From 42624362428df6253ce003b8fb1db6512c846016 Mon Sep 17 00:00:00 2001 From: Matt Hall Date: Mon, 6 Jul 2020 10:38:41 -0600 Subject: [PATCH 001/242] MoorDyn bug fix for tensions changing part-way through simulation. - Tension amplitudes were sometimes changing, typically at 512 s. - This was likely caused by use of single-precision time variable with MD_Input_ExtrapInterp. - All time variables are now double precision, solving the problem. --- modules/moordyn/src/MoorDyn.f90 | 33 +++++++++++++++------------------ 1 file changed, 15 insertions(+), 18 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index ca5684e585..b453ab6a4e 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -58,7 +58,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None ! local variables - REAL(ReKi) :: t ! instantaneous time, to be used during IC generation + REAL(DbKi) :: t ! instantaneous time, to be used during IC generation INTEGER(IntKi) :: I ! index INTEGER(IntKi) :: J ! index INTEGER(IntKi) :: K ! index @@ -391,7 +391,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er END DO END DO - t = 0.0_ReKi ! start time at zero + t = 0.0_DbKi ! start time at zero ! because TimeStep wants an array... call MD_CopyInput( u, uArray(1), MESH_NEWCOPY, ErrStat2, ErrMsg2 ) @@ -518,12 +518,12 @@ SUBROUTINE MD_UpdateStates( t, n, u, utimes, p, x, xd, z, other, m, ErrStat, Err ! moved to TimeStep TYPE(MD_InputType) :: u_interp ! INTEGER(IntKi) :: nTime - REAL(ReKi) :: t2 ! should work out a consistent data type for time... + REAL(DbKi) :: t2 ! copy of time passed to TimeStep nTime = size(u) ! the number of times of input data provided? - t2 = real(t, ReKi) + t2 = t ! >>> removing this section and putting it inside loop of TimeStep (to be done at every time step) <<< ! ! create space for arrays/meshes in u_interp @@ -604,7 +604,6 @@ SUBROUTINE MD_CalcOutput( t, u, p, x, xd, z, other, y, m, ErrStat, ErrMsg ) TYPE(MD_ContinuousStateType) :: dxdt ! time derivatives of continuous states (initialized in CalcContStateDeriv) INTEGER(IntKi) :: I ! counter INTEGER(IntKi) :: J ! counter - REAL(ReKi) :: t2 ! real version of t (double) INTEGER(IntKi) :: ErrStat2 ! Error status of the operation CHARACTER(ErrMsgLen) :: ErrMsg2 ! Error message if ErrStat2 /= ErrID_None @@ -612,8 +611,6 @@ SUBROUTINE MD_CalcOutput( t, u, p, x, xd, z, other, y, m, ErrStat, ErrMsg ) ! below updated to make sure outputs are current (based on provided x and u) - similar to what's in UpdateStates - t2 = real(t, ReKi) - ! go through fairleads and apply motions from driver DO I = 1, p%NFairs DO J = 1,3 @@ -623,7 +620,7 @@ SUBROUTINE MD_CalcOutput( t, u, p, x, xd, z, other, y, m, ErrStat, ErrMsg ) END DO ! call CalcContStateDeriv in order to run model and calculate dynamics with provided x and u - CALL MD_CalcContStateDeriv( t2, u, p, x, xd, z, other, m, dxdt, ErrStat, ErrMsg ) + CALL MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, ErrMsg ) ! assign net force on fairlead Connects to the output mesh @@ -635,7 +632,7 @@ SUBROUTINE MD_CalcOutput( t, u, p, x, xd, z, other, y, m, ErrStat, ErrMsg ) ! calculate outputs (y%WriteOutput) for glue code and write any m outputs to MoorDyn output files - CALL MDIO_WriteOutputs(REAL(t,DbKi) , p, m, y, ErrStat2, ErrMsg2) + CALL MDIO_WriteOutputs(t, p, m, y, ErrStat2, ErrMsg2) CALL CheckError(ErrStat2, 'In MDIO_WriteOutputs: '//trim(ErrMsg2)) IF ( ErrStat >= AbortErrLev ) RETURN @@ -680,7 +677,7 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er ! Tight coupling routine for computing derivatives of continuous states ! this is modelled off what used to be subroutine DoRHSmaster - REAL(ReKi), INTENT(IN ) :: t ! Current simulation time in seconds + REAL(DbKi), INTENT(IN ) :: t ! Current simulation time in seconds TYPE(MD_InputType), INTENT(IN ) :: u ! Inputs at t TYPE(MD_ParameterType), INTENT(IN ) :: p ! Parameters TYPE(MD_ContinuousStateType), INTENT(IN ) :: x ! Continuous states at t @@ -774,7 +771,7 @@ SUBROUTINE DoLineRHS (X, Xd, t, Line, LineProp, FairFtot, FairMtot, AnchFtot, An Real(ReKi), INTENT( IN ) :: X(:) ! state vector, provided Real(ReKi), INTENT( INOUT ) :: Xd(:) ! derivative of state vector, returned ! cahnged to INOUT - Real(ReKi), INTENT (IN) :: t ! instantaneous time + Real(DbKi), INTENT (IN) :: t ! instantaneous time TYPE(MD_Line), INTENT (INOUT) :: Line ! label for the current line, for convenience TYPE(MD_LineProp), INTENT(IN) :: LineProp ! the single line property set for the line of interest Real(ReKi), INTENT(INOUT) :: FairFtot(:) ! total force on Connect top of line is attached to @@ -1025,7 +1022,7 @@ SUBROUTINE DoConnectRHS (X, Xd, t, Connect) Real(ReKi), INTENT( IN ) :: X(:) ! state vector for this connect, provided Real(ReKi), INTENT( OUT ) :: Xd(:) ! derivative of state vector for this connect, returned - Real(ReKi), INTENT (IN) :: t ! instantaneous time + Real(DbKi), INTENT (IN) :: t ! instantaneous time Type(MD_Connect), INTENT (INOUT) :: Connect ! Connect number @@ -1040,7 +1037,7 @@ SUBROUTINE DoConnectRHS (X, Xd, t, Connect) ! itself, which will be added below. - IF (EqualRealNos(t, 0.0_ReKi)) THEN ! this is old: with current IC gen approach, we skip the first call to the line objects, because they're set AFTER the call to the connects + IF (EqualRealNos(t, 0.0_DbKi)) THEN ! this is old: with current IC gen approach, we skip the first call to the line objects, because they're set AFTER the call to the connects DO J = 1,3 Xd(3+J) = X(J) ! velocities - these are unused in integration @@ -1179,7 +1176,7 @@ END SUBROUTINE MD_End !======================================================================================================== SUBROUTINE TimeStep ( t, dtStep, u, utimes, p, x, xd, z, other, m, ErrStat, ErrMsg ) - REAL(ReKi) , INTENT(INOUT) :: t + REAL(DbKi) , INTENT(INOUT) :: t REAL(ReKi) , INTENT(IN ) :: dtStep ! how long to advance the time for TYPE( MD_InputType ) , INTENT(INOUT) :: u(:) ! INTENT(IN ) REAL(DbKi) , INTENT(IN ) :: utimes(:) ! times corresponding to elements of u(:)? @@ -1196,7 +1193,7 @@ SUBROUTINE TimeStep ( t, dtStep, u, utimes, p, x, xd, z, other, m, ErrStat, ErrM TYPE(MD_ContinuousStateType) :: dxdt ! time derivatives of continuous states (initialized in CalcContStateDeriv) TYPE(MD_ContinuousStateType) :: x2 ! temporary copy of continuous states used in RK2 calculations INTEGER(IntKi) :: NdtM ! the number of time steps to make with the mooring model - Real(ReKi) :: dtM ! the actual time step size to use + Real(DbKi) :: dtM ! the actual time step size to use INTEGER(IntKi) :: Nx ! size of states vector INTEGER(IntKi) :: I ! counter INTEGER(IntKi) :: J ! counter @@ -1224,7 +1221,7 @@ SUBROUTINE TimeStep ( t, dtStep, u, utimes, p, x, xd, z, other, m, ErrStat, ErrM DO I = 1, NdtM ! for (double ts=t; ts<=t+ICdt-dts; ts+=dts) - tDbKi = t ! get DbKi version of current time (why does ExtrapInterp except different time type than UpdateStates?) + !tDbKi = t ! get DbKi version of current time (why does ExtrapInterp except different time type than UpdateStates?) ! ------------------------------------------------------------------------------- @@ -1233,7 +1230,7 @@ SUBROUTINE TimeStep ( t, dtStep, u, utimes, p, x, xd, z, other, m, ErrStat, ErrM ! step 1 - CALL MD_Input_ExtrapInterp(u, utimes, u_interp, tDbKi , ErrStat, ErrMsg) ! interpolate input mesh to correct time (t) + CALL MD_Input_ExtrapInterp(u, utimes, u_interp, t , ErrStat, ErrMsg) ! interpolate input mesh to correct time (t) CALL MD_CalcContStateDeriv( t, u_interp, p, x, xd, z, other, m, dxdt, ErrStat, ErrMsg ) DO J = 1, Nx @@ -1242,7 +1239,7 @@ SUBROUTINE TimeStep ( t, dtStep, u, utimes, p, x, xd, z, other, m, ErrStat, ErrM ! step 2 - CALL MD_Input_ExtrapInterp(u, utimes, u_interp, tDbKi + 0.5_ReKi*dtM, ErrStat, ErrMsg) ! interpolate input mesh to correct time (t+0.5*dtM) + CALL MD_Input_ExtrapInterp(u, utimes, u_interp, t + 0.5_DbKi*dtM, ErrStat, ErrMsg) ! interpolate input mesh to correct time (t+0.5*dtM) CALL MD_CalcContStateDeriv( (t + 0.5_ReKi*dtM), u_interp, p, x2, xd, z, other, m, dxdt, ErrStat, ErrMsg ) !called with updated states x2 and time = t + dt/2.0 DO J = 1, Nx From 7f604a1c65ed5815d4a89f1a5c2bee53b214e010 Mon Sep 17 00:00:00 2001 From: Matt Hall Date: Tue, 24 Nov 2020 09:05:35 -0700 Subject: [PATCH 002/242] Edits to Waves, HydroDyn, and glue code changes to support MoorDyn v2 changes and to hard-code a simple wave kinematics grid to pass this info to MoorDyn for buoyancy cans. --- modules/hydrodyn/src/HydroDyn.f90 | 14 + modules/hydrodyn/src/HydroDyn.txt | 6 + modules/hydrodyn/src/HydroDyn_Input.f90 | 22 +- modules/hydrodyn/src/HydroDyn_Types.f90 | 344 +++++++++++++++++++ modules/hydrodyn/src/Waves.f90 | 22 ++ modules/hydrodyn/src/Waves.txt | 2 + modules/hydrodyn/src/Waves_Types.f90 | 66 ++++ modules/openfast-library/src/FAST_Solver.f90 | 34 +- modules/openfast-library/src/FAST_Subs.f90 | 51 ++- 9 files changed, 531 insertions(+), 30 deletions(-) diff --git a/modules/hydrodyn/src/HydroDyn.f90 b/modules/hydrodyn/src/HydroDyn.f90 index d92038f7f7..25b0caebc0 100644 --- a/modules/hydrodyn/src/HydroDyn.f90 +++ b/modules/hydrodyn/src/HydroDyn.f90 @@ -1348,10 +1348,24 @@ SUBROUTINE HydroDyn_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, I ErrStat, ErrMsg ) END IF + !::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + !@mhall: Making sure wave info from the grid points are saved for output here... + ALLOCATE ( InitOut%WaveVel (InitLocal%Morison%NStepWave, 200, 3) ) + ALLOCATE ( InitOut%WaveAcc (InitLocal%Morison%NStepWave, 200, 3) ) + ALLOCATE ( InitOut%WaveDynP (InitLocal%Morison%NStepWave, 200) ) + ALLOCATE ( InitOut%WaveElev (InitLocal%Morison%NStepWave, 25) ) ! unlike the above, this array is just 5x5, for the surface. + ALLOCATE ( InitOut%WaveTime (InitLocal%Morison%NStepWave) ) + InitOut%WaveVel = InitLocal%Morison%WaveVel( :,InitLocal%Morison%NNodes+1:,:) + InitOut%WaveAcc = InitLocal%Morison%WaveAcc( :,InitLocal%Morison%NNodes+1:,:) + InitOut%WaveDynP = InitLocal%Morison%WaveDynP(:,InitLocal%Morison%NNodes+1:) + InitOut%WaveElev = Waves_InitOut%WaveElevMD ! unlike the above, this array is just 5x5, for the surface. + InitOut%WaveTime = InitLocal%Morison%WaveTime(:) + !::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + ! Check the output switch to see if Morison is needing to send outputs back to HydroDyn via the WriteOutput array IF ( InitLocal%OutSwtch > 0 ) THEN diff --git a/modules/hydrodyn/src/HydroDyn.txt b/modules/hydrodyn/src/HydroDyn.txt index b8a4de1287..fca997a6c2 100644 --- a/modules/hydrodyn/src/HydroDyn.txt +++ b/modules/hydrodyn/src/HydroDyn.txt @@ -93,6 +93,12 @@ typedef ^ ^ CHARACTER(L typedef ^ InitOutputType INTEGER DerivOrder_x {:} - - "Integer that tells FAST/MBC3 the maximum derivative order of continuous states used in linearization" - typedef ^ ^ LOGICAL IsLoad_u {:} - - "Flag that tells FAST if the inputs used in linearization are loads (for preconditioning matrix)" - +typedef ^ ^ ReKi WaveVel {:}{:}{:} - - "output for now just to pass to MoorDyn" - +typedef ^ ^ ReKi WaveAcc {:}{:}{:} - - "output for now just to pass to MoorDyn" - +typedef ^ ^ ReKi WaveDynP {:}{:} - - "output for now just to pass to MoorDyn" - +typedef ^ ^ ReKi WaveElev {:}{:} - - "output for now just to pass to MoorDyn" - +typedef ^ ^ ReKi WaveTime {:} - - "output for now just to pass to MoorDyn" - + # ..... HD_ModuleMapType .................................................................................................................... typedef ^ HD_ModuleMapType MeshMapType uW_P_2_PRP_P - - - "Mesh mapping data: WAMIT body kinematics to PRP node at (0,0,0)" - diff --git a/modules/hydrodyn/src/HydroDyn_Input.f90 b/modules/hydrodyn/src/HydroDyn_Input.f90 index 120fe91630..0885111a8a 100644 --- a/modules/hydrodyn/src/HydroDyn_Input.f90 +++ b/modules/hydrodyn/src/HydroDyn_Input.f90 @@ -2383,6 +2383,7 @@ SUBROUTINE HydroDynInput_ProcessInitData( InitInp, ErrStat, ErrMsg ) INTEGER :: I ! Generic loop counter index INTEGER :: J ! Generic loop counter index INTEGER :: K ! Generic loop counter index + INTEGER :: Itemp ! @mhall: additional temporary index CHARACTER(1024) :: TmpPath ! Temporary storage for relative path name LOGICAL :: FoundID ! Boolean flag indicating whether an ID from one tables is found in one of the other input table REAL(ReKi) :: MinDepth ! The minimum depth entry in the Depth-based Hydrodynamic coefficents table @@ -4239,9 +4240,10 @@ SUBROUTINE HydroDynInput_ProcessInitData( InitInp, ErrStat, ErrMsg ) IF ( ErrStat >= AbortErrLev ) RETURN ! Set the number and global Z locations for the X and Y components of the current velocities - InitInp%Current%NMorisonNodes = InitInp%Morison%NNodes + ! @mhall: hard-coding an extra 200 points to make a water kinematics grid + InitInp%Current%NMorisonNodes = InitInp%Morison%NNodes + 200 - ALLOCATE ( InitInp%Current%MorisonNodezi(InitInp%Morison%NNodes), STAT = ErrStat2 ) + ALLOCATE ( InitInp%Current%MorisonNodezi(InitInp%Current%NMorisonNodes), STAT = ErrStat2 ) IF ( ErrStat2 /= ErrID_None ) THEN CALL SetErrStat( ErrID_Fatal,'Error allocating space for MorisonNodezi array.',ErrStat,ErrMsg,RoutineName) RETURN @@ -4250,7 +4252,8 @@ SUBROUTINE HydroDynInput_ProcessInitData( InitInp, ErrStat, ErrMsg ) ! Establish the number and locations where the wave kinematics will be computed - InitInp%Waves%NWaveKin = InitInp%Morison%NNodes ! Number of points where the incident wave kinematics will be computed (-) + ! @mhall: hard-coding an extra 200 points to make a water kinematics grid + InitInp%Waves%NWaveKin = InitInp%Morison%NNodes + 200 ! Number of points where the incident wave kinematics will be computed (-) ALLOCATE ( InitInp%Waves%WaveKinxi(InitInp%Waves%NWaveKin), STAT = ErrStat2 ) IF ( ErrStat2 /= ErrID_None ) THEN CALL SetErrStat( ErrID_Fatal,'Error allocating space for WaveKinxi array.',ErrStat,ErrMsg,RoutineName) @@ -4273,7 +4276,18 @@ SUBROUTINE HydroDynInput_ProcessInitData( InitInp, ErrStat, ErrMsg ) InitInp%Waves%WaveKinzi(I) = InitInp%Morison%Nodes(I)%Position(3) ! zi-coordinates for points where the incident wave kinematics will be computed; InitInp%Current%MorisonNodezi(I) = InitInp%Waves%WaveKinzi(I) END DO - + !@mhall: hard-coding the coordinates of those additional nodes for the grid (remember, must be in increasing order) + DO I=1,8 !z + DO J = 1,5 !y + DO K = 1,5 !x + Itemp = InitInp%Morison%NNodes + (I-1)*25.0 + (J-1)*5.0 + K ! index of actual node + InitInp%Waves%WaveKinzi(Itemp) = 1.0 - 2.0**(8-I) ! -127, -63, -31, -15, -7, -3, -1, 0 + InitInp%Waves%WaveKinyi(Itemp) = -60.0 + 20.0*J + InitInp%Waves%WaveKinxi(Itemp) = -60.0 + 20.0*K + InitInp%Current%MorisonNodezi(Itemp)= InitInp%Waves%WaveKinzi(I) + END DO + END DO + END DO ! If we are using the Waves module, the node information must be copied over. InitInp%Waves2%NWaveKin = InitInp%Waves%NWaveKin ! Number of points where the incident wave kinematics will be computed (-) diff --git a/modules/hydrodyn/src/HydroDyn_Types.f90 b/modules/hydrodyn/src/HydroDyn_Types.f90 index 7dc58021ba..c88ac495a6 100644 --- a/modules/hydrodyn/src/HydroDyn_Types.f90 +++ b/modules/hydrodyn/src/HydroDyn_Types.f90 @@ -112,6 +112,11 @@ MODULE HydroDyn_Types CHARACTER(LinChanLen) , DIMENSION(:), ALLOCATABLE :: LinNames_u !< Names of the inputs used in linearization [-] INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: DerivOrder_x !< Integer that tells FAST/MBC3 the maximum derivative order of continuous states used in linearization [-] LOGICAL , DIMENSION(:), ALLOCATABLE :: IsLoad_u !< Flag that tells FAST if the inputs used in linearization are loads (for preconditioning matrix) [-] + REAL(ReKi) , DIMENSION(:,:,:), ALLOCATABLE :: WaveVel !< output for now just to pass to MoorDyn [-] + REAL(ReKi) , DIMENSION(:,:,:), ALLOCATABLE :: WaveAcc !< output for now just to pass to MoorDyn [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: WaveDynP !< output for now just to pass to MoorDyn [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: WaveElev !< output for now just to pass to MoorDyn [-] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: WaveTime !< output for now just to pass to MoorDyn [-] END TYPE HydroDyn_InitOutputType ! ======================= ! ========= HD_ModuleMapType ======= @@ -2025,6 +2030,7 @@ SUBROUTINE HydroDyn_CopyInitOutput( SrcInitOutputData, DstInitOutputData, CtrlCo INTEGER(IntKi) :: i,j,k INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'HydroDyn_CopyInitOutput' @@ -2172,6 +2178,78 @@ SUBROUTINE HydroDyn_CopyInitOutput( SrcInitOutputData, DstInitOutputData, CtrlCo END IF END IF DstInitOutputData%IsLoad_u = SrcInitOutputData%IsLoad_u +ENDIF +IF (ALLOCATED(SrcInitOutputData%WaveVel)) THEN + i1_l = LBOUND(SrcInitOutputData%WaveVel,1) + i1_u = UBOUND(SrcInitOutputData%WaveVel,1) + i2_l = LBOUND(SrcInitOutputData%WaveVel,2) + i2_u = UBOUND(SrcInitOutputData%WaveVel,2) + i3_l = LBOUND(SrcInitOutputData%WaveVel,3) + i3_u = UBOUND(SrcInitOutputData%WaveVel,3) + IF (.NOT. ALLOCATED(DstInitOutputData%WaveVel)) THEN + ALLOCATE(DstInitOutputData%WaveVel(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitOutputData%WaveVel.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitOutputData%WaveVel = SrcInitOutputData%WaveVel +ENDIF +IF (ALLOCATED(SrcInitOutputData%WaveAcc)) THEN + i1_l = LBOUND(SrcInitOutputData%WaveAcc,1) + i1_u = UBOUND(SrcInitOutputData%WaveAcc,1) + i2_l = LBOUND(SrcInitOutputData%WaveAcc,2) + i2_u = UBOUND(SrcInitOutputData%WaveAcc,2) + i3_l = LBOUND(SrcInitOutputData%WaveAcc,3) + i3_u = UBOUND(SrcInitOutputData%WaveAcc,3) + IF (.NOT. ALLOCATED(DstInitOutputData%WaveAcc)) THEN + ALLOCATE(DstInitOutputData%WaveAcc(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitOutputData%WaveAcc.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitOutputData%WaveAcc = SrcInitOutputData%WaveAcc +ENDIF +IF (ALLOCATED(SrcInitOutputData%WaveDynP)) THEN + i1_l = LBOUND(SrcInitOutputData%WaveDynP,1) + i1_u = UBOUND(SrcInitOutputData%WaveDynP,1) + i2_l = LBOUND(SrcInitOutputData%WaveDynP,2) + i2_u = UBOUND(SrcInitOutputData%WaveDynP,2) + IF (.NOT. ALLOCATED(DstInitOutputData%WaveDynP)) THEN + ALLOCATE(DstInitOutputData%WaveDynP(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitOutputData%WaveDynP.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitOutputData%WaveDynP = SrcInitOutputData%WaveDynP +ENDIF +IF (ALLOCATED(SrcInitOutputData%WaveElev)) THEN + i1_l = LBOUND(SrcInitOutputData%WaveElev,1) + i1_u = UBOUND(SrcInitOutputData%WaveElev,1) + i2_l = LBOUND(SrcInitOutputData%WaveElev,2) + i2_u = UBOUND(SrcInitOutputData%WaveElev,2) + IF (.NOT. ALLOCATED(DstInitOutputData%WaveElev)) THEN + ALLOCATE(DstInitOutputData%WaveElev(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitOutputData%WaveElev.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitOutputData%WaveElev = SrcInitOutputData%WaveElev +ENDIF +IF (ALLOCATED(SrcInitOutputData%WaveTime)) THEN + i1_l = LBOUND(SrcInitOutputData%WaveTime,1) + i1_u = UBOUND(SrcInitOutputData%WaveTime,1) + IF (.NOT. ALLOCATED(DstInitOutputData%WaveTime)) THEN + ALLOCATE(DstInitOutputData%WaveTime(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitOutputData%WaveTime.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitOutputData%WaveTime = SrcInitOutputData%WaveTime ENDIF END SUBROUTINE HydroDyn_CopyInitOutput @@ -2222,6 +2300,21 @@ SUBROUTINE HydroDyn_DestroyInitOutput( InitOutputData, ErrStat, ErrMsg ) ENDIF IF (ALLOCATED(InitOutputData%IsLoad_u)) THEN DEALLOCATE(InitOutputData%IsLoad_u) +ENDIF +IF (ALLOCATED(InitOutputData%WaveVel)) THEN + DEALLOCATE(InitOutputData%WaveVel) +ENDIF +IF (ALLOCATED(InitOutputData%WaveAcc)) THEN + DEALLOCATE(InitOutputData%WaveAcc) +ENDIF +IF (ALLOCATED(InitOutputData%WaveDynP)) THEN + DEALLOCATE(InitOutputData%WaveDynP) +ENDIF +IF (ALLOCATED(InitOutputData%WaveElev)) THEN + DEALLOCATE(InitOutputData%WaveElev) +ENDIF +IF (ALLOCATED(InitOutputData%WaveTime)) THEN + DEALLOCATE(InitOutputData%WaveTime) ENDIF END SUBROUTINE HydroDyn_DestroyInitOutput @@ -2401,6 +2494,31 @@ SUBROUTINE HydroDyn_PackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, Int_BufSz = Int_BufSz + 2*1 ! IsLoad_u upper/lower bounds for each dimension Int_BufSz = Int_BufSz + SIZE(InData%IsLoad_u) ! IsLoad_u END IF + Int_BufSz = Int_BufSz + 1 ! WaveVel allocated yes/no + IF ( ALLOCATED(InData%WaveVel) ) THEN + Int_BufSz = Int_BufSz + 2*3 ! WaveVel upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%WaveVel) ! WaveVel + END IF + Int_BufSz = Int_BufSz + 1 ! WaveAcc allocated yes/no + IF ( ALLOCATED(InData%WaveAcc) ) THEN + Int_BufSz = Int_BufSz + 2*3 ! WaveAcc upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%WaveAcc) ! WaveAcc + END IF + Int_BufSz = Int_BufSz + 1 ! WaveDynP allocated yes/no + IF ( ALLOCATED(InData%WaveDynP) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! WaveDynP upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%WaveDynP) ! WaveDynP + END IF + Int_BufSz = Int_BufSz + 1 ! WaveElev allocated yes/no + IF ( ALLOCATED(InData%WaveElev) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! WaveElev upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%WaveElev) ! WaveElev + END IF + Int_BufSz = Int_BufSz + 1 ! WaveTime allocated yes/no + IF ( ALLOCATED(InData%WaveTime) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! WaveTime upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%WaveTime) ! WaveTime + END IF IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) IF (ErrStat2 /= 0) THEN @@ -2735,6 +2853,111 @@ SUBROUTINE HydroDyn_PackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, Int_Xferred = Int_Xferred + 1 END DO END IF + IF ( .NOT. ALLOCATED(InData%WaveVel) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveVel,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveVel,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveVel,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveVel,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveVel,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveVel,3) + Int_Xferred = Int_Xferred + 2 + + DO i3 = LBOUND(InData%WaveVel,3), UBOUND(InData%WaveVel,3) + DO i2 = LBOUND(InData%WaveVel,2), UBOUND(InData%WaveVel,2) + DO i1 = LBOUND(InData%WaveVel,1), UBOUND(InData%WaveVel,1) + ReKiBuf(Re_Xferred) = InData%WaveVel(i1,i2,i3) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%WaveAcc) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveAcc,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveAcc,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveAcc,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveAcc,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveAcc,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveAcc,3) + Int_Xferred = Int_Xferred + 2 + + DO i3 = LBOUND(InData%WaveAcc,3), UBOUND(InData%WaveAcc,3) + DO i2 = LBOUND(InData%WaveAcc,2), UBOUND(InData%WaveAcc,2) + DO i1 = LBOUND(InData%WaveAcc,1), UBOUND(InData%WaveAcc,1) + ReKiBuf(Re_Xferred) = InData%WaveAcc(i1,i2,i3) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%WaveDynP) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveDynP,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveDynP,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveDynP,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveDynP,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%WaveDynP,2), UBOUND(InData%WaveDynP,2) + DO i1 = LBOUND(InData%WaveDynP,1), UBOUND(InData%WaveDynP,1) + ReKiBuf(Re_Xferred) = InData%WaveDynP(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%WaveElev) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveElev,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveElev,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveElev,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveElev,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%WaveElev,2), UBOUND(InData%WaveElev,2) + DO i1 = LBOUND(InData%WaveElev,1), UBOUND(InData%WaveElev,1) + ReKiBuf(Re_Xferred) = InData%WaveElev(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%WaveTime) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveTime,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveTime,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%WaveTime,1), UBOUND(InData%WaveTime,1) + ReKiBuf(Re_Xferred) = InData%WaveTime(i1) + Re_Xferred = Re_Xferred + 1 + END DO + END IF END SUBROUTINE HydroDyn_PackInitOutput SUBROUTINE HydroDyn_UnPackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) @@ -2752,6 +2975,7 @@ SUBROUTINE HydroDyn_UnPackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrSt INTEGER(IntKi) :: i INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'HydroDyn_UnPackInitOutput' @@ -3162,6 +3386,126 @@ SUBROUTINE HydroDyn_UnPackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrSt Int_Xferred = Int_Xferred + 1 END DO END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WaveVel not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%WaveVel)) DEALLOCATE(OutData%WaveVel) + ALLOCATE(OutData%WaveVel(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%WaveVel.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i3 = LBOUND(OutData%WaveVel,3), UBOUND(OutData%WaveVel,3) + DO i2 = LBOUND(OutData%WaveVel,2), UBOUND(OutData%WaveVel,2) + DO i1 = LBOUND(OutData%WaveVel,1), UBOUND(OutData%WaveVel,1) + OutData%WaveVel(i1,i2,i3) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WaveAcc not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%WaveAcc)) DEALLOCATE(OutData%WaveAcc) + ALLOCATE(OutData%WaveAcc(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%WaveAcc.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i3 = LBOUND(OutData%WaveAcc,3), UBOUND(OutData%WaveAcc,3) + DO i2 = LBOUND(OutData%WaveAcc,2), UBOUND(OutData%WaveAcc,2) + DO i1 = LBOUND(OutData%WaveAcc,1), UBOUND(OutData%WaveAcc,1) + OutData%WaveAcc(i1,i2,i3) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WaveDynP not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%WaveDynP)) DEALLOCATE(OutData%WaveDynP) + ALLOCATE(OutData%WaveDynP(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%WaveDynP.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%WaveDynP,2), UBOUND(OutData%WaveDynP,2) + DO i1 = LBOUND(OutData%WaveDynP,1), UBOUND(OutData%WaveDynP,1) + OutData%WaveDynP(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WaveElev not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%WaveElev)) DEALLOCATE(OutData%WaveElev) + ALLOCATE(OutData%WaveElev(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%WaveElev.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%WaveElev,2), UBOUND(OutData%WaveElev,2) + DO i1 = LBOUND(OutData%WaveElev,1), UBOUND(OutData%WaveElev,1) + OutData%WaveElev(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WaveTime not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%WaveTime)) DEALLOCATE(OutData%WaveTime) + ALLOCATE(OutData%WaveTime(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%WaveTime.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%WaveTime,1), UBOUND(OutData%WaveTime,1) + OutData%WaveTime(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END IF END SUBROUTINE HydroDyn_UnPackInitOutput SUBROUTINE HydroDyn_CopyHD_ModuleMapType( SrcHD_ModuleMapTypeData, DstHD_ModuleMapTypeData, CtrlCode, ErrStat, ErrMsg ) diff --git a/modules/hydrodyn/src/Waves.f90 b/modules/hydrodyn/src/Waves.f90 index 127a882976..4054ccaa8f 100644 --- a/modules/hydrodyn/src/Waves.f90 +++ b/modules/hydrodyn/src/Waves.f90 @@ -1781,6 +1781,28 @@ SUBROUTINE VariousWaves_Init ( InitInp, InitOut, ErrStat, ErrMsg ) END IF END DO ! J - All points where the incident wave elevations can be output + ! :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + !@mhall: hard-coding some additional wave elevation time series output for now + + ALLOCATE ( InitOut%WaveElevMD (0:InitOut%NStepWave, 25), STAT=ErrStatTmp ) + IF (ErrStatTmp /= 0) CALL SetErrStat(ErrID_Fatal,'Cannot allocate array InitOut%WaveElevMD.', ErrStat,ErrMsg,'VariousWaves_Init') + + DO J = 1,5 !y = -60.0 + 20.0*J + DO K = 1,5 !x = -60.0 + 20.0*K + + I = (J-1)*5.0 + K ! index of actual node + + CALL WaveElevTimeSeriesAtXY( -60.0 + 20.0*K, -60.0 + 20.0*J, InitOut%WaveElevMD(:,I), ErrStatTmp, ErrMsgTmp ) + CALL SetErrStat(ErrStatTmp,'Error occured while applying the FFT to InitOut%WaveElevMD.',ErrStat,ErrMsg,'VariousWaves_Init') + IF ( ErrStat >= AbortErrLev ) THEN + CALL CleanUp() + RETURN + END IF + END DO + END DO + + ! :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + ! For creating animations of the sea surface, the WaveElevXY array is passed in with a series of x,y coordinates ! (index 1). The second index corresponds to the number of points passed in. A two dimensional time series diff --git a/modules/hydrodyn/src/Waves.txt b/modules/hydrodyn/src/Waves.txt index 366067469c..d5521beb0c 100644 --- a/modules/hydrodyn/src/Waves.txt +++ b/modules/hydrodyn/src/Waves.txt @@ -82,6 +82,8 @@ typedef ^ ^ SiKi PWaveVel0 typedef ^ ^ SiKi WaveElev {:}{:} - - "Instantaneous elevation time-series of incident waves at each of the NWaveElev points where the incident wave elevations can be output" (meters) typedef ^ ^ SiKi WaveElev0 {:} - - "Instantaneous elevation time-series of incident waves at the platform reference point" (meters) +typedef ^ ^ SiKi WaveElevMD {:}{:} - - "Instantaneous elevation time-series of incident waves at hard coded grid for temporary use in MoorDyn" (m) + typedef ^ ^ SiKi WaveElevSeries {:}{:} - - "Instantaneous elevation time-series at each of the points given by WaveElevXY. Used for making movies of the waves. First index is the timestep. Second index is XY point number corresponding to second index of WaveElevXY." (m) typedef ^ ^ SiKi WaveTime {:} - - "Simulation times at which the instantaneous elevation of, velocity of, acceleration of, and loads associated with the incident waves are determined" (sec) diff --git a/modules/hydrodyn/src/Waves_Types.f90 b/modules/hydrodyn/src/Waves_Types.f90 index 9b98fdc949..4a51f3c7c0 100644 --- a/modules/hydrodyn/src/Waves_Types.f90 +++ b/modules/hydrodyn/src/Waves_Types.f90 @@ -98,6 +98,7 @@ MODULE Waves_Types REAL(SiKi) , DIMENSION(:,:,:), ALLOCATABLE :: PWaveVel0 !< Instantaneous velocity of incident waves in the xi- (1), yi- (2), and zi- (3) directions, respectively, at the location (xi,yi,0), at each of the NWaveKin points where the incident wave kinematics will be computed (The values include both the velocity of incident waves and the velocity of current.) [(m/s)] REAL(SiKi) , DIMENSION(:,:), ALLOCATABLE :: WaveElev !< Instantaneous elevation time-series of incident waves at each of the NWaveElev points where the incident wave elevations can be output [(meters)] REAL(SiKi) , DIMENSION(:), ALLOCATABLE :: WaveElev0 !< Instantaneous elevation time-series of incident waves at the platform reference point [(meters)] + REAL(SiKi) , DIMENSION(:,:), ALLOCATABLE :: WaveElevMD !< Instantaneous elevation time-series of incident waves at hard coded grid for temporary use in MoorDyn [(m)] REAL(SiKi) , DIMENSION(:,:), ALLOCATABLE :: WaveElevSeries !< Instantaneous elevation time-series at each of the points given by WaveElevXY. Used for making movies of the waves. First index is the timestep. Second index is XY point number corresponding to second index of WaveElevXY. [(m)] REAL(SiKi) , DIMENSION(:), ALLOCATABLE :: WaveTime !< Simulation times at which the instantaneous elevation of, velocity of, acceleration of, and loads associated with the incident waves are determined [(sec)] REAL(DbKi) :: WaveTMax !< Analysis time for incident wave calculations; the actual analysis time may be larger than this value in order for the maintain an effecient FFT [(sec)] @@ -1206,6 +1207,20 @@ SUBROUTINE Waves_CopyInitOutput( SrcInitOutputData, DstInitOutputData, CtrlCode, END IF DstInitOutputData%WaveElev0 = SrcInitOutputData%WaveElev0 ENDIF +IF (ALLOCATED(SrcInitOutputData%WaveElevMD)) THEN + i1_l = LBOUND(SrcInitOutputData%WaveElevMD,1) + i1_u = UBOUND(SrcInitOutputData%WaveElevMD,1) + i2_l = LBOUND(SrcInitOutputData%WaveElevMD,2) + i2_u = UBOUND(SrcInitOutputData%WaveElevMD,2) + IF (.NOT. ALLOCATED(DstInitOutputData%WaveElevMD)) THEN + ALLOCATE(DstInitOutputData%WaveElevMD(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitOutputData%WaveElevMD.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitOutputData%WaveElevMD = SrcInitOutputData%WaveElevMD +ENDIF IF (ALLOCATED(SrcInitOutputData%WaveElevSeries)) THEN i1_l = LBOUND(SrcInitOutputData%WaveElevSeries,1) i1_u = UBOUND(SrcInitOutputData%WaveElevSeries,1) @@ -1294,6 +1309,9 @@ SUBROUTINE Waves_DestroyInitOutput( InitOutputData, ErrStat, ErrMsg ) IF (ALLOCATED(InitOutputData%WaveElev0)) THEN DEALLOCATE(InitOutputData%WaveElev0) ENDIF +IF (ALLOCATED(InitOutputData%WaveElevMD)) THEN + DEALLOCATE(InitOutputData%WaveElevMD) +ENDIF IF (ALLOCATED(InitOutputData%WaveElevSeries)) THEN DEALLOCATE(InitOutputData%WaveElevSeries) ENDIF @@ -1401,6 +1419,11 @@ SUBROUTINE Waves_PackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, Er Int_BufSz = Int_BufSz + 2*1 ! WaveElev0 upper/lower bounds for each dimension Re_BufSz = Re_BufSz + SIZE(InData%WaveElev0) ! WaveElev0 END IF + Int_BufSz = Int_BufSz + 1 ! WaveElevMD allocated yes/no + IF ( ALLOCATED(InData%WaveElevMD) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! WaveElevMD upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%WaveElevMD) ! WaveElevMD + END IF Int_BufSz = Int_BufSz + 1 ! WaveElevSeries allocated yes/no IF ( ALLOCATED(InData%WaveElevSeries) ) THEN Int_BufSz = Int_BufSz + 2*2 ! WaveElevSeries upper/lower bounds for each dimension @@ -1684,6 +1707,26 @@ SUBROUTINE Waves_PackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, Er Re_Xferred = Re_Xferred + 1 END DO END IF + IF ( .NOT. ALLOCATED(InData%WaveElevMD) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveElevMD,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveElevMD,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveElevMD,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveElevMD,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%WaveElevMD,2), UBOUND(InData%WaveElevMD,2) + DO i1 = LBOUND(InData%WaveElevMD,1), UBOUND(InData%WaveElevMD,1) + ReKiBuf(Re_Xferred) = InData%WaveElevMD(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF IF ( .NOT. ALLOCATED(InData%WaveElevSeries) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -2048,6 +2091,29 @@ SUBROUTINE Waves_UnPackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Re_Xferred = Re_Xferred + 1 END DO END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WaveElevMD not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%WaveElevMD)) DEALLOCATE(OutData%WaveElevMD) + ALLOCATE(OutData%WaveElevMD(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%WaveElevMD.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%WaveElevMD,2), UBOUND(OutData%WaveElevMD,2) + DO i1 = LBOUND(OutData%WaveElevMD,1), UBOUND(OutData%WaveElevMD,1) + OutData%WaveElevMD(i1,i2) = REAL(ReKiBuf(Re_Xferred), SiKi) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WaveElevSeries not allocated Int_Xferred = Int_Xferred + 1 ELSE diff --git a/modules/openfast-library/src/FAST_Solver.f90 b/modules/openfast-library/src/FAST_Solver.f90 index 6a0423af5f..d88b02d26e 100644 --- a/modules/openfast-library/src/FAST_Solver.f90 +++ b/modules/openfast-library/src/FAST_Solver.f90 @@ -1123,8 +1123,8 @@ SUBROUTINE Transfer_ED_to_HD_SD_BD_Mooring( p_FAST, y_ED, u_HD, u_SD, u_ExtPtfm, ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN ! motions: - CALL Transfer_Point_to_Point( y_ED%PlatformPtMesh, u_MD%PtFairleadDisplacement, MeshMapData%ED_P_2_Mooring_P, ErrStat2, ErrMsg2 ) - CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat, ErrMsg,RoutineName//'u_MD%PtFairleadDisplacement' ) + CALL Transfer_Point_to_Point( y_ED%PlatformPtMesh, u_MD%CoupledKinematics, MeshMapData%ED_P_2_Mooring_P, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat, ErrMsg,RoutineName//'u_MD%CoupledKinematics' ) ELSEIF ( p_FAST%CompMooring == Module_FEAM ) THEN ! motions: @@ -1761,10 +1761,10 @@ SUBROUTINE U_ED_HD_Residual( y_ED2, y_HD2, u_IN, U_Resid) ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN ! note: MD_InputSolve must be called before setting ED loads inputs (so that motions are known for loads [moment] mapping) - CALL Transfer_Point_to_Point( y_ED2%PlatformPtMesh, u_MD%PtFairleadDisplacement, MeshMapData%ED_P_2_Mooring_P, ErrStat, ErrMsg ) + CALL Transfer_Point_to_Point( y_ED2%PlatformPtMesh, u_MD%CoupledKinematics, MeshMapData%ED_P_2_Mooring_P, ErrStat, ErrMsg ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - CALL Transfer_Point_to_Point( y_MD%PtFairleadLoad, MeshMapData%u_ED_PlatformPtMesh, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2, u_MD%PtFairleadDisplacement, PlatformMotions ) !u_MD and y_ED contain the displacements needed for moment calculations + CALL Transfer_Point_to_Point( y_MD%CoupledLoads, MeshMapData%u_ED_PlatformPtMesh, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2, u_MD%CoupledKinematics, PlatformMotions ) !u_MD and y_ED contain the displacements needed for moment calculations CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) ELSEIF ( p_FAST%CompMooring == Module_FEAM ) THEN @@ -2669,10 +2669,10 @@ SUBROUTINE U_FullOpt1_Residual( y_ED2, y_SD2, y_HD2, y_BD2, y_Orca2, y_ExtPtfm2, ! note: MD_InputSolve must be called before setting ED loads inputs (so that motions are known for loads [moment] mapping) if ( p_FAST%CompSub == Module_SD ) then - CALL Transfer_Point_to_Point( y_SD2%y2Mesh, u_MD%PtFairleadDisplacement, MeshMapData%SD_P_2_Mooring_P, ErrStat, ErrMsg ) + CALL Transfer_Point_to_Point( y_SD2%y2Mesh, u_MD%CoupledKinematics, MeshMapData%SD_P_2_Mooring_P, ErrStat, ErrMsg ) CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) else - CALL Transfer_Point_to_Point( y_ED2%PlatformPtMesh, u_MD%PtFairleadDisplacement, MeshMapData%ED_P_2_Mooring_P, ErrStat, ErrMsg ) + CALL Transfer_Point_to_Point( y_ED2%PlatformPtMesh, u_MD%CoupledKinematics, MeshMapData%ED_P_2_Mooring_P, ErrStat, ErrMsg ) CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) end if @@ -2929,13 +2929,13 @@ SUBROUTINE U_FullOpt1_Residual( y_ED2, y_SD2, y_HD2, y_BD2, y_Orca2, y_ExtPtfm2, ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN if ( p_FAST%CompSub == Module_SD ) then - CALL Transfer_Point_to_Point( y_MD%PtFairleadLoad, MeshMapData%u_SD_LMesh_2, MeshMapData%Mooring_P_2_SD_P, ErrStat2, ErrMsg2, u_MD%PtFairleadDisplacement, y_SD2%Y2Mesh ) !u_MD and y_SD contain the displacements needed for moment calculations + CALL Transfer_Point_to_Point( y_MD%CoupledLoads, MeshMapData%u_SD_LMesh_2, MeshMapData%Mooring_P_2_SD_P, ErrStat2, ErrMsg2, u_MD%CoupledKinematics, y_SD2%Y2Mesh ) !u_MD and y_SD contain the displacements needed for moment calculations CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) MeshMapData%u_SD_LMesh%Force = MeshMapData%u_SD_LMesh%Force + MeshMapData%u_SD_LMesh_2%Force MeshMapData%u_SD_LMesh%Moment = MeshMapData%u_SD_LMesh%Moment + MeshMapData%u_SD_LMesh_2%Moment else - CALL Transfer_Point_to_Point( y_MD%PtFairleadLoad, MeshMapData%u_ED_PlatformPtMesh_2, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2, u_MD%PtFairleadDisplacement, PlatformMotions ) !u_MD and y_ED contain the displacements needed for moment calculations + CALL Transfer_Point_to_Point( y_MD%CoupledLoads, MeshMapData%u_ED_PlatformPtMesh_2, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2, u_MD%CoupledKinematics, PlatformMotions ) !u_MD and y_ED contain the displacements needed for moment calculations CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) MeshMapData%u_ED_PlatformPtMesh%Force = MeshMapData%u_ED_PlatformPtMesh%Force + MeshMapData%u_ED_PlatformPtMesh_2%Force @@ -3848,8 +3848,8 @@ SUBROUTINE ResetRemapFlags(p_FAST, ED, BD, AD14, AD, HD, SD, ExtPtfm, SrvD, MAPp MAPp%Input(1)%PtFairDisplacement%RemapFlag = .FALSE. MAPp%y%PtFairleadLoad%RemapFlag = .FALSE. ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN - MD%Input(1)%PtFairleadDisplacement%RemapFlag = .FALSE. - MD%y%PtFairleadLoad%RemapFlag = .FALSE. + MD%Input(1)%CoupledKinematics%RemapFlag = .FALSE. + MD%y%CoupledLoads%RemapFlag = .FALSE. ELSEIF ( p_FAST%CompMooring == Module_FEAM ) THEN FEAM%Input(1)%PtFairleadDisplacement%RemapFlag = .FALSE. FEAM%y%PtFairleadLoad%RemapFlag = .FALSE. @@ -4246,18 +4246,18 @@ SUBROUTINE InitModuleMappings(p_FAST, ED, BD, AD14, AD, HD, SD, ExtPtfm, SrvD, M ! SubDyn <-> MoorDyn !------------------------- ! MoorDyn point mesh to/from SubDyn point mesh - CALL MeshMapCreate( MD%y%PtFairleadLoad, SD%Input(1)%LMesh, MeshMapData%Mooring_P_2_SD_P, ErrStat2, ErrMsg2 ) + CALL MeshMapCreate( MD%y%CoupledLoads, SD%Input(1)%LMesh, MeshMapData%Mooring_P_2_SD_P, ErrStat2, ErrMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':Mooring_P_2_SD_P' ) - CALL MeshMapCreate( SD%y%y2Mesh, MD%Input(1)%PtFairleadDisplacement, MeshMapData%SD_P_2_Mooring_P, ErrStat2, ErrMsg2 ) + CALL MeshMapCreate( SD%y%y2Mesh, MD%Input(1)%CoupledKinematics, MeshMapData%SD_P_2_Mooring_P, ErrStat2, ErrMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':SD_P_2_Mooring_P' ) ELSE !------------------------- ! ElastoDyn <-> MoorDyn !------------------------- ! MoorDyn point mesh to/from ElastoDyn point mesh - CALL MeshMapCreate( MD%y%PtFairleadLoad, PlatformLoads, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2 ) + CALL MeshMapCreate( MD%y%CoupledLoads, PlatformLoads, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':Mooring_P_2_Ptfm' ) - CALL MeshMapCreate( PlatformMotion, MD%Input(1)%PtFairleadDisplacement, MeshMapData%ED_P_2_Mooring_P, ErrStat2, ErrMsg2 ) + CALL MeshMapCreate( PlatformMotion, MD%Input(1)%CoupledKinematics, MeshMapData%ED_P_2_Mooring_P, ErrStat2, ErrMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':Ptfm_2_Mooring_P' ) END IF ! p_FAST%CompSub == Module_SD @@ -4567,7 +4567,7 @@ SUBROUTINE CalcOutputs_And_SolveForInputs( n_t_global, this_time, this_state, ca CALL Transfer_Point_to_Point( SD%y%y2Mesh, MAPp%Input(1)%PtFairDisplacement, MeshMapData%SD_P_2_Mooring_P, ErrStat, ErrMsg ) CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN - CALL Transfer_Point_to_Point( SD%y%y2Mesh, MD%Input(1)%PtFairleadDisplacement, MeshMapData%SD_P_2_Mooring_P, ErrStat, ErrMsg ) + CALL Transfer_Point_to_Point( SD%y%y2Mesh, MD%Input(1)%CoupledKinematics, MeshMapData%SD_P_2_Mooring_P, ErrStat, ErrMsg ) CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) ELSEIF ( p_FAST%CompMooring == Module_FEAM ) THEN CALL Transfer_Point_to_Point( SD%y%y2Mesh, FEAM%Input(1)%PtFairleadDisplacement, MeshMapData%SD_P_2_Mooring_P, ErrStat, ErrMsg ) @@ -4772,10 +4772,10 @@ SUBROUTINE SolveOption1(this_time, this_state, calcJacobian, p_FAST, ED, BD, HD, ! note: MD_InputSolve must be called before setting ED loads inputs (so that motions are known for loads [moment] mapping) if ( p_FAST%CompSub == Module_SD ) then - CALL Transfer_Point_to_Point( SD%y%y2Mesh, MD%Input(1)%PtFairleadDisplacement, MeshMapData%SD_P_2_Mooring_P, ErrStat, ErrMsg ) + CALL Transfer_Point_to_Point( SD%y%y2Mesh, MD%Input(1)%CoupledKinematics, MeshMapData%SD_P_2_Mooring_P, ErrStat, ErrMsg ) CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) else - CALL Transfer_Point_to_Point( ED%y%PlatformPtMesh, MD%Input(1)%PtFairleadDisplacement, MeshMapData%ED_P_2_Mooring_P, ErrStat, ErrMsg ) + CALL Transfer_Point_to_Point( ED%y%PlatformPtMesh, MD%Input(1)%CoupledKinematics, MeshMapData%ED_P_2_Mooring_P, ErrStat, ErrMsg ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) endif diff --git a/modules/openfast-library/src/FAST_Subs.f90 b/modules/openfast-library/src/FAST_Subs.f90 index 2b43243063..0d99862827 100644 --- a/modules/openfast-library/src/FAST_Subs.f90 +++ b/modules/openfast-library/src/FAST_Subs.f90 @@ -927,19 +927,29 @@ SUBROUTINE FAST_InitializeAll( t_initial, p_FAST, y_FAST, m_FAST, ED, BD, SrvD, CALL Cleanup() RETURN END IF - ALLOCATE( FEAM%Input( p_FAST%InterpOrder+1 ), FEAM%InputTimes( p_FAST%InterpOrder+1 ), STAT = ErrStat2 ) + !@mhall: clean this up! <<<<<<<<<<<<<<<< ::::::::::::::::::::::::::::: + if (allocated(FEAM%Input)) then + print *, "FEAM%Input is already allocated!!" + end if + if (allocated(FEAM%InputTimes)) then + print *, "FEAM%InputTimes is already allocated!!" + end if + IF (p_FAST%CompMooring == Module_FEAM) THEN + ALLOCATE( FEAM%Input( p_FAST%InterpOrder+1 ), FEAM%InputTimes( p_FAST%InterpOrder+1 ), STAT = ErrStat2 ) IF (ErrStat2 /= 0) THEN CALL SetErrStat(ErrID_Fatal,"Error allocating FEAM%Input and FEAM%InputTimes.",ErrStat,ErrMsg,RoutineName) CALL Cleanup() RETURN - END IF - ALLOCATE( Orca%Input( p_FAST%InterpOrder+1 ), Orca%InputTimes( p_FAST%InterpOrder+1 ), STAT = ErrStat2 ) + END IF + ELSE IF (p_FAST%CompMooring == Module_Orca) THEN + ALLOCATE( Orca%Input( p_FAST%InterpOrder+1 ), Orca%InputTimes( p_FAST%InterpOrder+1 ), STAT = ErrStat2 ) IF (ErrStat2 /= 0) THEN CALL SetErrStat(ErrID_Fatal,"Error allocating Orca%Input and Orca%InputTimes.",ErrStat,ErrMsg,RoutineName) CALL Cleanup() RETURN END IF - + END IF + ! ........................ ! initialize MAP ! ........................ @@ -996,7 +1006,30 @@ SUBROUTINE FAST_InitializeAll( t_initial, p_FAST, y_FAST, m_FAST, ED, BD, SrvD, Init%InData_MD%g = Init%OutData_ED%Gravity ! This need to be according to g used in ElastoDyn Init%InData_MD%rhoW = Init%OutData_HD%WtrDens ! This needs to be set according to seawater density in HydroDyn Init%InData_MD%WtrDepth = Init%OutData_HD%WtrDpth ! This need to be set according to the water depth in HydroDyn - + + !::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + !@mhall: for now, passing some hardcoded wave kinematics grid info from HD to MD + + ALLOCATE ( Init%InData_MD%WaveVel (HD%p%NStepWave, 200, 3) ) + ALLOCATE ( Init%InData_MD%WaveAcc (HD%p%NStepWave, 200, 3) ) + ALLOCATE ( Init%InData_MD%WavePDyn (HD%p%NStepWave, 200) ) + ALLOCATE ( Init%InData_MD%WaveElev (HD%p%NStepWave, 200) ) + ALLOCATE ( Init%InData_MD%WaveTime (HD%p%NStepWave) ) + + Init%InData_MD%WaveVel = Init%OutData_HD%WaveVel + Init%InData_MD%WaveAcc = Init%OutData_HD%WaveAcc + Init%InData_MD%WavePDyn = Init%OutData_HD%WaveDynP + Init%InData_MD%WaveElev = Init%OutData_HD%WaveElev + Init%InData_MD%WaveTime = Init%OutData_HD%WaveTime + + !CALL MOVE_ALLOC( Init%OutData_HD%WaveVel , Init%InData_MD%WaveVel ) + !CALL MOVE_ALLOC( Init%OutData_HD%WaveAcc , Init%InData_MD%WaveAcc ) + !CALL MOVE_ALLOC( Init%OutData_HD%WaveDynP , Init%InData_MD%WavePDyn ) + !CALL MOVE_ALLOC( Init%OutData_HD%WaveElev , Init%InData_MD%WaveElev ) + !CALL MOVE_ALLOC( Init%OutData_HD%WaveTime , Init%InData_MD%WaveTime ) + + !::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + CALL MD_Init( Init%InData_MD, MD%Input(1), MD%p, MD%x(STATE_CURR), MD%xd(STATE_CURR), MD%z(STATE_CURR), & MD%OtherSt(STATE_CURR), MD%y, MD%m, p_FAST%dt_module( MODULE_MD ), Init%OutData_MD, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) @@ -5204,8 +5237,8 @@ SUBROUTINE WrVTK_AllMeshes(p_FAST, y_FAST, MeshMapData, ED, BD, AD, IfW, OpFM, H ! MoorDyn ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN if (allocated(MD%Input)) then - call MeshWrVTK(p_FAST%TurbinePos, MD%y%PtFairleadLoad, trim(p_FAST%VTK_OutFileRoot)//'.MD_PtFairlead', y_FAST%VTK_count, p_FAST%VTK_fields, ErrStat2, ErrMsg2, p_FAST%VTK_tWidth, MD%Input(1)%PtFairleadDisplacement ) - !call MeshWrVTK(p_FAST%TurbinePos, MD%Input(1)%PtFairleadDisplacement, trim(p_FAST%VTK_OutFileRoot)//'.MD_PtFair_motion', y_FAST%VTK_count, p_FAST%VTK_fields, ErrStat2, ErrMsg2, p_FAST%VTK_tWidth ) + call MeshWrVTK(p_FAST%TurbinePos, MD%y%CoupledLoads, trim(p_FAST%VTK_OutFileRoot)//'.MD_PtFairlead', y_FAST%VTK_count, p_FAST%VTK_fields, ErrStat2, ErrMsg2, p_FAST%VTK_tWidth, MD%Input(1)%CoupledKinematics ) + !call MeshWrVTK(p_FAST%TurbinePos, MD%Input(1)%CoupledKinematics, trim(p_FAST%VTK_OutFileRoot)//'.MD_PtFair_motion', y_FAST%VTK_count, p_FAST%VTK_fields, ErrStat2, ErrMsg2, p_FAST%VTK_tWidth ) end if ! FEAMooring @@ -5338,7 +5371,7 @@ SUBROUTINE WrVTK_BasicMeshes(p_FAST, y_FAST, MeshMapData, ED, BD, AD, IfW, OpFM, ! IF ( p_FAST%CompMooring == Module_MAP ) THEN ! call MeshWrVTK(p_FAST%TurbinePos, MAPp%Input(1)%PtFairDisplacement, trim(p_FAST%VTK_OutFileRoot)//'.MAP_PtFair_motion', y_FAST%VTK_count, p_FAST%VTK_fields, ErrStat2, ErrMsg2, p_FAST%VTK_tWidth ) ! ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN -! call MeshWrVTK(p_FAST%TurbinePos, MD%Input(1)%PtFairleadDisplacement, trim(p_FAST%VTK_OutFileRoot)//'.MD_PtFair_motion', y_FAST%VTK_count, p_FAST%VTK_fields, ErrStat2, ErrMsg2, p_FAST%VTK_tWidth ) +! call MeshWrVTK(p_FAST%TurbinePos, MD%Input(1)%CoupledKinematics, trim(p_FAST%VTK_OutFileRoot)//'.MD_PtFair_motion', y_FAST%VTK_count, p_FAST%VTK_fields, ErrStat2, ErrMsg2, p_FAST%VTK_tWidth ) ! ELSEIF ( p_FAST%CompMooring == Module_FEAM ) THEN ! call MeshWrVTK(p_FAST%TurbinePos, FEAM%Input(1)%PtFairleadDisplacement, trim(p_FAST%VTK_OutFileRoot)//'FEAM_PtFair_motion', y_FAST%VTK_count, p_FAST%VTK_fields, ErrStat2, ErrMsg2, p_FAST%VTK_tWidth ) ! END IF @@ -5458,7 +5491,7 @@ SUBROUTINE WrVTK_Surfaces(t_global, p_FAST, y_FAST, MeshMapData, ED, BD, AD, IfW ! IF ( p_FAST%CompMooring == Module_MAP ) THEN ! call MeshWrVTK(p_FAST%TurbinePos, MAPp%Input(1)%PtFairDisplacement, trim(p_FAST%VTK_OutFileRoot)//'.MAP_PtFair_motion', y_FAST%VTK_count, OutputFields, ErrStat2, ErrMsg2 ) ! ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN -! call MeshWrVTK(p_FAST%TurbinePos, MD%Input(1)%PtFairleadDisplacement, trim(p_FAST%VTK_OutFileRoot)//'.MD_PtFair_motion', y_FAST%VTK_count, OutputFields, ErrStat2, ErrMsg2 ) +! call MeshWrVTK(p_FAST%TurbinePos, MD%Input(1)%CoupledKinematics, trim(p_FAST%VTK_OutFileRoot)//'.MD_PtFair_motion', y_FAST%VTK_count, OutputFields, ErrStat2, ErrMsg2 ) ! ELSEIF ( p_FAST%CompMooring == Module_FEAM ) THEN ! call MeshWrVTK(p_FAST%TurbinePos, FEAM%Input(1)%PtFairleadDisplacement, trim(p_FAST%VTK_OutFileRoot)//'FEAM_PtFair_motion', y_FAST%VTK_count, OutputFields, ErrStat2, ErrMsg2 ) ! END IF From de02d56ad4576fd968247284c951c88fa6da25f6 Mon Sep 17 00:00:00 2001 From: Matt Hall Date: Wed, 30 Dec 2020 12:32:48 -0700 Subject: [PATCH 003/242] Added Mod_WaveField input to FAST.Farm for wave load phasing based on turbine positions: - New variable WaveFieldMod is passed through FAST.Farm into each OpenFAST instance to each HydroDyn module where it can adjust phases in the complex wave elevation amplitudes in Waves and Waves2. - This adds a new line for Mod_WaveField below the line for Mod_AmbWind in the FAST.Farm primary input file: - Mod_WaveField Wave field handling (-) (switch) {1: use individual HydroDyn inputs without adjustment, 2: adjust wave phases based on turbine offsets from farm origin} --- glue-codes/fast-farm/src/FASTWrapper.f90 | 1 + .../fast-farm/src/FASTWrapper_Registry.txt | 1 + .../fast-farm/src/FASTWrapper_Types.f90 | 7 +++++ .../fast-farm/src/FAST_Farm_Registry.txt | 1 + glue-codes/fast-farm/src/FAST_Farm_Subs.f90 | 9 ++++++ glue-codes/fast-farm/src/FAST_Farm_Types.f90 | 7 +++++ modules/hydrodyn/src/HydroDyn.f90 | 10 +++++-- modules/hydrodyn/src/HydroDyn.txt | 1 + modules/hydrodyn/src/HydroDyn_Input.f90 | 2 +- modules/hydrodyn/src/HydroDyn_Types.f90 | 7 +++++ modules/hydrodyn/src/Waves.f90 | 29 +++++++++++++++++++ modules/hydrodyn/src/Waves.txt | 3 ++ modules/hydrodyn/src/Waves_Types.f90 | 21 ++++++++++++++ .../openfast-library/src/FAST_Registry.txt | 5 +++- modules/openfast-library/src/FAST_Subs.f90 | 5 +++- modules/openfast-library/src/FAST_Types.f90 | 16 +++++++++- 16 files changed, 118 insertions(+), 7 deletions(-) diff --git a/glue-codes/fast-farm/src/FASTWrapper.f90 b/glue-codes/fast-farm/src/FASTWrapper.f90 index 5de5cf3cf8..cf3e0ed60b 100644 --- a/glue-codes/fast-farm/src/FASTWrapper.f90 +++ b/glue-codes/fast-farm/src/FASTWrapper.f90 @@ -140,6 +140,7 @@ SUBROUTINE FWrap_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, Init !.... multi-turbine options .... ExternInitData%TurbineID = InitInp%TurbNum ExternInitData%TurbinePos = InitInp%p_ref_Turbine + ExternInitData%WaveFieldMod = InitInp%WaveFieldMod ExternInitData%FarmIntegration = .true. ExternInitData%RootName = InitInp%RootName diff --git a/glue-codes/fast-farm/src/FASTWrapper_Registry.txt b/glue-codes/fast-farm/src/FASTWrapper_Registry.txt index 7cf5303c5e..1416eb4774 100644 --- a/glue-codes/fast-farm/src/FASTWrapper_Registry.txt +++ b/glue-codes/fast-farm/src/FASTWrapper_Registry.txt @@ -22,6 +22,7 @@ typedef ^ InitInputType CHARACTER(1024) FASTInFile typedef ^ InitInputType ReKi dr - - - "Radial increment of radial finite-difference grid" m typedef ^ InitInputType DbKi tmax - - - "Simulation length" s typedef ^ InitInputType ReKi p_ref_Turbine {3} - - "Undisplaced global coordinates of this turbine" m +typedef ^ InitInputType IntKi WaveFieldMod - - - "Wave field handling (-) (switch) 0: use individual HydroDyn inputs without adjustment, 1: adjust wave phases based on turbine offsets from farm origin" - typedef ^ InitInputType IntKi n_high_low - - - "Number of high-resolution time steps per low-resolution time step" - typedef ^ InitInputType DbKi dt_high - - - "High-resolution time step" s typedef ^ InitInputType ReKi p_ref_high {3} - - "Position of the origin of the high-resolution spatial domain for this turbine" m diff --git a/glue-codes/fast-farm/src/FASTWrapper_Types.f90 b/glue-codes/fast-farm/src/FASTWrapper_Types.f90 index 7dca73b15b..bfac7d2a45 100644 --- a/glue-codes/fast-farm/src/FASTWrapper_Types.f90 +++ b/glue-codes/fast-farm/src/FASTWrapper_Types.f90 @@ -83,6 +83,7 @@ MODULE FASTWrapper_Types REAL(ReKi) :: dr !< Radial increment of radial finite-difference grid [m] REAL(DbKi) :: tmax !< Simulation length [s] REAL(ReKi) , DIMENSION(1:3) :: p_ref_Turbine !< Undisplaced global coordinates of this turbine [m] + INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 1: use individual HydroDyn inputs without adjustment, 2: adjust wave phases based on turbine offsets from farm origin [-] INTEGER(IntKi) :: n_high_low !< Number of high-resolution time steps per low-resolution time step [-] REAL(DbKi) :: dt_high !< High-resolution time step [s] REAL(ReKi) , DIMENSION(1:3) :: p_ref_high !< Position of the origin of the high-resolution spatial domain for this turbine [m] @@ -187,6 +188,7 @@ SUBROUTINE FWrap_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, Er DstInitInputData%dr = SrcInitInputData%dr DstInitInputData%tmax = SrcInitInputData%tmax DstInitInputData%p_ref_Turbine = SrcInitInputData%p_ref_Turbine + DstInitInputData%WaveFieldMod = SrcInitInputData%WaveFieldMod DstInitInputData%n_high_low = SrcInitInputData%n_high_low DstInitInputData%dt_high = SrcInitInputData%dt_high DstInitInputData%p_ref_high = SrcInitInputData%p_ref_high @@ -285,6 +287,7 @@ SUBROUTINE FWrap_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, Err Re_BufSz = Re_BufSz + 1 ! dr Db_BufSz = Db_BufSz + 1 ! tmax Re_BufSz = Re_BufSz + SIZE(InData%p_ref_Turbine) ! p_ref_Turbine + Int_BufSz = Int_BufSz + 1 ! WaveFieldMod Int_BufSz = Int_BufSz + 1 ! n_high_low Db_BufSz = Db_BufSz + 1 ! dt_high Re_BufSz = Re_BufSz + SIZE(InData%p_ref_high) ! p_ref_high @@ -351,6 +354,8 @@ SUBROUTINE FWrap_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, Err ReKiBuf(Re_Xferred) = InData%p_ref_Turbine(i1) Re_Xferred = Re_Xferred + 1 END DO + IntKiBuf(Int_Xferred) = InData%WaveFieldMod + Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%n_high_low Int_Xferred = Int_Xferred + 1 DbKiBuf(Db_Xferred) = InData%dt_high @@ -464,6 +469,8 @@ SUBROUTINE FWrap_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, OutData%p_ref_Turbine(i1) = ReKiBuf(Re_Xferred) Re_Xferred = Re_Xferred + 1 END DO + OutData%WaveFieldMod = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 OutData%n_high_low = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 OutData%dt_high = DbKiBuf(Db_Xferred) diff --git a/glue-codes/fast-farm/src/FAST_Farm_Registry.txt b/glue-codes/fast-farm/src/FAST_Farm_Registry.txt index 0f20a1d3fd..74d9ca22ab 100644 --- a/glue-codes/fast-farm/src/FAST_Farm_Registry.txt +++ b/glue-codes/fast-farm/src/FAST_Farm_Registry.txt @@ -33,6 +33,7 @@ typedef ^ ParameterType CHARACTER(1024) WindFilePath - typedef ^ ParameterType CHARACTER(1024) SC_FileName - - - "Name/location of the dynamic library {.dll [Windows] or .so [Linux]} containing the Super Controller algorithms" - typedef ^ ParameterType LOGICAL UseSC - - - "Use a super controller?" - typedef ^ ParameterType ReKi WT_Position {:}{:} - - "X-Y-Z position of each wind turbine; index 1 = XYZ; index 2 = turbine number" meters +typedef ^ ParameterType IntKi WaveFieldMod - - - "Wave field handling (-) (switch) 0: use individual HydroDyn inputs without adjustment, 1: adjust wave phases based on turbine offsets from farm origin" - typedef ^ ParameterType CHARACTER(1024) WT_FASTInFile {:} - - "Name of input file for each turbine" - typedef ^ ParameterType CHARACTER(1024) FTitle - - - "The description line from the primary FAST.Farm input file" - typedef ^ ParameterType CHARACTER(1024) OutFileRoot - - - "The root name derived from the primary FAST.Farm input file" - diff --git a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 index e1e6fdfd1a..8b1ebd9d64 100644 --- a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 +++ b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 @@ -536,6 +536,14 @@ SUBROUTINE Farm_ReadPrimaryFile( InputFile, p, WD_InitInp, AWAE_InitInp, SC_Init RETURN end if + ! Mod_WaveField - Wave field handling (-) (switch) {1: use individual HydroDyn inputs without adjustment, 2: adjust wave phases based on turbine offsets from farm origin} + CALL ReadVar( UnIn, InputFile, p%WaveFieldMod, "Mod_WaveField", "Wave field handling (-) (switch) {1: use individual HydroDyn inputs without adjustment, 2: adjust wave phases based on turbine offsets from farm origin}", ErrStat2, ErrMsg2, UnEc) + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + if ( ErrStat >= AbortErrLev ) then + call cleanup() + RETURN + end if + !---------------------- SUPER CONTROLLER ------------------------------------------------------------------ CALL ReadCom( UnIn, InputFile, 'Section Header: Super Controller', ErrStat2, ErrMsg2, UnEc ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) @@ -1589,6 +1597,7 @@ SUBROUTINE Farm_InitFAST( farm, WD_InitInp, AWAE_InitOutput, SC_InitOutput, SC_y FWrap_InitInp%FASTInFile = farm%p%WT_FASTInFile(nt) FWrap_InitInp%p_ref_Turbine = farm%p%WT_Position(:,nt) + FWrap_InitInp%WaveFieldMod = farm%p%WaveFieldMod FWrap_InitInp%TurbNum = nt FWrap_InitInp%RootName = trim(farm%p%OutFileRoot)//'.T'//num2lstr(nt) diff --git a/glue-codes/fast-farm/src/FAST_Farm_Types.f90 b/glue-codes/fast-farm/src/FAST_Farm_Types.f90 index fdb0bcd874..d46bbf5b6e 100644 --- a/glue-codes/fast-farm/src/FAST_Farm_Types.f90 +++ b/glue-codes/fast-farm/src/FAST_Farm_Types.f90 @@ -97,6 +97,7 @@ MODULE FAST_Farm_Types CHARACTER(1024) :: SC_FileName !< Name/location of the dynamic library {.dll [Windows] or .so [Linux]} containing the Super Controller algorithms [-] LOGICAL :: UseSC !< Use a super controller? [-] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: WT_Position !< X-Y-Z position of each wind turbine; index 1 = XYZ; index 2 = turbine number [meters] + INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 1: use individual HydroDyn inputs without adjustment, 2: adjust wave phases based on turbine offsets from farm origin [-] CHARACTER(1024) , DIMENSION(:), ALLOCATABLE :: WT_FASTInFile !< Name of input file for each turbine [-] CHARACTER(1024) :: FTitle !< The description line from the primary FAST.Farm input file [-] CHARACTER(1024) :: OutFileRoot !< The root name derived from the primary FAST.Farm input file [-] @@ -247,6 +248,7 @@ SUBROUTINE Farm_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg END IF DstParamData%WT_Position = SrcParamData%WT_Position ENDIF + DstParamData%WaveFieldMod = SrcParamData%WaveFieldMod IF (ALLOCATED(SrcParamData%WT_FASTInFile)) THEN i1_l = LBOUND(SrcParamData%WT_FASTInFile,1) i1_u = UBOUND(SrcParamData%WT_FASTInFile,1) @@ -461,6 +463,7 @@ SUBROUTINE Farm_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Int_BufSz = Int_BufSz + 2*2 ! WT_Position upper/lower bounds for each dimension Re_BufSz = Re_BufSz + SIZE(InData%WT_Position) ! WT_Position END IF + Int_BufSz = Int_BufSz + 1 ! WaveFieldMod Int_BufSz = Int_BufSz + 1 ! WT_FASTInFile allocated yes/no IF ( ALLOCATED(InData%WT_FASTInFile) ) THEN Int_BufSz = Int_BufSz + 2*1 ! WT_FASTInFile upper/lower bounds for each dimension @@ -631,6 +634,8 @@ SUBROUTINE Farm_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, END DO END DO END IF + IntKiBuf(Int_Xferred) = InData%WaveFieldMod + Int_Xferred = Int_Xferred + 1 IF ( .NOT. ALLOCATED(InData%WT_FASTInFile) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -941,6 +946,8 @@ SUBROUTINE Farm_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMs END DO END DO END IF + OutData%WaveFieldMod = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WT_FASTInFile not allocated Int_Xferred = Int_Xferred + 1 ELSE diff --git a/modules/hydrodyn/src/HydroDyn.f90 b/modules/hydrodyn/src/HydroDyn.f90 index 9f1b60fb8e..f9119bc3cd 100644 --- a/modules/hydrodyn/src/HydroDyn.f90 +++ b/modules/hydrodyn/src/HydroDyn.f90 @@ -443,8 +443,12 @@ SUBROUTINE HydroDyn_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, I InitLocal%Waves%UnSum = InitLocal%UnSum InitLocal%WAMIT%Conv_Rdtn%UnSum = InitLocal%UnSum - InitLocal%Morison%UnSum = InitLocal%UnSum - + InitLocal%Morison%UnSum = InitLocal%UnSum + + ! distribute wave field and turbine location variables as needed to submodule initInputs + InitLocal%Waves%WaveFieldMod = InitLocal%WaveFieldMod + InitLocal%Waves%PtfmLocationX = InitLocal%PtfmLocationX + InitLocal%Waves%PtfmLocationY = InitLocal%PtfmLocationY ! Now call each sub-module's *_Init subroutine ! to fully initialize each sub-module based on the necessary initialization data @@ -694,7 +698,7 @@ SUBROUTINE HydroDyn_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, I InitLocal%Waves2%NWaveElev = InitLocal%Waves2%NWaveKin InitLocal%Waves2%WaveElevxi = InitLocal%Waves2%WaveKinxi - InitLocal%Waves2%WaveElevyi = InitLocal%Waves2%WaveKinyi + InitLocal%Waves2%WaveElevyi = InitLocal%Waves2%WaveKinyi CALL Waves2_Init(InitLocal%Waves2, m%u_Waves2, p%Waves2, x%Waves2, xd%Waves2, z%Waves2, OtherState%Waves2, & y%Waves2, m%Waves2, Interval, InitOut%Waves2, ErrStat2, ErrMsg2 ) diff --git a/modules/hydrodyn/src/HydroDyn.txt b/modules/hydrodyn/src/HydroDyn.txt index ca379172d8..55ecc818f4 100644 --- a/modules/hydrodyn/src/HydroDyn.txt +++ b/modules/hydrodyn/src/HydroDyn.txt @@ -34,6 +34,7 @@ typedef ^ ^ ReKi typedef ^ ^ DbKi TMax - - - "Supplied by Driver: The total simulation time" "(sec)" typedef ^ ^ LOGICAL HasIce - - - "Supplied by Driver: Whether this simulation has ice loading (flag)" - typedef ^ ^ SiKi WaveElevXY {:}{:} - - "Supplied by Driver: X-Y locations for WaveElevation output (for visualization). First dimension is the X (1) and Y (2) coordinate. Second dimension is the point number." "m,-" +typedef ^ ^ INTEGER WaveFieldMod - - - "Wave field handling (-) (switch) 0: use individual HydroDyn inputs without adjustment, 1: adjust wave phases based on turbine offsets from farm origin" - typedef ^ ^ ReKi PtfmLocationX - - - "Supplied by Driver: X coordinate of platform location in the wave field" "m" typedef ^ ^ ReKi PtfmLocationY - - - "Supplied by Driver: Y coordinate of platform location in the wave field" "m" typedef ^ ^ CHARACTER(80) PtfmSgFChr - - - "Platform horizontal surge translation force (flag) or DEFAULT" - diff --git a/modules/hydrodyn/src/HydroDyn_Input.f90 b/modules/hydrodyn/src/HydroDyn_Input.f90 index 025e7e3226..7352a5cad1 100644 --- a/modules/hydrodyn/src/HydroDyn_Input.f90 +++ b/modules/hydrodyn/src/HydroDyn_Input.f90 @@ -4312,7 +4312,7 @@ SUBROUTINE HydroDynInput_ProcessInitData( InitInp, ErrStat, ErrMsg ) CALL SetErrStat( ErrID_Fatal,'Error allocating space for WaveKinzi array for Waves2 module.',ErrStat,ErrMsg,RoutineName) RETURN END IF - + InitInp%Waves2%WaveKinxi = InitInp%Waves%WaveKinxi InitInp%Waves2%WaveKinyi = InitInp%Waves%WaveKinyi InitInp%Waves2%WaveKinzi = InitInp%Waves%WaveKinzi diff --git a/modules/hydrodyn/src/HydroDyn_Types.f90 b/modules/hydrodyn/src/HydroDyn_Types.f90 index 1b6b52b75c..b2a72222a1 100644 --- a/modules/hydrodyn/src/HydroDyn_Types.f90 +++ b/modules/hydrodyn/src/HydroDyn_Types.f90 @@ -54,6 +54,7 @@ MODULE HydroDyn_Types REAL(DbKi) :: TMax !< Supplied by Driver: The total simulation time [(sec)] LOGICAL :: HasIce !< Supplied by Driver: Whether this simulation has ice loading (flag) [-] REAL(SiKi) , DIMENSION(:,:), ALLOCATABLE :: WaveElevXY !< Supplied by Driver: X-Y locations for WaveElevation output (for visualization). First dimension is the X (1) and Y (2) coordinate. Second dimension is the point number. [m,-] + INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 1: use individual HydroDyn inputs without adjustment, 2: adjust wave phases based on turbine offsets from farm origin [-] REAL(ReKi) :: PtfmLocationX !< Supplied by Driver: X coordinate of platform location in the wave field [m] REAL(ReKi) :: PtfmLocationY !< Supplied by Driver: Y coordinate of platform location in the wave field [m] CHARACTER(80) :: PtfmSgFChr !< Platform horizontal surge translation force (flag) or DEFAULT [-] @@ -263,6 +264,7 @@ SUBROUTINE HydroDyn_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, END IF DstInitInputData%WaveElevXY = SrcInitInputData%WaveElevXY ENDIF + DstInitInputData%WaveFieldMod = SrcInitInputData%WaveFieldMod DstInitInputData%PtfmLocationX = SrcInitInputData%PtfmLocationX DstInitInputData%PtfmLocationY = SrcInitInputData%PtfmLocationY DstInitInputData%PtfmSgFChr = SrcInitInputData%PtfmSgFChr @@ -396,6 +398,7 @@ SUBROUTINE HydroDyn_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, Int_BufSz = Int_BufSz + 2*2 ! WaveElevXY upper/lower bounds for each dimension Re_BufSz = Re_BufSz + SIZE(InData%WaveElevXY) ! WaveElevXY END IF + Int_BufSz = Int_BufSz + 1 ! WaveFieldMod Re_BufSz = Re_BufSz + 1 ! PtfmLocationX Re_BufSz = Re_BufSz + 1 ! PtfmLocationY Int_BufSz = Int_BufSz + 1*LEN(InData%PtfmSgFChr) ! PtfmSgFChr @@ -601,6 +604,8 @@ SUBROUTINE HydroDyn_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, END DO END DO END IF + IntKiBuf(Int_Xferred) = InData%WaveFieldMod + Int_Xferred = Int_Xferred + 1 ReKiBuf(Re_Xferred) = InData%PtfmLocationX Re_Xferred = Re_Xferred + 1 ReKiBuf(Re_Xferred) = InData%PtfmLocationY @@ -955,6 +960,8 @@ SUBROUTINE HydroDyn_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrSta END DO END DO END IF + OutData%WaveFieldMod = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 OutData%PtfmLocationX = ReKiBuf(Re_Xferred) Re_Xferred = Re_Xferred + 1 OutData%PtfmLocationY = ReKiBuf(Re_Xferred) diff --git a/modules/hydrodyn/src/Waves.f90 b/modules/hydrodyn/src/Waves.f90 index 78fe5ab1bb..a3db769384 100644 --- a/modules/hydrodyn/src/Waves.f90 +++ b/modules/hydrodyn/src/Waves.f90 @@ -1670,6 +1670,35 @@ SUBROUTINE VariousWaves_Init ( InitInp, InitOut, ErrStat, ErrMsg ) SinWaveDir=SIN(D2R*InitOut%WaveDirArr) + !-------------------------------------------------------------------------------- + !> ## Phase shift the discrete Fourier transform of wave elevations at the WRP + !> This changes the phasing of all wave kinematics and loads to reflect the turbine's + !! location in the larger farm, in the case of FAST.Farm simulations, based on + !! specified PtfmLocationX and PtfmLocationY. + + IF (InitInp%WaveFieldMod == 2) THEN ! case 2: adjust wave phases based on turbine offsets from farm origin + + CALL WrScr ( ' Adjusting incident wave kinematics for turbine offset from array origin.' ) + + DO I = 0,InitOut%NStepWave2 + + tmpComplex = CMPLX( InitOut%WaveElevC0(1,I), InitOut%WaveElevC0(2,I)) + + ! some redundant calculations with later, but insignificant + Omega = I*InitOut%WaveDOmega + WaveNmbr = WaveNumber ( Omega, InitInp%Gravity, InitInp%WtrDpth ) + + ! apply the phase shift + tmpComplex = tmpComplex * EXP( -ImagNmbr*WaveNmbr*( InitInp%PtfmLocationX*CosWaveDir(I) + InitInp%PtfmLocationY*SinWaveDir(I) )) + + ! put shifted complex amplitudes back into the array for use in the remainder of this module and other modules (Waves2, WAMIT, WAMIT2) + InitOut%WaveElevC0 (1,I) = REAL( tmpComplex) + InitOut%WaveElevC0 (2,I) = AIMAG(tmpComplex) + + END DO + END IF + + !-------------------------------------------------------------------------------- !> ## Compute IFFTs !> Compute the discrete Fourier transform of the instantaneous elevation of diff --git a/modules/hydrodyn/src/Waves.txt b/modules/hydrodyn/src/Waves.txt index 366067469c..841ec9d53f 100644 --- a/modules/hydrodyn/src/Waves.txt +++ b/modules/hydrodyn/src/Waves.txt @@ -51,6 +51,9 @@ typedef ^ ^ INTEGER NWaveElev typedef ^ ^ SiKi WaveElevxi {:} - - "xi-coordinates for points where the incident wave elevations can be output" (meters) typedef ^ ^ SiKi WaveElevyi {:} - - "yi-coordinates for points where the incident wave elevations can be output" (meters) typedef ^ ^ SiKi WaveElevXY {:}{:} - - "Supplied by Driver: X-Y locations for WaveElevation output (for visualization). Index 1 corresponds to X or Y coordinate. Index 2 corresponds to point number." - +typedef ^ ^ ReKi PtfmLocationX - - - "Copy of X coordinate of platform location in the wave field, used to offset/phase-shift all wave kinematics to account for location in the farm" "m" +typedef ^ ^ ReKi PtfmLocationY - - - "Copy of Y coordinate of platform location in the wave field, used to offset/phase-shift all wave kinematics to account for location in the farm" "m" +typedef ^ ^ INTEGER WaveFieldMod - - - "Wave field handling (-) (switch) 0: use individual HydroDyn inputs without adjustment, 1: adjust wave phases based on turbine offsets from farm origin" - typedef ^ ^ INTEGER NWaveKin - - - "Number of points where the incident wave kinematics will be computed" - typedef ^ ^ SiKi WaveKinxi {:} - - "xi-coordinates for points where the incident wave kinematics will be computed; these are relative to the mean sea level" (meters) typedef ^ ^ SiKi WaveKinyi {:} - - "yi-coordinates for points where the incident wave kinematics will be computed; these are relative to the mean sea level" (meters) diff --git a/modules/hydrodyn/src/Waves_Types.f90 b/modules/hydrodyn/src/Waves_Types.f90 index 9b98fdc949..a5bb1dad44 100644 --- a/modules/hydrodyn/src/Waves_Types.f90 +++ b/modules/hydrodyn/src/Waves_Types.f90 @@ -68,6 +68,9 @@ MODULE Waves_Types REAL(SiKi) , DIMENSION(:), ALLOCATABLE :: WaveElevxi !< xi-coordinates for points where the incident wave elevations can be output [(meters)] REAL(SiKi) , DIMENSION(:), ALLOCATABLE :: WaveElevyi !< yi-coordinates for points where the incident wave elevations can be output [(meters)] REAL(SiKi) , DIMENSION(:,:), ALLOCATABLE :: WaveElevXY !< Supplied by Driver: X-Y locations for WaveElevation output (for visualization). Index 1 corresponds to X or Y coordinate. Index 2 corresponds to point number. [-] + REAL(ReKi) :: PtfmLocationX !< Copy of X coordinate of platform location in the wave field, used to offset/phase-shift all wave kinematics to account for location in the farm [m] + REAL(ReKi) :: PtfmLocationY !< Copy of Y coordinate of platform location in the wave field, used to offset/phase-shift all wave kinematics to account for location in the farm [m] + INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 1: use individual HydroDyn inputs without adjustment, 2: adjust wave phases based on turbine offsets from farm origin [-] INTEGER(IntKi) :: NWaveKin !< Number of points where the incident wave kinematics will be computed [-] REAL(SiKi) , DIMENSION(:), ALLOCATABLE :: WaveKinxi !< xi-coordinates for points where the incident wave kinematics will be computed; these are relative to the mean sea level [(meters)] REAL(SiKi) , DIMENSION(:), ALLOCATABLE :: WaveKinyi !< yi-coordinates for points where the incident wave kinematics will be computed; these are relative to the mean sea level [(meters)] @@ -237,6 +240,9 @@ SUBROUTINE Waves_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, Er END IF DstInitInputData%WaveElevXY = SrcInitInputData%WaveElevXY ENDIF + DstInitInputData%PtfmLocationX = SrcInitInputData%PtfmLocationX + DstInitInputData%PtfmLocationY = SrcInitInputData%PtfmLocationY + DstInitInputData%WaveFieldMod = SrcInitInputData%WaveFieldMod DstInitInputData%NWaveKin = SrcInitInputData%NWaveKin IF (ALLOCATED(SrcInitInputData%WaveKinxi)) THEN i1_l = LBOUND(SrcInitInputData%WaveKinxi,1) @@ -421,6 +427,9 @@ SUBROUTINE Waves_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, Err Int_BufSz = Int_BufSz + 2*2 ! WaveElevXY upper/lower bounds for each dimension Re_BufSz = Re_BufSz + SIZE(InData%WaveElevXY) ! WaveElevXY END IF + Re_BufSz = Re_BufSz + 1 ! PtfmLocationX + Re_BufSz = Re_BufSz + 1 ! PtfmLocationY + Int_BufSz = Int_BufSz + 1 ! WaveFieldMod Int_BufSz = Int_BufSz + 1 ! NWaveKin Int_BufSz = Int_BufSz + 1 ! WaveKinxi allocated yes/no IF ( ALLOCATED(InData%WaveKinxi) ) THEN @@ -616,6 +625,12 @@ SUBROUTINE Waves_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, Err END DO END DO END IF + ReKiBuf(Re_Xferred) = InData%PtfmLocationX + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%PtfmLocationY + Re_Xferred = Re_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%WaveFieldMod + Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%NWaveKin Int_Xferred = Int_Xferred + 1 IF ( .NOT. ALLOCATED(InData%WaveKinxi) ) THEN @@ -889,6 +904,12 @@ SUBROUTINE Waves_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, END DO END DO END IF + OutData%PtfmLocationX = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%PtfmLocationY = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%WaveFieldMod = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 OutData%NWaveKin = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WaveKinxi not allocated diff --git a/modules/openfast-library/src/FAST_Registry.txt b/modules/openfast-library/src/FAST_Registry.txt index 2a6b14ef21..77341c6daa 100644 --- a/modules/openfast-library/src/FAST_Registry.txt +++ b/modules/openfast-library/src/FAST_Registry.txt @@ -115,6 +115,8 @@ typedef ^ FAST_ParameterType IntKi CompMooring - - - "Compute mooring system (sw typedef ^ FAST_ParameterType IntKi CompIce - - - "Compute ice loading (switch) {Module_None; Module_IceF, Module_IceD}" - typedef ^ FAST_ParameterType LOGICAL UseDWM - - - "Use the DWM module in AeroDyn" - typedef ^ FAST_ParameterType LOGICAL Linearize - - - "Linearization analysis (flag)" - +typedef ^ FAST_ParameterType IntKi WaveFieldMod - - - "Wave field handling (-) (switch) 0: use individual HydroDyn inputs without adjustment, 1: adjust wave phases based on turbine offsets from farm origin" - + # Input file names: typedef ^ FAST_ParameterType CHARACTER(1024) EDFile - - - "The name of the ElastoDyn input file" - typedef ^ FAST_ParameterType CHARACTER(1024) BDBldFile {MaxNBlades} - - "Name of files containing BeamDyn inputs for each blade" - @@ -681,7 +683,8 @@ typedef ^ FAST_ExternInitType DbKi Tmax - -1 - "External code specified Tmax" s typedef ^ FAST_ExternInitType IntKi SensorType - SensorType_None - "lidar sensor type, which should not be pulsed at the moment; this input should be replaced with a section in the InflowWind input file" - typedef ^ FAST_ExternInitType LOGICAL LidRadialVel - - - "TRUE => return radial component, FALSE => return 'x' direction estimate" - typedef ^ FAST_ExternInitType IntKi TurbineID - 0 - "ID number for turbine (used to create output file naming convention)" - -typedef ^ FAST_ExternInitType ReKi TurbinePos {3} - - "Initial position of turbine base (origin used in future for graphics)" m +typedef ^ FAST_ExternInitType ReKi TurbinePos {3} - - "Initial position of turbine base (origin used for graphics or in FAST.Farm)" m +typedef ^ FAST_ExternInitType IntKi WaveFieldMod - - - "Wave field handling (-) (switch) 0: use individual HydroDyn inputs without adjustment, 1: adjust wave phases based on turbine offsets from farm origin" - typedef ^ FAST_ExternInitType IntKi NumSC2CtrlGlob - - - "number of global controller inputs [from supercontroller]" - typedef ^ FAST_ExternInitType IntKi NumSC2Ctrl - - - "number of turbine specific controller inputs [from supercontroller]" - typedef ^ FAST_ExternInitType IntKi NumCtrl2SC - - - "number of controller outputs [to supercontroller]" - diff --git a/modules/openfast-library/src/FAST_Subs.f90 b/modules/openfast-library/src/FAST_Subs.f90 index c47615befb..c30f9f800e 100644 --- a/modules/openfast-library/src/FAST_Subs.f90 +++ b/modules/openfast-library/src/FAST_Subs.f90 @@ -190,6 +190,7 @@ SUBROUTINE FAST_InitializeAll( t_initial, p_FAST, y_FAST, m_FAST, ED, BD, SrvD, p_FAST%UseSC = .FALSE. if (PRESENT(ExternInitData)) then p_FAST%TurbinePos = ExternInitData%TurbinePos + p_FAST%WaveFieldMod = ExternInitData%WaveFieldMod if( (ExternInitData%NumSC2CtrlGlob .gt. 0) .or. (ExternInitData%NumSC2Ctrl .gt. 0) .or. (ExternInitData%NumCtrl2SC .gt. 0)) then p_FAST%UseSC = .TRUE. end if @@ -202,6 +203,7 @@ SUBROUTINE FAST_InitializeAll( t_initial, p_FAST, y_FAST, m_FAST, ED, BD, SrvD, else p_FAST%TurbinePos = 0.0_ReKi + p_FAST%WaveFieldMod = 0 CALL FAST_Init( p_FAST, m_FAST, y_FAST, t_initial, InputFile, ErrStat2, ErrMsg2 ) ! We have the name of the input file from somewhere else (e.g. Simulink) end if @@ -801,7 +803,8 @@ SUBROUTINE FAST_InitializeAll( t_initial, p_FAST, y_FAST, m_FAST, ED, BD, SrvD, Init%InData_HD%hasIce = p_FAST%CompIce /= Module_None Init%InData_HD%Linearize = p_FAST%Linearize - ! if wave field needs an offset, modify these values (added at request of SOWFA developers): + ! these values support wave field handling + Init%InData_HD%WaveFieldMod = p_FAST%WaveFieldMod Init%InData_HD%PtfmLocationX = p_FAST%TurbinePos(1) Init%InData_HD%PtfmLocationY = p_FAST%TurbinePos(2) diff --git a/modules/openfast-library/src/FAST_Types.f90 b/modules/openfast-library/src/FAST_Types.f90 index 6c0f882be5..0e51943346 100644 --- a/modules/openfast-library/src/FAST_Types.f90 +++ b/modules/openfast-library/src/FAST_Types.f90 @@ -159,6 +159,7 @@ MODULE FAST_Types INTEGER(IntKi) :: CompIce !< Compute ice loading (switch) {Module_None; Module_IceF, Module_IceD} [-] LOGICAL :: UseDWM !< Use the DWM module in AeroDyn [-] LOGICAL :: Linearize !< Linearization analysis (flag) [-] + INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 1: use individual HydroDyn inputs without adjustment, 2: adjust wave phases based on turbine offsets from farm origin [-] CHARACTER(1024) :: EDFile !< The name of the ElastoDyn input file [-] CHARACTER(1024) , DIMENSION(MaxNBlades) :: BDBldFile !< Name of files containing BeamDyn inputs for each blade [-] CHARACTER(1024) :: InflowFile !< Name of file containing inflow wind input parameters [-] @@ -732,7 +733,8 @@ MODULE FAST_Types INTEGER(IntKi) :: SensorType = SensorType_None !< lidar sensor type, which should not be pulsed at the moment; this input should be replaced with a section in the InflowWind input file [-] LOGICAL :: LidRadialVel !< TRUE => return radial component, FALSE => return 'x' direction estimate [-] INTEGER(IntKi) :: TurbineID = 0 !< ID number for turbine (used to create output file naming convention) [-] - REAL(ReKi) , DIMENSION(1:3) :: TurbinePos !< Initial position of turbine base (origin used in future for graphics) [m] + REAL(ReKi) , DIMENSION(1:3) :: TurbinePos !< Initial position of turbine base (origin used for graphics or in FAST.Farm) [m] + INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 1: use individual HydroDyn inputs without adjustment, 2: adjust wave phases based on turbine offsets from farm origin [-] INTEGER(IntKi) :: NumSC2CtrlGlob !< number of global controller inputs [from supercontroller] [-] INTEGER(IntKi) :: NumSC2Ctrl !< number of turbine specific controller inputs [from supercontroller] [-] INTEGER(IntKi) :: NumCtrl2SC !< number of controller outputs [to supercontroller] [-] @@ -2107,6 +2109,7 @@ SUBROUTINE FAST_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg DstParamData%CompIce = SrcParamData%CompIce DstParamData%UseDWM = SrcParamData%UseDWM DstParamData%Linearize = SrcParamData%Linearize + DstParamData%WaveFieldMod = SrcParamData%WaveFieldMod DstParamData%EDFile = SrcParamData%EDFile DstParamData%BDBldFile = SrcParamData%BDBldFile DstParamData%InflowFile = SrcParamData%InflowFile @@ -2240,6 +2243,7 @@ SUBROUTINE FAST_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Int_BufSz = Int_BufSz + 1 ! CompIce Int_BufSz = Int_BufSz + 1 ! UseDWM Int_BufSz = Int_BufSz + 1 ! Linearize + Int_BufSz = Int_BufSz + 1 ! WaveFieldMod Int_BufSz = Int_BufSz + 1*LEN(InData%EDFile) ! EDFile Int_BufSz = Int_BufSz + SIZE(InData%BDBldFile)*LEN(InData%BDBldFile) ! BDBldFile Int_BufSz = Int_BufSz + 1*LEN(InData%InflowFile) ! InflowFile @@ -2412,6 +2416,8 @@ SUBROUTINE FAST_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = TRANSFER(InData%Linearize, IntKiBuf(1)) Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%WaveFieldMod + Int_Xferred = Int_Xferred + 1 DO I = 1, LEN(InData%EDFile) IntKiBuf(Int_Xferred) = ICHAR(InData%EDFile(I:I), IntKi) Int_Xferred = Int_Xferred + 1 @@ -2705,6 +2711,8 @@ SUBROUTINE FAST_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMs Int_Xferred = Int_Xferred + 1 OutData%Linearize = TRANSFER(IntKiBuf(Int_Xferred), OutData%Linearize) Int_Xferred = Int_Xferred + 1 + OutData%WaveFieldMod = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 DO I = 1, LEN(OutData%EDFile) OutData%EDFile(I:I) = CHAR(IntKiBuf(Int_Xferred)) Int_Xferred = Int_Xferred + 1 @@ -44116,6 +44124,7 @@ SUBROUTINE FAST_CopyExternInitType( SrcExternInitTypeData, DstExternInitTypeData DstExternInitTypeData%LidRadialVel = SrcExternInitTypeData%LidRadialVel DstExternInitTypeData%TurbineID = SrcExternInitTypeData%TurbineID DstExternInitTypeData%TurbinePos = SrcExternInitTypeData%TurbinePos + DstExternInitTypeData%WaveFieldMod = SrcExternInitTypeData%WaveFieldMod DstExternInitTypeData%NumSC2CtrlGlob = SrcExternInitTypeData%NumSC2CtrlGlob DstExternInitTypeData%NumSC2Ctrl = SrcExternInitTypeData%NumSC2Ctrl DstExternInitTypeData%NumCtrl2SC = SrcExternInitTypeData%NumCtrl2SC @@ -44209,6 +44218,7 @@ SUBROUTINE FAST_PackExternInitType( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, Int_BufSz = Int_BufSz + 1 ! LidRadialVel Int_BufSz = Int_BufSz + 1 ! TurbineID Re_BufSz = Re_BufSz + SIZE(InData%TurbinePos) ! TurbinePos + Int_BufSz = Int_BufSz + 1 ! WaveFieldMod Int_BufSz = Int_BufSz + 1 ! NumSC2CtrlGlob Int_BufSz = Int_BufSz + 1 ! NumSC2Ctrl Int_BufSz = Int_BufSz + 1 ! NumCtrl2SC @@ -44268,6 +44278,8 @@ SUBROUTINE FAST_PackExternInitType( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ReKiBuf(Re_Xferred) = InData%TurbinePos(i1) Re_Xferred = Re_Xferred + 1 END DO + IntKiBuf(Int_Xferred) = InData%WaveFieldMod + Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%NumSC2CtrlGlob Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%NumSC2Ctrl @@ -44369,6 +44381,8 @@ SUBROUTINE FAST_UnPackExternInitType( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrSt OutData%TurbinePos(i1) = ReKiBuf(Re_Xferred) Re_Xferred = Re_Xferred + 1 END DO + OutData%WaveFieldMod = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 OutData%NumSC2CtrlGlob = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 OutData%NumSC2Ctrl = IntKiBuf(Int_Xferred) From 701c947230b4e5b5d8ae8c5c9dfb550e33f7acca Mon Sep 17 00:00:00 2001 From: Matt Hall Date: Thu, 7 Jan 2021 18:22:52 -0700 Subject: [PATCH 004/242] Major MoorDyn v2 changes. Introducing all the v2 objects and structure. Still some things to work out for buoyancy can applications. --- modules/hydrodyn/src/HydroDyn_Input.f90 | 6 +- modules/moordyn/src/MoorDyn.f90 | 6549 +++++++++++++++++--- modules/moordyn/src/MoorDyn_Driver.f90 | 58 +- modules/moordyn/src/MoorDyn_IO.f90 | 1513 +++-- modules/moordyn/src/MoorDyn_Registry.txt | 279 +- modules/moordyn/src/MoorDyn_Types.f90 | 7220 +++++++++++++++++++--- 6 files changed, 13135 insertions(+), 2490 deletions(-) diff --git a/modules/hydrodyn/src/HydroDyn_Input.f90 b/modules/hydrodyn/src/HydroDyn_Input.f90 index 0885111a8a..173c49e972 100644 --- a/modules/hydrodyn/src/HydroDyn_Input.f90 +++ b/modules/hydrodyn/src/HydroDyn_Input.f90 @@ -33,6 +33,8 @@ MODULE HydroDyn_Input PRIVATE :: CleanupEchoFile PRIVATE :: CheckMeshOutput + + !>>> put parameters here (in waves) nx,ny,nz... and discretization coefficients, with unique names, it's like a global<<< CONTAINS @@ -4276,14 +4278,14 @@ SUBROUTINE HydroDynInput_ProcessInitData( InitInp, ErrStat, ErrMsg ) InitInp%Waves%WaveKinzi(I) = InitInp%Morison%Nodes(I)%Position(3) ! zi-coordinates for points where the incident wave kinematics will be computed; InitInp%Current%MorisonNodezi(I) = InitInp%Waves%WaveKinzi(I) END DO - !@mhall: hard-coding the coordinates of those additional nodes for the grid (remember, must be in increasing order) + !@mhall: hard-coding the coordinates of those additional nodes for the grid (remember, must be in increasing order) <<< move these to module global parameters<<<< DO I=1,8 !z DO J = 1,5 !y DO K = 1,5 !x Itemp = InitInp%Morison%NNodes + (I-1)*25.0 + (J-1)*5.0 + K ! index of actual node InitInp%Waves%WaveKinzi(Itemp) = 1.0 - 2.0**(8-I) ! -127, -63, -31, -15, -7, -3, -1, 0 InitInp%Waves%WaveKinyi(Itemp) = -60.0 + 20.0*J - InitInp%Waves%WaveKinxi(Itemp) = -60.0 + 20.0*K + InitInp%Waves%WaveKinxi(Itemp) = -60.0 + 20.0*K InitInp%Current%MorisonNodezi(Itemp)= InitInp%Waves%WaveKinzi(I) END DO END DO diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 4d1accbd50..4f644e665a 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -27,7 +27,7 @@ MODULE MoorDyn PRIVATE - TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn', 'v1.01.02F', '8-Apr-2016' ) + TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a1', '5 Jan. 2020' ) PUBLIC :: MD_Init @@ -53,32 +53,78 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er TYPE(MD_OutputType), INTENT( OUT) :: y ! INTENT( OUT) : Initial system outputs (outputs are not calculated; only the output mesh is initialized) TYPE(MD_MiscVarType), INTENT( OUT) :: m ! INTENT( OUT) : Initial misc/optimization variables REAL(DbKi), INTENT(INOUT) :: DTcoupling ! Coupling interval in seconds: the rate that Output is the actual coupling interval - TYPE(MD_InitOutputType), INTENT(INOUT) :: InitOut ! Output for initialization routine + TYPE(MD_InitOutputType), INTENT( OUT) :: InitOut ! Output for initialization routine INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None ! local variables REAL(DbKi) :: t ! instantaneous time, to be used during IC generation + INTEGER(IntKi) :: l ! index INTEGER(IntKi) :: I ! index INTEGER(IntKi) :: J ! index INTEGER(IntKi) :: K ! index + INTEGER(IntKi) :: Itemp ! index INTEGER(IntKi) :: Converged ! flag indicating whether the dynamic relaxation has converged INTEGER(IntKi) :: N ! convenience integer for readability: number of segments in the line REAL(ReKi) :: Pos(3) ! array for setting absolute fairlead positions in mesh - REAL(DbKi) :: TransMat(3,3) ! rotation matrix for setting fairlead positions correctly if there is initial platform rotation - REAL(DbKi), ALLOCATABLE :: FairTensIC(:,:)! array of size Nfairs, 3 to store three latest fairlead tensions of each line + REAL(ReKi) :: OrMat(3,3) ! rotation matrix for setting fairlead positions correctly if there is initial platform rotation + REAL(DbKi), ALLOCATABLE :: FairTensIC(:,:)! array of size nCpldCons, 3 to store three latest fairlead tensions of each line CHARACTER(20) :: TempString ! temporary string for incidental use INTEGER(IntKi) :: ErrStat2 ! Error status of the operation CHARACTER(ErrMsgLen) :: ErrMsg2 ! Error message if ErrStat2 /= ErrID_None - TYPE(MD_InputType) :: uArray(1) ! a size-one array for u to make call to TimeStep happy - REAL(DbKi) :: utimes(1) ! a size-one array saying time is 0 to make call to TimeStep happy + REAL(DbKi) :: dtM ! actual mooring dynamics time step + INTEGER(IntKi) :: NdtM ! number of time steps to integrate through with RK2 + + TYPE(MD_InputType) :: u_array(1) ! a size-one array for u to make call to TimeStep happy + REAL(DbKi) :: t_array(1) ! a size-one array saying time is 0 to make call to TimeStep happy + TYPE(MD_InputType) :: u_interp ! interpolated instantaneous input values to be calculated for each mooring time step + + CHARACTER(MaxWrScrLen) :: Message + + ! Local variables for reading file input (Previously in MDIO_ReadInput) + INTEGER(IntKi) :: UnIn ! Unit number for the input file + INTEGER(IntKi) :: UnEc ! The local unit number for this module's echo file + INTEGER(IntKi) :: UnOut ! for outputing wave kinematics data + CHARACTER(200) :: Frmt ! a string to hold a format statement + + CHARACTER(1024) :: EchoFile ! Name of MoorDyn echo file + CHARACTER(1024) :: Line ! String to temporarially hold value of read line + CHARACTER(20) :: LineOutString ! String to temporarially hold characters specifying line output options + CHARACTER(20) :: OptString ! String to temporarially hold name of option variable + CHARACTER(20) :: OptValue ! String to temporarially hold value of options variable input + INTEGER(IntKi) :: nOpts = 0 ! number of options lines in input file + CHARACTER(40) :: TempString1 ! + CHARACTER(40) :: TempString2 ! + CHARACTER(40) :: TempString3 ! + CHARACTER(40) :: TempString4 ! + CHARACTER(1024) :: FileName ! + + + CHARACTER(25) :: let1 ! strings used for splitting and parsing identifiers + CHARACTER(25) :: num1 + CHARACTER(25) :: let2 + CHARACTER(25) :: num2 + CHARACTER(25) :: let3 + + REAL(DbKi) :: tempArray(6) + REAL(ReKi) :: rRef(6) + REAL(DbKi) :: rRefDub(3) + + ! for reading output channels + CHARACTER(ChanLen),ALLOCATABLE :: OutList(:) ! array of output channel request (moved here from InitInput) + INTEGER :: MaxAryLen = 1000 ! Maximum length of the array being read + INTEGER :: NumWords ! Number of words contained on a line + INTEGER :: Nx + CHARACTER(*), PARAMETER :: RoutineName = 'MD_Init' + ErrStat = ErrID_None ErrMsg = "" + m%zeros6 = 0.0_DbKi ! Initialize the NWTC Subroutine Library CALL NWTC_Init( ) @@ -102,1034 +148,2499 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er p%RootName = TRIM(InitInp%RootName)//'.MD' ! all files written from this module will have this root name + !--------------------------------------------------------------------------------------------- + ! read input file and create cross-referenced mooring system objects + !--------------------------------------------------------------------------------------------- + + + UnEc = -1 - ! call function that reads input file and creates cross-referenced Connect and Line objects - CALL MDIO_ReadInput(InitInp, p, m, ErrStat2, ErrMsg2) - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN - - ! process the OutList array and set up the index arrays for the requested output quantities - CALL MDIO_ProcessOutList(InitInp%OutList, p, m, y, InitOut, ErrStat2, ErrMsg2 ) - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN - + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" !------------------------------------------------------------------------------------------------- - ! Connect mooring system together and make necessary allocations + ! Open the file !------------------------------------------------------------------------------------------------- + FileName = TRIM(InitInp%FileName) - CALL WrNR( ' Creating mooring system. ' ) - - p%NFairs = 0 ! this is the number of "vessel" type Connections. being consistent with MAP terminology - p%NConns = 0 ! this is the number of "connect" type Connections. not to be confused with NConnects, the number of Connections - p%NAnchs = 0 ! this is the number of "fixed" type Connections. - - ! cycle through Connects and identify Connect types - DO I = 1, p%NConnects - - TempString = m%ConnectList(I)%type - CALL Conv2UC(TempString) - if (TempString == 'FIXED') then - m%ConnectList(I)%TypeNum = 0 - p%NAnchs = p%NAnchs + 1 - else if (TempString == 'VESSEL') then - m%ConnectList(I)%TypeNum = 1 - p%NFairs = p%NFairs + 1 ! if a vessel connection, increment fairlead counter - else if (TempString == 'CONNECT') then - m%ConnectList(I)%TypeNum = 2 - p%NConns = p%NConns + 1 - else - CALL CheckError( ErrID_Fatal, 'Error in provided Connect type. Must be fixed, vessel, or connect.' ) + CALL GetNewUnit( UnIn ) + CALL OpenFInpFile( UnIn, FileName, ErrStat2, ErrMsg2 ) + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + IF ( ErrStat >= AbortErrLev ) THEN + CALL CleanUp() RETURN END IF - END DO - - CALL WrScr(trim(Num2LStr(p%NFairs))//' fairleads, '//trim(Num2LStr(p%NAnchs))//' anchors, '//trim(Num2LStr(p%NConns))//' connects.') - ! allocate fairleads list - ALLOCATE ( m%FairIdList(p%NFairs), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN - CALL CheckError( ErrID_Fatal, 'Error allocating space for FairIdList array.') - RETURN - END IF + !CALL WrScr( ' MD_Init: Opening MoorDyn input file: '//FileName ) - ! allocate connect list - ALLOCATE ( m%ConnIdList(p%NConns), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN - CALL CheckError( ErrID_Fatal, 'Error allocating space for ConnIdList array.') - RETURN - END IF + + ! ----------------- go through file contents a first time, counting each entry ----------------------- + + i = 0 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i !read a line + + do while ( ErrStat2 == 0 ) + + + if (INDEX(Line, "---") > 0) then ! look for a header line + + if ( ( INDEX(Line, "LINE DICTIONARY") > 0) .or. ( INDEX(Line, "LINE TYPES") > 0) ) then ! if line dictionary header +print *, "line dictionary" + ! skip following two lines (label line and unit line) + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! find how many elements of this type there are + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line + p%nLineTypes = p%nLineTypes + 1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," linetype on prev line" + END DO - ! now go back through and record the fairlead Id numbers (this is all the "connecting" that's required) - J = 1 ! counter for fairlead number - K = 1 ! counter for connect number - DO I = 1,p%NConnects - IF (m%ConnectList(I)%TypeNum == 1) THEN - m%FairIdList(J) = I ! if a vessel connection, add ID to list - J = J + 1 - ELSE IF (m%ConnectList(I)%TypeNum == 2) THEN - m%ConnIdList(K) = I ! if a connect connection, add ID to list - K = K + 1 - END IF - END DO + else if ( (INDEX(Line, "ROD DICTIONARY") > 0) .or. ( INDEX(Line, "ROD TYPES") > 0) ) then ! if rod dictionary header +print *, "rod dictionary" + ! skip following two lines (label line and unit line) + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! find how many elements of this type there are + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line + p%nRodTypes = p%nRodTypes + 1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," rod type on prev line" + END DO + + else if ((INDEX(Line, "BODY LIST") > 0 ) .or. (INDEX(Line, "BODY PROPERTIES") > 0 )) then +print *, "body list" + ! skip following two lines (label line and unit line) + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! find how many elements of this type there are + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line + p%nBodies = p%nBodies + 1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," body on prev line" + END DO + + else if ((INDEX(Line, "ROD LIST") > 0) .or. (INDEX(Line, "ROD PROPERTIES") > 0)) then ! if rod properties header +print *, "rod list" + ! skip following two lines (label line and unit line) + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! find how many elements of this type there are + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line + p%nRods = p%nRods + 1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," rod on prev line" + END DO + + else if ( (INDEX(Line, "CONNECTION PROPERTIES") > 0) .or. (INDEX(Line, "NODE PROPERTIES") > 0) ) then ! if node properties header +print *, "connections" + ! skip following two lines (label line and unit line) + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! find how many elements of this type there are + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line + p%nConnects = p%nConnects + 1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," con on prev line" + END DO + else if (INDEX(Line, "LINE PROPERTIES") > 0) then ! if line properties header +print *, "lines" + ! skip following two lines (label line and unit line) + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! find how many elements of this type there are + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line + p%nLines = p%nLines + 1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," line on prev line" + END DO - ! go through lines and allocate variables - DO I = 1, p%NLines - CALL SetupLine( m%LineList(I), m%LineTypeList(m%LineList(I)%PropsIdNum), p%rhoW , ErrStat2, ErrMsg2) - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN - END DO + else if (INDEX(Line, "FAILURE") > 0) then ! if failure conditions header + print *, " Reading failure conditions: "; + + ! skip following two lines (label line and unit line) + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! find how many elements of this type there are + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line + p%nFails = p%nFails + 1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + END DO + + + else if (INDEX(Line, "OPTIONS") > 0) then ! if options header +print *, "options" + ! don't skip any lines (no column headers for the options section) + + ! find how many options have been specified + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line + nOpts = nOpts + 1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," option on prev line" + END DO + - !------------------------------------------------------------------------------------ - ! prepare state vector - !------------------------------------------------------------------------------------ + else if (INDEX(Line, "OUTPUT") > 0) then ! if output header +print *, "output" + ! we don't need to count this section... - ! allocate list of starting state vector indices for each line - does this belong elsewhere? - ALLOCATE ( m%LineStateIndList(p%NLines), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN - CALL CheckError(ErrID_Fatal, ' Error allocating LineStateIndList array.') - RETURN - END IF + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i - ! figure out required size of state vector and how it will be apportioned to Connect and Lines (J is keeping track of the growing size of the state vector) - J = p%NConns*6 ! start index of first line's states (added six state variables for each "connect"-type connection) + else ! otherwise ignore this line that isn't a recognized header line and read the next line + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," --- unrecognized header" + end if + + else ! otherwise ignore this line, which doesn't have the "---" or header line and read the next line + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i, " .." + end if + + end do + + p%nConnectsExtra = p%nConnects + 2*p%nLines ! set maximum number of connections, accounting for possible detachment of each line end and a connection for that + + print *, "Identified ", p%nLineTypes , "LineTypes in input file." + print *, "Identified ", p%nRodTypes , "RodTypes in input file." + print *, "Identified ", p%nBodies , "Bodies in input file." + print *, "Identified ", p%nRods , "Rods in input file." + print *, "Identified ", p%nConnects , "Connections in input file." + print *, "Identified ", p%nLines , "Lines in input file." - DO I = 1, p%NLines - m%LineStateIndList(I) = J+1 ! assign start index of each line - J = J + 6*(m%LineList(I)%N - 1) !add 6 state variables for each internal node - END DO - ! allocate state vector for RK2 based on size just calculated - ALLOCATE ( x%states(J), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN - ErrMsg = ' Error allocating state vector.' - !CALL CleanUp() - RETURN - END IF + ! ----------------------------- misc checks to be sorted ----------------------------- - ! get header information for the FAST output file <<< what does this mean? + ! make sure nLineTypes isn't zero + IF ( p%nLineTypes < 1 ) THEN + CALL SetErrStat( ErrID_Fatal, 'nLineTypes parameter must be greater than zero.', ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF + + ! make sure NLines is at least one + IF ( p%NLines < 1 ) THEN + CALL SetErrStat( ErrID_Fatal, 'NLines parameter must be at least 1.', ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF - !-------------------------------------------------------------------------- - ! create i/o meshes for fairlead positions and forces - !-------------------------------------------------------------------------- + + - ! create input mesh for fairlead kinematics - CALL MeshCreate(BlankMesh=u%PtFairleadDisplacement , & - IOS= COMPONENT_INPUT , & - Nnodes=p%NFairs , & - TranslationDisp=.TRUE. , & - TranslationVel=.TRUE. , & - ErrStat=ErrStat2 , & - ErrMess=ErrMsg2) + ! ----------------------------- allocate necessary arrays ---------------------------- - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN + ! Allocate object arrays - ! --------------------------- set up initial condition of each fairlead ------------------------------- - DO i = 1,p%NFairs + ALLOCATE(m%LineTypeList(p%nLineTypes), STAT = ErrStat2 ); if(AllocateFailed("LineTypeList")) return + ALLOCATE(m%RodTypeList( p%nRodTypes ), STAT = ErrStat2 ); if(AllocateFailed("LineTypeList")) return + + ALLOCATE(m%BodyList( p%nBodies ), STAT = ErrStat2 ); if(AllocateFailed("BodyList" )) return + ALLOCATE(m%RodList( p%nRods ), STAT = ErrStat2 ); if(AllocateFailed("RodList" )) return + ALLOCATE(m%ConnectList( p%nConnects ), STAT = ErrStat2 ); if(AllocateFailed("ConnectList" )) return + ALLOCATE(m%LineList( p%nLines ), STAT = ErrStat2 ); if(AllocateFailed("LineList" )) return + + ALLOCATE(m%FailList( p%nFails ), STAT = ErrStat2 ); if(AllocateFailed("FailList" )) return + + + ! Allocate associated index arrays (note: some are allocated larger than will be used, for simplicity) + ALLOCATE(m%BodyStateIs1(p%nBodies ), m%BodyStateIsN(p%nBodies ), STAT=ErrStat2); if(AllocateFailed("BodyStateIs1/N")) return + ALLOCATE(m%RodStateIs1(p%nRods ), m%RodStateIsN(p%nRods ), STAT=ErrStat2); if(AllocateFailed("RodStateIs1/N" )) return + ALLOCATE(m%ConStateIs1(p%nConnects), m%ConStateIsN(p%nConnects), STAT=ErrStat2); if(AllocateFailed("ConStateIs1/N" )) return + ALLOCATE(m%LineStateIs1(p%nLines) , m%LineStateIsN(p%nLines) , STAT=ErrStat2); if(AllocateFailed("LineStateIs1/N")) return - Pos(1) = m%ConnectList(m%FairIdList(i))%conX ! set relative position of each fairlead i (I'm pretty sure this is just relative to ptfm origin) - Pos(2) = m%ConnectList(m%FairIdList(i))%conY - Pos(3) = m%ConnectList(m%FairIdList(i))%conZ + ALLOCATE(m%FreeBodyIs( p%nBodies ), m%CpldBodyIs(p%nBodies ), STAT=ErrStat2); if(AllocateFailed("BodyIs")) return + ALLOCATE(m%FreeRodIs( p%nRods ), m%CpldRodIs( p%nRods ), STAT=ErrStat2); if(AllocateFailed("RodIs")) return + ALLOCATE(m%FreeConIs( p%nConnects), m%CpldConIs(p%nConnects),STAT=ErrStat2); if(AllocateFailed("ConnectIs")) return - CALL MeshPositionNode(u%PtFairleadDisplacement,i,Pos,ErrStat2,ErrMsg2)! "assign the coordinates of each node in the global coordinate space" - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN + ! ---------------------- now go through again and process file contents -------------------- + REWIND(UnIn) ! rewind to start of input file + + ! note: no longer worrying about "Echo" option + + Nx = 0 ! set state counter to zero + i = 0 ! set line number counter to zero + + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i !read a line + + do while ( ErrStat2 == 0 ) + + if (INDEX(Line, "---") > 0) then ! look for a header line - ! set offset position of each node to according to initial platform position - CALL SmllRotTrans('initial fairlead positions due to platform rotation', InitInp%PtfmInit(4),InitInp%PtfmInit(5),InitInp%PtfmInit(6), TransMat, '', ErrStat2, ErrMsg2) ! account for possible platform rotation + !------------------------------------------------------------------------------------------- + if ( ( INDEX(Line, "LINE DICTIONARY") > 0) .or. ( INDEX(Line, "LINE TYPES") > 0) ) then ! if line dictionary header + + print *, "Reading line types" + + ! skip following two lines (label line and unit line) + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! process each line + DO l = 1,p%nLineTypes + + !read into a line + READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! parse out entries: Name Diam MassDenInAir EA cIntDamp >>EI(new)<< Can Cat Cdn Cdt + READ(Line,*,IOSTAT=ErrStat2) m%LineTypeList(l)%name, m%LineTypeList(l)%d, & + m%LineTypeList(l)%w, tempString1, tempString2, tempString3, & + m%LineTypeList(l)%Can, m%LineTypeList(l)%Cat, m%LineTypeList(l)%Cdn, m%LineTypeList(l)%Cdt + + IF ( ErrStat2 /= ErrID_None ) THEN + CALL SetErrStat( ErrID_Fatal, 'Failed to process line type inputs of entry '//trim(Num2LStr(l))//'. Check formatting and correct number of columns.', ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF + + ! process stiffness, damping, and bending coefficients (which might use lookup tables) + CALL getCoefficientOrCurve(tempString1, m%LineTypeList(l)%EA, & + m%LineTypeList(l)%nEApoints, & + m%LineTypeList(l)%stiffXs, & + m%LineTypeList(l)%stiffYs, ErrStat2, ErrMsg2) + CALL getCoefficientOrCurve(tempString2, m%LineTypeList(l)%BA, & + m%LineTypeList(l)%nBpoints, & + m%LineTypeList(l)%dampXs, & + m%LineTypeList(l)%dampYs, ErrStat2, ErrMsg2) + CALL getCoefficientOrCurve(tempString3, m%LineTypeList(l)%EI, & + m%LineTypeList(l)%nEIpoints, & + m%LineTypeList(l)%bstiffXs, & + m%LineTypeList(l)%bstiffYs, ErrStat2, ErrMsg2) + + ! specify IdNum of line type for error checking + m%LineTypeList(l)%IdNum = l + + + IF ( ErrStat2 /= ErrID_None ) THEN + CALL SetErrStat( ErrID_Fatal, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF + + END DO + + + !------------------------------------------------------------------------------------------- + else if ( (INDEX(Line, "ROD DICTIONARY") > 0) .or. ( INDEX(Line, "ROD TYPES") > 0) ) then ! if rod dictionary header + + print *, "Reading rod types" + + ! skip following two lines (label line and unit line) + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! process each line + DO l = 1,p%nRodTypes + + !read into a line + READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN + ! parse out entries: Name Diam MassDenInAir Can Cat Cdn Cdt + IF (ErrStat2 == 0) THEN + READ(Line,*,IOSTAT=ErrStat2) m%RodTypeList(l)%name, m%RodTypeList(l)%d, m%RodTypeList(l)%w, & + m%RodTypeList(l)%Can, m%RodTypeList(l)%Cat, m%RodTypeList(l)%Cdn, m%RodTypeList(l)%Cdt, & + m%RodTypeList(l)%CaEnd, m%RodTypeList(l)%CdEnd + END IF - ! Apply initial platform rotations and translations (fixed Jun 19, 2015) - u%PtFairleadDisplacement%TranslationDisp(1,i) = InitInp%PtfmInit(1) + Transmat(1,1)*Pos(1) + Transmat(2,1)*Pos(2) + TransMat(3,1)*Pos(3) - Pos(1) - u%PtFairleadDisplacement%TranslationDisp(2,i) = InitInp%PtfmInit(2) + Transmat(1,2)*Pos(1) + Transmat(2,2)*Pos(2) + TransMat(3,2)*Pos(3) - Pos(2) - u%PtFairleadDisplacement%TranslationDisp(3,i) = InitInp%PtfmInit(3) + Transmat(1,3)*Pos(1) + Transmat(2,3)*Pos(2) + TransMat(3,3)*Pos(3) - Pos(3) + ! specify IdNum of rod type for error checking + m%RodTypeList(l)%IdNum = l - ! set velocity of each node to zero - u%PtFairleadDisplacement%TranslationVel(1,i) = 0.0_DbKi - u%PtFairleadDisplacement%TranslationVel(2,i) = 0.0_DbKi - u%PtFairleadDisplacement%TranslationVel(3,i) = 0.0_DbKi - - !print *, 'Fairlead ', i, ' z TranslationDisp at start is ', u%PtFairleadDisplacement%TranslationDisp(3,i) - !print *, 'Fairlead ', i, ' z Position at start is ', u%PtFairleadDisplacement%Position(3,i) + IF ( ErrStat2 /= ErrID_None ) THEN + CALL SetErrStat( ErrID_Fatal, 'Failed to process rod type properties for rod '//trim(Num2LStr(l)), ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF - ! set each node as a point element - CALL MeshConstructElement(u%PtFairleadDisplacement, ELEMENT_POINT, ErrStat2, ErrMsg2, i) + END DO - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN - END DO ! I + !------------------------------------------------------------------------------------------- + else if ((INDEX(Line, "BODY LIST") > 0 ) .or. (INDEX(Line, "BODY PROPERTIES") > 0 )) then + ! skip following two lines (label line and unit line) + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! process each body + DO l = 1,p%nBodies + + !read into a line + READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! parse out entries: Node Type X Y Z M V FX FY FZ CdA Ca + IF (ErrStat2 == 0) THEN + READ(Line,*,IOSTAT=ErrStat2) tempString1, & + tempArray(1), tempArray(2), tempArray(3), tempArray(4), tempArray(5), tempArray(6), & + m%BodyList(l)%rCG(1), m%BodyList(l)%rCG(2), m%BodyList(l)%rCG(3), & + m%BodyList(l)%bodyM, m%BodyList(l)%bodyV, m%BodyList(l)%bodyI(1), m%BodyList(l)%bodyI(2), m%BodyList(l)%bodyI(3), & + m%BodyList(l)%bodyCdA(1), m%BodyList(l)%bodyCa(1) + END IF - CALL MeshCommit ( u%PtFairleadDisplacement, ErrStat, ErrMsg ) + IF ( ErrStat2 /= 0 ) THEN + CALL WrScr(' Unable to parse Body '//trim(Num2LStr(l))//' on row '//trim(Num2LStr(i))//' in input file.') ! Specific screen output because errors likely + CALL WrScr(' Ensure row has all 17 columns.') + CALL SetErrStat( ErrID_Fatal, 'Failed to read bodies.' , ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF + - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN + !----------- process body type (not considering input fixed bodies for now, only the GroundBody) ----------------- + + call DecomposeString(tempString1, let1, num1, let2, num2, let3) + + READ(num1, *) m%BodyList(l)%IdNum ! convert to int, representing parent body index + + if ((let2 == "COUPLED") .or. (let2 == "VESSEL")) then ! if a coupled body + + m%BodyList(l)%typeNum = -1 + p%nCpldBodies=p%nCpldBodies+1 ! add this rod to coupled list + m%CpldBodyIs(p%nCpldBodies) = l - ! copy the input fairlead kinematics mesh to make the output mesh for fairlead loads, PtFairleadLoad - CALL MeshCopy ( SrcMesh = u%PtFairleadDisplacement, DestMesh = y%PtFairleadLoad, & - CtrlCode = MESH_SIBLING, IOS = COMPONENT_OUTPUT, & - Force = .TRUE., ErrStat = ErrStat2, ErrMess=ErrMsg2 ) + ! body initial position due to coupling will be adjusted later + + else if ((let2 == "FREE") .or. (LEN_TRIM(let2)== 0)) then ! if a free body + m%BodyList(l)%typeNum = 0 + + p%nFreeBodies=p%nFreeBodies+1 ! add this pinned rod to the free list because it is half free + + m%BodyStateIs1(p%nFreeBodies) = Nx+1 + m%BodyStateIsN(p%nFreeBodies) = Nx+12 + Nx = Nx + 12 ! add 12 state variables for free Body + + m%FreeBodyIs(p%nFreeBodies) = l + + m%BodyList(l)%r6 = tempArray ! set initial body position and orientation + + else + CALL SetErrStat( ErrID_Fatal, "Unidentified Body type string for Body "//trim(Num2LStr(l))//": "//trim(tempString2), ErrStat, ErrMsg, RoutineName ) + return + end if + + + ! check for sequential IdNums + IF ( m%BodyList(l)%IdNum .NE. l ) THEN + CALL SetErrStat( ErrID_Fatal, 'Body numbers must be sequential starting from 1.', ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF + + ! set initial velocity to zero + m%BodyList(l)%v6 = 0.0_DbKi + + !also set number of attached rods and points to zero initially + m%BodyList(l)%nAttachedC = 0 + m%BodyList(l)%nAttachedR = 0 + + ! if there was a body setup function, it would get called here, but I don't think it's needed. + + IF ( ErrStat2 /= 0 ) THEN + CALL SetErrStat( ErrID_Fatal, 'Failed to read data for body '//trim(Num2LStr(l)), ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF + + print *, "Set up body ", l, " of type ", m%BodyList(l)%typeNum - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN + END DO + + + !------------------------------------------------------------------------------------------- + else if ((INDEX(Line, "ROD LIST") > 0) .or. (INDEX(Line, "ROD PROPERTIES") > 0)) then ! if rod properties header + print *, "Reading rods" + + ! skip following two lines (label line and unit line) + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! process each rod + DO l = 1,p%nRods + + !read into a line + READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! parse out entries: RodID Type/BodyID RodType Xa Ya Za Xb Yb Zb NumSegs Flags/Outputs + IF (ErrStat2 == 0) THEN + READ(Line,*,IOSTAT=ErrStat2) m%RodList(l)%IdNum, tempString1, tempString2, & + tempArray(1), tempArray(2), tempArray(3), tempArray(4), tempArray(5), tempArray(6), & + m%RodList(l)%N, LineOutString + END IF - ! -------------------------------------------------------------------- - ! go through all Connects and set position based on input file - ! -------------------------------------------------------------------- - ! first do it for all connections (connect and anchor types will be saved) - DO I = 1, p%NConnects - m%ConnectList(I)%r(1) = m%ConnectList(I)%conX - m%ConnectList(I)%r(2) = m%ConnectList(I)%conY - m%ConnectList(I)%r(3) = m%ConnectList(I)%conZ - m%ConnectList(I)%rd(1) = 0.0_DbKi - m%ConnectList(I)%rd(2) = 0.0_DbKi - m%ConnectList(I)%rd(3) = 0.0_DbKi - END DO + !----------- process rod type ----------------- + + call DecomposeString(tempString1, let1, num1, let2, num2, let3) + + if ((let1 == "ANCHOR") .or. (let1 == "FIXED") .or. (let1 == "FIX")) then + m%RodList(l)%typeNum = 2 + CALL Body_AddRod(m%GroundBody, l, tempArray) ! add rod l to Ground body + + + else if ((let1 == "PINNED") .or. (let1 == "PIN")) then + m%RodList(l)%typeNum = 1 + CALL Body_AddRod(m%GroundBody, l, tempArray) ! add rod l to Ground body + + p%nFreeRods=p%nFreeRods+1 ! add this pinned rod to the free list because it is half free + + m%RodStateIs1(p%nFreeRods) = Nx+1 + m%RodStateIsN(p%nFreeRods) = Nx+6 + Nx = Nx + 6 ! add 6 state variables for each pinned rod + + m%FreeRodIs(p%nFreeRods) = l + + else if (let1 == "BODY") then ! attached to a body (either rididly or pinned) + + if (len_trim(num1) > 0) then + + READ(num1,*) J ! convert to int, representing parent body index + + if ((J <= p%nBodies) .and. (J > 0)) then + + CALL Body_AddRod(m%BodyList(J), l, tempArray) ! add rod l to the body + + if ((INDEX(let2, "PINNED") > 0) .or. (INDEX(let2, "PIN") > 0)) then + m%RodList(l)%typeNum = 1 + + p%nFreeRods=p%nFreeRods+1 ! add this pinned rod to the free list because it is half free + + m%RodStateIs1(p%nFreeRods) = Nx+1 + m%RodStateIsN(p%nFreeRods) = Nx+6 + Nx = Nx + 6 ! add 6 state variables for each pinned rod + + m%FreeRodIs(p%nFreeRods) = l + + else + m%RodList(l)%typeNum = 2 + end if + + else + CALL SetErrStat( ErrID_Severe, "Body ID out of bounds for Rod "//trim(Num2LStr(l))//".", ErrStat, ErrMsg, RoutineName ) + return + end if + + else + CALL SetErrStat( ErrID_Severe, "No number provided for Rod "//trim(Num2LStr(l))//" Body attachment.", ErrStat, ErrMsg, RoutineName ) + return + end if + + else if ((let1 == "VESSEL") .or. (let1 == "VES")) then ! if a rigid fairlead, add to list and add + m%RodList(l)%typeNum = -2 + m%CpldRodIs(p%nCpldRods) = l; p%nCpldRods=p%nCpldRods+1 ! add this rod to coupled list + + else if ((let1 == "VESSELPINNED") .or. (let1 == "VESPIN")) then ! if a pinned fairlead, add to list and add + m%RodList(l)%typeNum = -1 + + p%nCpldRods=p%nCpldRods+1 ! add + p%nFreeRods=p%nFreeRods+1 ! add this pinned rod to the free list because it is half free + + m%RodStateIs1(p%nFreeRods) = Nx+1 + m%RodStateIsN(p%nFreeRods) = Nx+6 + Nx = Nx + 6 ! add 6 state variables for each pinned rod + + m%CpldRodIs(p%nCpldRods) = l + m%FreeRodIs(p%nFreeRods) = l + + else if ((let1 == "CONNECT") .or. (let1 == "CON") .or. (let1 == "FREE")) then + m%RodList(l)%typeNum = 0 + + p%nFreeRods=p%nFreeRods+1 ! add this pinned rod to the free list because it is half free + + m%RodStateIs1(p%nFreeRods) = Nx+1 + m%RodStateIsN(p%nFreeRods) = Nx+12 + Nx = Nx + 12 ! add 12 state variables for free Rod + + m%FreeRodIs(p%nFreeRods) = l + + else + + CALL SetErrStat( ErrID_Severe, "Unidentified Type/BodyID for Rod "//trim(Num2LStr(l))//": "//trim(tempString1), ErrStat, ErrMsg, RoutineName ) + return + end if + + + ! find Rod properties index + DO J = 1,p%nRodTypes + IF (trim(tempString2) == trim(m%RodTypeList(J)%name)) THEN + m%RodList(l)%PropsIdNum = J + EXIT + IF (J == p%nRodTypes) THEN ! call an error if there is no match + CALL SetErrStat( ErrID_Severe, 'Unable to find matching rod type name for Rod '//trim(Num2LStr(l))//": "//trim(tempString2), ErrStat, ErrMsg, RoutineName ) + END IF + END IF + END DO + + + ! process output flag characters (LineOutString) and set line output flag array (OutFlagList) + m%RodList(l)%OutFlagList = 0 ! first set array all to zero + ! per node, 3 component + IF ( scan( LineOutString, 'p') > 0 ) m%RodList(l)%OutFlagList(2 ) = 1 ! node position + IF ( scan( LineOutString, 'v') > 0 ) m%RodList(l)%OutFlagList(3 ) = 1 ! node velocity + IF ( scan( LineOutString, 'U') > 0 ) m%RodList(l)%OutFlagList(4 ) = 1 ! water velocity + IF ( scan( LineOutString, 'B') > 0 ) m%RodList(l)%OutFlagList(5 ) = 1 ! node buoyancy force + IF ( scan( LineOutString, 'D') > 0 ) m%RodList(l)%OutFlagList(6 ) = 1 ! drag force + IF ( scan( LineOutString, 'I') > 0 ) m%RodList(l)%OutFlagList(7 ) = 1 ! inertia force + IF ( scan( LineOutString, 'P') > 0 ) m%RodList(l)%OutFlagList(8 ) = 1 ! dynamic pressure force + IF ( scan( LineOutString, 'b') > 0 ) m%RodList(l)%OutFlagList(9 ) = 1 ! seabed contact forces + ! per node, 1 component + IF ( scan( LineOutString, 'W') > 0 ) m%RodList(l)%OutFlagList(10) = 1 ! node weight/buoyancy (positive up) + IF ( scan( LineOutString, 'K') > 0 ) m%RodList(l)%OutFlagList(11) = 1 ! curvature at node + ! per element, 1 component >>> these don't apply to a rod!! <<< + IF ( scan( LineOutString, 't') > 0 ) m%RodList(l)%OutFlagList(12) = 1 ! segment tension force (just EA) + IF ( scan( LineOutString, 'c') > 0 ) m%RodList(l)%OutFlagList(13) = 1 ! segment internal damping force + IF ( scan( LineOutString, 's') > 0 ) m%RodList(l)%OutFlagList(14) = 1 ! Segment strain + IF ( scan( LineOutString, 'd') > 0 ) m%RodList(l)%OutFlagList(15) = 1 ! Segment strain rate + + IF (SUM(m%RodList(l)%OutFlagList) > 0) m%RodList(l)%OutFlagList(1) = 1 ! this first entry signals whether to create any output file at all + ! the above letter-index combinations define which OutFlagList entry corresponds to which output type + + + ! specify IdNum of line for error checking + m%RodList(l)%IdNum = l + + ! check for sequential IdNums + IF ( m%RodList(l)%IdNum .NE. l ) THEN + CALL SetErrStat( ErrID_Fatal, 'Line numbers must be sequential starting from 1.', ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF + + ! set up rod + CALL Rod_Setup( m%RodList(l), m%RodTypeList(m%RodList(l)%PropsIdNum), tempArray, p%rhoW, ErrStat2, ErrMsg2) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN + + ! note: Rod was already added to its respective parent body if type > 0 + + IF ( ErrStat2 /= 0 ) THEN + CALL SetErrStat( ErrID_Fatal, 'Failed to read rod data for Rod '//trim(Num2LStr(l)), ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF - ! then do it for fairlead types - DO I = 1,p%NFairs - DO J = 1, 3 - m%ConnectList(m%FairIdList(I))%r(J) = u%PtFairleadDisplacement%Position(J,I) + u%PtFairleadDisplacement%TranslationDisp(J,I) - m%ConnectList(m%FairIdList(I))%rd(J) = 0.0_DbKi - END DO - END DO + END DO ! l = 1,p%nRods - ! for connect types, write the coordinates to the state vector - DO I = 1,p%NConns - x%states(6*I-2:6*I) = m%ConnectList(m%ConnIdList(I))%r ! double check order of r vs rd - x%states(6*I-5:6*I-3) = m%ConnectList(m%ConnIdList(I))%rd - END DO - ! -------------------------------------------------------------------- - ! open output file(s) and write header lines - CALL MDIO_OpenOutput( InitInp%FileName, p, m, InitOut, ErrStat2, ErrMsg2 ) - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN - ! -------------------------------------------------------------------- + !------------------------------------------------------------------------------------------- + else if ( (INDEX(Line, "CONNECTION PROPERTIES") > 0) .or. (INDEX(Line, "NODE PROPERTIES") > 0) ) then ! if node properties header + + print *, "Reading connections" + + ! skip following two lines (label line and unit line) + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! process each point + DO l = 1,p%nConnects + + !read into a line + READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! parse out entries: Node Type X Y Z M V FX FY FZ CdA Ca + IF (ErrStat2 == 0) THEN + READ(Line,*,IOSTAT=ErrStat2) m%ConnectList(l)%IdNum, tempString1, tempArray(1), & + tempArray(2), tempArray(3), m%ConnectList(l)%conM, & + m%ConnectList(l)%conV, m%ConnectList(l)%conFX, m%ConnectList(l)%conFY, & + m%ConnectList(l)%conFZ, m%ConnectList(l)%conCdA, m%ConnectList(l)%conCa + END IF + + + IF ( ErrStat2 /= 0 ) THEN + CALL WrScr(' Unable to parse Connection '//trim(Num2LStr(I))//' row in input file.') ! Specific screen output because errors likely + CALL WrScr(' Ensure row has all 12 columns, including CdA and Ca.') ! to be caused by non-updated input file formats. + CALL SetErrStat( ErrID_Fatal, 'Failed to read connects.' , ErrStat, ErrMsg, RoutineName ) ! would be nice to specify which line <<<<<<<<< + CALL CleanUp() + RETURN + END IF + - ! -------------------------------------------------------------------- - ! size active tensioning inputs arrays based on highest channel number read from input file for now <<<<<<< - ! -------------------------------------------------------------------- - - ! find the highest channel number - N = 0 - DO I = 1, p%NLines - IF ( m%LineList(I)%CtrlChan > N ) then - N = m%LineList(I)%CtrlChan - END IF - END DO - - ! allocate the input arrays - ALLOCATE ( u%DeltaL(N), u%DeltaLdot(N), STAT = ErrStat2 ) - + !----------- process connection type ----------------- + + call DecomposeString(tempString1, let1, num1, let2, num2, let3) + + if ((let1 == "ANCHOR") .or. (let1 == "FIXED") .or. (let1 == "FIX")) then + m%ConnectList(l)%typeNum = 1 + + m%ConnectList(l)%r = tempArray(1:3) ! set initial node position + + CALL Body_AddConnect(m%GroundBody, l, tempArray(1:3)) ! add connection l to Ground body + + + else if (let1 == "BODY") then ! attached to a body + if (len_trim(num1) > 0) then + READ(num1, *) J ! convert to int, representing parent body index + + if ((J <= p%nBodies) .and. (J > 0)) then + m%ConnectList(l)%typeNum = 1 + + CALL Body_AddConnect(m%BodyList(J), l, tempArray(1:3)) ! add connection l to Ground body + + else + CALL SetErrStat( ErrID_Severe, "Body ID out of bounds for Connection "//trim(Num2LStr(l))//".", ErrStat, ErrMsg, RoutineName ) + return + end if + else + CALL SetErrStat( ErrID_Severe, "No number provided for Connection "//trim(Num2LStr(l))//" Body attachment.", ErrStat, ErrMsg, RoutineName ) + return + end if + + else if ((let1 == "VESSEL") .or. (let1 == "VES")) then ! if a fairlead, add to list and add + m%ConnectList(l)%typeNum = -1 + p%nCpldCons=p%nCpldCons+1 ! add this rod to coupled list + m%CpldConIs(p%nCpldCons) = l + + ! this is temporary for backwards compatibility >>>>> will need to update for more versatile coupling >>>> + CALL SmllRotTrans('PtfmInit', InitInp%PtfmInit(4),InitInp%PtfmInit(5),InitInp%PtfmInit(6), OrMat, '', ErrStat2, ErrMsg2) + + ! set initial node position, including adjustments due to initial platform rotations and translations <<< could convert to array math + m%ConnectList(l)%r(1) = InitInp%PtfmInit(1) + OrMat(1,1)*tempArray(1) + OrMat(2,1)*tempArray(2) + OrMat(3,1)*tempArray(3) + m%ConnectList(l)%r(2) = InitInp%PtfmInit(2) + OrMat(1,2)*tempArray(1) + OrMat(2,2)*tempArray(2) + OrMat(3,2)*tempArray(3) + m%ConnectList(l)%r(3) = InitInp%PtfmInit(3) + OrMat(1,3)*tempArray(1) + OrMat(2,3)*tempArray(2) + OrMat(3,3)*tempArray(3) + + else if ((let1 == "CONNECT") .or. (let1 == "CON") .or. (let1 == "FREE")) then + m%ConnectList(l)%typeNum = 0 + + p%nFreeCons=p%nFreeCons+1 ! add this pinned rod to the free list because it is half free + + m%ConStateIs1(p%nFreeCons) = Nx+1 + m%ConStateIsN(p%nFreeCons) = Nx+6 + Nx = Nx + 6 ! add 12 state variables for free Connection + + m%FreeConIs(p%nFreeCons) = l + + m%ConnectList(l)%r = tempArray(1:3) ! set initial node position + + - ! -------------------------------------------------------------------- - ! go through lines and initialize internal node positions using Catenary() - ! -------------------------------------------------------------------- - DO I = 1, p%NLines + else + CALL SetErrStat( ErrID_Severe, "Unidentified Type/BodyID for Connection "//trim(Num2LStr(l))//": "//trim(tempString2), ErrStat, ErrMsg, RoutineName ) + return + end if + + ! set initial velocity to zero + m%ConnectList(l)%rd(1) = 0.0_DbKi + m%ConnectList(l)%rd(2) = 0.0_DbKi + m%ConnectList(l)%rd(3) = 0.0_DbKi + + ! possibly redundant <<< should revisit + m%ConnectList(l)%conX = tempArray(1) + m%ConnectList(l)%conY = tempArray(2) + m%ConnectList(l)%conZ = tempArray(3) - N = m%LineList(I)%N ! for convenience - !TODO: apply any initial adjustment of line length from active tensioning <<<<<<<<<<<< - ! >>> maybe this should be skipped <<<< - - ! set end node positions and velocities from connect objects - m%LineList(I)%r(:,N) = m%ConnectList(m%LineList(I)%FairConnect)%r - m%LineList(I)%r(:,0) = m%ConnectList(m%LineList(I)%AnchConnect)%r - m%LineList(I)%rd(:,N) = (/ 0.0, 0.0, 0.0 /) ! set anchor end velocities to zero - m%LineList(I)%rd(:,0) = (/ 0.0, 0.0, 0.0 /) ! set fairlead end velocities to zero + !also set number of attached lines to zero initially + m%ConnectList(l)%nAttached = 0 - ! set initial line internal node positions using quasi-static model or straight-line interpolation from anchor to fairlead - CALL InitializeLine( m%LineList(I), m%LineTypeList(m%LineList(I)%PropsIdNum), p%rhoW , ErrStat2, ErrMsg2) - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN - IF (ErrStat >= ErrId_Warn) CALL WrScr(" Catenary solver failed for one or more lines. Using linear node spacing.") ! make this statement more accurate - - ! assign the resulting internal node positions to the integrator initial state vector! (velocities leave at 0) - DO J = 1, N-1 - DO K = 1, 3 - x%states(m%LineStateIndList(I) + 3*N-3 + 3*J-3 + K-1 ) = m%LineList(I)%r(K,J) ! assign position - x%states(m%LineStateIndList(I) + 3*J-3 + K-1 ) = 0.0_DbKi ! assign velocities (of zero) - END DO - END DO - - END DO !I = 1, p%NLines + ! check for sequential IdNums + IF ( m%ConnectList(l)%IdNum .NE. l ) THEN + CALL SetErrStat( ErrID_Fatal, 'Connection numbers must be sequential starting from 1.', ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF + -! ! try writing output for troubleshooting purposes (TEMPORARY) -! CALL MDIO_WriteOutputs(-1.0_DbKi, p, m, y, ErrStat, ErrMsg) -! IF ( ErrStat >= AbortErrLev ) THEN -! ErrMsg = ' Error in MDIO_WriteOutputs: '//TRIM(ErrMsg) -! RETURN -! END IF + IF ( ErrStat2 /= 0 ) THEN + CALL SetErrStat( ErrID_Fatal, 'Failed to read rod data for Connection '//trim(Num2LStr(l)), ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF + + print *, "Set up connection ", l, " of type ", m%ConnectList(l)%typeNum + END DO ! l = 1,p%nRods - ! -------------------------------------------------------------------- - ! do dynamic relaxation to get ICs - ! -------------------------------------------------------------------- + !------------------------------------------------------------------------------------------- + else if (INDEX(Line, "LINE PROPERTIES") > 0) then ! if line properties header - CALL WrScr(" Finalizing ICs using dynamic relaxation."//NewLine) ! newline because next line writes over itself + print *, "Reading lines" + + ! skip following two lines (label line and unit line) + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! process each line + DO l = 1,p%nLines + + !read into a line + READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! parse out entries: LineID LineType UnstrLen NumSegs NodeA NodeB Flags/Outputs + IF (ErrStat2 == 0) THEN + READ(Line,*,IOSTAT=ErrStat2) m%LineList(l)%IdNum, tempString1, m%LineList(l)%UnstrLen, & + m%LineList(l)%N, tempString2, tempString3, LineOutString + END IF - ! boost drag coefficient of each line type - DO I = 1, p%NTypes - m%LineTypeList(I)%Cdn = m%LineTypeList(I)%Cdn * InitInp%CdScaleIC - m%LineTypeList(I)%Cdt = m%LineTypeList(I)%Cdt * InitInp%CdScaleIC - END DO + ! identify index of line type + DO J = 1,p%nLineTypes + IF (trim(tempString1) == trim(m%LineTypeList(J)%name)) THEN + m%LineList(l)%PropsIdNum = J + EXIT + IF (J == p%nLineTypes) THEN ! call an error if there is no match + CALL SetErrStat( ErrID_Severe, 'Unable to find matching line type name for Line '//trim(Num2LStr(l)), ErrStat, ErrMsg, RoutineName ) + END IF + END IF + END DO + + ! account for states of line + m%LineStateIs1(l) = Nx + 1 + m%LineStateIsN(l) = Nx + 6*m%LineList(l)%N - 6 + Nx = Nx + 6*(m%LineList(l)%N - 1) + + ! Process attachment identfiers and attach line ends + + ! First for the anchor (or end A)... + + call DecomposeString(tempString2, let1, num1, let2, num2, let3) + + if (len_trim(num1)<1) then + CALL SetErrStat( ErrID_Severe, "Error: no number provided for line "//trim(Num2LStr(l))//" end A attachment.", ErrStat, ErrMsg, RoutineName ) + return + end if + + READ(num1, *) J ! convert to int + + ! if id starts with an "R" or "Rod" + if ((let1 == "R") .or. (let1 == "ROD")) then + + if ((J <= p%nRods) .and. (J > 0)) then + if (let2 == "A") then + CALL Rod_AddLine(m%RodList(J), l, 0, 0) ! add line l (end A, denoted by 0) to rod J (end A, denoted by 0) + else if (let2 == "B") then + CALL Rod_AddLine(m%RodList(J), l, 0, 1) ! add line l (end A, denoted by 0) to rod J (end B, denoted by 1) + else + CALL SetErrStat( ErrID_Severe, "Error: rod end (A or B) must be specified for line "//trim(Num2LStr(l))//" end A attachment. Instead seeing "//let2, ErrStat, ErrMsg, RoutineName ) + return + end if + else + CALL SetErrStat( ErrID_Severe, "Error: rod connection ID out of bounds for line "//trim(Num2LStr(l))//" end A attachment.", ErrStat, ErrMsg, RoutineName ) + return + end if + + ! if J starts with a "C" or "Con" or goes straight ot the number then it's attached to a Connection + else if ((len_trim(let1)==0) .or. (let1 == "C") .or. (let1 == "CON")) then + + if ((J <= p%nConnects) .and. (J > 0)) then + CALL Connect_AddLine(m%ConnectList(J), l, 0) ! add line l (end A, denoted by 0) to connection J + else + CALL SetErrStat( ErrID_Severe, "Error: connection out of bounds for line "//trim(Num2LStr(l))//" end A attachment.", ErrStat, ErrMsg, RoutineName ) + return + end if + + end if + + + ! Then again for the fairlead (or end B)... + + call DecomposeString(tempString3, let1, num1, let2, num2, let3) + + if (len_trim(num1)<1) then + CALL SetErrStat( ErrID_Severe, "Error: no number provided for line "//trim(Num2LStr(l))//" end B attachment.", ErrStat, ErrMsg, RoutineName ) + return + end if + + READ(num1, *) J ! convert to int + + ! if id starts with an "R" or "Rod" + if ((let1 == "R") .or. (let1 == "ROD")) then + + if ((J <= p%nRods) .and. (J > 0)) then + if (let2 == "A") then + CALL Rod_AddLine(m%RodList(J), l, 1, 0) ! add line l (end B, denoted by 1) to rod J (end A, denoted by 0) + else if (let2 == "B") then + CALL Rod_AddLine(m%RodList(J), l, 1, 1) ! add line l (end B, denoted by 1) to rod J (end B, denoted by 1) + else + CALL SetErrStat( ErrID_Severe, "Error: rod end (A or B) must be specified for line "//trim(Num2LStr(l))//" end B attachment. Instead seeing "//let2, ErrStat, ErrMsg, RoutineName ) + return + end if + else + CALL SetErrStat( ErrID_Severe, "Error: rod connection ID out of bounds for line "//trim(Num2LStr(l))//" end B attachment.", ErrStat, ErrMsg, RoutineName ) + return + end if + + ! if J starts with a "C" or "Con" or goes straight ot the number then it's attached to a Connection + else if ((len_trim(let1)==0) .or. (let1 == "C") .or. (let1 == "CON")) then + + if ((J <= p%nConnects) .and. (J > 0)) then + CALL Connect_AddLine(m%ConnectList(J), l, 1) ! add line l (end B, denoted by 1) to connection J + else + CALL SetErrStat( ErrID_Severe, "Error: connection out of bounds for line "//trim(Num2LStr(l))//" end B attachment.", ErrStat, ErrMsg, RoutineName ) + return + end if + + end if + + + ! process output flag characters (LineOutString) and set line output flag array (OutFlagList) + m%LineList(l)%OutFlagList = 0 ! first set array all to zero + ! per node 3 component + IF ( scan( LineOutString, 'p') > 0 ) m%LineList(l)%OutFlagList(2) = 1 + IF ( scan( LineOutString, 'v') > 0 ) m%LineList(l)%OutFlagList(3) = 1 + IF ( scan( LineOutString, 'U') > 0 ) m%LineList(l)%OutFlagList(4) = 1 + IF ( scan( LineOutString, 'D') > 0 ) m%LineList(l)%OutFlagList(5) = 1 + IF ( scan( LineOutString, 'b') > 0 ) m%LineList(l)%OutFlagList(6) = 1 ! seabed contact forces + ! per node 1 component + IF ( scan( LineOutString, 'W') > 0 ) m%LineList(l)%OutFlagList(7) = 1 ! node weight/buoyancy (positive up) + IF ( scan( LineOutString, 'K') > 0 ) m%LineList(l)%OutFlagList(8) = 1 ! curvature at node + ! per element 1 component + IF ( scan( LineOutString, 't') > 0 ) m%LineList(l)%OutFlagList(10) = 1 ! segment tension force (just EA) + IF ( scan( LineOutString, 'c') > 0 ) m%LineList(l)%OutFlagList(11) = 1 ! segment internal damping force + IF ( scan( LineOutString, 's') > 0 ) m%LineList(l)%OutFlagList(12) = 1 ! Segment strain + IF ( scan( LineOutString, 'd') > 0 ) m%LineList(l)%OutFlagList(13) = 1 ! Segment strain rate + + IF (SUM(m%LineList(l)%OutFlagList) > 0) m%LineList(l)%OutFlagList(1) = 1 ! this first entry signals whether to create any output file at all + ! the above letter-index combinations define which OutFlagList entry corresponds to which output type + + + ! specify IdNum of line for error checking + m%LineList(l)%IdNum = l + + + ! check for sequential IdNums + IF ( m%LineList(l)%IdNum .NE. l ) THEN + CALL SetErrStat( ErrID_Fatal, 'Line numbers must be sequential starting from 1.', ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF + + + ! setup line + CALL SetupLine( m%LineList(l), m%LineTypeList(m%LineList(l)%PropsIdNum), p%rhoW , ErrStat2, ErrMsg2) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN + - ! allocate array holding three latest fairlead tensions - ALLOCATE ( FairTensIC(p%NFairs,3), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN - CALL CheckError( ErrID_Fatal, ErrMsg2 ) - RETURN - END IF + IF ( ErrStat2 /= 0 ) THEN + CALL SetErrStat( ErrID_Fatal, 'Failed to read line data for Line '//trim(Num2LStr(l)), ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF - ! initialize fairlead tension memory at zero - DO J = 1,p%NFairs - DO I = 1, 3 - FairTensIC(J,I) = 0.0_DbKi - END DO - END DO + END DO ! l = 1,p%nLines - t = 0.0_DbKi ! start time at zero - ! because TimeStep wants an array... - call MD_CopyInput( u, uArray(1), MESH_NEWCOPY, ErrStat2, ErrMsg2 ) + !------------------------------------------------------------------------------------------- + else if (INDEX(Line, "FAILURE") > 0) then ! if failure conditions header - DO I = 1, ceiling(InitInp%TMaxIC/InitInp%DTIC) ! loop through IC gen time steps, up to maximum + print *, " Reading failure conditions: (not implemented yet) "; + + ! TODO: add stuff <<<<<<<< - ! integrate the EOMs one DTIC s time step - CALL TimeStep ( t, InitInp%DTIC, uArray, utimes, p, x, xd, z, other, m, ErrStat, ErrMsg ) - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN + ! skip following two lines (label line and unit line) + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! process each line + DO l = 1,p%nFails + + !read into a line + READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + END DO - ! store new fairlead tension (and previous fairlead tensions for comparison) - DO J = 1, p%NFairs - FairTensIC(J,3) = FairTensIC(J,2) - FairTensIC(J,2) = FairTensIC(J,1) - FairTensIC(J,1) = TwoNorm(m%ConnectList(m%FairIdList(J))%Ftot(:)) - END DO + !------------------------------------------------------------------------------------------- + else if (INDEX(Line, "OPTIONS") > 0) then ! if options header - ! provide status message - ! bjj: putting this in a string so we get blanks to cover up previous values (if current string is shorter than previous one) - Message = ' t='//trim(Num2LStr(t))//' FairTen 1: '//trim(Num2LStr(FairTensIC(1,1)))// & - ', '//trim(Num2LStr(FairTensIC(1,2)))//', '//trim(Num2LStr(FairTensIC(1,3))) - CALL WrOver( Message ) - - ! check for convergence (compare current tension at each fairlead with previous two values) - IF (I > 2) THEN - Converged = 1 - DO J = 1, p%NFairs ! check for non-convergence - IF (( abs( FairTensIC(J,1)/FairTensIC(J,2) - 1.0 ) > InitInp%threshIC ) .OR. ( abs( FairTensIC(J,1)/FairTensIC(J,3) - 1.0 ) > InitInp%threshIC ) ) THEN - Converged = 0 - EXIT - END IF - END DO + print *, "Reading options" + + ! (don't skip any lines) + + ! process each line + DO l = 1,nOpts + + !read into a line + READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + + ! parse out entries: value, option keyword + READ(Line,*,IOSTAT=ErrStat2) OptValue, OptString ! look at first two entries, ignore remaining words in line, which should be comments + + + IF ( ErrStat2 /= 0 ) THEN + CALL SetErrStat( ErrID_Fatal, 'Failed to read options.', ErrStat, ErrMsg, RoutineName ) ! would be nice to specify which line had the error + CALL CleanUp() + RETURN + END IF + + CALL Conv2UC(OptString) + + ! check all possible options types and see if OptString is one of them, in which case set the variable. + if ( OptString == 'DTM') THEN + read (OptValue,*) p%dtM0 ! InitInp%DTmooring + else if ( OptString == 'G') then + read (OptValue,*) p%g + else if ( OptString == 'RHOW') then + read (OptValue,*) p%rhoW + else if ( OptString == 'WTRDPTH') then + read (OptValue,*) p%WtrDpth + else if ( OptString == 'KBOT') then + read (OptValue,*) p%kBot + else if ( OptString == 'CBOT') then + read (OptValue,*) p%cBot + else if ( OptString == 'DTIC') then + read (OptValue,*) InitInp%dtIC + else if ( OptString == 'TMAXIC') then + read (OptValue,*) InitInp%TMaxIC + else if ( OptString == 'CDSCALEIC') then + read (OptValue,*) InitInp%CdScaleIC + else if ( OptString == 'THRESHIC') then + read (OptValue,*) InitInp%threshIC + else if ( OptString == 'WATERKIN') then + read (OptValue,*) p%WaterKin + else + CALL SetErrStat( ErrID_Warn, 'unable to interpret input '//trim(OptString), ErrStat, ErrMsg, RoutineName ) + end if - IF (Converged == 1) THEN ! (J == p%NFairs) THEN ! if we made it with all cases satisfying the threshold - CALL WrScr(' Fairlead tensions converged to '//trim(Num2LStr(100.0*InitInp%threshIC))//'% after '//trim(Num2LStr(t))//' seconds.') - EXIT ! break out of the time stepping loop - END IF - END IF + END DO - IF (I == ceiling(InitInp%TMaxIC/InitInp%DTIC) ) THEN - CALL WrScr(' Fairlead tensions did not converge within TMaxIC='//trim(Num2LStr(InitInp%TMaxIC))//' seconds.') - !ErrStat = ErrID_Warn - !ErrMsg = ' MD_Init: ran dynamic convergence to TMaxIC without convergence' - END IF - END DO ! I ... looping through time steps + !------------------------------------------------------------------------------------------- + else if (INDEX(Line, "OUTPUT") > 0) then ! if output header - CALL MD_DestroyInput( uArray(1), ErrStat2, ErrMsg2 ) + print *, "Reading outputs" + + ! (don't skip any lines) + + ! allocate InitInp%Outliest (to a really big number for now...) + CALL AllocAry( OutList, MaxAryLen, "MoorDyn Input File's Outlist", ErrStat2, ErrMsg2 ); if(Failed()) return - ! UNboost drag coefficient of each line type - DO I = 1, p%NTypes - m%LineTypeList(I)%Cdn = m%LineTypeList(I)%Cdn / InitInp%CdScaleIC - m%LineTypeList(I)%Cdt = m%LineTypeList(I)%Cdt / InitInp%CdScaleIC - END DO + ! OutList - List of user-requested output channels (-): + !CALL ReadOutputList ( UnIn, FileName, InitInp%OutList, p%NumOuts, 'OutList', "List of user-requested output channels", ErrStat2, ErrMsg2, UnEc ); if(Failed()) return + ! customm implementation to avoid need for "END" keyword line + + ! Initialize some values + p%NumOuts = 0 ! start counter at zero + OutList = '' - p%dtCoupling = DTcoupling ! store coupling time step for use in updatestates - other%dummy = 0 - xd%dummy = 0 - z%dummy = 0 + ! Read in all of the lines containing output parameters and store them in OutList(:) + DO + ! read a line + READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i - CONTAINS + CALL Conv2UC(Line) ! convert to uppercase for easy string matching - SUBROUTINE CheckError(ErrID,Msg) - ! This subroutine sets the error message and level and cleans up if the error is >= AbortErrLev + if ((INDEX(Line, "---") > 0) .or. (INDEX(Line, "END") > 0)) EXIT ! stop if we hit a header line or the keyword "END" + + NumWords = CountWords( Line ) ! The number of words in Line. - ! Passed arguments - INTEGER(IntKi), INTENT(IN) :: ErrID ! The error identifier (ErrStat) - CHARACTER(*), INTENT(IN) :: Msg ! The error message (ErrMsg) + p%NumOuts = p%NumOuts + NumWords ! The total number of output channels read in so far. - INTEGER(IntKi) :: ErrStat3 ! The error identifier (ErrStat) - CHARACTER(ErrMsgLen) :: ErrMsg3 ! The error message (ErrMsg) - ! Set error status/message; - IF ( ErrID /= ErrID_None ) THEN + IF ( p%NumOuts > MaxAryLen ) THEN ! Check to see if the maximum # allowable in the array has been reached. - IF (ErrStat /= ErrID_None) ErrMsg = TRIM(ErrMsg)//NewLine ! if there's a pre-existing warning/error, retain the message and start a new line + ErrStat = ErrID_Fatal + ErrMsg = 'Error while reading output channels: The maximum number of output channels allowed is '//TRIM( Int2LStr(MaxAryLen) )//'.' + EXIT - ErrMsg = TRIM(ErrMsg)//' MD_Init:'//TRIM(Msg) - ErrStat = MAX(ErrStat, ErrID) + ELSE + CALL GetWords ( Line, OutList((p%NumOuts - NumWords + 1):p%NumOuts), NumWords ) - ! Clean up if we're going to return on error: close files, deallocate local arrays + END IF + END DO - IF ( ErrStat >= AbortErrLev ) THEN - IF (ALLOCATED(m%FairIdList )) DEALLOCATE(m%FairIdList ) - IF (ALLOCATED(m%ConnIdList )) DEALLOCATE(m%ConnIdList ) - IF (ALLOCATED(m%LineStateIndList )) DEALLOCATE(m%LineStateIndList ) - IF (ALLOCATED(x%states )) DEALLOCATE(x%states ) - IF (ALLOCATED(FairTensIC )) DEALLOCATE(FairTensIC ) - END IF - END IF + ! process the OutList array and set up the index arrays for the requested output quantities + CALL MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat2, ErrMsg2 ) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN - END SUBROUTINE CheckError - END SUBROUTINE MD_Init - !============================================================================================== + !------------------------------------------------------------------------------------------- + else ! otherwise ignore this line that isn't a recognized header line and read the next line + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," --- unrecognized header" + end if + + !------------------------------------------------------------------------------------------- + + else ! otherwise ignore this line, which doesn't have the "---" or header line and read the next line + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i, " .." + end if + + end do + ! this is the end of reading the input file, so close it now + CALL CleanUp() + + + + + + + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN - !============================================================================================== - SUBROUTINE MD_UpdateStates( t, n, u, utimes, p, x, xd, z, other, m, ErrStat, ErrMsg) - REAL(DbKi) , INTENT(IN ) :: t - INTEGER(IntKi) , INTENT(IN ) :: n - TYPE(MD_InputType) , INTENT(INOUT) :: u(:) ! INTENT(INOUT) ! had to change this to INOUT - REAL(DbKi) , INTENT(IN ) :: utimes(:) - TYPE(MD_ParameterType) , INTENT(IN ) :: p ! INTENT(IN ) - TYPE(MD_ContinuousStateType) , INTENT(INOUT) :: x ! INTENT(INOUT) - TYPE(MD_DiscreteStateType) , INTENT(INOUT) :: xd ! INTENT(INOUT) - TYPE(MD_ConstraintStateType) , INTENT(INOUT) :: z ! INTENT(INOUT) - TYPE(MD_OtherStateType) , INTENT(INOUT) :: other ! INTENT(INOUT) - TYPE(MD_MiscVarType) , INTENT(INOUT) :: m ! INTENT(INOUT) - INTEGER(IntKi) , INTENT( OUT) :: ErrStat ! Error status of the operation - CHARACTER(*) , INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + !------------------------------------------------------------------------------------------------- + ! Connect mooring system together and make necessary allocations + !------------------------------------------------------------------------------------------------- - INTEGER(IntKi) :: ErrStat2 ! Error status of the operation - CHARACTER(ErrMsgLen) :: ErrMsg2 ! Error message if ErrStat2 /= ErrID_None + CALL WrNR( ' Created mooring system. ' ) + +! p%NAnchs = 0 ! this is the number of "fixed" type Connections. <<<<<<<<<<<<<< + + +! CALL WrScr(trim(Num2LStr(p%nCpldCons))//' fairleads, '//trim(Num2LStr(p%NAnchs))//' anchors, '//trim(Num2LStr(p%nFreeCons))//' connects.') + + + + + ! ! now go back through and record the fairlead Id numbers (this >>>WAS<<< all the "connecting" that's required) <<<< + ! J = 1 ! counter for fairlead number + ! K = 1 ! counter for connect number + ! DO I = 1,p%NConnects + ! IF (m%ConnectList(I)%typeNum == 1) THEN + ! m%CpldConIs(J) = I ! if a vessel connection, add ID to list + ! J = J + 1 + ! ELSE IF (m%ConnectList(I)%typeNum == 2) THEN + ! m%FreeConIs(K) = I ! if a connect connection, add ID to list + ! K = K + 1 + ! END IF + ! END DO + + print *, "nLineTypes = ",p%nLineTypes + print *, "nRodTypes = ",p%nRodTypes + print *, "nConnects = ",p%nConnects + print *, "nConnectsExtra = ",p%nConnectsExtra + print *, "nBodies = ",p%nBodies + print *, "nRods = ",p%nRods + print *, "nLines = ",p%nLines + print *, "nFails = ",p%nFails + print *, "nFreeBodies = ",p%nFreeBodies + print *, "nFreeRods = ",p%nFreeRods + print *, "nFreeCons = ",p%nFreeCons + print *, "nCpldBodies = ",p%nCpldBodies + print *, "nCpldRods = ",p%nCpldRods + print *, "nCpldCons = ",p%nCpldCons + print *, "NConns = ",p%NConns + print *, "NAnchs = ",p%NAnchs + + print *, "FreeConIs are ", m%FreeConIs + print *, "CpldConIs are ", m%CpldConIs -! moved to TimeStep TYPE(MD_InputType) :: u_interp ! - INTEGER(IntKi) :: nTime - REAL(DbKi) :: t2 ! copy of time passed to TimeStep + !------------------------------------------------------------------------------------ + ! fill in state vector index record holders + !------------------------------------------------------------------------------------ + ! allocate state vector index record holders... - nTime = size(u) ! the number of times of input data provided? + - t2 = t + ! ! allocate list of starting and ending state vector indices for each free connection + ! ALLOCATE ( m%ConStateIs1(p%nFreeCons), m%ConStateIsN(p%nFreeCons), STAT = ErrStat ) + ! IF ( ErrStat /= ErrID_None ) THEN + ! CALL CheckError(ErrID_Fatal, ' Error allocating ConStateIs array.') + ! RETURN + ! END IF + ! + ! ! allocate list of starting and ending state vector indices for each line - does this belong elsewhere? + ! ALLOCATE ( m%LineStateIs1(p%nLines), m%LineStateIsN(p%nLines), STAT = ErrStat ) + ! IF ( ErrStat /= ErrID_None ) THEN + ! CALL CheckError(ErrID_Fatal, ' Error allocating LineStateIs arrays.') + ! RETURN + ! END IF + ! + ! + ! ! fill in values for state vector index record holders... + ! + ! J=0 ! start off index counter at zero + ! + ! ! Free Bodies... + ! ! Free Rods... + ! + ! ! Free Connections... + ! DO l = 1, p%nFreeCons + ! J = J + 1 ! assign start index + ! m%ConStateIs1(l) = J + ! + ! J = J + 5 ! assign end index (5 entries further, since nodes have 2*3 states) + ! m%ConStateIsN(l) = J + ! END DO + ! + ! ! Lines + ! DO l = 1, p%nLines + ! J = J + 1 ! assign start index + ! m%LineStateIs1(l) = J + ! + ! J = J + 6*(m%LineList(l)%N - 1) - 1 ! !add 6 state variables for each internal node + ! m%LineStateIsN(l) = J + ! END DO + ! + ! + ! ! record number of states + ! m%Nx = J + + + !------------------------------------------------------------------------------------ + ! prepare state vector etc. + !------------------------------------------------------------------------------------ -! >>> removing this section and putting it inside loop of TimeStep (to be done at every time step) <<< -! ! create space for arrays/meshes in u_interp -! CALL MD_CopyInput(u(1), u_interp, MESH_NEWCOPY, ErrStat2, ErrMsg2) -! CALL CheckError( ErrStat2, ErrMsg2 ) -! IF (ErrStat >= AbortErrLev) RETURN -! -! ! interpolate input mesh to correct time -! CALL MD_Input_ExtrapInterp(u, utimes, u_interp, t, ErrStat2, ErrMsg2) -! CALL CheckError( ErrStat2, ErrMsg2 ) -! IF (ErrStat >= AbortErrLev) RETURN -! -! -! ! go through fairleads and apply motions from driver -! DO I = 1, p%NFairs -! DO J = 1,3 -! m%ConnectList(m%FairIdList(I))%r(J) = u_interp%PtFairleadDisplacement%Position(J,I) + u_interp%PtFairleadDisplacement%TranslationDisp(J,I) -! m%ConnectList(m%FairIdList(I))%rd(J) = u_interp%PtFairleadDisplacement%TranslationVel(J,I) ! is this right? <<< -! END DO -! END DO -! + ! the number of states is Nx + m%Nx = Nx + + print *, "allocating state vectors to size ", Nx - ! call function that loops through mooring model time steps - CALL TimeStep ( t2, p%dtCoupling, u, utimes, p, x, xd, z, other, m, ErrStat2, ErrMsg2 ) - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN + ! allocate state vector and temporary state vectors based on size just calculated + ALLOCATE ( x%states(m%Nx), m%xTemp%states(m%Nx), m%xdTemp%states(m%Nx), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) THEN + ErrMsg = ' Error allocating state vectors.' + !CALL CleanUp() + RETURN + END IF - ! clean up input interpolation stuff - ! moved to TimeStep CALL MD_DestroyInput(u_interp, ErrStat, ErrMsg) - CONTAINS + ! ================================ initialize system ================================ - SUBROUTINE CheckError(ErrId, Msg) - ! This subroutine sets the error message and level and cleans up if the error is >= AbortErrLev - INTEGER(IntKi), INTENT(IN) :: ErrID ! The error identifier (ErrStat) - CHARACTER(*), INTENT(IN) :: Msg ! The error message (ErrMsg) + ! call ground body to update all the fixed things... + CALL Body_SetDependentKin(m%GroundBody, 0.0_DbKi, m) - IF ( ErrID /= ErrID_None ) THEN + ! m%GroundBody%OrMat = EulerConstruct( m%GroundBody%r6(4:6) ) ! make sure it's OrMat is set up <<< need to check this approach + + ! ! first set/update the kinematics of all the fixed things (>>>> eventually do this by using a ground body <<<<) + ! ! only doing connections so far + ! DO J = 1,p%nConnects + ! if (m%ConnectList(J)%typeNum == 1) then + ! ! set the attached line endpoint positions: + ! CALL Connect_SetKinematics(m%ConnectList(J), m%ConnectList(J)%r, (/0.0_DbKi,0.0_DbKi,0.0_DbKi/), 0.0_DbKi, m%LineList) + ! end if + ! END DO + + + + + ! Initialize coupled objects based on passed kinematics + ! (set up initial condition of each coupled object based on values specified by glue code) + ! Also create i/o meshes + + ! <<<<<<<< look here when changing to shared mooring compatibility - IF (ErrStat /= ErrID_None) ErrMsg = TRIM(ErrMsg)//NewLine ! keep existing error message if there is one - ErrMsg = TRIM(ErrMsg)//' MD_UpdateStates:'//TRIM(Msg) ! add current error message - ErrStat = MAX(ErrStat, ErrID) + ! create input mesh for all coupled objects (as mesh node points) + CALL MeshCreate(BlankMesh=u%CoupledKinematics, IOS= COMPONENT_INPUT, & + Nnodes = p%nCpldBodies + p%nCpldRods + p%nCpldCons, & + TranslationDisp=.TRUE., TranslationVel=.TRUE., & + Orientation=.TRUE., RotationVel=.TRUE., & + TranslationAcc=.TRUE., RotationAcc= .TRUE., & + ErrStat=ErrStat2, ErrMess=ErrMsg2) - CALL WrScr( ErrMsg ) ! do this always or only if warning level? + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN + + ! note: in MoorDyn-F v2, the points in the mesh correspond in order to all the coupled bodies, then rods, then connections + ! NOTE: InitInp%PtfmInit should be replaced by specific values for each coupled body/rod/connect at some point <<<<< + + J = 0 ! this is the counter through the mesh points + + DO l = 1,p%nCpldBodies + + J = J + 1 + + rRef = m%BodyList(m%CpldBodyIs(l))%r6 ! for now set reference position as per input file <<< + + CALL MeshPositionNode(u%CoupledKinematics, J, rRef(1:3), ErrStat2, ErrMsg2) ! defaults to identity orientation matrix + u%CoupledKinematics%TranslationDisp(:,J) = 0.0_ReKi ! no displacement from reference position - IF( ErrStat > ErrID_Warn ) THEN - ! CALL MD_DestroyInput( u_interp, ErrStat, ErrMsg ) - RETURN - END IF - END IF + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN - END SUBROUTINE CheckError + ! ! Apply offsets due to initial platform rotations and translations (fixed Jun 19, 2015) + ! CALL SmllRotTrans('body rotation matrix', InitInp%PtfmInit(4),InitInp%PtfmInit(5),InitInp%PtfmInit(6), OrMat, '', ErrStat2, ErrMsg2) + ! u%CoupledKinematics%TranslationDisp(1, i) = InitInp%PtfmInit(1) + OrMat(1,1)*rRef(1) + OrMat(2,1)*rRef(2) + OrMat(3,1)*rRef(3) - rRef(1) + ! u%CoupledKinematics%TranslationDisp(2, i) = InitInp%PtfmInit(2) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2) + ! u%CoupledKinematics%TranslationDisp(3, i) = InitInp%PtfmInit(3) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) + + CALL MeshConstructElement(u%CoupledKinematics, ELEMENT_POINT, ErrStat2, ErrMsg2, J) ! set node as point element + CALL Body_InitializeUnfree( m%BodyList(m%CpldBodyIs(l)), m ) - END SUBROUTINE MD_UpdateStates - !======================================================================================== + END DO + + DO l = 1,p%nCpldRods ! keeping this one simple for now, positioning at whatever is specified in input file <<<<< should change to glue code! + J = J + 1 + + rRef = m%RodList(m%CpldRodIs(l))%r6 ! for now set reference position as per input file <<< + + CALL MeshPositionNode(u%CoupledKinematics, J, rRef, ErrStat2, ErrMsg2) ! defaults to identity orientation matrix + u%CoupledKinematics%TranslationDisp(:,J) = 0.0_ReKi ! no displacement from reference position + CALL MeshConstructElement(u%CoupledKinematics, ELEMENT_POINT, ErrStat2, ErrMsg2, J) + CALL Rod_SetKinematics(m%RodList(m%CpldRodIs(l)), tempArray, m%zeros6, m%zeros6, 0.0_DbKi, m%LineList) + END DO + DO l = 1,p%nCpldCons ! keeping this one simple for now, positioning at whatever is specified by glue code <<< + + J = J + 1 + + rRef(1:3) = m%ConnectList(m%CpldConIs(l))%r ! for now set reference position as per input file <<< + CALL MeshPositionNode(u%CoupledKinematics, J, rRef, ErrStat2, ErrMsg2) ! defaults to identity orientation matrix + u%CoupledKinematics%TranslationDisp(:,J) = 0.0_ReKi ! no displacement from reference position + CALL MeshConstructElement(u%CoupledKinematics, ELEMENT_POINT, ErrStat2, ErrMsg2, J) - !======================================================================================== - SUBROUTINE MD_CalcOutput( t, u, p, x, xd, z, other, y, m, ErrStat, ErrMsg ) + ! lastly, do this to set the attached line endpoint positions: + rRefDub = rRef(1:3) + CALL Connect_SetKinematics(m%ConnectList(m%CpldConIs(l)), rRefDub, m%zeros6(1:3), m%zeros6(1:3), 0.0_DbKi, m%LineList) + END DO - REAL(DbKi) , INTENT(IN ) :: t - TYPE( MD_InputType ) , INTENT(IN ) :: u ! INTENT(IN ) - TYPE( MD_ParameterType ) , INTENT(IN ) :: p ! INTENT(IN ) - TYPE( MD_ContinuousStateType ) , INTENT(IN ) :: x ! INTENT(IN ) - TYPE( MD_DiscreteStateType ) , INTENT(IN ) :: xd ! INTENT(IN ) - TYPE( MD_ConstraintStateType ) , INTENT(IN ) :: z ! INTENT(IN ) - TYPE( MD_OtherStateType ) , INTENT(IN ) :: other ! INTENT(IN ) - TYPE( MD_OutputType ) , INTENT(INOUT) :: y ! INTENT(INOUT) - TYPE(MD_MiscVarType) , INTENT(INOUT) :: m ! INTENT(INOUT) - INTEGER(IntKi) , INTENT(INOUT) :: ErrStat - CHARACTER(*) , INTENT(INOUT) :: ErrMsg + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN + + ! set velocities/accelerations of all mesh nodes to zero + u%CoupledKinematics%TranslationVel = 0.0_ReKi + u%CoupledKinematics%TranslationAcc = 0.0_ReKi + u%CoupledKinematics%RotationVel = 0.0_ReKi + u%CoupledKinematics%RotationAcc = 0.0_ReKi - TYPE(MD_ContinuousStateType) :: dxdt ! time derivatives of continuous states (initialized in CalcContStateDeriv) - INTEGER(IntKi) :: I ! counter - INTEGER(IntKi) :: J ! counter + CALL MeshCommit ( u%CoupledKinematics, ErrStat, ErrMsg ) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN - INTEGER(IntKi) :: ErrStat2 ! Error status of the operation - CHARACTER(ErrMsgLen) :: ErrMsg2 ! Error message if ErrStat2 /= ErrID_None + ! copy the input fairlead kinematics mesh to make the output mesh for fairlead loads, PtFairleadLoad + CALL MeshCopy ( SrcMesh = u%CoupledKinematics, DestMesh = y%CoupledLoads, & + CtrlCode = MESH_SIBLING, IOS = COMPONENT_OUTPUT, & + Force=.TRUE., Moment=.TRUE., ErrStat = ErrStat2, ErrMess=ErrMsg2 ) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN - ! below updated to make sure outputs are current (based on provided x and u) - similar to what's in UpdateStates + + ! ----------------------------- Arrays for active tensioning --------------------------- + + ! size active tensioning inputs arrays based on highest channel number read from input file for now <<<<<<< + + ! find the highest channel number + J = 0 + DO I = 1, p%NLines + IF ( m%LineList(I)%CtrlChan > J ) then + J = m%LineList(I)%CtrlChan + END IF + END DO - ! go through fairleads and apply motions from driver - DO I = 1, p%NFairs - DO J = 1,3 - m%ConnectList(m%FairIdList(I))%r(J) = u%PtFairleadDisplacement%Position(J,I) + u%PtFairleadDisplacement%TranslationDisp(J,I) - m%ConnectList(m%FairIdList(I))%rd(J) = u%PtFairleadDisplacement%TranslationVel(J,I) ! is this right? <<< + ! allocate the input arrays + ALLOCATE ( u%DeltaL(J), u%DeltaLdot(J), STAT = ErrStat2 ) + + + + + ! ----------------------------- Arrays for wave kinematics ----------------------------- + ! :::::::::::::: BELOW WILL BE USED EVENTUALLY WHEN WAVE INFO IS AN INPUT :::::::::::::::::: + ! ! The rAll array contains all nodes or reference points in the system + ! ! (x,y,z global coordinates for each) in the order of bodies, rods, points, internal line nodes. + ! + ! ! count the number of nodes to use for passing wave kinematics + ! J=0 + ! ! Body reference point coordinates + ! J = J + p%nBodies + ! ! Rod node coordinates (including ends) + ! DO l = 1, p%nRods + ! J = J + (m%RodList(l)%N + 1) + ! END DO + ! ! Point reference point coordinates + ! J = J + p%nConnects + ! ! Line internal node coordinates + ! DO l = 1, p%nLines + ! J = J + (m%LineList(l)%N - 1) + ! END DO + ! + ! ! allocate all relevant arrays + ! ! allocate state vector and temporary state vectors based on size just calculated + ! ALLOCATE ( y%rAll(3,J), u%U(3,J), u%Ud(3,J), u%zeta(J), u%PDyn(J), STAT = ErrStat ) + ! IF ( ErrStat /= ErrID_None ) THEN + ! ErrMsg = ' Error allocating wave kinematics vectors.' + ! RETURN + ! END IF + ! + ! + ! ! go through the nodes and fill in the data (this should maybe be turned into a global function) + ! J=0 + ! ! Body reference point coordinates + ! DO I = 1, p%nBodies + ! J = J + 1 + ! y%rAll(:,J) = m%BodyList(I)%r6(1:3) + ! END DO + ! ! Rod node coordinates + ! DO I = 1, p%nRods + ! DO K = 0,m%RodList(I)%N + ! J = J + 1 + ! y%rAll(:,J) = m%RodList(I)%r(:,K) + ! END DO + ! END DO + ! ! Point reference point coordinates + ! DO I = 1, p%nConnects + ! J = J + 1 + ! y%rAll(:,J) = m%ConnectList(I)%r + ! END DO + ! ! Line internal node coordinates + ! DO I = 1, p%nLines + ! DO K = 1,m%LineList(I)%N-1 + ! J = J + 1 + ! y%rAll(:,J) = m%LineList(I)%r(:,K) + ! END DO + ! END DO + + ! :::::::::::::::: the above will be used eventually. For now, let's store wave info grids within this module ::::::::::::::::: + ! allocate arrays + I = SIZE(InitInp%WaveTime) + ALLOCATE ( p%ux (I,8,5,5), STAT = ErrStat2 ) + ALLOCATE ( p%uy (I,8,5,5), STAT = ErrStat2 ) + ALLOCATE ( p%uz (I,8,5,5), STAT = ErrStat2 ) + ALLOCATE ( p%ax (I,8,5,5), STAT = ErrStat2 ) + ALLOCATE ( p%ay (I,8,5,5), STAT = ErrStat2 ) + ALLOCATE ( p%az (I,8,5,5), STAT = ErrStat2 ) + ALLOCATE ( p%PDyn(I,8,5,5), STAT = ErrStat2 ) + ALLOCATE ( p%zeta(I,5,5), STAT = ErrStat2 ) ! 2D grid over x and y only + ALLOCATE ( p%px(5), STAT = ErrStat2 ) + ALLOCATE ( p%py(5), STAT = ErrStat2 ) + ALLOCATE ( p%pz(8), STAT = ErrStat2 ) + + ! get grid and time info (currenltly this is hard-coded to match what's in HydroDyn_Input + DO I=1,8 + p%pz(I) = 1.0 - 2.0**(8-I) ! -127, -63, -31, -15, -7, -3, -1, 0 + END DO + DO J = 1,5 + p%py(J) = -60.0 + 20.0*J + END DO + DO K = 1,5 + p%px(K) = -60.0 + 20.0*K + END DO + p%dtWave = InitInp%WaveTime(2) - InitInp%WaveTime(1) + + ! fill in the grid data (the for loops match those in HydroDyn_Input) + DO I=1,8 + DO J = 1,5 + DO K = 1,5 + Itemp = (I-1)*25.0 + (J-1)*5.0 + K ! index of actual node on 3D grid + + p%ux (:,I,J,K) = InitInp%WaveVel( :,Itemp,1) ! note: indices are t, z, y, x + p%uy (:,I,J,K) = InitInp%WaveVel( :,Itemp,2) + p%uz (:,I,J,K) = InitInp%WaveVel( :,Itemp,3) + p%ax (:,I,J,K) = InitInp%WaveAcc( :,Itemp,1) + p%ay (:,I,J,K) = InitInp%WaveAcc( :,Itemp,2) + p%az (:,I,J,K) = InitInp%WaveAcc( :,Itemp,3) + p%PDyn(:,I,J,K) = InitInp%WavePDyn(:,Itemp) + END DO + END DO + END DO + + ! fill in the grid data (the for loops match those in HydroDyn_Input) + DO J = 1,5 + DO K = 1,5 + Itemp = (J-1)*5.0 + K ! index of actual node on surface 2D grid + p%zeta(:,J,K) = InitInp%WaveElev(:,Itemp) END DO END DO + + + ! write the date to an output file for testing purposes + + CALL GetNewUnit( UnOut) - ! call CalcContStateDeriv in order to run model and calculate dynamics with provided x and u - CALL MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, ErrMsg ) + CALL OpenFOutFile ( UnOut, "waves.txt", ErrStat, ErrMsg ) + IF ( ErrStat > ErrID_None ) THEN + ErrMsg = ' Error opening wave grid file: '//TRIM(ErrMsg) + ErrStat = ErrID_Fatal + RETURN + END IF + WRITE(UnOut, *, IOSTAT=ErrStat2) TRIM( 'MoorDyn v2 wave/current kinematics grid file' ) + WRITE(UnOut, *, IOSTAT=ErrStat2) TRIM( '---------------------------------------------' ) + WRITE(UnOut, *, IOSTAT=ErrStat2) TRIM( 'The following 6 lines (4-9) specify the input type then the inputs for x, then, y, then z coordinates.' ) + + WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - X input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) + Frmt = '('//TRIM(Int2LStr(5))//'(A1,e10.4))' + WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%px(I))), I=1,5 ) + + WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - Y input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) + Frmt = '('//TRIM(Int2LStr(5))//'(A1,e10.4))' + WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%py(I))), I=1,5 ) + + WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - Z input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) + Frmt = '('//TRIM(Int2LStr(8))//'(A1,e10.4))' + WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%pz(I))), I=1,8 ) + + CLOSE(UnOut, IOSTAT = ErrStat ) + IF ( ErrStat /= 0 ) THEN + ErrMsg = 'Error closing wave grid file' + END IF + + + CALL GetNewUnit( UnOut) - ! assign net force on fairlead Connects to the output mesh - DO i = 1, p%NFairs - DO J=1,3 - y%PtFairleadLoad%Force(J,I) = m%ConnectList(m%FairIdList(I))%Ftot(J) + CALL OpenFOutFile ( UnOut, "wave data.txt", ErrStat, ErrMsg ) + IF ( ErrStat > ErrID_None ) THEN + ErrMsg = ' Error opening wave grid file: '//TRIM(ErrMsg) + ErrStat = ErrID_Fatal + RETURN + END IF + + ! write channel labels + + + ! time + WRITE(UnOut,"(A10)", IOSTAT=ErrStat2, advance="no") "Time" + + DO J = 1,5 !y + DO K = 1,5 !x + WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ze0", Num2Lstr(J+10*K) END DO END DO + DO I=1,8 !z + DO J = 1,5 !y + DO K = 1,5 !x + WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ux", Num2Lstr(I+10*J+100*K) + WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " uy", Num2Lstr(I+10*J+100*K) + WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " uz", Num2Lstr(I+10*J+100*K) + END DO + END DO + END DO + DO I=1,8 !z + DO J = 1,5 !y + DO K = 1,5 !x + WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ax", Num2Lstr(I+10*J+100*K) + WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ay", Num2Lstr(I+10*J+100*K) + WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " az", Num2Lstr(I+10*J+100*K) + END DO + END DO + END DO + DO I=1,8 !z + DO J = 1,5 !y + DO K = 1,5 !x + WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " pd", Num2Lstr(I+10*J+100*K) + END DO + END DO + END DO + + ! end the line + WRITE(UnOut, "(A1)", IOSTAT=ErrStat2) " " + + + + DO l=1, SIZE(InitInp%WaveTime) ! loop through all time steps + + ! time + WRITE(UnOut,"(F10.4)", IOSTAT=ErrStat2, advance="no") (l-1)*p%dtWave + + ! wave elevation (all slices for now, to check) + DO J = 1,5 !y + DO K = 1,5 !x + Itemp = (J-1)*5.0 + K ! index of actual node + + WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%zeta(l,J,K) + END DO + END DO + + ! wave velocities + DO I=1,8 !z + DO J = 1,5 !y + DO K = 1,5 !x + Itemp = (I-1)*25.0 + (J-1)*5.0 + K ! index of actual node + + WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%ux(l,I,J,K) + WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%uy(l,I,J,K) + WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%uz(l,I,J,K) + END DO + END DO + END DO + + ! wave accelerations + DO I=1,8 !z + DO J = 1,5 !y + DO K = 1,5 !x + Itemp = (I-1)*25.0 + (J-1)*5.0 + K ! index of actual node + + WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%ax(l,I,J,K) + WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%ay(l,I,J,K) + WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%az(l,I,J,K) + END DO + END DO + END DO + + ! dynamic pressure + DO I=1,8 !z + DO J = 1,5 !y + DO K = 1,5 !x + Itemp = (I-1)*25.0 + (J-1)*5.0 + K ! index of actual node + + WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%PDyn(l,I,J,K) + END DO + END DO + END DO + + ! end the line + WRITE(UnOut, "(A1)", IOSTAT=ErrStat2) " " + + + END DO + + + CLOSE(UnOut, IOSTAT = ErrStat ) + IF ( ErrStat /= 0 ) THEN + ErrMsg = 'Error closing wave grid file' + END IF + + + + ! Frmt = '(A10,'//TRIM(Int2LStr(p%NumOuts))//'(A1,A12))' + ! + ! WRITE(p%MDUnOut,Frmt, IOSTAT=ErrStat2) TRIM( 'Time' ), ( p%Delim, TRIM( p%OutParam(I)%Name), I=1,p%NumOuts ) + ! + ! WRITE(p%MDUnOut,Frmt) TRIM( '(s)' ), ( p%Delim, TRIM( p%OutParam(I)%Units ), I=1,p%NumOuts ) + ! + ! + ! + ! ! Write the output parameters to the file + ! + ! Frmt = '(F10.4,'//TRIM(Int2LStr(p%NumOuts))//'(A1,e10.4))' + ! + ! WRITE(p%MDUnOut,Frmt) Time, ( p%Delim, y%WriteOutput(I), I=1,p%NumOuts ) + + + + ! ::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: - ! calculate outputs (y%WriteOutput) for glue code and write any m outputs to MoorDyn output files - CALL MDIO_WriteOutputs(t, p, m, y, ErrStat2, ErrMsg2) - CALL CheckError(ErrStat2, 'In MDIO_WriteOutputs: '//trim(ErrMsg2)) - IF ( ErrStat >= AbortErrLev ) RETURN - + ! if any of the coupled objects need initialization steps, that should have been taken care of already <<<< + + + ! initialize objects with states, writing their initial states to the master state vector (x%states) + + + !TODO: apply any initial adjustment of line length from active tensioning <<<<<<<<<<<< + ! >>> maybe this should be skipped <<<< + + + ! Go through Bodys and write the coordinates to the state vector + DO l = 1,p%nFreeBodies + CALL Body_Initialize(m%BodyList(m%FreeBodyIs(l)), x%states(m%BodyStateIs1(l) : m%BodyStateIsN(l)), m) + END DO + + ! Go through independent (including pinned) Rods and write the coordinates to the state vector + DO l = 1,p%nFreeRods + CALL Rod_Initialize(m%RodList(m%FreeRodIs(l)), x%states(m%RodStateIs1(l):m%RodStateIsN(l)), m%LineList) + END DO - ! destroy dxdt - CALL MD_DestroyContState( dxdt, ErrStat2, ErrMsg2) - CALL CheckError(ErrStat2, 'When destroying dxdt: '//trim(ErrMsg2)) - IF ( ErrStat >= AbortErrLev ) RETURN + ! Go through independent connections (Connects) and write the coordinates to the state vector and set positions of attached line ends + DO l = 1, p%nFreeCons + CALL Connect_Initialize(m%ConnectList(m%FreeConIs(l)), x%states(m%ConStateIs1(l) : m%conStateIsN(l)), m%LineList) + END DO + ! Lastly, go through lines and initialize internal node positions using quasi-static model + DO l = 1, p%NLines - CONTAINS + N = m%LineList(l)%N ! for convenience - SUBROUTINE CheckError(ErrId, Msg) - ! This subroutine sets the error message and level and cleans up if the error is >= AbortErrLev + ! ! set end node positions and velocities from connect objects + ! m%LineList(l)%r(:,N) = m%ConnectList(m%LineList(l)%FairConnect)%r + ! m%LineList(l)%r(:,0) = m%ConnectList(m%LineList(l)%AnchConnect)%r + ! m%LineList(l)%rd(:,N) = (/ 0.0, 0.0, 0.0 /) ! set anchor end velocities to zero + ! m%LineList(l)%rd(:,0) = (/ 0.0, 0.0, 0.0 /) ! set fairlead end velocities to zero - INTEGER(IntKi), INTENT(IN) :: ErrID ! The error identifier (ErrStat) - CHARACTER(*), INTENT(IN) :: Msg ! The error message (ErrMsg) + ! set initial line internal node positions using quasi-static model or straight-line interpolation from anchor to fairlead + CALL Line_Initialize( m%LineList(l), m%LineTypeList(m%LineList(l)%PropsIdNum), p%rhoW , ErrStat2, ErrMsg2) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN + IF (ErrStat >= ErrId_Warn) CALL WrScr(" Catenary solver failed for one or more lines. Using linear node spacing.") ! make this statement more accurate +! print *, "Line ", l, " with NumSegs =", N +! print *, "its states range from index ", m%LineStateIs1(l), " to ", m%LineStateIsN(l) - IF ( ErrID /= ErrID_None ) THEN + ! assign the resulting internal node positions to the integrator initial state vector! (velocities leave at 0) + DO I = 1, N-1 +! print *, "I=", I + DO J = 1, 3 +! print*, J, " ... writing position state to index ", 1*(m%LineStateIs1(l) + 3*N-3 + 3*I-3 + J-1) + x%states(m%LineStateIs1(l) + 3*N-3 + 3*I-3 + J-1 ) = m%LineList(l)%r(J,I) ! assign position + x%states(m%LineStateIs1(l) + 3*I-3 + J-1 ) = 0.0_DbKi ! assign velocities (of zero) + END DO +! print *, m%LineList(l)%r(:,I) + END DO - IF (ErrStat /= ErrID_None) ErrMsg = TRIM(ErrMsg)//NewLine ! keep existing error message if there is one - ErrMsg = TRIM(ErrMsg)//' MD_CalcOutput:'//TRIM(Msg) ! add current error message - ErrStat = MAX(ErrStat, ErrID) + END DO !l = 1, p%NLines - CALL WrScr( ErrMsg ) ! do this always or only if warning level? <<<<<<<<<<<<<<<<<<<<<< probably should remove all instances - IF( ErrStat > ErrID_Warn ) THEN - CALL MD_DestroyContState( dxdt, ErrStat2, ErrMsg2) - END IF - END IF - END SUBROUTINE CheckError + ! -------------------------------------------------------------------- + ! open output file(s) and write header lines + CALL MDIO_OpenOutput( InitInp%FileName, p, m, InitOut, ErrStat2, ErrMsg2 ) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN + ! -------------------------------------------------------------------- - END SUBROUTINE MD_CalcOutput - !============================================================================================= + +! print *,"Done setup of the system (before any dynamic relaxation. State vector is as follows:" + +! DO I = 1, m%Nx +! print *, x%states(I) - !============================================================================================= - SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, ErrMsg ) - ! Tight coupling routine for computing derivatives of continuous states - ! this is modelled off what used to be subroutine DoRHSmaster - - REAL(DbKi), INTENT(IN ) :: t ! Current simulation time in seconds - TYPE(MD_InputType), INTENT(IN ) :: u ! Inputs at t - TYPE(MD_ParameterType), INTENT(IN ) :: p ! Parameters - TYPE(MD_ContinuousStateType), INTENT(IN ) :: x ! Continuous states at t - TYPE(MD_DiscreteStateType), INTENT(IN ) :: xd ! Discrete states at t - TYPE(MD_ConstraintStateType), INTENT(IN ) :: z ! Constraint states at t - TYPE(MD_OtherStateType), INTENT(IN ) :: other ! Other states at t - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables - TYPE(MD_ContinuousStateType), INTENT( OUT) :: dxdt ! Continuous state derivatives at t - INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation - CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None - - - INTEGER(IntKi) :: L ! index - INTEGER(IntKi) :: J ! index - INTEGER(IntKi) :: K ! index - INTEGER(IntKi) :: Istart ! start index of line/connect in state vector - INTEGER(IntKi) :: Iend ! end index of line/connect in state vector +! ! try writing output for troubleshooting purposes (TEMPORARY) +! CALL MDIO_WriteOutputs(-1.0_DbKi, p, m, y, ErrStat, ErrMsg) +! IF ( ErrStat >= AbortErrLev ) THEN +! ErrMsg = ' Error in MDIO_WriteOutputs: '//TRIM(ErrMsg) +! RETURN +! END IF +! END DO - ! Initialize ErrStat - ErrStat = ErrID_None - ErrMsg = "" + ! -------------------------------------------------------------------- + ! do dynamic relaxation to get ICs + ! -------------------------------------------------------------------- - ! allocations of dxdt (as in SubDyn. "INTENT(OUT) automatically deallocates the arrays on entry, we have to allocate them here" is this right/efficient?) - ALLOCATE ( dxdt%states(size(x%states)), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN - ErrMsg = ' Error allocating dxdt%states array.' - RETURN - END IF + ! only do this if TMaxIC > 0 + if (InitInp%TMaxIC > 0.0_DbKi) then + CALL WrScr(" Finalizing ICs using dynamic relaxation."//NewLine) ! newline because next line writes over itself - ! clear connection force and mass values - DO L = 1, p%NConnects - DO J = 1,3 - m%ConnectList(L)%Ftot(J) = 0.0_DbKi - m%ConnectList(L)%Ftot(J) = 0.0_DbKi - DO K = 1,3 - m%ConnectList(L)%Mtot(K,J) = 0.0_DbKi - m%ConnectList(L)%Mtot(K,J) = 0.0_DbKi - END DO - END DO - END DO - - ! update fairlead positions for instantaneous values (fixed 2015-06-22) - DO K = 1, p%NFairs - DO J = 1,3 - m%ConnectList(m%FairIdList(K))%r(J) = u%PtFairleadDisplacement%Position(J,K) + u%PtFairleadDisplacement%TranslationDisp(J,K) - m%ConnectList(m%FairIdList(K))%rd(J) = u%PtFairleadDisplacement%TranslationVel(J,K) ! is this right? <<< + ! boost drag coefficient of each line type <<<<<<<< does this actually do anything or do lines hold these coefficients??? + DO I = 1, p%nLineTypes + m%LineTypeList(I)%Cdn = m%LineTypeList(I)%Cdn * InitInp%CdScaleIC + m%LineTypeList(I)%Cdt = m%LineTypeList(I)%Cdt * InitInp%CdScaleIC ! <<<<< need to update this to apply to all objects' drag END DO - END DO - ! apply line length changes from active tensioning if applicable - DO L = 1, p%NLines - IF (m%LineList(L)%CtrlChan > 0) then - - ! do a bounds check to prohibit excessive segment length changes (until a method to add/remove segments is created) - IF ( u%DeltaL(m%LineList(L)%CtrlChan) > m%LineList(L)%UnstrLen / m%LineList(L)%N ) then - ErrStat = ErrID_Fatal - ErrMsg = ' Active tension command will make a segment longer than the limit of twice its original length.' - print *, u%DeltaL(m%LineList(L)%CtrlChan), " is an increase of more than ", (m%LineList(L)%UnstrLen / m%LineList(L)%N) - print *, u%DeltaL - print*, m%LineList(L)%CtrlChan - RETURN - END IF - IF ( u%DeltaL(m%LineList(L)%CtrlChan) < -0.5 * m%LineList(L)%UnstrLen / m%LineList(L)%N ) then - ErrStat = ErrID_Fatal - ErrMsg = ' Active tension command will make a segment shorter than the limit of half its original length.' - print *, u%DeltaL(m%LineList(L)%CtrlChan), " is a reduction of more than half of ", (m%LineList(L)%UnstrLen / m%LineList(L)%N) - print *, u%DeltaL - print*, m%LineList(L)%CtrlChan - RETURN - END IF - - ! for now this approach only acts on the fairlead end segment, and assumes all segment lengths are otherwise equal size - m%LineList(L)%l( m%LineList(L)%N) = m%LineList(L)%UnstrLen/m%LineList(L)%N + u%DeltaL(m%LineList(L)%CtrlChan) - m%LineList(L)%ld(m%LineList(L)%N) = u%DeltaLdot(m%LineList(L)%CtrlChan) + ! allocate array holding 10 latest fairlead tensions + ALLOCATE ( FairTensIC(p%nLines, 10), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) THEN + CALL CheckError( ErrID_Fatal, ErrMsg2 ) + RETURN END IF - END DO - - ! do Line force and acceleration calculations, also add end masses/forces to respective Connects - DO L = 1, p%NLines - Istart = m%LineStateIndList(L) - Iend = Istart + 6*(m%LineList(L)%N - 1) - 1 - CALL DoLineRHS(x%states(Istart:Iend), dxdt%states(Istart:Iend), t, m%LineList(L), & - m%LineTypeList(m%LineList(L)%PropsIdNum), & - m%ConnectList(m%LineList(L)%FairConnect)%Ftot, m%ConnectList(m%LineList(L)%FairConnect)%Mtot, & - m%ConnectList(m%LineList(L)%AnchConnect)%Ftot, m%ConnectList(m%LineList(L)%AnchConnect)%Mtot ) - END DO - - ! perform connection force and mass calculations (done to all connects for sake of calculating fairlead/anchor loads) - DO L = 1, p%NConnects - ! add Connect's own forces including buoyancy and weight - m%ConnectList(L)%Ftot(1) =m%ConnectList(L)%Ftot(1) + m%ConnectList(L)%conFX - m%ConnectList(L)%Ftot(2) =m%ConnectList(L)%Ftot(2) + m%ConnectList(L)%conFY - m%ConnectList(L)%Ftot(3) =m%ConnectList(L)%Ftot(3) + m%ConnectList(L)%conFZ + m%ConnectList(L)%conV*p%rhoW*p%g - m%ConnectList(L)%conM*p%g - - ! add Connect's own mass - DO J = 1,3 - m%ConnectList(L)%Mtot(J,J) = m%ConnectList(L)%Mtot(J,J) + m%ConnectList(L)%conM + ! initialize fairlead tension memory at changing values so things start unconverged + DO J = 1,p%nLines + DO I = 1, 10 + FairTensIC(J,I) = I + END DO END DO - END DO ! L - - - ! do Connect acceleration calculations - changed to do only connect types - DO L = 1, p%NConns - Istart = L*6-5 - Iend = L*6 - CALL DoConnectRHS(x%states(Istart:Iend), dxdt%states(Istart:Iend), t, m%ConnectList(m%ConnIDList(L))) - END DO - CONTAINS - + ! round dt to integer number of time steps + NdtM = ceiling(InitInp%DTIC/p%dtM0) ! get number of mooring time steps to do based on desired time step size + dtM = InitInp%DTIC/real(NdtM, DbKi) ! adjust desired time step to satisfy dt with an integer number of time steps - !====================================================================== - SUBROUTINE DoLineRHS (X, Xd, t, Line, LineProp, FairFtot, FairMtot, AnchFtot, AnchMtot) + t = 0.0_DbKi ! start time at zero - Real(DbKi), INTENT( IN ) :: X(:) ! state vector, provided - Real(DbKi), INTENT( INOUT ) :: Xd(:) ! derivative of state vector, returned ! cahnged to INOUT - Real(DbKi), INTENT (IN) :: t ! instantaneous time - TYPE(MD_Line), INTENT (INOUT) :: Line ! label for the current line, for convenience - TYPE(MD_LineProp), INTENT(IN) :: LineProp ! the single line property set for the line of interest - Real(DbKi), INTENT(INOUT) :: FairFtot(:) ! total force on Connect top of line is attached to - Real(DbKi), INTENT(INOUT) :: FairMtot(:,:) ! total mass of Connect top of line is attached to - Real(DbKi), INTENT(INOUT) :: AnchFtot(:) ! total force on Connect bottom of line is attached to - Real(DbKi), INTENT(INOUT) :: AnchMtot(:,:) ! total mass of Connect bottom of line is attached to + ! because TimeStep wants an array... + call MD_CopyInput( u, u_array(1), MESH_NEWCOPY, ErrStat2, ErrMsg2 ) ! make a size=1 array of inputs (since MD_RK2 expects an array to InterpExtrap) + call MD_CopyInput( u, u_interp, MESH_NEWCOPY, ErrStat2, ErrMsg2 ) ! also make an inputs object to interpExtrap to + t_array(1) = t ! fill in the times "array" for u_array + DO I = 1, ceiling(InitInp%TMaxIC/InitInp%DTIC) ! loop through IC gen time steps, up to maximum - INTEGER(IntKi) :: I ! index of segments or nodes along line - INTEGER(IntKi) :: J ! index - INTEGER(IntKi) :: K ! index - INTEGER(IntKi) :: N ! number of segments in line - Real(DbKi) :: d ! line diameter - Real(DbKi) :: rho ! line material density [kg/m^3] - Real(DbKi) :: Sum1 ! for summing squares - Real(DbKi) :: m_i ! node mass - Real(DbKi) :: v_i ! node submerged volume - Real(DbKi) :: Vi(3) ! relative water velocity at a given node - Real(DbKi) :: Vp(3) ! transverse relative water velocity component at a given node - Real(DbKi) :: Vq(3) ! tangential relative water velocity component at a given node - Real(DbKi) :: SumSqVp ! - Real(DbKi) :: SumSqVq ! - Real(DbKi) :: MagVp ! - Real(DbKi) :: MagVq ! + !loop through line integration time steps + DO J = 1, NdtM ! for (double ts=t; ts<=t+ICdt-dts; ts+=dts) - N = Line%N ! for convenience - d = LineProp%d ! for convenience - rho = LineProp%w/(Pi/4.0*d*d) - + CALL MD_RK2(t, dtM, u_interp, u_array, t_array, p, x, xd, z, other, m, ErrStat2, ErrMsg2) + + ! check for NaNs - is this a good place/way to do it? + DO K = 1, m%Nx + IF (Is_NaN(REAL(x%states(K),DbKi))) THEN + ErrStat = ErrID_Fatal + ErrMsg = ' NaN state detected.' + EXIT + END IF + END DO + + IF (ErrStat == ErrID_Fatal) THEN + print *, "NaN detected at time ", t, " during IC gen. Here is the state vector: " + print *, x%states + EXIT + END IF + END DO ! J time steps - ! set end node positions and velocities from connect objects' states - DO J = 1, 3 - Line%r( J,N) = m%ConnectList(Line%FairConnect)%r(J) - Line%r( J,0) = m%ConnectList(Line%AnchConnect)%r(J) - Line%rd(J,N) = m%ConnectList(Line%FairConnect)%rd(J) - Line%rd(J,0) = m%ConnectList(Line%AnchConnect)%rd(J) - END DO + ! ! integrate the EOMs one DTIC s time step + ! CALL TimeStep ( t, InitInp%DTIC, u_array, t_array, p, x, xd, z, other, m, ErrStat, ErrMsg ) + ! CALL CheckError( ErrStat2, ErrMsg2 ) + ! IF (ErrStat >= AbortErrLev) RETURN - ! set interior node positions and velocities - DO I = 1, N-1 - DO J = 1, 3 - Line%r( J,I) = X( 3*N-3 + 3*I-3 + J) ! r(J,I) = X[3*N-3 + 3*i-3 + J]; // get positions .. used to start from m%LineStateIndList(Line%IdNum) in whole state vector - Line%rd(J,I) = X( 3*I-3 + J) ! rd(J,I) = X[ 3*i-3 + J]; // get velocities + ! store new fairlead tension (and previous fairlead tensions for comparison) + DO l = 1, p%nLines + + DO K=0,8 ! we want to count down from 10 to 2 . + FairTensIC(l, 10-K) = FairTensIC(l, 9-K) ! this pushes stored values up in the array + END DO + + ! now store latest value of each line's fairlead (end B) tension + FairTensIC(l,1) = TwoNorm(m%LineList(l)%Fnet(:, m%LineList(l)%N)) END DO - END DO - ! calculate instantaneous (stretched) segment lengths and rates << should add catch here for if lstr is ever zero - DO I = 1, N - Sum1 = 0.0_DbKi - DO J = 1, 3 - Sum1 = Sum1 + (Line%r(J,I) - Line%r(J,I-1)) * (Line%r(J,I) - Line%r(J,I-1)) - END DO - Line%lstr(I) = sqrt(Sum1) ! stretched segment length - Sum1 = 0.0_DbKi - DO J = 1, 3 - Sum1 = Sum1 + (Line%r(J,I) - Line%r(J,I-1))*(Line%rd(J,I) - Line%rd(J,I-1)) - END DO - Line%lstrd(I) = Sum1/Line%lstr(I) ! segment stretched length rate of change + ! provide status message + ! bjj: putting this in a string so we get blanks to cover up previous values (if current string is shorter than previous one) + Message = ' t='//trim(Num2LStr(t))//' FairTen 1: '//trim(Num2LStr(FairTensIC(1,1)))// & + ', '//trim(Num2LStr(FairTensIC(1,2)))//', '//trim(Num2LStr(FairTensIC(1,3))) + CALL WrOver( Message ) - ! Line%V(I) = Pi/4.0 * d*d*Line%l(I) !volume attributed to segment - END DO + ! check for convergence (compare current tension at each fairlead with previous 9 values) + IF (I > 9) THEN + + Converged = 1 + + ! check for non-convergence + + DO l = 1, p%nLines + DO K = 1,9 + IF ( abs( FairTensIC(l,K)/FairTensIC(l,K+1) - 1.0 ) > InitInp%threshIC ) THEN + Converged = 0 + EXIT + END IF + END DO + + IF (Converged == 0) EXIT ! make sure we exit this loop too + END DO - !calculate unit tangent vectors (q) for each node (including ends) note: I think these are pointing toward 0 rather than N! - CALL UnitVector(Line%q(:,0), Line%r(:,1), Line%r(:,0)) ! compute unit vector q - DO I = 1, N-1 - CALL UnitVector(Line%q(:,I), Line%r(:,I+1), Line%r(:,I-1)) ! compute unit vector q ... using adjacent two nodes! - END DO - CALL UnitVector(Line%q(:,N), Line%r(:,N), Line%r(:,N-1)) ! compute unit vector q + IF (Converged == 1) THEN ! if we made it with all cases satisfying the threshold + CALL WrScr(' Fairlead tensions converged to '//trim(Num2LStr(100.0*InitInp%threshIC))//'% after '//trim(Num2LStr(t))//' seconds.') + EXIT ! break out of the time stepping loop + END IF + END IF + IF (I == ceiling(InitInp%TMaxIC/InitInp%DTIC) ) THEN + CALL WrScr(' Fairlead tensions did not converge within TMaxIC='//trim(Num2LStr(InitInp%TMaxIC))//' seconds.') + !ErrStat = ErrID_Warn + !ErrMsg = ' MD_Init: ran dynamic convergence to TMaxIC without convergence' + END IF - ! wave kinematics not implemented yet + END DO ! I ... looping through time steps - !calculate mass (including added mass) matrix for each node - DO I = 0, N - IF (I==0) THEN - m_i = Pi/8.0 *d*d*Line%l(1)*rho - v_i = 0.5 *Line%V(1) - ELSE IF (I==N) THEN - m_i = pi/8.0 *d*d*Line%l(N)*rho; - v_i = 0.5*Line%V(N) - ELSE - m_i = pi/8.0 * d*d*rho*(Line%l(I) + Line%l(I+1)) - v_i = 0.5 *(Line%V(I) + Line%V(I+1)) - END IF - DO J=1,3 - DO K=1,3 - IF (J==K) THEN - Line%M(K,J,I) = m_i + p%rhoW*v_i*( LineProp%Can*(1 - Line%q(J,I)*Line%q(K,I)) + LineProp%Cat*Line%q(J,I)*Line%q(K,I) ) - ELSE - Line%M(K,J,I) = p%rhoW*v_i*( LineProp%Can*(-Line%q(J,I)*Line%q(K,I)) + LineProp%Cat*Line%q(J,I)*Line%q(K,I) ) - END IF - END DO - END DO + CALL MD_DestroyInput( u_array(1), ErrStat2, ErrMsg2 ) - CALL Inverse3by3(Line%S(:,:,I), Line%M(:,:,I)) ! invert mass matrix + ! UNboost drag coefficient of each line type <<< + DO I = 1, p%nLineTypes + m%LineTypeList(I)%Cdn = m%LineTypeList(I)%Cdn / InitInp%CdScaleIC + m%LineTypeList(I)%Cdt = m%LineTypeList(I)%Cdt / InitInp%CdScaleIC END DO + end if ! InitInp%TMaxIC > 0 + - ! ------------------ CALCULATE FORCES ON EACH NODE ---------------------------- - - ! loop through the segments - DO I = 1, N + p%dtCoupling = DTcoupling ! store coupling time step for use in updatestates - ! line tension, inherently including possibility of dynamic length changes in l term - IF (Line%lstr(I)/Line%l(I) > 1.0) THEN - DO J = 1, 3 - Line%T(J,I) = LineProp%EA *( 1.0/Line%l(I) - 1.0/Line%lstr(I) ) * (Line%r(J,I)-Line%r(J,I-1)) - END DO - ELSE - DO J = 1, 3 - Line%T(J,I) = 0.0_DbKi ! cable can't "push" - END DO - END if + other%dummy = 0 + xd%dummy = 0 + z%dummy = 0 + + + ! TODO: add feature for automatic water depth increase based on max anchor depth! - ! line internal damping force based on line-specific BA value, including possibility of dynamic length changes in l and ld terms - DO J = 1, 3 - Line%Td(J,I) = Line%BA* ( Line%lstrd(I) - Line%lstr(I)*Line%ld(I)/Line%l(I) )/Line%l(I) * (Line%r(J,I)-Line%r(J,I-1)) / Line%lstr(I) - END DO - END DO + CONTAINS + LOGICAL FUNCTION AllocateFailed(arrayName) - ! loop through the nodes - DO I = 0, N + CHARACTER(*), INTENT(IN ) :: arrayName ! The array name + + call SetErrStat(ErrStat2, "Error allocating space for "//trim(arrayName)//" array.", ErrStat, ErrMsg, 'MD_Init') + AllocateFailed = ErrStat2 >= AbortErrLev + if (AllocateFailed) call CleanUp() !<<<<<<<<<< need to fix this up + END FUNCTION AllocateFailed + + + LOGICAL FUNCTION Failed() - !submerged weight (including buoyancy) - IF (I==0) THEN - Line%W(3,I) = Pi/8.0*d*d* Line%l(1)*(rho - p%rhoW) *(-p%g) ! assuming g is positive - ELSE IF (i==N) THEN - Line%W(3,I) = pi/8.0*d*d* Line%l(N)*(rho - p%rhoW) *(-p%g) - ELSE - Line%W(3,I) = pi/8.0*d*d* (Line%l(I)*(rho - p%rhoW) + Line%l(I+1)*(rho - p%rhoW) )*(-p%g) ! left in this form for future free surface handling - END IF + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'MDIO_ReadInput') + Failed = ErrStat >= AbortErrLev + if (Failed) call CleanUp() + END FUNCTION Failed - !relative flow velocities - DO J = 1, 3 - Vi(J) = 0.0 - Line%rd(J,I) ! relative flow velocity over node -- this is where wave velicites would be added - END DO - ! decomponse relative flow into components - SumSqVp = 0.0_DbKi ! start sums of squares at zero - SumSqVq = 0.0_DbKi - DO J = 1, 3 - Vq(J) = DOT_PRODUCT( Vi , Line%q(:,I) ) * Line%q(J,I); ! tangential relative flow component - Vp(J) = Vi(J) - Vq(J) ! transverse relative flow component - SumSqVq = SumSqVq + Vq(J)*Vq(J) - SumSqVp = SumSqVp + Vp(J)*Vp(J) - END DO - MagVp = sqrt(SumSqVp) ! get magnitudes of flow components - MagVq = sqrt(SumSqVq) + SUBROUTINE CheckError(ErrID,Msg) + ! This subroutine sets the error message and level and cleans up if the error is >= AbortErrLev - ! transverse and tangenential drag - IF (I==0) THEN - DO J = 1, 3 - Line%Dp(J,I) = 0.25*p%rhoW*LineProp%Cdn* d*Line%l(1) * MagVp * Vp(J) - Line%Dq(J,I) = 0.25*p%rhoW*LineProp%Cdt* Pi*d*Line%l(1) * MagVq * Vq(J) - END DO - ELSE IF (I==N) THEN - DO J = 1, 3 - Line%Dp(J,I) = 0.25*p%rhoW*LineProp%Cdn* d*Line%l(N) * MagVp * Vp(J); - Line%Dq(J,I) = 0.25*p%rhoW*LineProp%Cdt* Pi*d*Line%l(N) * MagVq * Vq(J) - END DO - ELSE - DO J = 1, 3 - Line%Dp(J,I) = 0.25*p%rhoW*LineProp%Cdn* d*(Line%l(I) + Line%l(I+1)) * MagVp * vp(J); - Line%Dq(J,I) = 0.25*p%rhoW*LineProp%Cdt* Pi*d*(Line%l(I) + Line%l(I+1)) * MagVq * vq(J); - END DO - END IF + ! Passed arguments + INTEGER(IntKi), INTENT(IN) :: ErrID ! The error identifier (ErrStat) + CHARACTER(*), INTENT(IN) :: Msg ! The error message (ErrMsg) - ! F-K force from fluid acceleration not implemented yet + INTEGER(IntKi) :: ErrStat3 ! The error identifier (ErrStat) + CHARACTER(1024) :: ErrMsg3 ! The error message (ErrMsg) - ! bottom contact (stiffness and damping, vertical-only for now) - updated Nov 24 for general case where anchor and fairlead ends may deal with bottom contact forces + ! Set error status/message; + IF ( ErrID /= ErrID_None ) THEN - IF (Line%r(3,I) < -p%WtrDpth) THEN - IF (I==0) THEN - Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*( Line%l(I+1) ) - ELSE IF (I==N) THEN - Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) ) - ELSE - Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) + Line%l(I+1) ) + IF (ErrStat /= ErrID_None) ErrMsg = TRIM(ErrMsg)//NewLine ! if there's a pre-existing warning/error, retain the message and start a new line + ErrMsg = TRIM(ErrMsg)//' MD_Init:'//TRIM(Msg) + ErrStat = MAX(ErrStat, ErrID) + ! Clean up if we're going to return on error: close files, deallocate local arrays - END IF - ELSE - Line%B(3,I) = 0.0_DbKi - END IF - ! total forces - IF (I==0) THEN - DO J = 1, 3 - Line%F(J,I) = Line%T(J,1) + Line%Td(J,1) + Line%W(J,I) + Line%Dp(J,I) + Line%Dq(J,I) + Line%B(J,I) - END DO - ELSE IF (I==N) THEN - DO J = 1, 3 - Line%F(J,I) = -Line%T(J,N) - Line%Td(J,N) + Line%W(J,I) + Line%Dp(J,I) + Line%Dq(J,I) + Line%B(J,I) - END DO - ELSE - DO J = 1, 3 - Line%F(J,I) = Line%T(J,I+1) - Line%T(J,I) + Line%Td(J,I+1) - Line%Td(J,I) + Line%W(J,I) + Line%Dp(J,I) + Line%Dq(J,I) + Line%B(J,I) - END DO + IF ( ErrStat >= AbortErrLev ) THEN + IF (ALLOCATED(m%CpldConIs )) DEALLOCATE(m%CpldConIs ) + IF (ALLOCATED(m%FreeConIs )) DEALLOCATE(m%FreeConIs ) + IF (ALLOCATED(m%LineStateIs1 )) DEALLOCATE(m%LineStateIs1 ) + IF (ALLOCATED(m%LineStateIsN )) DEALLOCATE(m%LineStateIsN ) + IF (ALLOCATED(m%ConStateIs1 )) DEALLOCATE(m%ConStateIs1 ) + IF (ALLOCATED(m%ConStateIsN )) DEALLOCATE(m%ConStateIsN ) + IF (ALLOCATED(x%states )) DEALLOCATE(x%states ) + IF (ALLOCATED(FairTensIC )) DEALLOCATE(FairTensIC ) END IF + END IF - END DO ! I - done looping through nodes + END SUBROUTINE CheckError + SUBROUTINE CleanUp() + ! ErrStat = ErrID_Fatal + CLOSE( UnIn ) + ! IF (InitInp%Echo) CLOSE( UnEc ) + END SUBROUTINE - ! loop through internal nodes and update their states - DO I=1, N-1 - DO J=1,3 + END SUBROUTINE MD_Init + !----------------------------------------------------------------------------------------====== - ! calculate RHS constant (premultiplying force vector by inverse of mass matrix ... i.e. rhs = S*Forces) - Sum1 = 0.0_DbKi ! reset temporary accumulator - DO K = 1, 3 - Sum1 = Sum1 + Line%S(K,J,I) * Line%F(K,I) ! matrix-vector multiplication [S i]{Forces i} << double check indices - END DO ! K - ! update states - Xd(3*N-3 + 3*I-3 + J) = X(3*I-3 + J); ! dxdt = V (velocities) - Xd( 3*I-3 + J) = Sum1 ! dVdt = RHS * A (accelerations) - END DO ! J - END DO ! I + !----------------------------------------------------------------------------------------====== + SUBROUTINE MD_UpdateStates( t, n, u, t_array, p, x, xd, z, other, m, ErrStat, ErrMsg) - ! add force and mass of end nodes to the Connects they correspond to - DO J = 1,3 - FairFtot(J) = FairFtot(J) + Line%F(J,N) - AnchFtot(J) = AnchFtot(J) + Line%F(J,0) - DO K = 1,3 - FairMtot(K,J) = FairMtot(K,J) + Line%M(K,J,N) - AnchMtot(K,J) = AnchMtot(K,J) + Line%M(K,J,0) - END DO - END DO + REAL(DbKi) , INTENT(IN ) :: t + INTEGER(IntKi) , INTENT(IN ) :: n + TYPE(MD_InputType) , INTENT(INOUT) :: u(:) ! INTENT(INOUT) ! had to change this to INOUT + REAL(DbKi) , INTENT(IN ) :: t_array(:) + TYPE(MD_ParameterType) , INTENT(IN ) :: p ! INTENT(IN ) + TYPE(MD_ContinuousStateType) , INTENT(INOUT) :: x ! INTENT(INOUT) + TYPE(MD_DiscreteStateType) , INTENT(INOUT) :: xd ! INTENT(INOUT) + TYPE(MD_ConstraintStateType) , INTENT(INOUT) :: z ! INTENT(INOUT) + TYPE(MD_OtherStateType) , INTENT(INOUT) :: other ! INTENT(INOUT) + TYPE(MD_MiscVarType) , INTENT(INOUT) :: m ! INTENT(INOUT) + INTEGER(IntKi) , INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*) , INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None - END SUBROUTINE DoLineRHS - !===================================================================== + INTEGER(IntKi) :: ErrStat2 ! Error status of the operation + CHARACTER(ErrMsgLen) :: ErrMsg2 ! Error message if ErrStat2 /= ErrID_None +! moved to TimeStep TYPE(MD_InputType) :: u_interp ! + INTEGER(IntKi) :: nTime + + TYPE(MD_InputType) :: u_interp ! interpolated instantaneous input values to be calculated for each mooring time step - !====================================================================== - SUBROUTINE DoConnectRHS (X, Xd, t, Connect) + REAL(DbKi) :: t2 ! copy of time variable that will get advanced by the integrator (not sure this is necessary<<<) + REAL(DbKi) :: dtM ! actual mooring dynamics time step + INTEGER(IntKi) :: NdtM ! number of time steps to integrate through with RK2 + INTEGER(IntKi) :: I + INTEGER(IntKi) :: J - ! This subroutine is for the "Connect" type of Connections only. Other types don't have their own state variables. - - Real(DbKi), INTENT( IN ) :: X(:) ! state vector for this connect, provided - Real(DbKi), INTENT( OUT ) :: Xd(:) ! derivative of state vector for this connect, returned - Real(DbKi), INTENT (IN) :: t ! instantaneous time - Type(MD_Connect), INTENT (INOUT) :: Connect ! Connect number + nTime = size(u) ! the number of times of input data provided? <<<<<<< not used + t2 = t - !INTEGER(IntKi) :: I ! index of segments or nodes along line - INTEGER(IntKi) :: J ! index - INTEGER(IntKi) :: K ! index - Real(DbKi) :: Sum1 ! for adding things +! >>> removing this section and putting it inside loop of TimeStep (to be done at every time step) <<< +! ! create space for arrays/meshes in u_interp +! CALL MD_CopyInput(u(1), u_interp, MESH_NEWCOPY, ErrStat2, ErrMsg2) +! CALL CheckError( ErrStat2, ErrMsg2 ) +! IF (ErrStat >= AbortErrLev) RETURN +! +! ! interpolate input mesh to correct time +! CALL MD_Input_ExtrapInterp(u, t_array, u_interp, t, ErrStat2, ErrMsg2) +! CALL CheckError( ErrStat2, ErrMsg2 ) +! IF (ErrStat >= AbortErrLev) RETURN +! +! +! ! go through fairleads and apply motions from driver +! DO I = 1, p%nCpldCons +! DO J = 1,3 +! m%ConnectList(m%CpldConIs(I))%r(J) = u_interp%PtFairleadDisplacement%Position(J,I) + u_interp%PtFairleadDisplacement%TranslationDisp(J,I) +! m%ConnectList(m%CpldConIs(I))%rd(J) = u_interp%PtFairleadDisplacement%TranslationVel(J,I) ! is this right? <<< +! END DO +! END DO +! - ! When this sub is called, the force and mass contributions from the attached Lines should already have been added to - ! Fto and Mtot by the Line RHS function. Also, any self weight, buoyancy, or external forcing should have already been - ! added by the calling subroutine. The only thing left is any added mass or drag forces from the connection (e.g. float) - ! itself, which will be added below. - IF (EqualRealNos(t, 0.0_DbKi)) THEN ! this is old: with current IC gen approach, we skip the first call to the line objects, because they're set AFTER the call to the connects +! ! call function that loops through mooring model time steps +! CALL TimeStep ( t2, p%dtCoupling, u, t_array, p, x, xd, z, other, m, ErrStat2, ErrMsg2 ) +! CALL CheckError( ErrStat2, ErrMsg2 ) +! IF (ErrStat >= AbortErrLev) RETURN - DO J = 1,3 - Xd(3+J) = X(J) ! velocities - these are unused in integration - Xd(J) = 0.0_DbKi ! accelerations - these are unused in integration - END DO - ELSE - ! from state values, get r and rdot values - DO J = 1,3 - Connect%r(J) = X(3 + J) ! get positions - Connect%rd(J) = X(J) ! get velocities - END DO - END IF + ! create space for arrays/meshes in u_interp ... is it efficient to do this every time step??? + CALL MD_CopyInput(u(1), u_interp, MESH_NEWCOPY, ErrStat, ErrMsg) - ! add any added mass and drag forces from the Connect body itself - DO J = 1,3 - Connect%Ftot(J) = Connect%Ftot(J) - 0.5 * p%rhoW * Connect%rd(J) * abs(Connect%rd(J)) * Connect%conCdA; ! add drag forces - corrected Nov 24 - Connect%Mtot(J,J) = Connect%Mtot(J,J) + Connect%conV*p%rhoW*Connect%conCa; ! add added mass - END DO - - ! invert node mass matrix - CALL Inverse3by3(Connect%S, Connect%Mtot) - DO J = 1,3 - ! RHS constant - (premultiplying force vector by inverse of mass matrix ... i.e. rhs = S*Forces - Sum1 = 0.0_DbKi ! reset accumulator - DO K = 1, 3 - Sum1 = Sum1 + Connect%S(K,J) * Connect%Ftot(K) ! matrix multiplication [S i]{Forces i} - END DO + ! round dt to integer number of time steps <<<< should this be calculated only once, up front? + NdtM = ceiling(p%dtCoupling/p%dtM0) ! get number of mooring time steps to do based on desired time step size + dtM = p%dtCoupling/float(NdtM) ! adjust desired time step to satisfy dt with an integer number of time steps - ! update states - Xd(3 + J) = X(J) ! dxdt = V (velocities) - Xd(J) = Sum1 ! dVdt = RHS * A (accelerations) + + !loop through line integration time steps + DO I = 1, NdtM ! for (double ts=t; ts<=t+ICdt-dts; ts+=dts) + + CALL MD_RK2(t2, dtM, u_interp, u, t_array, p, x, xd, z, other, m, ErrStat2, ErrMsg2) + + + ! check for NaNs - is this a good place/way to do it? + DO J = 1, m%Nx + IF (Is_NaN(REAL(x%states(J),DbKi))) THEN + ErrStat = ErrID_Fatal + ErrMsg = ' NaN state detected.' + EXIT + END IF END DO + + IF (ErrStat == ErrID_Fatal) THEN + print *, "NaN detected at time ", t2, ". Here is the state vector: " + print *, x%states + EXIT + END IF + + END DO ! I time steps - END SUBROUTINE DoConnectRHS - !===================================================================== - END SUBROUTINE MD_CalcContStateDeriv - !============================================================================================= + ! destroy dxdt and x2, and u_interp + !CALL MD_DestroyContState( dxdt, ErrStat, ErrMsg) + !CALL MD_DestroyContState( x2, ErrStat, ErrMsg) + CALL MD_DestroyInput(u_interp, ErrStat, ErrMsg) + IF ( ErrStat /= ErrID_None ) THEN + ErrMsg = ' Error destroying dxdt or x2.' + END IF + ! CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'MD_UpdateStates') - !=============================================================================================== - SUBROUTINE MD_End(u, p, x, xd, z, other, y, m, ErrStat , ErrMsg) - TYPE(MD_InputType) , INTENT(INOUT) :: u + ! check for NaNs - is this a good place/way to do it? + DO J = 1, m%Nx + IF (Is_NaN(REAL(x%states(J),DbKi))) THEN + ErrStat = ErrID_Fatal + ErrMsg = ' NaN state detected.' + EXIT + END IF + END DO + + IF (ErrStat == ErrID_Fatal) THEN + print *, "NaN detected at time ", t2, ". Here is the state vector: " + print *, x%states + END IF + + CONTAINS + + SUBROUTINE CheckError(ErrId, Msg) + ! This subroutine sets the error message and level and cleans up if the error is >= AbortErrLev + + INTEGER(IntKi), INTENT(IN) :: ErrID ! The error identifier (ErrStat) + CHARACTER(*), INTENT(IN) :: Msg ! The error message (ErrMsg) + + + IF ( ErrID /= ErrID_None ) THEN + + IF (ErrStat /= ErrID_None) ErrMsg = TRIM(ErrMsg)//NewLine ! keep existing error message if there is one + ErrMsg = TRIM(ErrMsg)//' MD_UpdateStates:'//TRIM(Msg) ! add current error message + ErrStat = MAX(ErrStat, ErrID) + + CALL WrScr( ErrMsg ) ! do this always or only if warning level? + + IF( ErrStat > ErrID_Warn ) THEN + ! CALL MD_DestroyInput( u_interp, ErrStat, ErrMsg ) + RETURN + END IF + END IF + + END SUBROUTINE CheckError + + END SUBROUTINE MD_UpdateStates + !---------------------------------------------------------------------------------------- + + + + !---------------------------------------------------------------------------------------- + SUBROUTINE MD_CalcOutput( t, u, p, x, xd, z, other, y, m, ErrStat, ErrMsg ) + + REAL(DbKi) , INTENT(IN ) :: t + TYPE( MD_InputType ) , INTENT(IN ) :: u ! INTENT(IN ) + TYPE( MD_ParameterType ) , INTENT(IN ) :: p ! INTENT(IN ) + TYPE( MD_ContinuousStateType ) , INTENT(IN ) :: x ! INTENT(IN ) + TYPE( MD_DiscreteStateType ) , INTENT(IN ) :: xd ! INTENT(IN ) + TYPE( MD_ConstraintStateType ) , INTENT(IN ) :: z ! INTENT(IN ) + TYPE( MD_OtherStateType ) , INTENT(IN ) :: other ! INTENT(IN ) + TYPE( MD_OutputType ) , INTENT(INOUT) :: y ! INTENT(INOUT) + TYPE(MD_MiscVarType) , INTENT(INOUT) :: m ! INTENT(INOUT) + INTEGER(IntKi) , INTENT(INOUT) :: ErrStat + CHARACTER(*) , INTENT(INOUT) :: ErrMsg + + ! TYPE(MD_ContinuousStateType) :: dxdt ! time derivatives of continuous states (initialized in CalcContStateDeriv) + INTEGER(IntKi) :: I ! counter + INTEGER(IntKi) :: J ! counter + INTEGER(IntKi) :: K ! counter + INTEGER(IntKi) :: l ! index used for objects + + Real(DbKi) :: F6net(6) ! net force and moment calculated on coupled objects + + INTEGER(IntKi) :: ErrStat2 ! Error status of the operation + CHARACTER(ErrMsgLen) :: ErrMsg2 ! Error message if ErrStat2 /= ErrID_None + + + ! below updated to make sure outputs are current (based on provided x and u) - similar to what's in UpdateStates + + ! ! go through fairleads and apply motions from driver + ! DO I = 1, p%nCpldCons + ! DO J = 1,3 + ! m%ConnectList(m%CpldConIs(I))%r(J) = u%CoupledKinematics%Position(J,I) + u%CoupledKinematics%TranslationDisp(J,I) + ! m%ConnectList(m%CpldConIs(I))%rd(J) = u%CoupledKinematics%TranslationVel(J,I) ! is this right? <<< + ! END DO + ! END DO + + + ! ! go through nodes and apply wave kinematics from driver + ! IF (p%WaterKin > 0) THEN + ! + ! J=0 + ! ! Body reference point coordinates + ! DO I = 1, p%nBodies + ! J = J + 1 + ! m%BodyList(I)%U = u%U(:,J) + ! m%BodyList(I)%Ud = u%Ud(:,J) + ! m%BodyList(I)%zeta = u%zeta(J) + ! END DO + ! ! Rod node coordinates + ! DO I = 1, p%nRods + ! DO K = 0,m%RodList(I)%N + ! J = J + 1 + ! m%RodList(I)%U (:,K) = u%U(:,J) + ! m%RodList(I)%Ud(:,K) = u%Ud(:,J) + ! m%RodList(I)%zeta(K) = u%zeta(J) + ! m%RodList(I)%PDyn(K) = u%PDyn(J) + ! END DO + ! END DO + ! ! Point reference point coordinates + ! DO I = 1, p%nConnects + ! J = J + 1 + ! m%ConnectList(I)%U = u%U(:,J) + ! m%ConnectList(I)%Ud = u%Ud(:,J) + ! m%ConnectList(I)%zeta = u%zeta(J) + ! END DO + ! ! Line internal node coordinates + ! DO I = 1, p%nLines + ! DO K = 1, m%LineList(I)%N-1 + ! J = J + 1 + ! m%LineList(I)%U (:,K) = u%U(:,J) + ! m%LineList(I)%Ud(:,K) = u%Ud(:,J) + ! m%LineList(I)%zeta(K) = u%zeta(J) + ! END DO + ! END DO + ! + ! END IF + + + + ! call CalcContStateDeriv in order to run model and calculate dynamics with provided x and u + CALL MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, m%xdTemp, ErrStat, ErrMsg ) + + ! ! assign net force on fairlead Connects to the fairlead force output mesh + ! DO i = 1, p%nCpldCons + ! DO J=1,3 + ! y%PtFairleadLoad%Force(J,I) = m%ConnectList(m%CpldConIs(I))%Fnet(J) + ! END DO + ! END DO + + ! now that forces have been updated, write them to the output mesh + J = 0 + DO l = 1,p%nCpldBodies + J = J + 1 + CALL Body_GetCoupledForce(m%BodyList(m%CpldBodyIs(l)), F6net, m, p) + y%CoupledLoads%Force( :,J) = F6net(1:3) + y%CoupledLoads%Moment(:,J) = F6net(4:6) + END DO + + DO l = 1,p%nCpldRods + J = J + 1 + CALL Rod_GetCoupledForce(m%RodList(m%CpldRodIs(l)), F6net, m%LineList, p) + y%CoupledLoads%Force( :,J) = F6net(1:3) + y%CoupledLoads%Moment(:,J) = F6net(4:6) + END DO + + DO l = 1,p%nCpldCons + J = J + 1 + CALL Connect_GetCoupledForce(m%ConnectList(m%CpldConIs(l)), F6net(1:3), m%LineList, p) + y%CoupledLoads%Force(:,J) = F6net(1:3) + END DO + + + + + + ! ! write all node positions to the node positons output array + ! ! go through the nodes and fill in the data (this should maybe be turned into a global function) + ! J=0 + ! ! Body reference point coordinates + ! DO I = 1, p%nBodies + ! J = J + 1 + ! y%rAll(:,J) = m%BodyList(I)%r6(1:3) + ! END DO + ! ! Rod node coordinates + ! DO I = 1, p%nRods + ! DO K = 0,m%RodList(I)%N + ! J = J + 1 + ! y%rAll(:,J) = m%RodList(I)%r(:,K) + ! END DO + ! END DO + ! ! Point reference point coordinates + ! DO I = 1, p%nConnects + ! J = J + 1 + ! y%rAll(:,J) = m%ConnectList(I)%r + ! END DO + ! ! Line internal node coordinates + ! DO I = 1, p%nLines + ! DO K = 1, m%LineList(I)%N-1 + ! J = J + 1 + ! y%rAll(:,J) = m%LineList(I)%r(:,K) + ! END DO + ! END DO + + + ! calculate outputs (y%WriteOutput) for glue code and write any m outputs to MoorDyn output files + CALL MDIO_WriteOutputs(REAL(t,DbKi) , p, m, y, ErrStat2, ErrMsg2) + CALL CheckError(ErrStat2, 'In MDIO_WriteOutputs: '//trim(ErrMsg2)) + IF ( ErrStat >= AbortErrLev ) RETURN + + + ! ! destroy dxdt + ! CALL MD_DestroyContState( dxdt, ErrStat2, ErrMsg2) + ! CALL CheckError(ErrStat2, 'When destroying dxdt: '//trim(ErrMsg2)) + ! IF ( ErrStat >= AbortErrLev ) RETURN + + + + CONTAINS + + SUBROUTINE CheckError(ErrId, Msg) + ! This subroutine sets the error message and level and cleans up if the error is >= AbortErrLev + + INTEGER(IntKi), INTENT(IN) :: ErrID ! The error identifier (ErrStat) + CHARACTER(*), INTENT(IN) :: Msg ! The error message (ErrMsg) + + + IF ( ErrID /= ErrID_None ) THEN + + IF (ErrStat /= ErrID_None) ErrMsg = TRIM(ErrMsg)//NewLine ! keep existing error message if there is one + ErrMsg = TRIM(ErrMsg)//' MD_CalcOutput:'//TRIM(Msg) ! add current error message + ErrStat = MAX(ErrStat, ErrID) + + CALL WrScr( ErrMsg ) ! do this always or only if warning level? <<<<<<<<<<<<<<<<<<<<<< probably should remove all instances + + ! IF( ErrStat > ErrID_Warn ) THEN + ! CALL MD_DestroyContState( dxdt, ErrStat2, ErrMsg2) + ! END IF + END IF + + END SUBROUTINE CheckError + + END SUBROUTINE MD_CalcOutput + !---------------------------------------------------------------------------------------- + + + !---------------------------------------------------------------------------------------- + SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, ErrMsg ) + ! Tight coupling routine for computing derivatives of continuous states + ! this is modelled off what used to be subroutine DoRHSmaster + + REAL(DbKi), INTENT(IN ) :: t ! Current simulation time in seconds + TYPE(MD_InputType), INTENT(IN ) :: u ! Inputs at t + TYPE(MD_ParameterType), INTENT(IN ) :: p ! Parameters + TYPE(MD_ContinuousStateType), INTENT(IN ) :: x ! Continuous states at t + TYPE(MD_DiscreteStateType), INTENT(IN ) :: xd ! Discrete states at t + TYPE(MD_ConstraintStateType), INTENT(IN ) :: z ! Constraint states at t + TYPE(MD_OtherStateType), INTENT(IN ) :: other ! Other states at t + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables + TYPE(MD_ContinuousStateType), INTENT(INOUT) :: dxdt ! Continuous state derivatives at t + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + + + INTEGER(IntKi) :: L ! index + INTEGER(IntKi) :: I ! index + INTEGER(IntKi) :: J ! index + INTEGER(IntKi) :: K ! index + INTEGER(IntKi) :: Istart ! start index of line/connect in state vector + INTEGER(IntKi) :: Iend ! end index of line/connect in state vector + + REAL(DbKi) :: r6_in(6) ! temporary for passing kinematics + REAL(DbKi) :: v6_in(6) ! temporary for passing kinematics + REAL(DbKi) :: a6_in(6) ! temporary for passing kinematics + REAL(DbKi) :: r_in(3) ! temporary for passing kinematics + REAL(DbKi) :: rd_in(3) ! temporary for passing kinematics + REAL(DbKi) :: a_in(3) ! temporary for passing kinematics + + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + +! ! allocations of dxdt (as in SubDyn. "INTENT(OUT) automatically deallocates the arrays on entry, we have to allocate them here" is this right/efficient?) +! ALLOCATE ( dxdt%states(size(x%states)), STAT = ErrStat ) +! IF ( ErrStat /= ErrID_None ) THEN +! ErrMsg = ' Error allocating dxdt%states array.' +! RETURN +! END IF + + ! clear connection force and mass values updateFairlead( t ); <<<< manually set anchored connection stuff for now here + r6_in = 0.0_DbKi + v6_in = 0.0_DbKi + CALL Body_SetKinematics(m%GroundBody, r6_in, v6_in, m%zeros6, t, m) + + ! ---------------------------------- coupled things --------------------------------- + ! Apply displacement and velocity terms here. Accelerations will be considered to calculate inertial loads at the end. + + J = 0 ! J is the index of the coupling points in the input mesh CoupledKinematics + ! any coupled bodies (type -1) + DO l = 1,p%nCpldBodies + J = J + 1 + r6_in(1:3) = u%CoupledKinematics%Position(:,J) + u%CoupledKinematics%TranslationDisp(:,J) + r6_in(4:6) = EulerExtract( TRANSPOSE( u%CoupledKinematics%Orientation(:,:,J) ) ) + v6_in(1:3) = u%CoupledKinematics%TranslationVel(:,J) + v6_in(4:6) = u%CoupledKinematics%RotationVel(:,J) + a6_in(1:3) = u%CoupledKinematics%TranslationAcc(:,J) + a6_in(4:6) = u%CoupledKinematics%RotationAcc(:,J) + + + if ((t >= 1.0) .and. (t < 1.001)) then + print *, "orientation matrix from mesh:" + print *, u%CoupledKinematics%Orientation(:,:,J) + print *, "Euler extract:" + print *, EulerExtract( u%CoupledKinematics%Orientation(:,:,J) ) + print *, "Euler extract of transpose" + print *, EulerExtract( transpose(u%CoupledKinematics%Orientation(:,:,J) )) + + print *, "done" + end if + + CALL Body_SetKinematics(m%BodyList(m%CpldBodyIs(l)), r6_in, v6_in, a6_in, t, m) + END DO + + ! any coupled rods (type -1 or -2) note, rotations ignored if it's a pinned rod + DO l = 1,p%nCpldRods + J = J + 1 + + r6_in(1:3) = u%CoupledKinematics%Position(:,J) + u%CoupledKinematics%TranslationDisp(:,J) + r6_in(4:6) = EulerExtract( TRANSPOSE( u%CoupledKinematics%Orientation(:,:,J) ) ) ! <<<< should make sure this works <<< + v6_in(1:3) = u%CoupledKinematics%TranslationVel(:,J) + v6_in(4:6) = u%CoupledKinematics%RotationVel(:,J) + a6_in(1:3) = u%CoupledKinematics%TranslationAcc(:,J) + a6_in(4:6) = u%CoupledKinematics%RotationAcc(:,J) + + if ((t >= 1.0) .and. (t < 1.001)) then + print *, "orientation matrix from mesh:" + print *, u%CoupledKinematics%Orientation(:,:,J) + print *, "Euler extract:" + print *, EulerExtract( u%CoupledKinematics%Orientation(:,:,J) ) + print *, "Euler extract of transpose" + print *, EulerExtract( transpose(u%CoupledKinematics%Orientation(:,:,J) )) + + print *, "done" + end if + + CALL Rod_SetKinematics(m%RodList(m%CpldRodIs(l)), r6_in, v6_in, a6_in, t, m%LineList) + + END DO + + ! any coupled points (type -1) + DO l = 1, p%nCpldCons + J = J + 1 + + r_in = u%CoupledKinematics%Position(:,J) + u%CoupledKinematics%TranslationDisp(:,J) + rd_in = u%CoupledKinematics%TranslationVel(:,J) + a_in(1:3) = u%CoupledKinematics%TranslationAcc(:,J) + CALL Connect_SetKinematics(m%ConnectList(m%CpldConIs(l)), r_in, rd_in, a_in, t, m%LineList) + + !print *, u%PtFairleadDisplacement%Position(:,l) + u%PtFairleadDisplacement%TranslationDisp(:,l) + !print *, u%PtFairleadDisplacement%TranslationVel(:,l) + + END DO + + + ! apply line length changes from active tensioning if applicable + DO L = 1, p%NLines + IF (m%LineList(L)%CtrlChan > 0) then + + ! do a bounds check to prohibit excessive segment length changes (until a method to add/remove segments is created) + IF ( u%DeltaL(m%LineList(L)%CtrlChan) > m%LineList(L)%UnstrLen / m%LineList(L)%N ) then + ErrStat = ErrID_Fatal + ErrMsg = ' Active tension command will make a segment longer than the limit of twice its original length.' + print *, u%DeltaL(m%LineList(L)%CtrlChan), " is an increase of more than ", (m%LineList(L)%UnstrLen / m%LineList(L)%N) + print *, u%DeltaL + print*, m%LineList(L)%CtrlChan + RETURN + END IF + IF ( u%DeltaL(m%LineList(L)%CtrlChan) < -0.5 * m%LineList(L)%UnstrLen / m%LineList(L)%N ) then + ErrStat = ErrID_Fatal + ErrMsg = ' Active tension command will make a segment shorter than the limit of half its original length.' + print *, u%DeltaL(m%LineList(L)%CtrlChan), " is a reduction of more than half of ", (m%LineList(L)%UnstrLen / m%LineList(L)%N) + print *, u%DeltaL + print*, m%LineList(L)%CtrlChan + RETURN + END IF + + ! for now this approach only acts on the fairlead end segment, and assumes all segment lengths are otherwise equal size + m%LineList(L)%l( m%LineList(L)%N) = m%LineList(L)%UnstrLen/m%LineList(L)%N + u%DeltaL(m%LineList(L)%CtrlChan) + m%LineList(L)%ld(m%LineList(L)%N) = u%DeltaLdot(m%LineList(L)%CtrlChan) + END IF + END DO + + + ! ! go through nodes and apply wave kinematics from driver + ! IF (p%WaterKin > 0) THEN + ! + ! J=0 + ! ! Body reference point coordinates + ! DO I = 1, p%nBodies + ! J = J + 1 + ! m%BodyList(I)%U = u%U(:,J) + ! m%BodyList(I)%Ud = u%Ud(:,J) + ! m%BodyList(I)%zeta = u%zeta(J) + ! END DO + ! ! Rod node coordinates + ! DO I = 1, p%nRods + ! DO K = 0,m%RodList(I)%N + ! J = J + 1 + ! m%RodList(I)%U (:,K) = u%U(:,J) + ! m%RodList(I)%Ud(:,K) = u%Ud(:,J) + ! m%RodList(I)%zeta(K) = u%zeta(J) + ! m%RodList(I)%PDyn(K) = u%PDyn(J) + ! END DO + ! END DO + ! ! Point reference point coordinates + ! DO I = 1, p%nConnects + ! J = J + 1 + ! m%ConnectList(I)%U = u%U(:,J) + ! m%ConnectList(I)%Ud = u%Ud(:,J) + ! m%ConnectList(I)%zeta = u%zeta(J) + ! END DO + ! ! Line internal node coordinates + ! DO I = 1, p%nLines + ! DO K = 1, m%LineList(I)%N-1 + ! J = J + 1 + ! m%LineList(I)%U (:,K) = u%U(:,J) + ! m%LineList(I)%Ud(:,K) = u%Ud(:,J) + ! m%LineList(I)%zeta(K) = u%zeta(J) + ! END DO + ! END DO + ! + ! END IF + + + ! independent or semi-independent things with their own states... + + ! give Bodies latest state variables (kinematics will also be assigned to dependent connections and rods, and thus line ends) + DO l = 1,p%nFreeBodies + CALL Body_SetState(m%BodyList(m%FreeBodyIs(l)), x%states(m%BodyStateIs1(l):m%BodyStateIsN(l)), t, m) + END DO + + ! give independent or pinned rods' latest state variables (kinematics will also be assigned to attached line ends) + DO l = 1,p%nFreeRods + CALL Rod_SetState(m%RodList(m%FreeRodIs(l)), x%states(m%RodStateIs1(l):m%RodStateIsN(l)), t, m%LineList) + END DO + + ! give Connects (independent connections) latest state variable values (kinematics will also be assigned to attached line ends) + DO l = 1,p%nFreeCons + ! Print *, "calling SetState for free connection, con#", m%FreeConIs(l), " with state range: ", m%ConStateIs1(l), "-", m%ConStateIsN(l) + !K=K+1 + CALL Connect_SetState(m%ConnectList(m%FreeConIs(l)), x%states(m%ConStateIs1(l):m%ConStateIsN(l)), t, m%LineList) + END DO + + ! give Lines latest state variable values for internal nodes + DO l = 1,p%nLines + CALL Line_SetState(m%LineList(l), x%states(m%LineStateIs1(l):m%LineStateIsN(l)), t) + END DO + + ! calculate dynamics of free objects (will also calculate forces (doRHS()) from any child/dependent objects)... + + ! calculate line dynamics (and calculate line forces and masses attributed to connections) + DO l = 1,p%nLines + CALL Line_GetStateDeriv(m%LineList(l), dxdt%states(m%LineStateIs1(l):m%LineStateIsN(l)), p) !dt might also be passed for fancy friction models + END DO + + ! calculate connect dynamics (including contributions from attached lines + ! as well as hydrodynamic forces etc. on connect object itself if applicable) + DO l = 1,p%nFreeCons + CALL Connect_GetStateDeriv(m%ConnectList(m%FreeConIs(l)), dxdt%states(m%ConStateIs1(l):m%ConStateIsN(l)), m%LineList, p) + END DO + + ! calculate dynamics of independent Rods + DO l = 1,p%nFreeRods + CALL Rod_GetStateDeriv(m%RodList(m%FreeRodIs(l)), dxdt%states(m%RodStateIs1(l):m%RodStateIsN(l)), m%LineList, p) + END DO + + ! calculate dynamics of Bodies + DO l = 1,p%nFreeBodies + CALL Body_GetStateDeriv(m%BodyList(m%FreeBodyIs(l)), dxdt%states(m%BodyStateIs1(l):m%BodyStateIsN(l)), m, p) + END DO + + + + ! get dynamics/forces (doRHS()) of coupled objects, which weren't addressed in above calls (this includes inertial loads) + ! note: can do this in any order since there are no dependencies among coupled objects + + + DO l = 1,p%nCpldCons + + ! >>>>>>>> here we should pass along accelerations and include inertial loads in the calculation!!! <<>> below should no longer be necessary thanks to using ExtrapInterp of u(:) within the mooring time stepping loop.. <<< ! ! update Fairlead positions by integrating velocity and last position (do this AFTER the processing of the time step rather than before) - ! DO J = 1, p%NFairs + ! DO J = 1, p%nCpldCons ! DO K = 1, 3 - ! m%ConnectList(m%FairIdList(J))%r(K) = m%ConnectList(m%FairIdList(J))%r(K) + m%ConnectList(m%FairIdList(J))%rd(K)*dtM + ! m%ConnectList(m%CpldConIs(J))%r(K) = m%ConnectList(m%CpldConIs(J))%r(K) + m%ConnectList(m%CpldConIs(J))%rd(K)*dtM ! END DO ! END DO @@ -1331,17 +2896,18 @@ SUBROUTINE TimeStep ( t, dtStep, u, utimes, p, x, xd, z, other, m, ErrStat, ErrM END SUBROUTINE TimeStep - !====================================================================== + !-------------------------------------------------------------- - !======================================================================= + !----------------------------------------------------------------------- + ! >>>>>>>>>>>>>> rename/reorganize this subroutine >>>>>>>>>>>>> SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) ! allocate arrays in line object TYPE(MD_Line), INTENT(INOUT) :: Line ! the single line object of interest TYPE(MD_LineProp), INTENT(INOUT) :: LineProp ! the single line property set for the line of interest - REAL(ReKi), INTENT(IN) :: rhoW + REAL(DbKi), INTENT(IN) :: rhoW INTEGER, INTENT( INOUT ) :: ErrStat ! returns a non-zero value when an error occurs CHARACTER(*), INTENT( INOUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None @@ -1351,6 +2917,41 @@ SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) N = Line%N ! number of segments in this line (for code readability) + ! -------------- save some section properties to the line object itself ----------------- + + Line%d = LineProp%d + Line%rho = LineProp%w/(Pi/4.0 * Line%d * Line%d) + + Line%EA = LineProp%EA + + Line%Can = LineProp%Can + Line%Cat = LineProp%Cat + Line%Cdn = LineProp%Cdn + Line%Cdt = LineProp%Cdt + + ! Specify specific internal damping coefficient (BA) for this line. + ! Will be equal to inputted BA of LineType if input value is positive. + ! If input value is negative, it is considered to be desired damping ratio (zeta) + ! from which the line's BA can be calculated based on the segment natural frequency. + IF (LineProp%BA < 0) THEN + ! - we assume desired damping coefficient is zeta = -LineProp%BA + ! - highest axial vibration mode of a segment is wn = sqrt(k/m) = 2N/UnstrLen*sqrt(EA/w) + Line%BA = -LineProp%BA * Line%UnstrLen / Line%N * SQRT(LineProp%EA * LineProp%w) + ! print *, 'Based on zeta, BA set to ', Line%BA + + ! print *, 'Negative BA input detected, treating as -zeta. For zeta = ', -LineProp%BA, ', setting BA to ', Line%BA + + ELSE + Line%BA = LineProp%BA + ! temp = Line%N * Line%BA / Line%UnstrLen * SQRT(1.0/(LineProp%EA * LineProp%w)) + ! print *, 'BA set as input to ', Line%BA, '. Corresponding zeta is ', temp + END IF + + !temp = 2*Line%N / Line%UnstrLen * sqrt( LineProp%EA / LineProp%w) / TwoPi + !print *, 'Segment natural frequency is ', temp, ' Hz' + + + ! allocate node positions and velocities (NOTE: these arrays start at ZERO) ALLOCATE ( Line%r(3, 0:N), Line%rd(3, 0:N), STAT = ErrStat ) IF ( ErrStat /= ErrID_None ) THEN @@ -1375,12 +2976,20 @@ SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) RETURN END IF - ! assign values for l and V + ! assign values for l, ld, and V DO J=1,N Line%l(J) = Line%UnstrLen/REAL(N, DbKi) Line%ld(J)= 0.0_DbKi Line%V(J) = Line%l(J)*0.25*Pi*LineProp%d*LineProp%d END DO + + ! allocate water related vectors + ALLOCATE ( Line%U(3, 0:N), Line%Ud(3, 0:N), Line%zeta(0:N), Line%PDyn(0:N), STAT = ErrStat ) + ! set to zero initially (important of wave kinematics are not being used) + Line%U = 0.0_DbKi + Line%Ud = 0.0_DbKi + Line%zeta = 0.0_DbKi + Line%PDyn = 0.0_DbKi ! allocate segment tension and internal damping force vectors ALLOCATE ( Line%T(3, N), Line%Td(3, N), STAT = ErrStat ) @@ -1392,7 +3001,7 @@ SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) ! allocate node force vectors ALLOCATE ( Line%W(3, 0:N), Line%Dp(3, 0:N), Line%Dq(3, 0:N), Line%Ap(3, 0:N), & - Line%Aq(3, 0:N), Line%B(3, 0:N), Line%F(3, 0:N), STAT = ErrStat ) + Line%Aq(3, 0:N), Line%B(3, 0:N), Line%Fnet(3, 0:N), STAT = ErrStat ) IF ( ErrStat /= ErrID_None ) THEN ErrMsg = ' Error allocating node force arrays.' !CALL CleanUp() @@ -1415,44 +3024,25 @@ SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) RETURN END IF - ! Specify specific internal damping coefficient (BA) for this line. - ! Will be equal to inputted BA of LineType if input value is positive. - ! If input value is negative, it is considered to be desired damping ratio (zeta) - ! from which the line's BA can be calculated based on the segment natural frequency. - IF (LineProp%BA < 0) THEN - ! - we assume desired damping coefficient is zeta = -LineProp%BA - ! - highest axial vibration mode of a segment is wn = sqrt(k/m) = 2N/UnstrLen*sqrt(EA/w) - Line%BA = -LineProp%BA * Line%UnstrLen / Line%N * SQRT(LineProp%EA * LineProp%w) - ! print *, 'Based on zeta, BA set to ', Line%BA - - ! print *, 'Negative BA input detected, treating as -zeta. For zeta = ', -LineProp%BA, ', setting BA to ', Line%BA - - ELSE - Line%BA = LineProp%BA - ! temp = Line%N * Line%BA / Line%UnstrLen * SQRT(1.0/(LineProp%EA * LineProp%w)) - ! print *, 'BA set as input to ', Line%BA, '. Corresponding zeta is ', temp - END IF - - !temp = 2*Line%N / Line%UnstrLen * sqrt( LineProp%EA / LineProp%w) / TwoPi - !print *, 'Segment natural frequency is ', temp, ' Hz' - + ! need to add cleanup sub <<< END SUBROUTINE SetupLine - !====================================================================== + !-------------------------------------------------------------- + - !=============================================================================================== - SUBROUTINE InitializeLine (Line, LineProp, rhoW, ErrStat, ErrMsg) + !----------------------------------------------------------------------------------------======= + SUBROUTINE Line_Initialize (Line, LineProp, rhoW, ErrStat, ErrMsg) ! calculate initial profile of the line using quasi-static model TYPE(MD_Line), INTENT(INOUT) :: Line ! the single line object of interest TYPE(MD_LineProp), INTENT(INOUT) :: LineProp ! the single line property set for the line of interest - REAL(ReKi), INTENT(IN) :: rhoW + REAL(DbKi), INTENT(IN) :: rhoW INTEGER, INTENT( INOUT ) :: ErrStat ! returns a non-zero value when an error occurs CHARACTER(*), INTENT( INOUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None @@ -1550,7 +3140,7 @@ SUBROUTINE InitializeLine (Line, LineProp, rhoW, ErrStat, ErrMsg) ! coordinate system of the current line to the inertial frame coordinate ! system: - DO J = 0,Line%N ! Loop through all nodes per line where the line position and tension can be output + DO J = 0,N ! Loop through all nodes per line where the line position and tension can be output Line%r(1,J) = Line%r(1,0) + LNodesX(J+1)*COSPhi Line%r(2,J) = Line%r(2,0) + LNodesX(J+1)*SINPhi Line%r(3,J) = Line%r(3,0) + LNodesZ(J+1) @@ -1559,13 +3149,21 @@ SUBROUTINE InitializeLine (Line, LineProp, rhoW, ErrStat, ErrMsg) ELSE ! if there is a problem with the catenary approach, just stretch the nodes linearly between fairlead and anchor - CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'InitializeLine') + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Line_Initialize') - DO J = 0,Line%N ! Loop through all nodes per line where the line position and tension can be output +! print *, "Node positions: " + + DO J = 0,N ! Loop through all nodes per line where the line position and tension can be output Line%r(1,J) = Line%r(1,0) + (Line%r(1,N) - Line%r(1,0))*REAL(J, DbKi)/REAL(N, DbKi) Line%r(2,J) = Line%r(2,0) + (Line%r(2,N) - Line%r(2,0))*REAL(J, DbKi)/REAL(N, DbKi) Line%r(3,J) = Line%r(3,0) + (Line%r(3,N) - Line%r(3,0))*REAL(J, DbKi)/REAL(N, DbKi) + +! print*, Line%r(:,J) ENDDO + +! print*,"FYI line end A and B node coords are" +! print*, Line%r(:,0) +! print*, Line%r(:,N) ENDIF @@ -1577,7 +3175,7 @@ SUBROUTINE InitializeLine (Line, LineProp, rhoW, ErrStat, ErrMsg) CONTAINS - !======================================================================= + !----------------------------------------------------------------------- SUBROUTINE CleanUp() ! deallocate temporary arrays @@ -1586,10 +3184,10 @@ SUBROUTINE CleanUp() IF (ALLOCATED(LNodesZ)) DEALLOCATE(LNodesZ) END SUBROUTINE CleanUp - !======================================================================= + !----------------------------------------------------------------------- - !======================================================================= + !----------------------------------------------------------------------- SUBROUTINE Catenary ( XF_In, ZF_In, L_In , EA_In, & W_In , CB_In, Tol_In, N , & s_In , X_In , Z_In , ErrStat, ErrMsg ) @@ -1610,6 +3208,7 @@ SUBROUTINE Catenary ( XF_In, ZF_In, L_In , EA_In, & ! double precision, so all of the input/output arguments are ! converteds to/from double precision from/to default precision. + ! >>>> TO DO: streamline this function, if it's still to be used at all <<<< ! USE Precision @@ -1997,192 +3596,3088 @@ SUBROUTINE Catenary ( XF_In, ZF_In, L_In , EA_In, & ! at each node (again, these depend on the configuration of the mooring ! line): - IF ( ( CB < 0.0_DbKi ) .OR. ( W < 0.0_DbKi ) .OR. ( VFMinWL > 0.0_DbKi ) ) THEN ! .TRUE. when no portion of the line rests on the seabed + IF ( ( CB < 0.0_DbKi ) .OR. ( W < 0.0_DbKi ) .OR. ( VFMinWL > 0.0_DbKi ) ) THEN ! .TRUE. when no portion of the line rests on the seabed + + ! Anchor tensions: + + HA = HF + VA = VFMinWL + + + ! Line position and tension at each node: + + DO I = 1,N ! Loop through all nodes where the line position and tension are to be computed + + IF ( ( s(I) < 0.0_DbKi ) .OR. ( s(I) > L ) ) THEN + ErrStat = ErrID_Warn + ErrMsg = ' All line nodes must be located between the anchor ' & + //'and fairlead (inclusive) in routine Catenary().' + RETURN + END IF + + Ws = W *s(I) ! Initialize + VFMinWLs = VFMinWL + Ws ! some commonly + VFMinWLsOvrHF = VFMinWLs/HF ! used terms + sOvrEA = s(I) /EA ! that depend + SQRT1VFMinWLsOvrHF2 = SQRT( 1.0_DbKi + VFMinWLsOvrHF*VFMinWLsOvrHF ) ! on s(I) + + X (I) = ( LOG( VFMinWLsOvrHF + SQRT1VFMinWLsOvrHF2 ) & + - LOG( VFMinWLOvrHF + SQRT1VFMinWLOvrHF2 ) )*HFOvrW & + + sOvrEA* HF + Z (I) = ( SQRT1VFMinWLsOvrHF2 & + - SQRT1VFMinWLOvrHF2 )*HFOvrW & + + sOvrEA*( VFMinWL + 0.5_DbKi*Ws ) + Te(I) = SQRT( HF*HF + VFMinWLs*VFMinWLs ) + + ENDDO ! I - All nodes where the line position and tension are to be computed + + + ELSEIF ( -CB*VFMinWL < HF ) THEN ! .TRUE. when a portion of the line rests on the seabed and the anchor tension is nonzero + + ! Anchor tensions: + + HA = HF + CB*VFMinWL + VA = 0.0_DbKi + + + ! Line position and tension at each node: + + DO I = 1,N ! Loop through all nodes where the line position and tension are to be computed + + IF ( ( s(I) < 0.0_DbKi ) .OR. ( s(I) > L ) ) THEN + ErrStat = ErrID_Warn + ErrMsg = ' All line nodes must be located between the anchor ' & + //'and fairlead (inclusive) in routine Catenary().' + RETURN + END IF + + Ws = W *s(I) ! Initialize + VFMinWLs = VFMinWL + Ws ! some commonly + VFMinWLsOvrHF = VFMinWLs/HF ! used terms + sOvrEA = s(I) /EA ! that depend + SQRT1VFMinWLsOvrHF2 = SQRT( 1.0_DbKi + VFMinWLsOvrHF*VFMinWLsOvrHF ) ! on s(I) + + IF ( s(I) <= LMinVFOvrW ) THEN ! .TRUE. if this node rests on the seabed and the tension is nonzero + + X (I) = s(I) & + + sOvrEA*( HF + CB*VFMinWL + 0.5_DbKi*Ws*CB ) + Z (I) = 0.0_DbKi + Te(I) = HF + CB*VFMinWLs + + ELSE ! LMinVFOvrW < s <= L ! This node must be above the seabed + + X (I) = LOG( VFMinWLsOvrHF + SQRT1VFMinWLsOvrHF2 ) *HFOvrW & + + sOvrEA* HF + LMinVFOvrW - 0.5_DbKi*CB*VFMinWL*VFMinWL/WEA + Z (I) = ( - 1.0_DbKi + SQRT1VFMinWLsOvrHF2 )*HFOvrW & + + sOvrEA*( VFMinWL + 0.5_DbKi*Ws ) + 0.5_DbKi* VFMinWL*VFMinWL/WEA + Te(I) = SQRT( HF*HF + VFMinWLs*VFMinWLs ) + + ENDIF + + ENDDO ! I - All nodes where the line position and tension are to be computed + + + ELSE ! 0.0_DbKi < HF <= -CB*VFMinWL ! A portion of the line must rest on the seabed and the anchor tension is zero + + ! Anchor tensions: + + HA = 0.0_DbKi + VA = 0.0_DbKi + + + ! Line position and tension at each node: + + DO I = 1,N ! Loop through all nodes where the line position and tension are to be computed + + IF ( ( s(I) < 0.0_DbKi ) .OR. ( s(I) > L ) ) THEN + ErrStat = ErrID_Warn + ErrMsg = ' All line nodes must be located between the anchor ' & + //'and fairlead (inclusive) in routine Catenary().' + RETURN + END IF + + Ws = W *s(I) ! Initialize + VFMinWLs = VFMinWL + Ws ! some commonly + VFMinWLsOvrHF = VFMinWLs/HF ! used terms + sOvrEA = s(I) /EA ! that depend + SQRT1VFMinWLsOvrHF2 = SQRT( 1.0_DbKi + VFMinWLsOvrHF*VFMinWLsOvrHF ) ! on s(I) + + IF ( s(I) <= LMinVFOvrW - HFOvrW/CB ) THEN ! .TRUE. if this node rests on the seabed and the tension is zero + + X (I) = s(I) + Z (I) = 0.0_DbKi + Te(I) = 0.0_DbKi + + ELSEIF ( s(I) <= LMinVFOvrW ) THEN ! .TRUE. if this node rests on the seabed and the tension is nonzero + + X (I) = s(I) - ( LMinVFOvrW - 0.5_DbKi*HFOvrW/CB )*HF/EA & + + sOvrEA*( HF + CB*VFMinWL + 0.5_DbKi*Ws*CB ) + 0.5_DbKi*CB*VFMinWL*VFMinWL/WEA + Z (I) = 0.0_DbKi + Te(I) = HF + CB*VFMinWLs + + ELSE ! LMinVFOvrW < s <= L ! This node must be above the seabed + + X (I) = LOG( VFMinWLsOvrHF + SQRT1VFMinWLsOvrHF2 ) *HFOvrW & + + sOvrEA* HF + LMinVFOvrW - ( LMinVFOvrW - 0.5_DbKi*HFOvrW/CB )*HF/EA + Z (I) = ( - 1.0_DbKi + SQRT1VFMinWLsOvrHF2 )*HFOvrW & + + sOvrEA*( VFMinWL + 0.5_DbKi*Ws ) + 0.5_DbKi* VFMinWL*VFMinWL/WEA + Te(I) = SQRT( HF*HF + VFMinWLs*VFMinWLs ) + + ENDIF + + ENDDO ! I - All nodes where the line position and tension are to be computed + + + ENDIF + + + + ! The Newton-Raphson iteration is only accurate in double precision, so + ! convert the output arguments back into the default precision for real + ! numbers: + + !HA_In = REAL( HA , DbKi ) !mth: for this I only care about returning node positions + !HF_In = REAL( HF , DbKi ) + !Te_In(:) = REAL( Te(:), DbKi ) + !VA_In = REAL( VA , DbKi ) + !VF_In = REAL( VF , DbKi ) + X_In (:) = REAL( X (:), DbKi ) + Z_In (:) = REAL( Z (:), DbKi ) + + END SUBROUTINE Catenary + !----------------------------------------------------------------------- + + + END SUBROUTINE Line_Initialize + !-------------------------------------------------------------- + + + !-------------------------------------------------------------- + SUBROUTINE Line_SetState(Line, X, t) + + TYPE(MD_Line), INTENT(INOUT) :: Line ! the current Line object + Real(DbKi), INTENT(IN ) :: X(:) ! state vector section for this line + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + + INTEGER(IntKi) :: i ! index of segments or nodes along line + INTEGER(IntKi) :: J ! index + + + ! store current time + Line%time = t + + ! set interior node positions and velocities based on state vector + DO I=1,Line%N-1 + DO J=1,3 + + Line%r( J,I) = X( 3*Line%N-3 + 3*I-3 + J) ! get positions + Line%rd(J,I) = X( 3*I-3 + J) ! get velocities + + END DO + END DO + + END SUBROUTINE Line_SetState + !-------------------------------------------------------------- + + !-------------------------------------------------------------- + SUBROUTINE Line_GetStateDeriv(Line, Xd, p) !, FairFtot, FairMtot, AnchFtot, AnchMtot) + + TYPE(MD_Line), INTENT(INOUT) :: Line ! the current Line object + Real(DbKi), INTENT(INOUT) :: Xd(:) ! state derivative vector section for this line + TYPE(MD_ParameterType), INTENT(IN ) :: p ! Parameters + + + ! Real(DbKi), INTENT( IN ) :: X(:) ! state vector, provided + ! Real(DbKi), INTENT( INOUT ) :: Xd(:) ! derivative of state vector, returned ! cahnged to INOUT + ! Real(DbKi), INTENT (IN) :: t ! instantaneous time + ! TYPE(MD_Line), INTENT (INOUT) :: Line ! label for the current line, for convenience + ! TYPE(MD_LineProp), INTENT(IN) :: LineProp ! the single line property set for the line of interest + ! Real(DbKi), INTENT(INOUT) :: FairFtot(:) ! total force on Connect top of line is attached to + ! Real(DbKi), INTENT(INOUT) :: FairMtot(:,:) ! total mass of Connect top of line is attached to + ! Real(DbKi), INTENT(INOUT) :: AnchFtot(:) ! total force on Connect bottom of line is attached to + ! Real(DbKi), INTENT(INOUT) :: AnchMtot(:,:) ! total mass of Connect bottom of line is attached to + + + INTEGER(IntKi) :: i ! index of segments or nodes along line + INTEGER(IntKi) :: J ! index + INTEGER(IntKi) :: K ! index + INTEGER(IntKi) :: N ! number of segments in line + Real(DbKi) :: d ! line diameter + Real(DbKi) :: rho ! line material density [kg/m^3] + Real(DbKi) :: Sum1 ! for summing squares + Real(DbKi) :: dummyLength ! + Real(DbKi) :: m_i ! node mass + Real(DbKi) :: v_i ! node submerged volume + Real(DbKi) :: Vi(3) ! relative water velocity at a given node + Real(DbKi) :: Vp(3) ! transverse relative water velocity component at a given node + Real(DbKi) :: Vq(3) ! tangential relative water velocity component at a given node + Real(DbKi) :: SumSqVp ! + Real(DbKi) :: SumSqVq ! + Real(DbKi) :: MagVp ! + Real(DbKi) :: MagVq ! + + + N = Line%N ! for convenience + d = Line%d + rho = Line%rho + + ! note that end node kinematics should have already been set by attached objects + +! ! set end node positions and velocities from connect objects' states +! DO J = 1, 3 +! Line%r( J,N) = m%ConnectList(Line%FairConnect)%r(J) +! Line%r( J,0) = m%ConnectList(Line%AnchConnect)%r(J) +! Line%rd(J,N) = m%ConnectList(Line%FairConnect)%rd(J) +! Line%rd(J,0) = m%ConnectList(Line%AnchConnect)%rd(J) +! END DO + + + + ! calculate instantaneous (stretched) segment lengths and rates << should add catch here for if lstr is ever zero + DO I = 1, N + Sum1 = 0.0_DbKi + DO J = 1, 3 + Sum1 = Sum1 + (Line%r(J,I) - Line%r(J,I-1)) * (Line%r(J,I) - Line%r(J,I-1)) + END DO + Line%lstr(I) = sqrt(Sum1) ! stretched segment length + + Sum1 = 0.0_DbKi + DO J = 1, 3 + Sum1 = Sum1 + (Line%r(J,I) - Line%r(J,I-1))*(Line%rd(J,I) - Line%rd(J,I-1)) + END DO + Line%lstrd(I) = Sum1/Line%lstr(I) ! segment stretched length rate of change + + ! Line%V(I) = Pi/4.0 * d*d*Line%l(I) !volume attributed to segment + END DO + + !calculate unit tangent vectors (q) for each node (including ends) note: I think these are pointing toward 0 rather than N! + CALL UnitVector(Line%r(:,0), Line%r(:,1), Line%q(:,0), dummyLength) ! compute unit vector q + DO I = 1, N-1 + CALL UnitVector(Line%r(:,I-1), Line%r(:,I+1), Line%q(:,I), dummyLength) ! compute unit vector q ... using adjacent two nodes! + END DO + CALL UnitVector(Line%r(:,N-1), Line%r(:,N), Line%q(:,N), dummyLength) ! compute unit vector q + + + ! --------------------------------- apply wave kinematics ------------------------------------ + + IF (p%WaterKin > 0) THEN ! wave kinematics interpolated from global grid in Waves object + DO i=0,N + CALL getWaveKin(p, Line%r(1,i), Line%r(2,i), Line%r(3,i), Line%time, Line%U(:,i), Line%Ud(:,i), Line%zeta(i), Line%PDyn(i)) + END DO + END IF + + + ! --------------- calculate mass (including added mass) matrix for each node ----------------- + DO I = 0, N + IF (I==0) THEN + m_i = Pi/8.0 *d*d*Line%l(1)*rho + v_i = 0.5 *Line%V(1) + ELSE IF (I==N) THEN + m_i = pi/8.0 *d*d*Line%l(N)*rho; + v_i = 0.5*Line%V(N) + ELSE + m_i = pi/8.0 * d*d*rho*(Line%l(I) + Line%l(I+1)) + v_i = 0.5 *(Line%V(I) + Line%V(I+1)) + END IF + + DO J=1,3 + DO K=1,3 + IF (J==K) THEN + Line%M(K,J,I) = m_i + p%rhoW*v_i*( Line%Can*(1 - Line%q(J,I)*Line%q(K,I)) + Line%Cat*Line%q(J,I)*Line%q(K,I) ) + ELSE + Line%M(K,J,I) = p%rhoW*v_i*( Line%Can*(-Line%q(J,I)*Line%q(K,I)) + Line%Cat*Line%q(J,I)*Line%q(K,I) ) + END IF + END DO + END DO + + CALL Inverse3by3(Line%S(:,:,I), Line%M(:,:,I)) ! invert mass matrix + END DO + + + ! ------------------ CALCULATE FORCES ON EACH NODE ---------------------------- + + ! loop through the segments + DO I = 1, N + + ! line tension, inherently including possibility of dynamic length changes in l term + IF (Line%lstr(I)/Line%l(I) > 1.0) THEN + DO J = 1, 3 + Line%T(J,I) = Line%EA *( 1.0/Line%l(I) - 1.0/Line%lstr(I) ) * (Line%r(J,I)-Line%r(J,I-1)) + END DO + ELSE + DO J = 1, 3 + Line%T(J,I) = 0.0_DbKi ! cable can't "push" + END DO + END if + + ! line internal damping force based on line-specific BA value, including possibility of dynamic length changes in l and ld terms + DO J = 1, 3 + !Line%Td(J,I) = Line%BA* ( Line%lstrd(I) / Line%l(I) ) * (Line%r(J,I)-Line%r(J,I-1)) / Line%lstr(I) ! note new form of damping coefficient, BA rather than Cint + Line%Td(J,I) = Line%BA* ( Line%lstrd(I) - Line%lstr(I)*Line%ld(I)/Line%l(I) )/Line%l(I) * (Line%r(J,I)-Line%r(J,I-1)) / Line%lstr(I) + END DO + END DO + + + + ! loop through the nodes + DO I = 0, N + + !submerged weight (including buoyancy) + IF (I==0) THEN + Line%W(3,I) = Pi/8.0*d*d* Line%l(1)*(rho - p%rhoW) *(-p%g) ! assuming g is positive + ELSE IF (i==N) THEN + Line%W(3,I) = pi/8.0*d*d* Line%l(N)*(rho - p%rhoW) *(-p%g) + ELSE + Line%W(3,I) = pi/8.0*d*d* (Line%l(I)*(rho - p%rhoW) + Line%l(I+1)*(rho - p%rhoW) )*(-p%g) ! left in this form for future free surface handling + END IF + + !relative flow velocities + DO J = 1, 3 + Vi(J) = 0.0 - Line%rd(J,I) ! relative flow velocity over node -- this is where wave velicites would be added + END DO + + ! decomponse relative flow into components + SumSqVp = 0.0_DbKi ! start sums of squares at zero + SumSqVq = 0.0_DbKi + DO J = 1, 3 + Vq(J) = DOT_PRODUCT( Vi , Line%q(:,I) ) * Line%q(J,I); ! tangential relative flow component + Vp(J) = Vi(J) - Vq(J) ! transverse relative flow component + SumSqVq = SumSqVq + Vq(J)*Vq(J) + SumSqVp = SumSqVp + Vp(J)*Vp(J) + END DO + MagVp = sqrt(SumSqVp) ! get magnitudes of flow components + MagVq = sqrt(SumSqVq) + + ! transverse and tangenential drag + IF (I==0) THEN + Line%Dp(:,I) = 0.25*p%rhoW*Line%Cdn* d*Line%l(1) * MagVp * Vp + Line%Dq(:,I) = 0.25*p%rhoW*Line%Cdt* Pi*d*Line%l(1) * MagVq * Vq + ELSE IF (I==N) THEN + Line%Dp(:,I) = 0.25*p%rhoW*Line%Cdn* d*Line%l(N) * MagVp * Vp + Line%Dq(:,I) = 0.25*p%rhoW*Line%Cdt* Pi*d*Line%l(N) * MagVq * Vq + ELSE + Line%Dp(:,I) = 0.25*p%rhoW*Line%Cdn* d*(Line%l(I) + Line%l(I+1)) * MagVp * vp + Line%Dq(:,I) = 0.25*p%rhoW*Line%Cdt* Pi*d*(Line%l(I) + Line%l(I+1)) * MagVq * vq + END IF + + ! F-K force from fluid acceleration not implemented yet + + ! bottom contact (stiffness and damping, vertical-only for now) - updated Nov 24 for general case where anchor and fairlead ends may deal with bottom contact forces + + IF (Line%r(3,I) < -p%WtrDpth) THEN + IF (I==0) THEN + Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*( Line%l(I+1) ) + ELSE IF (I==N) THEN + Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) ) + ELSE + Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) + Line%l(I+1) ) + + + + END IF + ELSE + Line%B(3,I) = 0.0_DbKi + END IF + + ! total forces + IF (I==0) THEN + Line%Fnet(:,I) = Line%T(:,1) + Line%Td(:,1) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) + ELSE IF (I==N) THEN + Line%Fnet(:,I) = -Line%T(:,N) - Line%Td(:,N) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) + ELSE + Line%Fnet(:,I) = Line%T(:,I+1) - Line%T(:,I) + Line%Td(:,I+1) - Line%Td(:,I) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) + END IF + + END DO ! I - done looping through nodes + + ! print *, "line ", Line%IdNum, " has N=", N + ! print *, " and rd shape", shape(Line%rd) + ! print *, " and Xd shape", shape(Xd) + + ! loop through internal nodes and update their states <<< should/could convert to matrix operations instead of all these loops + DO I=1, N-1 + DO J=1,3 + + ! calculate RHS constant (premultiplying force vector by inverse of mass matrix ... i.e. rhs = S*Forces) + Sum1 = 0.0_DbKi ! reset temporary accumulator + DO K = 1, 3 + Sum1 = Sum1 + Line%S(K,J,I) * Line%Fnet(K,I) ! matrix-vector multiplication [S i]{Forces i} << double check indices + END DO ! K + + ! print *, "writing Xd indices ", 3*N-3 + 3*I-3 + J, " and " , 3*I-3 + J + ! + ! print*, Line%rd(J,I) + + ! update states + Xd(3*N-3 + 3*I-3 + J) = Line%rd(J,I); ! dxdt = V (velocities) + Xd( 3*I-3 + J) = Sum1 ! dVdt = RHS * A (accelerations) + + END DO ! J + END DO ! I + + + ! check for NaNs + DO J = 1, 6*(N-1) + IF (Is_NaN(REAL(Xd(J),DbKi))) THEN + print *, "NaN detected at time ", Line%time, " in Line ", Line%IdNum, " state derivatives:" + print *, Xd + + + + print *, "m_i p%rhoW v_i Line%Can Line%Cat" + print *, m_i + print *, p%rhoW + print *, v_i + print *, Line%Can + print *, Line%Cat + + print *, "Line%q" + print *, Line%q + + print *, "Line%r" + print *, Line%r + + + print *, "Here is the mass matrix set" + print *, Line%M + + print *, "Here is the inverted mass matrix set" + print *, Line%S + + print *, "Here is the net force set" + print *, Line%Fnet + + + + EXIT + END IF + END DO + + + ! ! add force and mass of end nodes to the Connects they correspond to <<<<<<<<<<<< do this from Connection instead now! + ! DO J = 1,3 + ! FairFtot(J) = FairFtot(J) + Line%F(J,N) + ! AnchFtot(J) = AnchFtot(J) + Line%F(J,0) + ! DO K = 1,3 + ! FairMtot(K,J) = FairMtot(K,J) + Line%M(K,J,N) + ! AnchMtot(K,J) = AnchMtot(K,J) + Line%M(K,J,0) + ! END DO + ! END DO + + END SUBROUTINE Line_GetStateDeriv + !===================================================================== + + + !-------------------------------------------------------------- + SUBROUTINE Line_SetEndKinematics(Line, r_in, rd_in, t, topOfLine) + + TYPE(MD_Line), INTENT(INOUT) :: Line ! the current Line object + Real(DbKi), INTENT(IN ) :: r_in( 3) ! state vector section for this line + Real(DbKi), INTENT(IN ) :: rd_in(3) ! state vector section for this line + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + INTEGER(IntKi), INTENT(IN ) :: topOfLine ! 0 for end A (Node 0), 1 for end B (node N) + + Integer(IntKi) :: I,J + INTEGER(IntKi) :: inode + + IF (topOfLine==1) THEN + inode = Line%N + Line%endTypeB = 0 ! set as ball rather than rigid connection (unless changed later by SetEndOrientation) + ELSE + inode = 0 + Line%endTypeA = 0 ! set as ball rather than rigid connection (unless changed later by SetEndOrientation) + END IF + + !Line%r( :,inode) = r_in + !Line%rd(:,inode) = rd_in + + DO J = 1,3 + Line%r( :,inode) = r_in + Line%rd(:,inode) = rd_in + END DO + + ! print *, "SetEndKinematics of line ", Line%idNum, " top?:", topOfLine + ! print *, r_in + ! print *, Line%r( :,inode), " - confirming, node ", inode + ! print *, rd_in + + Line%time = t + + END SUBROUTINE Line_SetEndKinematics + !-------------------------------------------------------------- + + + ! get force, moment, and mass of line at line end node + !-------------------------------------------------------------- + SUBROUTINE Line_GetEndStuff(Line, Fnet_out, Moment_out, M_out, topOfLine) + + TYPE(MD_Line), INTENT(INOUT) :: Line ! label for the current line, for convenience + REAL(DbKi), INTENT( OUT) :: Fnet_out(3) ! net force on end node + REAL(DbKi), INTENT( OUT) :: Moment_out(3) ! moment on end node (future capability) + REAL(DbKi), INTENT( OUT) :: M_out(3,3) ! mass matrix of end node + INTEGER(IntKi), INTENT(IN ) :: topOfLine ! 0 for end A (Node 0), 1 for end B (node N) + + Integer(IntKi) :: I,J + INTEGER(IntKi) :: inode + + IF (topOfLine==1) THEN ! end B of line + Fnet_out = Line%Fnet(:, Line%N) + Moment_out = Line%endMomentB + M_out = Line%M(:,:, Line%N) + ELSE ! end A of line + Fnet_out = Line%Fnet(:, 0) + Moment_out = Line%endMomentA + M_out = Line%M(:,:, 0) + END IF + + END SUBROUTINE Line_GetEndStuff + !-------------------------------------------------------------- + + + ! set end kinematics of a line that's attached to a Rod, and this includes rotational information + !-------------------------------------------------------------- + SUBROUTINE Line_GetEndSegmentInfo(Line, qEnd, EIout, dlOut, topOfLine) + + TYPE(MD_Line), INTENT(INOUT) :: Line ! label for the current line, for convenience + REAL(DbKi), INTENT( OUT) :: qEnd(3) + REAL(DbKi), INTENT( OUT) :: EIout + REAL(DbKi), INTENT( OUT) :: dlOut + INTEGER(IntKi), INTENT(IN ) :: topOfLine ! 0 for end A (Node 0), 1 for end B (node N) + + EIout = Line%EI; + + if (topOfLine==1) then + CALL UnitVector(Line%r(:,Line%N-1), Line%r(:,Line%N), qEnd, dlOut) ! unit vector of last line segment + else + CALL UnitVector(Line%r(:,0 ), Line%r(:,1 ), qEnd, dlOut) ! unit vector of first line segment + END IF + + END SUBROUTINE Line_GetEndSegmentInfo + !-------------------------------------------------------------- + + + ! set end node unit vector of a line (this is called by an attached to a Rod, only applicable for bending stiffness) + !-------------------------------------------------------------- + SUBROUTINE Line_SetEndOrientation(Line, qin, topOfLine, rodEndB) + + TYPE(MD_Line), INTENT(INOUT) :: Line ! label for the current line, for convenience + REAL(DbKi), INTENT(IN ) :: qin(3) ! the rod's axis unit vector + INTEGER(IntKi), INTENT(IN ) :: topOfLine ! 0 for end A (Node 0), 1 for end B (node N) + INTEGER(IntKi), INTENT(IN ) :: rodEndB ! =0 means the line is attached to Rod end A, =1 means attached to Rod end B (implication for unit vector sign) + + if (topOfLine==1) then + + Line%endTypeB = 1 ! indicate attached to Rod (at every time step, just in case line get detached) + + if (rodEndB==1) then + Line%q(:,Line%N) = -qin ! -----line----->[B<==ROD==A] + else + Line%q(:,Line%N) = qin ! -----line----->[A==ROD==>B] + end if + else + + Line%endTypeA = 1 ! indicate attached to Rod (at every time step, just in case line get detached) ! indicate attached to Rod + + if (rodEndB==1) then + Line%q(:,0 ) = qin ! [A==ROD==>B]-----line-----> + else + Line%q(:,0 ) = -qin ! [B<==ROD==A]-----line-----> + end if + end if + + END SUBROUTINE Line_SetEndOrientation + !-------------------------------------------------------------- + + +!-------------------------------------------------------------- +! Connection-Specific Subroutines +!-------------------------------------------------------------- + + + + + + !-------------------------------------------------------------- + SUBROUTINE Connect_Initialize(Connect, states, LineList) + + Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object + Real(DbKi), INTENT(INOUT) :: states(6) ! state vector section for this Connection + Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + + INTEGER(IntKi) :: l + + + if (Connect%typeNum == 0) then ! error check + + ! pass kinematics to any attached lines so they have initial positions at this initialization stage + DO l=1,Connect%nAttached + print *, "Connect ", Connect%IdNum, " setting end kinematics of line ", Connect%attached(l), " to ", Connect%r + CALL Line_SetEndKinematics(LineList(Connect%attached(l)), Connect%r, Connect%rd, 0.0_DbKi, Connect%Top(l)) + END DO + + + ! assign initial node kinematics to state vector + states(4:6) = Connect%r + states(1:3) = Connect%rd + + + print *, "Initialized Connection ", Connect%IdNum + + else + print *," Error: wrong connection type given to Connect_Initialize for number ", Connect%idNum + end if + + END SUBROUTINE Connect_Initialize + !-------------------------------------------------------------- + + + !-------------------------------------------------------------- + SUBROUTINE Connect_SetKinematics(Connect, r_in, rd_in, a_in, t, LineList) + + Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object + Real(DbKi), INTENT(IN ) :: r_in( 3) ! position + Real(DbKi), INTENT(IN ) :: rd_in(3) ! velocity + Real(DbKi), INTENT(IN ) :: a_in(3) ! acceleration (only used for coupled connects) + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + + INTEGER(IntKi) :: l + + ! store current time + Connect%time = t + + + ! if (Connect%typeNum==0) THEN ! anchor ( <<< to be changed/expanded) ... in MoorDyn F also used for coupled connections + + ! set position and velocity + Connect%r = r_in + Connect%rd = rd_in + Connect%a = a_in + + ! pass latest kinematics to any attached lines + DO l=1,Connect%nAttached + CALL Line_SetEndKinematics(LineList(Connect%attached(l)), Connect%r, Connect%rd, t, Connect%Top(l)) + END DO + + ! else + ! + ! PRINT*,"Error: setKinematics called for wrong Connection type. Connection ", Connect%IdNum, " type ", Connect%typeNum + + ! END IF + + + END SUBROUTINE Connect_SetKinematics + !-------------------------------------------------------------- + + !-------------------------------------------------------------- + SUBROUTINE Connect_SetState(Connect, X, t, LineList) + + Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object + Real(DbKi), INTENT(IN ) :: X(:) ! state vector section for this line + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + + INTEGER(IntKi) :: l ! index of segments or nodes along line + INTEGER(IntKi) :: J ! index + + + ! store current time + Connect%time = t + + ! from state values, get r and rdot values + DO J=1,3 + Connect%r( J) = X(3 + J) ! get positions + Connect%rd(J) = X( J) ! get velocities + END DO + + ! pass latest kinematics to any attached lines + DO l=1,Connect%nAttached + CALL Line_SetEndKinematics(LineList(Connect%attached(l)), Connect%r, Connect%rd, t, Connect%Top(l)) + END DO + + END SUBROUTINE Connect_SetState + !-------------------------------------------------------------- + + !-------------------------------------------------------------- + SUBROUTINE Connect_GetStateDeriv(Connect, Xd, LineList, p) + + Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object + Real(DbKi), INTENT(INOUT) :: Xd(:) ! state derivative vector section for this line + Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables + + !INTEGER(IntKi) :: l ! index of attached lines + INTEGER(IntKi) :: J ! index + INTEGER(IntKi) :: K ! index + Real(DbKi) :: Sum1 ! for adding things + + Real(DbKi) :: S(3,3) ! inverse mass matrix + + + CALL Connect_DoRHS(Connect, LineList, p) + +! // solve for accelerations in [M]{a}={f} using LU decomposition +! double M_tot[9]; // serialize total mass matrix for easy processing +! for (int I=0; I<3; I++) for (int J=0; J<3; J++) M_tot[3*I+J]=M[I][J]; +! double LU[9]; // serialized matrix that will hold LU matrices combined +! Crout(3, M_tot, LU); // perform LU decomposition on mass matrix +! double acc[3]; // acceleration vector to solve for +! solveCrout(3, LU, Fnet, acc); // solve for acceleration vector + + ! solve for accelerations in [M]{a}={f} using LU decomposition +! CALL LUsolve(6, M_out, LU_temp, Fnet_out, y_temp, acc) + + + ! invert node mass matrix + CALL Inverse3by3(S, Connect%M) + + ! accelerations + Connect%a = MATMUL(S, Connect%Fnet) + + ! fill in state derivatives + Xd(4:6) = Connect%rd ! dxdt = V (velocities) + Xd(1:3) = Connect%a ! dVdt = RHS * A (accelerations) + + + ! check for NaNs + DO J = 1, 6 + IF (Is_NaN(REAL(Xd(J),DbKi))) THEN + print *, "NaN detected at time ", Connect%time, " in Connection ",Connect%IdNum, " state derivatives:" + print *, Xd + EXIT + END IF + END DO + + END SUBROUTINE Connect_GetStateDeriv + !-------------------------------------------------------------- + + !-------------------------------------------------------------- + SUBROUTINE Connect_DoRHS(Connect, LineList, p) + + Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object + Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables + + INTEGER(IntKi) :: l ! index of attached lines + INTEGER(IntKi) :: I ! index + INTEGER(IntKi) :: J ! index + INTEGER(IntKi) :: K ! index + + Real(DbKi) :: Fnet_i(3) ! force from an attached line + Real(DbKi) :: Moment_dummy(3) ! dummy vector to hold unused line end moments + Real(DbKi) :: M_i(3,3) ! mass from an attached line + + + ! start with the Connection's own forces including buoyancy and weight, and its own mass + Connect%Fnet(1) = Connect%conFX + Connect%Fnet(2) = Connect%conFY + Connect%Fnet(3) = Connect%conFZ + Connect%conV*p%rhoW*p%g - Connect%conM*p%g + + Connect%M = 0.0_DbKi ! clear (zero) the connect mass matrix + + DO J = 1,3 + Connect%M (J,J) = Connect%conM ! set the diagonals to the self-mass (to start with) + END DO + + + ! print *, "connection number", Connect%IdNum + ! print *, "attached lines: ", Connect%attached + ! print *, "size of line list" , size(m%LineList) + + ! loop through attached lines, adding force and mass contributions + DO l=1,Connect%nAttached + + ! print *, " l", l + ! print *, Connect%attached(l) + ! print *, m%LineList(Connect%attached(l))%Fnet + ! + ! + ! print *, " attached line ID", m%LineList(Connect%attached(l))%IdNum + + CALL Line_GetEndStuff(LineList(Connect%attached(l)), Fnet_i, Moment_dummy, M_i, Connect%Top(l)) + + ! sum quantitites + Connect%Fnet = Connect%Fnet + Fnet_i + Connect%M = Connect%M + M_i + + END DO + + + ! XXXWhen this sub is called, any self weight, buoyancy, or external forcing should have already been + ! added by the calling subroutine. The only thing left is any added mass or drag forces from the connection (e.g. float) + ! itself, which will be added below.XXX + + + ! IF (EqualRealNos(t, 0.0_DbKi)) THEN ! this is old: with current IC gen approach, we skip the first call to the line objects, because they're set AFTER the call to the connects + ! + ! DO J = 1,3 + ! Xd(3+J) = X(J) ! velocities - these are unused in integration + ! Xd(J) = 0.0_DbKi ! accelerations - these are unused in integration + ! END DO + ! ELSE + ! ! from state values, get r and rdot values + ! DO J = 1,3 + ! Connect%r(J) = X(3 + J) ! get positions + ! Connect%rd(J) = X(J) ! get velocities + ! END DO + ! END IF + + + ! add any added mass and drag forces from the Connect body itself + DO J = 1,3 + Connect%Fnet(J) = Connect%Fnet(J) - 0.5 * p%rhoW * Connect%rd(J) * abs(Connect%rd(J)) * Connect%conCdA; ! add drag forces - corrected Nov 24 + Connect%M (J,J) = Connect%M (J,J) + Connect%conV*p%rhoW*Connect%conCa; ! add added mass + + END DO + + END SUBROUTINE Connect_DoRHS + !===================================================================== + + + ! calculate the force including inertial loads on connect that is coupled + !-------------------------------------------------------------- + SUBROUTINE Connect_GetCoupledForce(Connect, Fnet_out, LineList, p) + + Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connect object + Real(DbKi), INTENT( OUT) :: Fnet_out(3) ! force and moment vector about rRef + Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + Real(DbKi) :: F_iner(3) ! inertial force + + IF (Connect%typeNum == -1) then + ! calculate forces and masses of connect + CALL Connect_DoRHS(Connect, LineList, p) + + ! add inertial loads as appropriate + F_iner = -MATMUL(Connect%M, Connect%a) ! inertial loads + Fnet_out = Connect%Fnet + F_iner ! add inertial loads + + ELSE + print *, "Connect_GetCoupledForce called for wrong (uncoupled) connect type!" + END IF + + END SUBROUTINE Connect_GetCoupledForce + + + ! calculate the force and mass contributions of the connect on the parent body (only for type 3 connects?) + !-------------------------------------------------------------- + SUBROUTINE Connect_GetNetForceAndMass(Connect, rRef, Fnet_out, M_out, LineList, p) + + Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connect object + Real(DbKi), INTENT(IN ) :: rRef(3) ! global coordinates of reference point (i.e. the parent body) + Real(DbKi), INTENT( OUT) :: Fnet_out(6) ! force and moment vector about rRef + Real(DbKi), INTENT( OUT) :: M_out(6,6) ! mass and inertia matrix about rRef + Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + Real(DbKi) :: rRel( 3) ! position of connection relative to the body reference point (global orientation frame) + + + CALL Connect_DoRHS(Connect, LineList, p) + + rRel = Connect%r - rRef ! vector from body reference point to node + + ! convert net force into 6dof force about body ref point + CALL translateForce3to6DOF(rRel, Connect%Fnet, Fnet_out) + + ! convert mass matrix to 6by6 mass matrix about body ref point + CALL translateMass3to6DOF(rRel, Connect%M, M_out) + + END SUBROUTINE Connect_GetNetForceAndMass + + + + + ! this function handles assigning a line to a connection node + !-------------------------------------------------------------- + SUBROUTINE Connect_AddLine(Connect, lineID, TopOfLine) + + Type(MD_Connect), INTENT (INOUT) :: Connect ! the Connection object + Integer(IntKi), INTENT( IN ) :: lineID + Integer(IntKi), INTENT( IN ) :: TopOfLine + + Print*, "L", lineID, "->C", Connect%IdNum + + IF (Connect%nAttached <10) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. + Connect%nAttached = Connect%nAttached + 1 ! add the line to the number connected + Connect%Attached(Connect%nAttached) = lineID + Connect%Top(Connect%nAttached) = TopOfLine ! attached to line ... 1 = top/fairlead(end B), 0 = bottom/anchor(end A) + ELSE + Print*, "too many lines connected!" + END IF + + END SUBROUTINE Connect_AddLine + + + ! this function handles removing a line from a connection node + !-------------------------------------------------------------- + SUBROUTINE Connect_RemoveLine(Connect, lineID, TopOfLine, rEnd, rdEnd) + + Type(MD_Connect), INTENT (INOUT) :: Connect ! the Connection object + Integer(IntKi), INTENT( IN ) :: lineID + Integer(IntKi), INTENT( OUT) :: TopOfLine + REAL(DbKi), INTENT(INOUT) :: rEnd(3) + REAL(DbKi), INTENT(INOUT) :: rdEnd(3) + + Integer(IntKi) :: l,m,J + + DO l = 1,Connect%nAttached ! look through attached lines + + IF (Connect%Attached(l) == lineID) THEN ! if this is the line's entry in the attachment list + + TopOfLine = Connect%Top(l); ! record which end of the line was attached + + DO m = l,Connect%nAttached-1 + + Connect%Attached(m) = Connect%Attached(m+1) ! move subsequent line links forward one spot in the list to eliminate this line link + Connect%Top( m) = Connect%Top(m+1) + + Connect%nAttached = Connect%nAttached - 1 ! reduce attached line counter by 1 + + ! also pass back the kinematics at the end + DO J = 1,3 + rEnd( J) = Connect%r( J) + rdEnd(J) = Connect%rd(J) + END DO + + print*, "Detached line ", lineID, " from Connection ", Connect%IdNum + + EXIT + END DO + + IF (l == Connect%nAttached) THEN ! detect if line not found + print *, "Error: failed to find line to remove during removeLineFromConnect call to connection ", Connect%IdNum, ". Line ", lineID + END IF + + END IF + + END DO + + END SUBROUTINE Connect_RemoveLine + + + + + + + + +!-------------------------------------------------------------- +! Rod-Specific Subroutines +!-------------------------------------------------------------- + + + + !----------------------------------------------------------------------- + SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) + ! calculate initial profile of the line using quasi-static model + + TYPE(MD_Rod), INTENT(INOUT) :: Rod ! the single rod object of interest + TYPE(MD_RodProp), INTENT(INOUT) :: RodProp ! the single rod property set for the line of interest + REAL(DbKi), INTENT(IN) :: endCoords(6) + REAL(DbKi), INTENT(IN) :: rhoW + INTEGER, INTENT( INOUT ) :: ErrStat ! returns a non-zero value when an error occurs + CHARACTER(*), INTENT( INOUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None + + INTEGER(4) :: J ! Generic index + INTEGER(4) :: K ! Generic index + INTEGER(IntKi) :: N + + N = Rod%N ! number of segments in this line (for code readability) + + ! -------------- save some section properties to the line object itself ----------------- + + Rod%d = RodProp%d + Rod%rho = RodProp%w/(Pi/4.0 * Rod%d * Rod%d) + + Rod%Can = RodProp%Can + Rod%Cat = RodProp%Cat + Rod%Cdn = RodProp%Cdn + Rod%Cdt = RodProp%Cdt + + + ! allocate node positions and velocities (NOTE: these arrays start at ZERO) + ALLOCATE ( Rod%r(3, 0:N), Rod%rd(3, 0:N), STAT = ErrStat ) ! <<<<<< add error checks here + IF ( ErrStat /= ErrID_None ) print *, "Alloc error 1!!!!!!!!!!!!!" + + ! allocate segment scalar quantities + ALLOCATE ( Rod%l(N), Rod%V(N), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) print *, "Alloc error 2!!!!!!!!!!!!!" + + ! allocate water related vectors + ALLOCATE ( Rod%U(3, 0:N), Rod%Ud(3, 0:N), Rod%zeta(0:N), Rod%PDyn(0:N), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) print *, "Alloc error 3!!!!!!!!!!!!!" + ! set to zero initially (important of wave kinematics are not being used) + Rod%U = 0.0_DbKi + Rod%Ud = 0.0_DbKi + Rod%zeta = 0.0_DbKi + Rod%PDyn = 0.0_DbKi + + ! allocate node force vectors + ALLOCATE ( Rod%W(3, 0:N), Rod%Bo(3, 0:N), Rod%Dp(3, 0:N), Rod%Dq(3, 0:N), Rod%Ap(3, 0:N), & + Rod%Aq(3, 0:N), Rod%Pd(3, 0:N), Rod%B(3, 0:N), Rod%Fnet(3, 0:N), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) print *, "Alloc error 4!!!!!!!!!!!!!" + + ! allocate mass and inverse mass matrices for each node (including ends) + ALLOCATE ( Rod%M(3, 3, 0:N), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) print *, "Alloc error 5!!!!!!!!!!!!!" + + + + ! ------------------------- set some geometric properties and the starting kinematics ------------------------- + + CALL UnitVector(endCoords(1:3), endCoords(4:6), Rod%q, Rod%UnstrLen) ! get Rod axis direction vector and Rod length + + ! set Rod positions if applicable + if (Rod%typeNum==0) then ! for an independent rod, set the position right off the bat + + Rod%r6(1:3) = endCoords(1:3) ! (end A coordinates) + Rod%v6(1:3) = 0.0_DbKi ! (end A velocity, unrotated axes) + + Rod%r6(4:6) = Rod%q ! (Rod direction unit vector) + Rod%v6(4:6) = 0.0_DbKi ! (rotational velocities about unrotated axes) + + + else if (abs(Rod%typeNum)==1) then ! for a pinned rod, just set the orientation (position will be set later by parent object) + + Rod%r6(4:6) = Rod%q ! (Rod direction unit vector) + Rod%v6(4:6) = 0.0_DbKi ! (rotational velocities about unrotated axes) + + end if + ! otherwise (for a fixed rod) the positions will be set by the parent body or via coupling + + + + ! save mass for future calculations >>>> should calculate I_l and I_r here in future <<<< + Rod%mass = Rod%UnstrLen*RodProp%w + + + ! assign values for l and V + DO J=1,N + Rod%l(J) = Rod%UnstrLen/REAL(N, DbKi) + Rod%V(J) = Rod%l(J)*0.25*Pi*RodProp%d*RodProp%d + END DO + + + + ! set gravity and bottom contact forces to zero initially (because the horizontal components should remain at zero) + DO J = 0,N + DO K = 1,3 + Rod%W(K,J) = 0.0_DbKi + Rod%B(K,J) = 0.0_DbKi + END DO + END DO + + ! >>> why are the above assignments making l V W and B appear as "undefined pointer/array"s??? <<< + + print *, "Set up Rod ",Rod%IdNum, ", type ", Rod%typeNum + + ! need to add cleanup sub <<< + + END SUBROUTINE Rod_Setup + !-------------------------------------------------------------- + + + + + ! Make output file for Rod and set end kinematics of any attached lines. + ! For free Rods, fill in the initial states into the state vector. + ! Notes: r6 and v6 must already be set. + ! ground- or body-pinned rods have already had setKinematics called to set first 3 elements of r6, v6. + !-------------------------------------------------------------- + SUBROUTINE Rod_Initialize(Rod, states, LineList) + + TYPE(MD_Rod), INTENT(INOUT) :: Rod ! the rod object + Real(DbKi), INTENT(INOUT) :: states(:) ! state vector section for this line + TYPE(MD_Line), INTENT(INOUT) :: LineList(:) ! passing along all mooring objects + + INTEGER(IntKi) :: l ! index of segments or nodes along line + REAL(DbKi) :: rRef(3) ! reference position of mesh node + REAL(DbKi) :: OrMat(3,3) ! DCM for body orientation based on r6_in + + print *, "initializing Rod ", Rod%idNum + + ! the r6 and v6 vectors should have already been set + ! r and rd of ends have already been set by setup function or by parent object <<<<< right? <<<<< + + + ! Pass kinematics to any attached lines (this is just like what a Connection does, except for both ends) + ! so that they have the correct initial positions at this initialization stage. + + if (Rod%typeNum >- 2) CALL Rod_SetDependentKin(Rod, 0.0_DbKi, LineList) ! don't call this for type -2 coupled Rods as it's already been called + + + ! assign the resulting kinematics to its part of the state vector (only matters if it's an independent Rod) + + if (Rod%typeNum == 0) then ! free Rod type + + states(1:6) = 0.0_DbKi ! zero velocities for initialization + states(7:9) = Rod%r(:,0) ! end A position + states(10:12) = Rod%q ! rod direction unit vector + + else if (abs(Rod%typeNum) ==1 ) then ! pinned rod type (coupled or attached to something previously via setPinKin) + + states(1:3) = 0.0_DbKi ! zero velocities for initialization + states(4:6) = Rod%q ! rod direction unit vector + + end if + + ! note: this may also be called by a coupled rod (type = -1) in which case states will be empty + + print *, " states: ", states + print *, " r0: ", Rod%r(:,0) + print *, " q : ", Rod%q + + + END SUBROUTINE Rod_Initialize + !-------------------------------------------------------------- + + + + + ! set kinematics for Rods ONLY if they are attached to a body (including a coupled body) or coupled (otherwise shouldn't be called) + !-------------------------------------------------------------- + SUBROUTINE Rod_SetKinematics(Rod, r6_in, v6_in, a6_in, t, LineList) + + Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object + Real(DbKi), INTENT(IN ) :: r6_in(6) ! 6-DOF position + Real(DbKi), INTENT(IN ) :: v6_in(6) ! 6-DOF velocity + Real(DbKi), INTENT(IN ) :: a6_in(6) ! 6-DOF acceleration (only used for coupled rods) + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + + INTEGER(IntKi) :: l + + Rod%time = t ! store current time + + + if (abs(Rod%typeNum) == 2) then ! rod rigidly coupled to a body, or ground, or coupling point + Rod%r6 = r6_in + Rod%v6 = v6_in + Rod%a6 = a6_in + + call ScaleVector(Rod%r6(4:6), 1.0_DbKi, Rod%r6(4:6)); ! enforce direction vector to be a unit vector + + ! since this rod has no states and all DOFs have been set, pass its kinematics to dependent Lines + CALL Rod_SetDependentKin(Rod, t, LineList) + + else if (abs(Rod%typeNum) == 1) then ! rod end A pinned to a body, or ground, or coupling point + + ! set Rod *end A only* kinematics based on BCs (linear model for now) + Rod%r6(1:3) = r6_in(1:3) + Rod%v6(1:3) = v6_in(1:3) + Rod%a6(1:3) = a6_in(1:3) + + + ! Rod is pinned so only end A is specified, rotations are left alone and will be + ! handled, along with passing kinematics to dependent lines, by separate call to setState + + else + print *, "Error: Rod_SetKinematics called for a free Rod." ! <<< + end if + + + ! update Rod direction unit vector (simply equal to last three entries of r6, presumably these were set elsewhere for pinned Rods) + Rod%q = Rod%r6(4:6) + + + END SUBROUTINE Rod_SetKinematics + !-------------------------------------------------------------- + + ! pass the latest states to the rod if it has any DOFs/states (then update rod end kinematics including attached lines) + !-------------------------------------------------------------- + SUBROUTINE Rod_SetState(Rod, X, t, LineList) + + Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object + Real(DbKi), INTENT(IN ) :: X(:) ! state vector section for this line + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + + INTEGER(IntKi) :: J ! index + + + ! for a free Rod, there are 12 states: + ! [ x, y, z velocity of end A, then rate of change of u/v/w coordinates of unit vector pointing toward end B, + ! then x, y, z coordinate of end A, u/v/w coordinates of unit vector pointing toward end B] + + ! for a pinned Rod, there are 6 states (rotational only): + ! [ rate of change of u/v/w coordinates of unit vector pointing toward end B, + ! then u/v/w coordinates of unit vector pointing toward end B] + + + ! store current time + Rod%time = t + + + ! copy over state values for potential use during derivative calculations + if (Rod%typeNum == 0) then ! free Rod type + + ! CALL ScaleVector(X(10:12), 1.0, X(10:12)) ! enforce direction vector to be a unit vector <<<< can't do this with FAST frameowrk, could be a problem!! + + ! TODO: add "controller" adjusting state derivatives of X(10:12) to artificially force X(10:12) to remain a unit vector <<<<<<<<<<< + + + Rod%r6(1:3) = X(7:9) ! (end A coordinates) + Rod%v6(1:3) = X(1:3) ! (end A velocity, unrotated axes) + CALL ScaleVector(X(10:12), 1.0_DbKi, Rod%r6(4:6)) !Rod%r6(4:6) = X(10:12) ! (Rod direction unit vector) + Rod%v6(4:6) = X(4:6) ! (rotational velocities about unrotated axes) + + + CALL Rod_SetDependentKin(Rod, t, LineList) + + else if (abs(Rod%typeNum) == 1) then ! pinned rod type (coupled or attached to something)t previously via setPinKin) + + !CALL ScaleVector(X(4:6), 1.0, X(4:6)) ! enforce direction vector to be a unit vector + + + CALL ScaleVector(X(4:6), 1.0_DbKi, Rod%r6(4:6)) !Rod%r6(3+J) = X(3+J) ! (Rod direction unit vector) + Rod%v6(4:6) = X(1:3) ! (rotational velocities about unrotated axes) + + + CALL Rod_SetDependentKin(Rod, t, LineList) + + else + print *, "Error: Rod::setState called for a non-free rod type" ! <<< + end if + + ! update Rod direction unit vector (simply equal to last three entries of r6) + Rod%q = Rod%r6(4:6) + + END SUBROUTINE Rod_SetState + !-------------------------------------------------------------- + + + ! Set the Rod end kinematics then set the kinematics of dependent objects (any attached lines). + ! This also determines the orientation of zero-length rods. + !-------------------------------------------------------------- + SUBROUTINE Rod_SetDependentKin(Rod, t, LineList) + + Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + + INTEGER(IntKi) :: l ! index of segments or nodes along line + INTEGER(IntKi) :: J ! index + INTEGER(IntKi) :: N ! number of segments + + REAL(DbKi) :: qEnd(3) ! unit vector of attached line end segment, following same direction convention as Rod's q vector + REAL(DbKi) :: EIend ! bending stiffness of attached line end segment + REAL(DbKi) :: dlEnd ! stretched length of attached line end segment + REAL(DbKi) :: qMomentSum(3) ! summation of qEnd*EI/dl_stretched (with correct sign) for each attached line + + + ! in future pass accelerations here too? <<<< + + N = Rod%N + + ! from state values, set positions of end nodes + ! end A + Rod%r(:,0) = Rod%r6(1:3) ! positions + Rod%rd(:,0) = Rod%v6(1:3) ! velocities + + !print *, Rod%r6(1:3) + !print *, Rod%r(:,0) + + if (Rod%N > 0) then ! set end B nodes only if the rod isn't zero length + call transformKinematicsAtoB(Rod%r6(1:3), Rod%r6(4:6), Rod%UnstrLen, Rod%v6, Rod%r(:,N), Rod%rd(:,N)) ! end B + end if + + ! pass end node kinematics to any attached lines (this is just like what a Connection does, except for both ends) + DO l=1,Rod%nAttachedA + CALL Line_SetEndKinematics(LineList(Rod%attachedA(l)), Rod%r(:,0), Rod%rd(:,0), t, Rod%TopA(l)) + END DO + DO l=1,Rod%nAttachedB + CALL Line_SetEndKinematics(LineList(Rod%attachedB(l)), Rod%r(:,N), Rod%rd(:,N), t, Rod%TopB(l)) + END DO + + + ! if this is a zero-length Rod, get bending moment-related information from attached lines and compute Rod's equilibrium orientation + if (N==0) then + + DO l=1,Rod%nAttachedA + + CALL Line_GetEndSegmentInfo(LineList(Rod%attachedA(l)), qEnd, EIend, dlEnd, Rod%TopA(l)) + + qMomentSum = qMomentSum + qEnd*EIend/dlEnd ! add each component to the summation vector + + END DO + + DO l=1,Rod%nAttachedB + + CALL Line_GetEndSegmentInfo(LineList(Rod%attachedB(l)), qEnd, EIend, dlEnd, Rod%TopB(l)) + + qMomentSum = qMomentSum + qEnd*EIend/dlEnd ! add each component to the summation vector + + END DO + + ! solve for line unit vector that balances all moments (unit vector of summation of qEnd*EI/dl_stretched over each line) + CALL ScaleVector(qMomentSum, 1.0_DbKi, Rod%q) + END IF + + ! pass Rod orientation to any attached lines (this is just like what a Connection does, except for both ends) + DO l=1,Rod%nAttachedA + CALL Line_SetEndOrientation(LineList(Rod%attachedA(l)), Rod%q, Rod%TopA(l), 0) + END DO + DO l=1,Rod%nAttachedB + CALL Line_SetEndOrientation(LineList(Rod%attachedB(l)), Rod%q, Rod%TopB(l), 1) + END DO + + END SUBROUTINE Rod_SetDependentKin + !-------------------------------------------------------------- + + !-------------------------------------------------------------- + SUBROUTINE Rod_GetStateDeriv(Rod, Xd, LineList, p) + + Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object + Real(DbKi), INTENT(INOUT) :: Xd(:) ! state derivative vector section for this line + Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables + + INTEGER(IntKi) :: J ! index + + Real(DbKi) :: Fnet (6) ! net force and moment about reference point + Real(DbKi) :: M_out (6,6) ! mass matrix about reference point + + Real(DbKi) :: acc(6) ! 6DOF acceleration vector about reference point + + Real(DbKi) :: Mcpl(3) ! moment in response to end A acceleration due to inertial coupling + + Real(DbKi) :: y_temp (6) ! temporary vector for LU decomposition + Real(DbKi) :: LU_temp(6,6) ! temporary matrix for LU decomposition + + + CALL Rod_GetNetForceAndMass(Rod, Rod%r(:,0), Fnet, M_out, LineList, p) + + + + ! TODO: add "controller" adjusting state derivatives of X(10:12) to artificially force X(10:12) to remain a unit vector <<<<<<<<<<< + + ! fill in state derivatives + IF (Rod%typeNum == 0) THEN ! free Rod type, 12 states + + ! solve for accelerations in [M]{a}={f} using LU decomposition + CALL LUsolve(6, M_out, LU_temp, Fnet, y_temp, acc) + + Xd(7:9) = Rod%v6(1:3) !Xd[6 + I] = v6[ I]; ! dxdt = V (velocities) + Xd(1:6) = acc !Xd[ I] = acc[ I]; ! dVdt = a (accelerations) + !Xd[3 + I] = acc[3+I]; ! rotational accelerations + + ! rate of change of unit vector components!! CHECK! <<<<< + Xd(10) = - Rod%v6(6)*Rod%r6(5) + Rod%v6(5)*Rod%r6(6) ! i.e. u_dot_x = -omega_z*u_y + omega_y*u_z + Xd(11) = Rod%v6(6)*Rod%r6(4) - Rod%v6(4)*Rod%r6(6) ! i.e. u_dot_y = omega_z*u_x - omega_x*u_z + Xd(12) = -Rod%v6(5)*Rod%r6(4) + Rod%v6(4)*Rod%r6(5) ! i.e. u_dot_z = -omega_y*u_x - omega_x*u_y + + ! store accelerations in case they're useful as output + Rod%a6 = acc + + ELSE ! pinned rod, 6 states (rotational only) + + ! account for moment in response to end A acceleration due to inertial coupling (off-diagonal sub-matrix terms) + !Fnet(4:6) = Fnet(4:6) - MATMUL(M_out(4:6,1:3), Rod%a6(1:3)) ! <<>> do we need to ensure zero moment is passed if it's pinned? <<< + !if (abs(Rod%typeNum)==1) then + ! Fnet_out(4:6) = 0.0_DbKi + !end if + + + END SUBROUTINE Rod_GetNetForceAndMass + !-------------------------------------------------------------- + + + ! calculate the forces on the rod, including from attached lines + !-------------------------------------------------------------- + SUBROUTINE Rod_DoRHS(Rod, LineList, p) + + Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rodion object + Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables + + INTEGER(IntKi) :: l ! index of attached lines + INTEGER(IntKi) :: I,J,K ! index + + + INTEGER(IntKi) :: N ! number of rod elements for convenience + + Real(DbKi) :: phi, beta, sinPhi, cosPhi, tanPhi, sinBeta, cosBeta ! various orientation things + Real(DbKi) :: k_hat(3) ! unit vector (redundant, not used) <<<< + Real(DbKi) :: Ftemp ! temporary force component + Real(DbKi) :: Mtemp ! temporary moment component + + Real(DbKi) :: m_i, v_i ! + Real(DbKi) :: zeta ! wave elevation above/below a given node + Real(DbKi) :: h0 ! distance along rod centerline from end A to the waterplane + Real(DbKi) :: deltaL ! submerged length of a given segment + Real(DbKi) :: Lsum ! cumulative length along rod axis from bottom + Real(DbKi) :: dL ! length attributed to node + Real(DbKi) :: VOF ! fraction of volume associated with node that is submerged + + Real(DbKi) :: Vi(3) ! relative flow velocity over a node + Real(DbKi) :: SumSqVp, SumSqVq, MagVp, MagVq + Real(DbKi) :: Vp(3), Vq(3) ! transverse and axial components of water velocity at a given node + Real(DbKi) :: ap(3), aq(3) ! transverse and axial components of water acceleration at a given node + Real(DbKi) :: Fnet_i(3) ! force from an attached line + Real(DbKi) :: Mnet_i(3) ! moment from an attached line + Real(DbKi) :: Mass_i(3,3) ! mass from an attached line + + ! used in lumped 6DOF calculations: + Real(DbKi) :: rRel( 3) ! relative position of each node i from rRef + Real(DbKi) :: OrMat(3,3) ! rotation matrix to rotate global z to rod's axis + Real(DbKi) :: F6_i(6) ! a node's contribution to the total force vector + Real(DbKi) :: M6_i(6,6) ! a node's contribution to the total mass matrix + Real(DbKi) :: I_l ! axial inertia of rod + Real(DbKi) :: I_r ! radial inertia of rod about CG + Real(DbKi) :: Imat_l(3,3) ! inertia about CG aligned with Rod axis + Real(DbKi) :: Imat(3,3) ! inertia about CG in global frame + Real(DbKi) :: h_c ! location of CG along axis + Real(DbKi) :: r_c(3) ! 3d location of CG relative to node A + Real(DbKi) :: Fcentripetal(3) ! centripetal force + Real(DbKi) :: Mcentripetal(3) ! centripetal moment + + + N = Rod%N + + ! ------------------------------ zero some things -------------------------- + + Rod%Mext = 0.0_DbKi ! zero the external moment sum + + Lsum = 0.0_DbKi + + + ! ---------------------------- initial rod and node calculations ------------------------ + + ! calculate some orientation information for the Rod as a whole + call GetOrientationAngles(Rod%r( :,0), Rod%r( :,N), phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat) + + ! save to internal roll and pitch variables for use in output <<< should check these, make Euler angles isntead of independent <<< + Rod%roll = -180.0/Pi * phi*sinBeta + Rod%pitch = 180.0/Pi * phi*cosBeta + + ! set interior node positions and velocities (stretch the nodes between the endpoints linearly) (skipped for zero-length Rods) + DO i=1,N-1 + Rod%r( :,i) = Rod%r( :,0) + (Rod%r( :,N) - Rod%r( :,0)) * (REAL(i)/REAL(N)) + Rod%rd(:,i) = Rod%rd(:,0) + (Rod%rd(:,N) - Rod%rd(:,0)) * (REAL(i)/REAL(N)) + + + Rod%V(i) = 0.25*pi * Rod%d*Rod%d * Rod%l(i) ! volume attributed to segment + END DO + + + ! --------------------------------- apply wave kinematics ------------------------------------ + + IF (p%WaterKin == 1) THEN ! wave kinematics interpolated from global grid in Waves object + DO i=0,N + CALL getWaveKin(p, Rod%r(1,i), Rod%r(2,i), Rod%r(3,i), Rod%time, Rod%U(:,i), Rod%Ud(:,i), Rod%zeta(i), Rod%PDyn(i)) + !F(i) = 1.0 ! set VOF value to one for now (everything submerged - eventually this should be element-based!!!) <<<< + ! <<<< currently F is not being used and instead a VOF variable is used within the node loop + END DO + END IF + + + ! ! wave kinematics not implemented yet <<< + ! ap = 0.0_DbKi + ! aq = 0.0_DbKi + ! ! set U and Ud herem as well as pDyn and zeta... + ! Rod%U = 0.0_DbKi + ! Rod%Ud = 0.0_DbKi + ! pDyn = 0.0_DbKi + ! zeta = 0.0_DbKi + + ! >>> remember to check for violated conditions, if there are any... <<< + + zeta = Rod%zeta(N)! just use the wave elevation computed at the location of the top node for now + + if ((Rod%r(3,0) < zeta) .and. (Rod%r(3,N) > zeta)) then ! check if it's crossing the water plane (should also add some limits to avoid near-horizontals at some point) + h0 = (zeta - Rod%r(3,0))/Rod%q(3) ! distance along rod centerline from end A to the waterplane + else if (Rod%r(3,0) < zeta) then + h0 = 2.0*Rod%UnstrLen ! fully submerged case + else + h0 = 0.0_DbKi ! fully unsubmerged case (ever applicable?) + end if + + + ! -------------------------- loop through all the nodes ----------------------------------- + DO I = 0, N + + + ! ------------------ calculate added mass matrix for each node ------------------------- + + ! get mass and volume considering adjacent segment lengths + IF (I==0) THEN + dL = 0.5*Rod%l(1) + m_i = 0.25*Pi * Rod%d*Rod%d * dL *Rod%rho ! (will be zero for zero-length Rods) + v_i = 0.5 *Rod%V(1) + ELSE IF (I==N) THEN + dL = 0.5*Rod%l(N) + m_i = 0.25*pi * Rod%d*Rod%d * dL *Rod%rho + v_i = 0.5*Rod%V(N) + ELSE + dL = 0.5*(Rod%l(I) + Rod%l(I+1)) + m_i = 0.25*pi * Rod%d*Rod%d * dL *Rod%rho + v_i = 0.5 *(Rod%V(I) + Rod%V(I+1)) + END IF + + ! get scalar for submerged portion + IF (Lsum + dL < h0) THEN ! if fully submerged + VOF = 1.0_DbKi + ELSE IF (Lsum < h0) THEN ! if partially below waterline + VOF = (h0 - Lsum)/dL + ELSE ! must be out of water + VOF = 0.0_DbKi + END IF + + Lsum = Lsum + dL ! add length attributed to this node to the total + + ! build mass and added mass matrix + DO J=1,3 + DO K=1,3 + IF (J==K) THEN + Rod%M(K,J,I) = m_i + VOF*p%rhoW*v_i*( Rod%Can*(1 - Rod%q(J)*Rod%q(K)) + Rod%Cat*Rod%q(J)*Rod%q(K) ) + ELSE + Rod%M(K,J,I) = VOF*p%rhoW*v_i*( Rod%Can*(-Rod%q(J)*Rod%q(K)) + Rod%Cat*Rod%q(J)*Rod%q(K) ) + END IF + END DO + END DO + + ! <<<< what about accounting for offset of half segment from node location for end nodes? <<<< + + +! CALL Inverse3by3(Rod%S(:,:,I), Rod%M(:,:,I)) ! invert mass matrix + + + ! ------------------ CALCULATE FORCES ON EACH NODE ---------------------------- + + if (N > 0) then ! the following force calculations are only nonzero for finite-length rods (skipping for zero-length Rods) + + ! >>> no nodal axial elasticity loads calculated since it's assumed rigid, but should I calculate tension/compression due to other loads? <<< + + ! weight (now only the dry weight) + Rod%W(:,I) = (/ 0.0_DbKi, 0.0_DbKi, -m_i * p%g /) ! assuming g is positive + + ! buoyance (now calculated based on outside pressure, for submerged portion only) + ! radial buoyancy force from sides + Ftemp = -VOF * 0.25*Pi*dL*Rod%d*Rod%d * p%rhoW*p%g * sinPhi + Rod%Bo(:,I) = (/ Ftemp*cosBeta*cosPhi, Ftemp*sinBeta*cosPhi, -Ftemp*sinPhi /) + + !relative flow velocities + DO J = 1, 3 + Vi(J) = Rod%U(J,I) - Rod%rd(J,I) ! relative flow velocity over node -- this is where wave velicites would be added + END DO + + ! decomponse relative flow into components + SumSqVp = 0.0_DbKi ! start sums of squares at zero + SumSqVq = 0.0_DbKi + DO J = 1, 3 + Vq(J) = DOT_PRODUCT( Vi , Rod%q ) * Rod%q(J); ! tangential relative flow component + Vp(J) = Vi(J) - Vq(J) ! transverse relative flow component + SumSqVq = SumSqVq + Vq(J)*Vq(J) + SumSqVp = SumSqVp + Vp(J)*Vp(J) + END DO + MagVp = sqrt(SumSqVp) ! get magnitudes of flow components + MagVq = sqrt(SumSqVq) + + ! transverse and tangenential drag + Rod%Dp(:,I) = VOF * 0.5*p%rhoW*Rod%Cdn* Rod%d* dL * MagVp * Vp + Rod%Dq(:,I) = 0.0_DbKi ! 0.25*p%rhoW*Rod%Cdt* Pi*Rod%d* dL * MagVq * Vq <<< should these axial side loads be included? + + ! fluid acceleration components for current node + aq = DOT_PRODUCT(Rod%Ud(:,I), Rod%q) * Rod%q ! tangential component of fluid acceleration + ap = Rod%Ud(:,I) - aq ! normal component of fluid acceleration + ! transverse Froude-Krylov force + Rod%Ap(:,I) = VOF * p%rhoW*(1.0+Rod%Can)*0.5* v_i * ap ! <<< are these equations right?? + ! axial Froude-Krylov force + Rod%Aq(:,I) = 0.0_DbKi ! p%rhoW*(1.0+Rod%Cat)*0.5* v_i * aq ! <<< are these equations right?? + + ! dynamic pressure + Rod%Pd(:,I) = 0.0_DbKi ! assuming zero for sides + + ! bottom contact (stiffness and damping, vertical-only for now) - updated Nov 24 for general case where anchor and fairlead ends may deal with bottom contact forces + IF (Rod%r(3,I) < -p%WtrDpth) THEN + IF (I==0) THEN + Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*( Rod%l(I+1) ) + ELSE IF (I==N) THEN + Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) ) + ELSE + Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) + Rod%l(I+1) ) + END IF + ELSE + Rod%B(3,I) = 0.0_DbKi + END IF + + ELSE ! zero-length (N=0) Rod case + + ! >>>>>>>>>>>>>> still need to check handling of zero length rods <<<<<<<<<<<<<<<<<<< + + ! for zero-length rods, make sure various forces are zero + Rod%W = 0.0_DbKi + Rod%Bo = 0.0_DbKi + Rod%Dp = 0.0_DbKi + Rod%Dq= 0.0_DbKi + Rod%B = 0.0_DbKi + Rod%Pd = 0.0_DbKi + + END IF + + + ! ------ now add forces, moments, and added mass from Rod end effects (these can exist even if N==0) ------- + + ! end A + IF ((I==0) .and. (h0 > 0.0_ReKi)) THEN ! if this is end A and it is submerged + + ! >>> eventually should consider a VOF approach for the ends hTilt = 0.5*Rod%d/cosPhi <<< + + ! buoyancy force + Ftemp = -VOF * 0.25*Pi*Rod%d*Rod%d * p%rhoW*p%g*Rod%r(3,I) + Rod%Bo(:,I) = Rod%Bo(:,I) + (/ Ftemp*cosBeta*sinPhi, Ftemp*sinBeta*sinPhi, Ftemp*cosPhi /) + + ! buoyancy moment + Mtemp = -VOF * 1.0/64.0*Pi*Rod%d**4 * p%rhoW*p%g * sinPhi + Rod%Mext = Rod%Mext + (/ Mtemp*sinBeta, -Mtemp*cosBeta, 0.0_DbKi /) + + ! axial drag + Rod%Dq(:,I) = Rod%Dq(:,I) + VOF * 0.25* Pi*Rod%d*Rod%d * p%rhoW*Rod%CdEnd * MagVq * Vq + + ! >>> what about rotational drag?? <<< eqn will be Pi* Rod%d**4/16.0 omega_rel?^2... *0.5 * Cd... + + ! Froud-Krylov force + Rod%Aq(:,I) = Rod%Aq(:,I) + VOF * p%rhoW*(1.0+Rod%CaEnd)*0.5* (2.0/3.0*Pi*Rod%d**3 /8) * aq + + ! dynamic pressure force + Rod%Pd(:,I) = Rod%Pd(:,I) + VOF * 0.25* Pi*Rod%d*Rod%d * Rod%PDyn(I) * Rod%q + + ! added mass + DO J=1,3 + DO K=1,3 + IF (J==K) THEN + Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* (Pi*Rod%d**3/6.0) * Rod%CaEnd*Rod%q(J)*Rod%q(K) + ELSE + Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* (Pi*Rod%d**3/6.0) * Rod%CaEnd*Rod%q(J)*Rod%q(K) + END IF + END DO + END DO + + END IF + + IF ((I==N) .and. (h0 > Rod%UnstrLen)) THEN ! if this end B and it is submerged (note, if N=0, both this and previous if statement are true) + + ! buoyancy force + Ftemp = VOF * 0.25*Pi*Rod%d*Rod%d * p%rhoW*p%g*Rod%r(3,I) + Rod%Bo(:,I) = Rod%Bo(:,I) + (/ Ftemp*cosBeta*sinPhi, Ftemp*sinBeta*sinPhi, Ftemp*cosPhi /) + + ! buoyancy moment + Mtemp = VOF * 1.0/64.0*Pi*Rod%d**4 * p%rhoW*p%g * sinPhi + Rod%Mext = Rod%Mext + (/ Mtemp*sinBeta, -Mtemp*cosBeta, 0.0_DbKi /) + + ! axial drag + Rod%Dq(:,I) = Rod%Dq(:,I) + VOF * 0.25* Pi*Rod%d*Rod%d * p%rhoW*Rod%CdEnd * MagVq * Vq + + ! Froud-Krylov force + Rod%Aq(:,I) = Rod%Aq(:,I) + VOF * p%rhoW*(1.0+Rod%CaEnd)*0.5* (2.0/3.0*Pi*Rod%d**3 /8) * aq + + ! dynamic pressure force + Rod%Pd(:,I) = Rod%Pd(:,I) - VOF * 0.25* Pi*Rod%d*Rod%d * Rod%PDyn(I) * Rod%q + + ! added mass + DO J=1,3 + DO K=1,3 + IF (J==K) THEN + Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* (Pi*Rod%d**3/6.0) * Rod%CaEnd*Rod%q(J)*Rod%q(K) + ELSE + Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* (Pi*Rod%d**3/6.0) * Rod%CaEnd*Rod%q(J)*Rod%q(K) + END IF + END DO + END DO + + END IF + + + + ! ---------------------------- total forces for this node ----------------------------- + + Rod%Fnet(:,I) = Rod%W(:,I) + Rod%Bo(:,I) + Rod%Dp(:,I) + Rod%Dq(:,I) & + + Rod%Ap(:,I) + Rod%Aq(:,I) + Rod%Pd(:,I) + Rod%B(:,I) + + + END DO ! I - done looping through nodes + + + ! ----- add waterplane moment of inertia moment if applicable ----- + IF ((Rod%r(3,0) < zeta) .and. (Rod%r(3,N) > zeta)) then ! check if it's crossing the water plane + Mtemp = 1.0/16.0 *Pi*Rod%d**4 * p%rhoW*p%g * sinPhi * (1.0 + 0.5* tanPhi**2) + Rod%Mext = Rod%Mext + (/ Mtemp*sinBeta, -Mtemp*cosBeta, 0.0_DbKi /) + END IF + + ! ---------------- now add in forces on end nodes from attached lines ------------------ + + ! loop through lines attached to end A + DO l=1,Rod%nAttachedA + + CALL Line_GetEndStuff(LineList(Rod%attachedA(l)), Fnet_i, Mnet_i, Mass_i, Rod%TopA(l)) + + ! sum quantitites + Rod%Fnet(:,0)= Rod%Fnet(:,0) + Fnet_i ! total force + Rod%Mext = Rod%Mext + Mnet_i ! externally applied moment + Rod%M(:,:,0) = Rod%M(:,:,0) + Mass_i ! mass at end node + + END DO + + ! loop through lines attached to end B + DO l=1,Rod%nAttachedB + + CALL Line_GetEndStuff(LineList(Rod%attachedB(l)), Fnet_i, Mnet_i, Mass_i, Rod%TopB(l)) + + ! sum quantitites + Rod%Fnet(:,N)= Rod%Fnet(:,N) + Fnet_i ! total force + Rod%Mext = Rod%Mext + Mnet_i ! externally applied moment + Rod%M(:,:,N) = Rod%M(:,:,N) + Mass_i ! mass at end node + + END DO + + ! ---------------- now lump everything in 6DOF about end A ----------------------------- + + ! question: do I really want to neglect the rotational inertia/drag/etc across the length of each segment? + + ! make sure 6DOF quantiaties are zeroed before adding them up + Rod%F6net = 0.0_DbKi + Rod%M6net = 0.0_DbKi + + ! now go through each node's contributions, put them about end A, and sum them + DO i = 0,Rod%N + + rRel = Rod%r(:,i) - Rod%r(:,0) ! vector from reference point to node + + ! convert segment net force into 6dof force about body ref point (if the Rod itself, end A) + CALL translateForce3to6DOF(rRel, Rod%Fnet(:,i), F6_i) + + ! convert segment mass matrix to 6by6 mass matrix about body ref point (if the Rod itself, end A) + CALL translateMass3to6DOF(rRel, Rod%M(:,:,i), M6_i) + + ! sum contributions + Rod%F6net = Rod%F6net + F6_i + Rod%M6net = Rod%M6net + M6_i + + END DO + + ! ------------- Calculate some items for the Rod as a whole here ----------------- + + ! >>> could some of these be precalculated just once? <<< + + ! add inertia terms for the Rod assuming it is uniform density (radial terms add to existing matrix which contains parallel-axis-theorem components only) + I_l = 0.125*Rod%mass * Rod%d*Rod%d ! axial moment of inertia + I_r = Rod%mass/12 * (0.75*Rod%d*Rod%d + (Rod%UnstrLen/Rod%N)**2 ) * Rod%N ! summed radial moment of inertia for each segment individually + + !h_c = [value from registry] + + Imat_l(1,1) = I_r ! inertia about CG in local orientations (as if Rod is vertical) + Imat_l(2,2) = I_r + Imat_l(3,3) = I_l + + OrMat = CalcOrientation(phi, beta, 0.0_DbKi) ! get rotation matrix to put things in global rather than rod-axis orientations + + Imat = RotateM3(Imat_l, OrMat) ! rotate to give inertia matrix about CG in global frame + + ! these supplementary inertias can then be added the matrix (these are the terms ASIDE from the parallel axis terms) + Rod%M6net(4:6,4:6) = Rod%M6net(4:6,4:6) + Imat + + + ! now add centripetal and gyroscopic forces/moments, and that should be everything + h_c = 0.5*Rod%UnstrLen ! distance to center of mass + r_c = h_c*Rod%q ! vector to center of mass + + ! note that Rod%v6(4:6) is the rotational velocity vector, omega + Fcentripetal = 0.0_DbKi !<<R", Rod%IdNum , "b" + + IF (Rod%nAttachedB <10) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. + Rod%nAttachedB = Rod%nAttachedB + 1 ! add the line to the number connected + Rod%AttachedB(Rod%nAttachedB) = lineID + Rod%TopB(Rod%nAttachedB) = TopOfLine ! attached to line ... 1 = top/fairlead(end B), 0 = bottom/anchor(end A) + ELSE + Print*, "too many lines connected!" + END IF + + else ! attaching to end A + + Print*, "L", lineID, "->R", Rod%IdNum , "a" + + IF (Rod%nAttachedA <10) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. + Rod%nAttachedA = Rod%nAttachedA + 1 ! add the line to the number connected + Rod%AttachedA(Rod%nAttachedA) = lineID + Rod%TopA(Rod%nAttachedA) = TopOfLine ! attached to line ... 1 = top/fairlead(end B), 0 = bottom/anchor(end A) + ELSE + Print*, "too many lines connected!" + END IF + + end if + + END SUBROUTINE Rod_AddLine + + + ! this function handles removing a line from a connection node + SUBROUTINE Rod_RemoveLine(Rod, lineID, TopOfLine, endB, rEnd, rdEnd) + + Type(MD_Rod), INTENT (INOUT) :: Rod ! the Connection object + + Integer(IntKi), INTENT( IN ) :: lineID + Integer(IntKi), INTENT( OUT) :: TopOfLine + Integer(IntKi), INTENT( IN ) :: endB ! end B if 1, end A if 0 + REAL(DbKi), INTENT(INOUT) :: rEnd(3) + REAL(DbKi), INTENT(INOUT) :: rdEnd(3) + + Integer(IntKi) :: l,m,J + + if (endB==1) then ! attaching to end B + + DO l = 1,Rod%nAttachedB ! look through attached lines + + IF (Rod%AttachedB(l) == lineID) THEN ! if this is the line's entry in the attachment list + + TopOfLine = Rod%TopB(l); ! record which end of the line was attached + + DO m = l,Rod%nAttachedB-1 + + Rod%AttachedB(m) = Rod%AttachedB(m+1) ! move subsequent line links forward one spot in the list to eliminate this line link + Rod%TopB( m) = Rod%TopB(m+1) + + Rod%nAttachedB = Rod%nAttachedB - 1 ! reduce attached line counter by 1 + + ! also pass back the kinematics at the end + DO J = 1,3 + rEnd( J) = Rod%r( J,Rod%N) + rdEnd(J) = Rod%rd(J,Rod%N) + END DO + + print*, "Detached line ", lineID, " from Rod ", Rod%IdNum, " end B" + + EXIT + END DO + + IF (l == Rod%nAttachedB) THEN ! detect if line not found + print *, "Error: failed to find line to remove during RemoveLine call to Rod ", Rod%IdNum, ". Line ", lineID + END IF + END IF + END DO + + else ! attaching to end A + + DO l = 1,Rod%nAttachedA ! look through attached lines + + IF (Rod%AttachedA(l) == lineID) THEN ! if this is the line's entry in the attachment list + + TopOfLine = Rod%TopA(l); ! record which end of the line was attached + + DO m = l,Rod%nAttachedA-1 + + Rod%AttachedA(m) = Rod%AttachedA(m+1) ! move subsequent line links forward one spot in the list to eliminate this line link + Rod%TopA( m) = Rod%TopA(m+1) + + Rod%nAttachedA = Rod%nAttachedA - 1 ! reduce attached line counter by 1 + + ! also pass back the kinematics at the end + DO J = 1,3 + rEnd( J) = Rod%r( J,0) + rdEnd(J) = Rod%rd(J,0) + END DO + + print*, "Detached line ", lineID, " from Rod ", Rod%IdNum, " end A" + + EXIT + END DO + + IF (l == Rod%nAttachedA) THEN ! detect if line not found + print *, "Error: failed to find line to remove during RemoveLine call to Rod ", Rod%IdNum, ". Line ", lineID + END IF + END IF + END DO + + end if + + END SUBROUTINE Rod_RemoveLine + + + + + + + + +!-------------------------------------------------------------- +! Body-Specific Subroutines +!-------------------------------------------------------------- + + +! ! used to initialize bodies that aren't free i.e. don't have states +! !-------------------------------------------------------------- +! SUBROUTINE Body_InitializeUnfree(Body, r6_in, mesh, mesh_index, m) +! +! Type(MD_Body), INTENT(INOUT) :: Body ! the Body object +! Real(DbKi), INTENT(IN ) :: r6_in(6) ! state vector section for this line +! TYPE(MeshType), INTENT(INOUT) :: mesh ! +! Integer(IntKi), INTENT(IN ) :: mesh_index ! index of the node in the mesh for the current object being initialized +! TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects +! +! INTEGER(IntKi) :: l ! index of segments or nodes along line +! REAL(DbKi) :: rRef(3) ! reference position of mesh node +! REAL(DbKi) :: OrMat(3,3) ! DCM for body orientation based on r6_in +! REAL(DbKi) :: dummyStates(12) +! +! +! rRef = 0.0_DbKi ! <<< maybe this should be the offsets of the local platform origins from the global origins in future? And that's what's specificed by the Body input coordinates? +! +! CALL MeshPositionNode(mesh, mesh+index, rRef,ErrStat2,ErrMsg2)! "assign the coordinates (u%PtFairleadDisplacement%Position) of each node in the global coordinate space" +! +! CALL CheckError( ErrStat2, ErrMsg2 ) +! IF (ErrStat >= AbortErrLev) RETURN +! +! ! Apply offsets due to initial platform rotations and translations (fixed Jun 19, 2015) +! CALL SmllRotTrans('body rotation matrix', r6_in(4),r6_in(5),r6_in(6), OrMat, '', ErrStat2, ErrMsg2) +! mesh%TranslationDisp(1, mesh_index) = r6_in(1) + OrMat(1,1)*rRef(1) + OrMat(2,1)*rRef(2) + OrMat(3,1)*rRef(3) - rRef(1) +! mesh%TranslationDisp(2, mesh_index) = r6_in(2) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2) +! mesh%TranslationDisp(3, mesh_index) = r6_in(3) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) +! +! ! what about node point orientation ??? +! +! ! If any Rod is fixed to the body (not pinned), initialize it now because otherwise it won't be initialized +! DO l=1, Body%nAttachedR +! if (m%RodList(Body%attachedR(l))%typeNum == 2) CALL Rod_Initialize(m%RodList(Body%attachedR(l)), dummyStates, m%LineList) +! END DO +! +! ! Note: Connections don't need any initialization +! +! END SUBROUTINE Body_InitializeUnfree +! !-------------------------------------------------------------- + + + ! used to initialize bodies that are free + !-------------------------------------------------------------- + SUBROUTINE Body_Initialize(Body, states, m) + + Type(MD_Body), INTENT(INOUT) :: Body ! the Body object + Real(DbKi), INTENT(INOUT) :: states(6) ! state vector section for this Body + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + + INTEGER(IntKi) :: l ! index of segments or nodes along line + REAL(DbKi) :: dummyStates(12) ! dummy vector to mimic states when initializing a rigidly attached rod + + + ! assign initial body kinematics to state vector + states(7:12) = Body%r6 + states(1:6 ) = Body%v6 + + + ! set positions of any dependent connections and rods now (before they are initialized) + CALL Body_SetDependentKin(Body, 0.0_DbKi, m) + + ! If any Rod is fixed to the body (not pinned), initialize it now because otherwise it won't be initialized + DO l=1, Body%nAttachedR + if (m%RodList(Body%attachedR(l))%typeNum == 2) CALL Rod_Initialize(m%RodList(Body%attachedR(l)), dummyStates, m%LineList) + END DO + + ! Note: Connections don't need any initialization + + END SUBROUTINE Body_Initialize + !-------------------------------------------------------------- + + ! used to initialize bodies that are coupled or fixed + !-------------------------------------------------------------- + SUBROUTINE Body_InitializeUnfree(Body, m) + + Type(MD_Body), INTENT(INOUT) :: Body ! the Body object + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + + INTEGER(IntKi) :: l ! index of segments or nodes along line + REAL(DbKi) :: dummyStates(12) ! dummy vector to mimic states when initializing a rigidly attached rod + + + ! set positions of any dependent connections and rods now (before they are initialized) + CALL Body_SetDependentKin(Body, 0.0_DbKi, m) + + ! If any Rod is fixed to the body (not pinned), initialize it now because otherwise it won't be initialized + DO l=1, Body%nAttachedR + if (m%RodList(Body%attachedR(l))%typeNum == 2) CALL Rod_Initialize(m%RodList(Body%attachedR(l)), dummyStates, m%LineList) + END DO + + ! Note: Connections don't need any initialization + + END SUBROUTINE Body_InitializeUnfree + !-------------------------------------------------------------- + + + + !-------------------------------------------------------------- + SUBROUTINE Body_SetState(Body, X, t, m) + + Type(MD_Body), INTENT(INOUT) :: Body ! the Body object + Real(DbKi), INTENT(IN ) :: X(:) ! state vector section for this line + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + + INTEGER(IntKi) :: l ! index of segments or nodes along line + INTEGER(IntKi) :: J ! index + + ! store current time + Body%time = t + + + + Body%r6 = X(7:12) ! get positions + Body%v6 = X(1:6) ! get velocities + + + ! set positions of any dependent connections and rods + CALL Body_SetDependentKin(Body, t, m) + + END SUBROUTINE Body_SetState + !-------------------------------------------------------------- + + + ! set kinematics for Bodies if they are coupled (or ground) + !-------------------------------------------------------------- + SUBROUTINE Body_SetKinematics(Body, r_in, v_in, a_in, t, m) + + Type(MD_Body), INTENT(INOUT) :: Body ! the Body object + Real(DbKi), INTENT(IN ) :: r_in(6) ! 6-DOF position + Real(DbKi), INTENT(IN ) :: v_in(6) ! 6-DOF velocity + Real(DbKi), INTENT(IN ) :: a_in(6) ! 6-DOF acceleration (only used for coupled rods) + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects (for simplicity, since Bodies deal with Rods and Connections) + + + INTEGER(IntKi) :: l + + ! store current time + Body%time = t + + ! if (abs(Body%typeNum) == 2) then ! body coupled in 6 DOF, or ground + Body%r6 = r_in + Body%v6 = v_in + Body%a6 = a_in + + ! since this body has no states and all DOFs have been set, pass its kinematics to dependent attachments + CALL Body_SetDependentKin(Body, t, m) + + ! else if (abs(Body%typeNum) == 1) then ! body pinned at reference point + ! + ! ! set Body *end A only* kinematics based on BCs (linear model for now) + ! Body%r6(1:3) = r_in(1:3) + ! Body%v6(1:3) = v_in(1:3) + ! + ! ! Body is pinned so only ref point posiiton is specified, rotations are left alone and will be + ! ! handled, along with passing kinematics to attached objects, by separate call to setState + ! + ! else + ! print *, "Error: Body_SetKinematics called for a free Body." ! <<< + ! end if + + END SUBROUTINE Body_SetKinematics + !-------------------------------------------------------------- + + + ! set the states (positions and velocities) of any connects or rods that are part of this body + ! also computes the orientation matrix (never skip this sub!) + !-------------------------------------------------------------- + SUBROUTINE Body_SetDependentKin(Body, t, m) + + Type(MD_Body), INTENT(INOUT) :: Body ! the Bodyion object + REAL(DbKi), INTENT(IN ) :: t + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects (for simplicity, since Bodies deal with Rods and Connections) + + INTEGER(IntKi) :: l ! index of attached objects + + Real(DbKi) :: rConnect(3) + Real(DbKi) :: rdConnect(3) + Real(DbKi) :: rRod(6) + Real(DbKi) :: vRod(6) + Real(DbKi) :: aRod(6) + + + + ! calculate orientation matrix based on latest angles + !CALL SmllRotTrans('', Body%r6(4), Body%r6(5), Body%r6(6), Body%TransMat, '', ErrStat2, ErrMsg2) + Body%OrMat = EulerConstruct( Body%r6(4:6) ) ! full Euler angle approach <<<< need to check order + +if ((t >= 1.0) .and. (t < 1.001)) then + print *, "orientation matrix OrMat of Body:" + print *, Body%OrMat + print *, "based on position vector:" + print *, Body%r6 +end if + + ! set kinematics of any dependent connections + do l = 1,Body%nAttachedC + + CALL transformKinematics(Body%rConnectRel(:,l), Body%r6, Body%OrMat, Body%v6, rConnect, rdConnect) !<<< should double check this function + + ! >>> need to add acceleration terms here too? <<< + + ! pass above to the connection and get it to calculate the forces + CALL Connect_SetKinematics( m%ConnectList(Body%attachedC(l)), rConnect, rdConnect, m%zeros6(1:3), t, m%LineList) + end do + + ! set kinematics of any dependent Rods + do l=1,Body%nAttachedR + + ! calculate displaced coordinates/orientation and velocities of each rod <<<<<<<<<<<<< + ! do 3d details of Rod ref point + CALL TransformKinematicsA( Body%r6RodRel(1:3,l), Body%r6(1:3), Body%OrMat, Body%v6, Body%a6, rRod(1:3), vRod(1:3), aRod(1:3)) ! set first three entires (end A translation) of rRod and rdRod + ! does the above function need to take in all 6 elements of r6RodRel?? + + ! do rotational stuff + rRod(4:6) = MATMUL(Body%OrMat, Body%r6RodRel(4:6,l)) !<<<<<< correct? <<<<< rotateVector3(r6RodRel[i]+3, OrMat, rRod+3); ! rotate rod relative unit vector by OrMat to get unit vec in reference coords + vRod(4:6) = Body%v6(4:6) ! transformed rotational velocity. <<< is this okay as is? <<<< + aRod(4:6) = Body%a6(4:6) + + ! pass above to the rod and get it to calculate the forces + CALL Rod_SetKinematics(m%RodList(Body%attachedR(l)), rRod, vRod, aRod, t, m%LineList) + end do + + END SUBROUTINE Body_SetDependentKin + !-------------------------------------------------------------- + + ! calculate the aggregate 3/6DOF rigid-body loads of a coupled rod including inertial loads + !-------------------------------------------------------------- + SUBROUTINE Body_GetCoupledForce(Body, Fnet_out, m, p) + + Type(MD_Body), INTENT(INOUT) :: Body ! the Body object + Real(DbKi), INTENT( OUT) :: Fnet_out(6) ! force and moment vector + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + Real(DbKi) :: F6_iner(6) ! inertial reaction force + + ! do calculations of forces and masses on the body + CALL Body_DoRHS(Body, m, p) + + ! add inertial loads as appropriate + if (Body%typeNum == -1) then + + F6_iner = 0.0_DbKi !-MATMUL(Body%M, Body%a6) <<<<<<<< why does including F6_iner cause instability??? + Fnet_out = Body%F6net + F6_iner ! add inertial loads + + else + print *, "ERROR, Body_GetCoupledForce called for wrong (non-coupled) body type!" + end if + + END SUBROUTINE Body_GetCoupledForce + !-------------------------------------------------------------- + + + !-------------------------------------------------------------- + SUBROUTINE Body_GetStateDeriv(Body, Xd, m, p) + + Type(MD_Body), INTENT(INOUT) :: Body ! the Bodyion object + Real(DbKi), INTENT(INOUT) :: Xd(:) ! state derivative vector section for this line + + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + INTEGER(IntKi) :: J ! index + + Real(DbKi) :: acc(6) ! 6DOF acceleration vector + + Real(DbKi) :: y_temp (6) ! temporary vector for LU decomposition + Real(DbKi) :: LU_temp(6,6) ! temporary matrix for LU decomposition + + + CALL Body_DoRHS(Body, m, p) - ! Anchor tensions: + ! solve for accelerations in [M]{a}={f} using LU decomposition + CALL LUsolve(6, Body%M, LU_temp, Body%F6net, y_temp, acc) - HA = HF - VA = VFMinWL + ! fill in state derivatives + Xd(7:12) = Body%v6 ! dxdt = V (velocities) + Xd(1:6) = acc ! dVdt = a (accelerations) + ! store accelerations in case they're useful as output + Body%a6 = acc + + ! check for NaNs (should check all state derivatives, not just first 6) + DO J = 1, 6 + IF (Is_NaN(REAL(Xd(J),DbKi))) THEN + print *, "NaN detected at time ", Body%time, " in Body ",Body%IdNum, " state derivatives:" + print *, Xd + EXIT + END IF + END DO - ! Line position and tension at each node: - DO I = 1,N ! Loop through all nodes where the line position and tension are to be computed + END SUBROUTINE Body_GetStateDeriv + !-------------------------------------------------------------- - IF ( ( s(I) < 0.0_DbKi ) .OR. ( s(I) > L ) ) THEN - ErrStat = ErrID_Warn - ErrMsg = ' All line nodes must be located between the anchor ' & - //'and fairlead (inclusive) in routine Catenary().' - RETURN - END IF + !-------------------------------------------------------------- + SUBROUTINE Body_DoRHS(Body, m, p) - Ws = W *s(I) ! Initialize - VFMinWLs = VFMinWL + Ws ! some commonly - VFMinWLsOvrHF = VFMinWLs/HF ! used terms - sOvrEA = s(I) /EA ! that depend - SQRT1VFMinWLsOvrHF2 = SQRT( 1.0_DbKi + VFMinWLsOvrHF*VFMinWLsOvrHF ) ! on s(I) + Type(MD_Body), INTENT(INOUT) :: Body ! the Bodyion object + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables + + INTEGER(IntKi) :: l ! index of attached lines + INTEGER(IntKi) :: I ! index + INTEGER(IntKi) :: J ! index + INTEGER(IntKi) :: K ! index + + Real(DbKi) :: Fgrav(3) ! body weight force + Real(DbKi) :: body_rCGrotated(3) ! instantaneous vector from body ref point to CG + Real(DbKi) :: U(3) ! water velocity - zero for now + Real(DbKi) :: Ud(3) ! water acceleration- zero for now + Real(DbKi) :: vi(6) ! relative water velocity (last 3 terms are rotatonal and will be set to zero + Real(DbKi) :: F6_i(6) ! net force and moments from an attached object + Real(DbKi) :: M6_i(6,6) ! mass and inertia from an attached object + + ! First, the body's own mass matrix must be adjusted based on its orientation so that + ! we have a mass matrix in the global orientation frame + Body%M = RotateM6(Body%M0, Body%OrMat) + + !gravity on core body + + Fgrav(3) = Body%bodyV * p%rhow * p%g - Body%bodyM * p%g ! weight+buoyancy vector + + body_rCGrotated = MATMUL(Body%OrMat, Body%rCG) ! rotateVector3(body_rCG, OrMat, body_rCGrotated); ! relative vector to body CG in inertial orientation + CALL translateForce3to6DOF(body_rCGrotated, Fgrav, Body%F6net) ! gravity forces and moments about body ref point given CG location + + + ! --------------------------------- apply wave kinematics ------------------------------------ + !env->waves->getU(r6, t, U); ! call generic function to get water velocities <<<<<<<<< all needs updating + + ! for (int J=0; J<3; J++) + ! Ud[J] = 0.0; ! set water accelerations as zero for now + ! ------------------------------------------------------------------------------------------ + + ! viscous drag calculation (on core body) + vi(1:3) = U - Body%v6(1:3) ! relative flow velocity over body ref point + vi(4:6) = - Body%v6(4:6) ! for rotation, this is just the negative of the body's rotation for now (not allowing flow rotation) + + Body%F6net = Body%F6net + 0.5*p%rhoW * vi * abs(vi) * Body%bodyCdA + ! <<< NOTE, for body this should be fixed to account for orientation!! <<< what about drag in rotational DOFs??? <<<<<<<<<<<<<< + + + + ! Get contributions from any dependent connections + do l = 1,Body%nAttachedC + + ! get net force and mass from Connection on body ref point (global orientation) + CALL Connect_GetNetForceAndMass( m%ConnectList(Body%attachedC(l)), Body%r6(1:3), F6_i, M6_i, m%LineList, p) + + ! sum quantitites + Body%F6net = Body%F6net + F6_i + Body%M = Body%M + M6_i + end do + + ! Get contributions from any dependent Rods + do l=1,Body%nAttachedR + + ! get net force and mass from Rod on body ref point (global orientation) + CALL Rod_GetNetForceAndMass(m%RodList(Body%attachedR(l)), Body%r6(1:3), F6_i, M6_i, m%LineList, p) + + ! sum quantitites + Body%F6net = Body%F6net + F6_i + Body%M = Body%M + M6_i + end do + - X (I) = ( LOG( VFMinWLsOvrHF + SQRT1VFMinWLsOvrHF2 ) & - - LOG( VFMinWLOvrHF + SQRT1VFMinWLOvrHF2 ) )*HFOvrW & - + sOvrEA* HF - Z (I) = ( SQRT1VFMinWLsOvrHF2 & - - SQRT1VFMinWLOvrHF2 )*HFOvrW & - + sOvrEA*( VFMinWL + 0.5_DbKi*Ws ) - Te(I) = SQRT( HF*HF + VFMinWLs*VFMinWLs ) + END SUBROUTINE Body_DoRHS + !===================================================================== - ENDDO ! I - All nodes where the line position and tension are to be computed - ELSEIF ( -CB*VFMinWL < HF ) THEN ! .TRUE. when a portion of the line rests on the seabed and the anchor tension is nonzero - ! Anchor tensions: + ! this function handles assigning a connection to a body + !-------------------------------------------------------------- + SUBROUTINE Body_AddConnect(Body, connectID, coords) - HA = HF + CB*VFMinWL - VA = 0.0_DbKi + Type(MD_Body), INTENT(INOUT) :: Body ! the Connection object + Integer(IntKi), INTENT(IN ) :: connectID + REAL(DbKi), INTENT(IN ) :: coords(3) - ! Line position and tension at each node: + Print*, "C", connectID, "->B", Body%IdNum + + IF(Body%nAttachedC < 30) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. + Body%nAttachedC = Body%nAttachedC + 1 ! increment the number connected + Body%AttachedC(Body%nAttachedC) = connectID + Body%rConnectRel(:,Body%nAttachedC) = coords ! store relative position of connect on body + ELSE + Print*, "too many connections attached!" + END IF - DO I = 1,N ! Loop through all nodes where the line position and tension are to be computed + END SUBROUTINE Body_AddConnect - IF ( ( s(I) < 0.0_DbKi ) .OR. ( s(I) > L ) ) THEN - ErrStat = ErrID_Warn - ErrMsg = ' All line nodes must be located between the anchor ' & - //'and fairlead (inclusive) in routine Catenary().' - RETURN - END IF - Ws = W *s(I) ! Initialize - VFMinWLs = VFMinWL + Ws ! some commonly - VFMinWLsOvrHF = VFMinWLs/HF ! used terms - sOvrEA = s(I) /EA ! that depend - SQRT1VFMinWLsOvrHF2 = SQRT( 1.0_DbKi + VFMinWLsOvrHF*VFMinWLsOvrHF ) ! on s(I) + ! this function handles assigning a rod to a body + !-------------------------------------------------------------- + SUBROUTINE Body_AddRod(Body, rodID, coords) - IF ( s(I) <= LMinVFOvrW ) THEN ! .TRUE. if this node rests on the seabed and the tension is nonzero + Type(MD_Body), INTENT(INOUT) :: Body ! the Connection object + Integer(IntKi), INTENT(IN ) :: rodID + REAL(DbKi), INTENT(IN ) :: coords(6) ! positions of rod ends A and B relative to body + + REAL(DbKi) :: tempUnitVec(3) + REAL(DbKi) :: dummyLength - X (I) = s(I) & - + sOvrEA*( HF + CB*VFMinWL + 0.5_DbKi*Ws*CB ) - Z (I) = 0.0_DbKi - Te(I) = HF + CB*VFMinWLs + Print*, "R", rodID, "->B", Body%IdNum + + IF(Body%nAttachedR < 30) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. + Body%nAttachedR = Body%nAttachedR + 1 ! increment the number connected + + ! store rod ID + Body%AttachedR(Body%nAttachedR) = rodID + + ! store Rod end A relative position and unit vector from end A to B + CALL UnitVector(coords(1:3), coords(4:6), tempUnitVec, dummyLength) + Body%r6RodRel(1:3, Body%nAttachedR) = coords(1:3) + Body%r6RodRel(4:6, Body%nAttachedR) = tempUnitVec + + ELSE + Print*, "too many rods attached!" + END IF - ELSE ! LMinVFOvrW < s <= L ! This node must be above the seabed + END SUBROUTINE Body_AddRod - X (I) = LOG( VFMinWLsOvrHF + SQRT1VFMinWLsOvrHF2 ) *HFOvrW & - + sOvrEA* HF + LMinVFOvrW - 0.5_DbKi*CB*VFMinWL*VFMinWL/WEA - Z (I) = ( - 1.0_DbKi + SQRT1VFMinWLsOvrHF2 )*HFOvrW & - + sOvrEA*( VFMinWL + 0.5_DbKi*Ws ) + 0.5_DbKi* VFMinWL*VFMinWL/WEA - Te(I) = SQRT( HF*HF + VFMinWLs*VFMinWLs ) - ENDIF + ! :::::::::::::::::::::::::: below are some wave related functions ::::::::::::::::::::::::::::::: + + + ! master function to get wave/water kinematics at a given point -- called by each object fro grid-based data + SUBROUTINE getWaveKin(p, x, y, z, t, U, Ud, zeta, PDyn) + + ! note, this whole approach assuems that px, py, and pz are in increasing order + + TYPE(MD_ParameterType),INTENT (IN ) :: p ! MoorDyn parameters (contains the wave info for now) + Real(DbKi), INTENT (IN ) :: x + Real(DbKi), INTENT (IN ) :: y + Real(DbKi), INTENT (IN ) :: z + Real(DbKi), INTENT (IN ) :: t + Real(DbKi), INTENT (INOUT) :: U(3) + Real(DbKi), INTENT (INOUT) :: Ud(3) + Real(DbKi), INTENT (INOUT) :: zeta + Real(DbKi), INTENT (INOUT) :: PDyn + + + INTEGER(IntKi) :: ix, iy, iz, it ! indeces for interpolation + INTEGER(IntKi) :: N ! number of rod elements for convenience + Real(DbKi) :: fx, fy, fz, ft ! interpolation fractions + Real(DbKi) :: qt ! used in time step interpolation + + + CALL getInterpNums(p%px, x, ix, fx) + CALL getInterpNums(p%py, y, iy, fy) + CALL getInterpNums(p%pz, z, iz, fz) + + qt = t / real(p%dtWave, DbKi) + it = floor(qt) + 1 ! adjust by 1 for fortran's indexing starting at 1 + ft = qt - it + 1.0; !(t-(it*dtWave))/dtWave ! //remainder(t,dtWave)/dtWave; + + CALL calculate3Dinterpolation(p%zeta, ix, iy, it, fx, fy, ft, zeta) + + CALL calculate4Dinterpolation(p%PDyn, ix, iy, iz, it, fx, fy, fz, ft, PDyn) + + CALL calculate4Dinterpolation(p%ux, ix, iy, iz, it, fx, fy, fz, ft, U(1) ) + CALL calculate4Dinterpolation(p%uy, ix, iy, iz, it, fx, fy, fz, ft, U(2) ) + CALL calculate4Dinterpolation(p%uz, ix, iy, iz, it, fx, fy, fz, ft, U(3) ) + CALL calculate4Dinterpolation(p%ax, ix, iy, iz, it, fx, fy, fz, ft, Ud(1) ) + CALL calculate4Dinterpolation(p%ay, ix, iy, iz, it, fx, fy, fz, ft, Ud(2) ) + CALL calculate4Dinterpolation(p%az, ix, iy, iz, it, fx, fy, fz, ft, Ud(3) ) + + END SUBROUTINE + + + SUBROUTINE getInterpNums(xlist, xin, i, fout) + + Real(DbKi), INTENT (IN ) :: xlist(:) + Real(DbKi), INTENT (IN ) :: xin + Integer(IntKi),INTENT ( OUT) :: i + Real(DbKi), INTENT ( OUT) :: fout + + Integer(IntKi) :: nx + ! Parameters: list of x values, number of x values, x value to be interpolated, fraction to return + ! Returns the lower index to interpolate from. such that y* = y[i] + fout*(y[i+1]-y[i]) + + nx = SIZE(xlist) + + if (xin <= xlist(1)) THEN ! below lowest data point + i = 1_IntKi + fout = 0.0_DbKi + + else if (xin >= xlist(nx)) THEN ! above highest data point + i = nx + fout = 0.0_DbKi + + else ! within the data range + DO i = 1, nx-1 + IF (xlist(i+1) > xin) THEN + fout = (xin - xlist(i) )/( xlist(i+1) - xlist(i) ) + exit + END IF + END DO + END IF + + END SUBROUTINE + + + SUBROUTINE calculate4Dinterpolation(f, ix0, iy0, iz0, it0, fx, fy, fz, ft, c) + + Real(DbKi), INTENT (IN ) :: f(:,:,:,:) ! data array + INTEGER(IntKi), INTENT (IN ) :: ix0, iy0, iz0, it0 ! indeces for interpolation + Real(DbKi), INTENT (IN ) :: fx, fy, fz, ft ! interpolation fractions + Real(DbKi), INTENT ( OUT) :: c ! the output value + + INTEGER(IntKi) :: ix1, iy1, iz1, it1 ! second indices + REAL(DbKi) :: c000, c001, c010, c011, c100, c101, c110, c111 + REAL(DbKi) :: c00, c01, c10, c11, c0, c1 + + ! handle end case conditions + if (fx == 0) then + ix1 = ix0 + else + ix1 = ix0+1 + end if + + if (fy == 0) then + iy1 = iy0 + else + iy1 = iy0+1 + end if + + if (fz == 0) then + iz1 = iz0 + else + iz1 = iz0+1 + end if + + if (ft == 0) then + it1 = it0 + else + it1 = it0+1 + end if + + c000 = f(it0,iz0,iy0,ix0)*(1-ft) + f(it1,iz0,iy0,ix0)*ft + c001 = f(it0,iz1,iy0,ix0)*(1-ft) + f(it1,iz1,iy0,ix0)*ft + c010 = f(it0,iz0,iy1,ix0)*(1-ft) + f(it1,iz0,iy1,ix0)*ft + c011 = f(it0,iz1,iy1,ix0)*(1-ft) + f(it1,iz1,iy1,ix0)*ft + c100 = f(it0,iz0,iy0,ix1)*(1-ft) + f(it1,iz0,iy0,ix1)*ft + c101 = f(it0,iz1,iy0,ix1)*(1-ft) + f(it1,iz1,iy0,ix1)*ft + c110 = f(it0,iz0,iy1,ix1)*(1-ft) + f(it1,iz0,iy1,ix1)*ft + c111 = f(it0,iz1,iy1,ix1)*(1-ft) + f(it1,iz1,iy1,ix1)*ft + + c00 = c000*(1-fx) + c100*fx + c01 = c001*(1-fx) + c101*fx + c10 = c010*(1-fx) + c110*fx + c11 = c011*(1-fx) + c111*fx - ENDDO ! I - All nodes where the line position and tension are to be computed + c0 = c00 *(1-fy) + c10 *fy + c1 = c01 *(1-fy) + c11 *fy + c = c0 *(1-fz) + c1 *fz + + END SUBROUTINE - ELSE ! 0.0_DbKi < HF <= -CB*VFMinWL ! A portion of the line must rest on the seabed and the anchor tension is zero - ! Anchor tensions: + SUBROUTINE calculate3Dinterpolation(f, ix0, iy0, iz0, fx, fy, fz, c) - HA = 0.0_DbKi - VA = 0.0_DbKi + Real(DbKi), INTENT (IN ) :: f(:,:,:) ! data array + INTEGER(IntKi), INTENT (IN ) :: ix0, iy0, iz0 ! indeces for interpolation + Real(DbKi), INTENT (IN ) :: fx, fy, fz ! interpolation fractions + Real(DbKi), INTENT ( OUT) :: c ! the output value + + INTEGER(IntKi) :: ix1, iy1, iz1 ! second indices + REAL(DbKi) :: c000, c001, c010, c011, c100, c101, c110, c111 + REAL(DbKi) :: c00, c01, c10, c11, c0, c1 + + ! note that "z" could also be "t" - dimension names are arbitrary + + ! handle end case conditions + if (fx == 0) then + ix1 = ix0 + else + ix1 = ix0+1 + end if + + if (fy == 0) then + iy1 = iy0 + else + iy1 = iy0+1 + end if + + if (fz == 0) then + iz1 = iz0 + else + iz1 = iz0+1 + end if + + c000 = f(iz0,iy0,ix0) + c001 = f(iz1,iy0,ix0) + c010 = f(iz0,iy1,ix0) + c011 = f(iz1,iy1,ix0) + c100 = f(iz0,iy0,ix1) + c101 = f(iz1,iy0,ix1) + c110 = f(iz0,iy1,ix1) + c111 = f(iz1,iy1,ix1) + + c00 = c000*(1-fx) + c100*fx + c01 = c001*(1-fx) + c101*fx + c10 = c010*(1-fx) + c110*fx + c11 = c011*(1-fx) + c111*fx + c0 = c00 *(1-fy) + c10 *fy + c1 = c01 *(1-fy) + c11 *fy - ! Line position and tension at each node: + c = c0 *(1-fz) + c1 *fz - DO I = 1,N ! Loop through all nodes where the line position and tension are to be computed + END SUBROUTINE - IF ( ( s(I) < 0.0_DbKi ) .OR. ( s(I) > L ) ) THEN - ErrStat = ErrID_Warn - ErrMsg = ' All line nodes must be located between the anchor ' & - //'and fairlead (inclusive) in routine Catenary().' - RETURN - END IF - Ws = W *s(I) ! Initialize - VFMinWLs = VFMinWL + Ws ! some commonly - VFMinWLsOvrHF = VFMinWLs/HF ! used terms - sOvrEA = s(I) /EA ! that depend - SQRT1VFMinWLsOvrHF2 = SQRT( 1.0_DbKi + VFMinWLsOvrHF*VFMinWLsOvrHF ) ! on s(I) - IF ( s(I) <= LMinVFOvrW - HFOvrW/CB ) THEN ! .TRUE. if this node rests on the seabed and the tension is zero + ! ============ below are some math convenience functions =============== + ! should add error checking if I keep these, but hopefully there are existing NWTCLib functions to replace them - X (I) = s(I) - Z (I) = 0.0_DbKi - Te(I) = 0.0_DbKi - ELSEIF ( s(I) <= LMinVFOvrW ) THEN ! .TRUE. if this node rests on the seabed and the tension is nonzero + ! return unit vector (u) and in direction from r1 to r2 and distance between points + !----------------------------------------------------------------------- + SUBROUTINE UnitVector( r1, r2, u, Length ) ! note: order of parameters chagned in this function + + REAL(DbKi), INTENT(IN ) :: r1(:) + REAL(DbKi), INTENT(IN ) :: r2(:) + REAL(DbKi), INTENT( OUT) :: u(:) + REAL(DbKi), INTENT( OUT) :: length - X (I) = s(I) - ( LMinVFOvrW - 0.5_DbKi*HFOvrW/CB )*HF/EA & - + sOvrEA*( HF + CB*VFMinWL + 0.5_DbKi*Ws*CB ) + 0.5_DbKi*CB*VFMinWL*VFMinWL/WEA - Z (I) = 0.0_DbKi - Te(I) = HF + CB*VFMinWLs + u = r2 - r1 + length = TwoNorm(u) - ELSE ! LMinVFOvrW < s <= L ! This node must be above the seabed + if ( .NOT. EqualRealNos(length, 0.0_DbKi ) ) THEN + u = u / Length + END IF - X (I) = LOG( VFMinWLsOvrHF + SQRT1VFMinWLsOvrHF2 ) *HFOvrW & - + sOvrEA* HF + LMinVFOvrW - ( LMinVFOvrW - 0.5_DbKi*HFOvrW/CB )*HF/EA - Z (I) = ( - 1.0_DbKi + SQRT1VFMinWLsOvrHF2 )*HFOvrW & - + sOvrEA*( VFMinWL + 0.5_DbKi*Ws ) + 0.5_DbKi* VFMinWL*VFMinWL/WEA - Te(I) = SQRT( HF*HF + VFMinWLs*VFMinWLs ) + END SUBROUTINE UnitVector + !----------------------------------------------------------------------- + + ! scale vector to desired length + !----------------------------------------------------------------------- + SUBROUTINE ScaleVector( u_in, newlength, u_out ) + REAL(DbKi), INTENT(IN ) :: u_in(3) ! input vector + REAL(DbKi), INTENT(IN ) :: newlength ! desired length of output vector + REAL(DbKi), INTENT(INOUT) :: u_out(3) ! output vector (hopefully can be the same as u_in without issue) + + REAL(DbKi) :: length_squared + REAL(DbKi) :: scaler + INTEGER(IntKi) :: J + + length_squared = 0.0; + DO J=1,3 + length_squared = length_squared + u_in(J)*u_in(J) + END DO + + if (length_squared > 0) then + scaler = newlength/sqrt(length_squared) + else ! if original vector is zero, return zero + scaler = 0_DbKi + end if + + DO J=1,3 + u_out(J) = u_in(J) * scaler + END DO - ENDIF + END SUBROUTINE ScaleVector + !----------------------------------------------------------------------- - ENDDO ! I - All nodes where the line position and tension are to be computed + ! calculate orientation angles of a cylindrical object + !----------------------------------------------------------------------- + subroutine GetOrientationAngles(p1, p2, phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat) + real(DbKi), intent(in ) :: p1(3),p2(3) + real(DbKi), intent( out) :: phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat(3) + + real(DbKi) :: vec(3), vecLen, vecLen2D + + ! calculate isntantaneous incline angle and heading, and related trig values + ! the first and last NodeIndx values point to the corresponding Joint nodes idices which are at the start of the Mesh + vec = p2 - p1 + vecLen = SQRT(Dot_Product(vec,vec)) + vecLen2D = SQRT(vec(1)**2+vec(2)**2) + if ( vecLen < 0.000001 ) then + print *, "ERROR in GetOrientationAngles" !call SeterrStat(ErrID_Fatal, 'An element of the Morison structure has co-located endpoints! This should never occur. Please review your model.', errStat, errMsg, 'Morison_CalcOutput' ) + print *, p1 + print *, p2 + k_hat = 1.0/0.0 + else + k_hat = vec / vecLen + phi = atan2(vecLen2D, vec(3)) ! incline angle + end if + if ( phi < 0.000001) then + beta = 0.0_ReKi + else + beta = atan2(vec(2), vec(1)) ! heading of incline + endif + sinPhi = sin(phi) + cosPhi = cos(phi) + tanPhi = tan(phi) + sinBeta = sin(beta) + cosBeta = cos(beta) + + end subroutine GetOrientationAngles + !----------------------------------------------------------------------- + - ENDIF + ! calculate position and velocity of point based on its position relative to moving 6DOF body + !----------------------------------------------------------------------- + SUBROUTINE TransformKinematics(rRelBody, r_in, TransMat, rd_in, r_out, rd_out) + REAL(DbKi), INTENT(IN ) :: rRelBody(:) ! coordinate of end A + REAL(DbKi), INTENT(IN ) :: r_in(3) ! Rod unit vector + REAL(DbKi), INTENT(IN ) :: TransMat(3,3)! + REAL(DbKi), INTENT(IN ) :: rd_in(6) ! 6DOF velecity vector about Rod end A, in global orientation frame + REAL(DbKi), INTENT( OUT) :: r_out(3) ! coordinates of end B + REAL(DbKi), INTENT( OUT) :: rd_out(3) ! velocity of end B + REAL(DbKi) :: rRel(3) + ! rd_in should be in global orientation frame + ! note: it's okay if r_out and rd_out are 6-size. Only the first 3 will be written, and 4-6 will + ! already be correct or can be assigned seperately from r_in and rd_in (assuming orientation frames are identical) - ! The Newton-Raphson iteration is only accurate in double precision, so - ! convert the output arguments back into the default precision for real - ! numbers: - !HA_In = REAL( HA , DbKi ) !mth: for this I only care about returning node positions - !HF_In = REAL( HF , DbKi ) - !Te_In(:) = REAL( Te(:), DbKi ) - !VA_In = REAL( VA , DbKi ) - !VF_In = REAL( VF , DbKi ) - X_In (:) = REAL( X (:), DbKi ) - Z_In (:) = REAL( Z (:), DbKi ) + ! locations (unrotated reference frame) about platform reference point (2021-01-05: just transposed TransMat, it was incorrect before) + rRel(1) = TransMat(1,1)*rRelBody(1) + TransMat(1,2)*rRelBody(2) + TransMat(1,3)*rRelBody(3) ! x + rRel(2) = TransMat(2,1)*rRelBody(1) + TransMat(2,2)*rRelBody(2) + TransMat(2,3)*rRelBody(3) ! y + rRel(3) = TransMat(3,1)*rRelBody(1) + TransMat(3,2)*rRelBody(2) + TransMat(3,3)*rRelBody(3) ! z - END SUBROUTINE Catenary - !======================================================================= + ! absolute locations + r_out = rRel + r_in + ! absolute velocities + rd_out(1) = - rd_in(6)*rRel(2) + rd_in(5)*rRel(3) + rd_in(1) ! x + rd_out(2) = rd_in(6)*rRel(1) - rd_in(4)*rRel(3) + rd_in(2) ! y + rd_out(3) = -rd_in(5)*rRel(1) + rd_in(4)*rRel(2) + rd_in(3) ! z + + ! absolute accelerations + rd_out(1) = - rd_in(6)*rRel(2) + rd_in(5)*rRel(3) + rd_in(1) ! x + rd_out(2) = rd_in(6)*rRel(1) - rd_in(4)*rRel(3) + rd_in(2) ! y + rd_out(3) = -rd_in(5)*rRel(1) + rd_in(4)*rRel(2) + rd_in(3) ! z - END SUBROUTINE InitializeLine - !====================================================================== + !rRel = MATMUL(TransMat, rRelBody) + !H = getH(rRel) + !! absolute locations + !r_out = rRel + r_in + !! absolute velocities + !rd_out = MATMUL( H, rd_in(4:6)) + rd_in(1:3) + - ! ============ below are some math convenience functions =============== - ! should add error checking if I keep these, but hopefully there are existing NWTCLib functions to replace them + END SUBROUTINE TransformKinematics + !----------------------------------------------------------------------- + + + ! calculate position, velocity, and acceleration of point based on its position relative to moving 6DOF body + !----------------------------------------------------------------------- + SUBROUTINE TransformKinematicsA(rRelBody, r_in, TransMat, v_in, a_in, r_out, v_out, a_out) + REAL(DbKi), INTENT(IN ) :: rRelBody(:) ! relative location of point about reference point, in local/reference coordinate system + REAL(DbKi), INTENT(IN ) :: r_in(3) ! translation applied to reference point + REAL(DbKi), INTENT(IN ) :: TransMat(3,3)! rotation matrix describing rotation about reference point + REAL(DbKi), INTENT(IN ) :: v_in(6) ! 6DOF velecity vector about ref point in global orientation frame + REAL(DbKi), INTENT(IN ) :: a_in(6) ! 6DOF acceleration vector + REAL(DbKi), INTENT( OUT) :: r_out(3) ! coordinates of point of interest + REAL(DbKi), INTENT( OUT) :: v_out(3) ! velocity of point + REAL(DbKi), INTENT( OUT) :: a_out(3) ! acceleration of point + + REAL(DbKi) :: rRel(3) + REAL(DbKi) :: rRel2(3) + + REAL(DbKi) :: r_out2(3) + REAL(DbKi) :: rd_out2(3) + REAL(DbKi) :: H(3,3) + + ! rd_in should be in global orientation frame + ! note: it's okay if r_out and rd_out are 6-size. Only the first 3 will be written, and 4-6 will + ! already be correct or can be assigned seperately from r_in and rd_in (assuming orientation frames are identical) + + + ! locations about ref point in *unrotated* reference frame + !rRel2(1) = TransMat(1,1)*rRelBody(1) + TransMat(2,1)*rRelBody(2) + TransMat(3,1)*rRelBody(3) ! x + !rRel2(2) = TransMat(1,2)*rRelBody(1) + TransMat(2,2)*rRelBody(2) + TransMat(3,2)*rRelBody(3) ! y + !rRel2(3) = TransMat(1,3)*rRelBody(1) + TransMat(2,3)*rRelBody(2) + TransMat(3,3)*rRelBody(3) ! z + + rRel = MATMUL(TransMat, rRelBody) + + H = getH(rRel) + + ! absolute locations + r_out = rRel + r_in - ! return unit vector (u) in direction from r1 to r2 - !======================================================================= - SUBROUTINE UnitVector( u, r1, r2 ) - REAL(DbKi), INTENT(OUT) :: u(:) - REAL(DbKi), INTENT(IN) :: r1(:) - REAL(DbKi), INTENT(IN) :: r2(:) + ! absolute velocities + !rd_out2(1) = - v_in(6)*rRel(2) + v_in(5)*rRel(3) + v_in(1) ! x + !rd_out2(2) = v_in(6)*rRel(1) - v_in(4)*rRel(3) + v_in(2) ! y + !rd_out2(3) = -v_in(5)*rRel(1) + v_in(4)*rRel(2) + v_in(3) ! z + + v_out = MATMUL( H, v_in(4:6)) + v_in(1:3) + + ! absolute accelerations + a_out = MATMUL( H, a_in(4:6)) + a_in(1:3) ! << should add second order terms! + - REAL(DbKi) :: Length + END SUBROUTINE TransformKinematicsA + !----------------------------------------------------------------------- + + ! calculate position and velocity of point along rod (distance L along direction u) + !----------------------------------------------------------------------- + SUBROUTINE TransformKinematicsAtoB(rA, u, L, rd_in, r_out, rd_out) + REAL(DbKi), INTENT(IN ) :: rA(3) ! coordinate of end A + REAL(DbKi), INTENT(IN ) :: u(3) ! Rod unit vector + REAL(DbKi), INTENT(IN ) :: L ! Rod length from end A to B + REAL(DbKi), INTENT(IN ) :: rd_in(6) ! 6DOF velecity vector about Rod end A, in global orientation frame + REAL(DbKi), INTENT( OUT) :: r_out(3) ! coordinates of end B + REAL(DbKi), INTENT( OUT) :: rd_out(3) ! velocity of end B + + REAL(DbKi) :: rRel(3) + + + ! locations (unrotated reference frame) + rRel = L*u ! relative location of point B from point A + r_out = rRel + rA ! absolute location of point B + + ! absolute velocities + rd_out(1) = - rd_in(6)*rRel(2) + rd_in(5)*rRel(3) + rd_in(1) ! x + rd_out(2) = rd_in(6)*rRel(1) - rd_in(4)*rRel(3) + rd_in(2) ! y + rd_out(3) = -rd_in(5)*rRel(1) + rd_in(4)*rRel(2) + rd_in(3) ! z + + + END SUBROUTINE TransformKinematicsAtoB + !----------------------------------------------------------------------- + + ! + !----------------------------------------------------------------------- + SUBROUTINE TranslateForce3to6DOF(dx, F, Fout) + REAL(DbKi), INTENT(IN ) :: dx(3) ! displacement vector from ref point to point of force (F) application + REAL(DbKi), INTENT(IN ) :: F(3) ! applied force + REAL(DbKi), INTENT( OUT) :: Fout(6) ! resultant applied force and moment about ref point + + Fout(1:3) = F + + Fout(4:6) = CROSS_PRODUCT(dx, F) + + END SUBROUTINE TranslateForce3to6DOF + !----------------------------------------------------------------------- + + + ! + !----------------------------------------------------------------------- + SUBROUTINE TranslateMass3to6DOF(dx, Min, Mout) + REAL(DbKi), INTENT(IN ) :: dx(3) ! displacement vector from ref point to point of mass matrix (Min) + REAL(DbKi), INTENT(IN ) :: Min( 3,3) ! original mass matrix (assumed at center of mass, or a point mass) + REAL(DbKi), INTENT( OUT) :: Mout(6,6) ! resultant mass and inertia matrix about ref point + + REAL(DbKi) :: H( 3,3) ! "anti-symmetric tensor components" from Sadeghi and Incecik + REAL(DbKi) :: tempM( 3,3) + REAL(DbKi) :: tempM2(3,3) + REAL(DbKi) :: Htrans(3,3) + Integer(IntKi) :: I,J + + ! sub-matrix definitions are accordint to | m J | + ! | J^T I | + + H = getH(dx); + + ! mass matrix [m'] = [m] + Mout(1:3,1:3) = Min + + ! product of inertia matrix [J'] = [m][H] + [J] + Mout(1:3,4:6) = MATMUL(Min, H) + Mout(4:6,1:3) = TRANSPOSE(Mout(1:3,4:6)) + + !moment of inertia matrix [I'] = [H][m][H]^T + [J]^T [H] + [H]^T [J] + [I] + Mout(4:6,4:6) = MATMUL(MATMUL(H, Min), TRANSPOSE(H)) + + END SUBROUTINE TranslateMass3to6DOF + !----------------------------------------------------------------------- + + ! + !----------------------------------------------------------------------- + SUBROUTINE TranslateMass6to6DOF(dx, Min, Mout) + REAL(DbKi), INTENT(IN ) :: dx(3) ! displacement vector from ref point to point of mass matrix (Min) + REAL(DbKi), INTENT(IN ) :: Min( 6,6) ! original mass matrix + REAL(DbKi), INTENT( OUT) :: Mout(6,6) ! resultant mass and inertia matrix about ref point + + REAL(DbKi) :: H( 3,3) ! "anti-symmetric tensor components" from Sadeghi and Incecik + + H = getH(dx); + + ! mass matrix [m'] = [m] + Mout(1:3,1:3) = Min(1:3,1:3) + + ! product of inertia matrix [J'] = [m][H] + [J] + Mout(1:3,4:6) = MATMUL(Min(1:3,1:3), H) + Min(1:3,4:6) + Mout(4:6,1:3) = TRANSPOSE(Mout(1:3,4:6)) + + !moment of inertia matrix [I'] = [H][m][H]^T + [J]^T [H] + [H]^T [J] + [I] + Mout(4:6,4:6) = MATMUL(MATMUL(H, Min(1:3,1:3)), TRANSPOSE(H)) + MATMUL(Min(4:6,1:3),H) + MATMUL(TRANSPOSE(H),Min(1:3,4:6)) + Min(4:6,4:6) + + END SUBROUTINE TranslateMass6to6DOF + !----------------------------------------------------------------------- + + ! produce alternator matrix + !----------------------------------------------------------------------- + FUNCTION GetH(r) + Real(DbKi), INTENT(IN) :: r(3) ! inputted vector + Real(DbKi) :: GetH(3,3) ! outputted matrix + + GetH(2,1) = -r(3) + GetH(1,2) = r(3) + GetH(3,1) = r(2) + GetH(1,3) = -r(2) + GetH(3,2) = -r(1) + GetH(2,3) = r(1) + + GetH(1,1) = 0.0_DbKi + GetH(2,2) = 0.0_DbKi + GetH(3,3) = 0.0_DbKi + + END FUNCTION GetH + !----------------------------------------------------------------------- + + + + ! apply a rotation to a 6-by-6 mass/inertia tensor (see Sadeghi and Incecik 2005 for theory) + !----------------------------------------------------------------------- + FUNCTION RotateM6(Min, rotMat) result(outMat) + + Real(DbKi), INTENT(IN) :: Min(6,6) ! inputted matrix to be rotated + Real(DbKi), INTENT(IN) :: rotMat(3,3) ! rotation matrix (DCM) + Real(DbKi) :: outMat(6,6) ! rotated matrix + + Real(DbKi) :: tempM(3,3) + Real(DbKi) :: tempMrot(3,3) + + ! the process for each of the following is to + ! 1. copy out the relevant 3x3 matrix section, + ! 2. rotate it, and + ! 3. paste it into the output 6x6 matrix + + + ! mass matrix + outMat(1:3,1:3) = rotateM3(Min(1:3,1:3), rotMat) - u = r2 - r1 - Length = TwoNorm(u) + ! product of inertia matrix + outMat(1:3,4:6) = rotateM3(Min(1:3,4:6), rotMat) + outMat(4:6,1:3) = TRANSPOSE(outMat(1:3,4:6)) - if ( .NOT. EqualRealNos(length, 0.0_DbKi ) ) THEN - u = u / Length - END IF + ! moment of inertia matrix + outMat(4:6,4:6) = rotateM3(Min(4:6,4:6), rotMat) + + END FUNCTION RotateM6 - END SUBROUTINE UnitVector - !======================================================================= + ! apply a rotation to a 3-by-3 mass matrix or any other second order tensor + !----------------------------------------------------------------------- + FUNCTION RotateM3(Min, rotMat) result(outMat) + + Real(DbKi), INTENT(IN) :: Min(3,3) ! inputted matrix to be rotated + Real(DbKi), INTENT(IN) :: rotMat(3,3) ! rotation matrix (DCM) + Real(DbKi) :: outMat(3,3) ! rotated matrix + + ! overall operation is [m'] = [a]*[m]*[a]^T + + outMat = MATMUL( MATMUL(rotMat, Min), TRANSPOSE(rotMat) ) + + END FUNCTION RotateM3 + + + + + + ! calculates rotation matrix R to rotate from global axes to a member's local axes + !----------------------------------------------------------------------- + FUNCTION CalcOrientation(phi, beta, gamma) result(R) + + REAL(DbKi), INTENT ( IN ) :: phi ! member incline angle + REAL(DbKi), INTENT ( IN ) :: beta ! member incline heading + REAL(DbKi), INTENT ( IN ) :: gamma ! member twist angle + REAL(DbKi) :: R(3,3) ! rotation matrix + + INTEGER(IntKi) :: errStat + CHARACTER(100) :: errMsg + + REAL(DbKi) :: s1, c1, s2, c2, s3, c3 + + + ! trig terms for Euler angles rotation based on beta, phi, and gamma + s1 = sin(beta) + c1 = cos(beta) + s2 = sin(phi) + c2 = cos(phi) + s3 = sin(gamma) + c3 = cos(gamma) + + ! calculate rotation matrix based on Z1Y2Z3 Euler rotation sequence from https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + + R(1,1) = c1*c2*c3-s1*s3 + R(1,2) = -c3*s1-c1*c2*s3 + R(1,3) = c1*s2 + R(2,1) = c1*s3+c2*c3*s1 + R(2,2) = c1*c3-c2*s1*s3 + R(2,3) = s1*s2 + R(3,1) = -c3*s2 + R(3,2) = s2*s3 + R(3,3) = c2 + + ! could also calculate unit normals p1 and p2 for rectangular cross sections + !p1 = matmul( R, [1,0,0] ) ! unit vector that is perpendicular to the 'beta' plane if gamma is zero + !p2 = cross( q, p1 ) ! unit vector orthogonal to both p1 and q + + END FUNCTION CalcOrientation + !compute the inverse of a 3-by-3 matrix m - !======================================================================= + !----------------------------------------------------------------------- SUBROUTINE Inverse3by3( Minv, M ) - Real(DbKi), INTENT(OUT) :: Minv(:,:) ! returned inverse matrix - Real(DbKi), INTENT(IN) :: M(:,:) ! inputted matrix + Real(DbKi), INTENT(OUT) :: Minv(3,3) ! returned inverse matrix + Real(DbKi), INTENT(IN) :: M(3,3) ! inputted matrix Real(DbKi) :: det ! the determinant Real(DbKi) :: invdet ! inverse of the determinant @@ -2204,7 +6699,75 @@ SUBROUTINE Inverse3by3( Minv, M ) Minv(3, 3) = (M(1, 1) * M(2, 2) - M(2, 1) * M(1, 2)) * invdet END SUBROUTINE Inverse3by3 - !======================================================================= + !----------------------------------------------------------------------- + + + ! One-function implementation of Crout LU Decomposition. Solves Ax=b for x + SUBROUTINE LUsolve(n, A, LU, b, y, x) + + INTEGER(intKi), INTENT(IN ) :: n ! size of matrices and vectors + Real(DbKi), INTENT(IN ) :: A( n,n) ! LHS matrix (e.g. mass matrix) + Real(DbKi), INTENT(INOUT) :: LU(n,n) ! stores LU matrix data + Real(DbKi), INTENT(IN ) :: b(n) ! RHS vector + Real(DbKi), INTENT(INOUT) :: y(n) ! temporary vector + Real(DbKi), INTENT( OUT) :: x(n) ! LHS vector to solve for + + INTEGER(intKi) :: i,j,k,p + Real(DbKi) :: sum + + DO k = 1,n + DO i = k,n + + sum = 0.0_DbKi + + DO p=1,k-1 !for(int p=0; p=0; --i) + + sum = 0.0_DbKi + + DO k=i+1, n + sum = sum + LU(i,k)*x(k) + END DO + + x(i) = (y(i)-sum) + + END DO !j (actually decrementing i) + + END SUBROUTINE LUsolve + + END MODULE MoorDyn diff --git a/modules/moordyn/src/MoorDyn_Driver.f90 b/modules/moordyn/src/MoorDyn_Driver.f90 index 99ad3b8019..a286b5f58a 100644 --- a/modules/moordyn/src/MoorDyn_Driver.f90 +++ b/modules/moordyn/src/MoorDyn_Driver.f90 @@ -85,6 +85,7 @@ PROGRAM MoorDyn_Driver ! ------------------------------------------------------------------------- ! Initialize MoorDyn + ! ------------------------------------------------------------------------- dtC = 0.01 ! desired coupling time step size for communicating with MoorDyn @@ -104,6 +105,21 @@ PROGRAM MoorDyn_Driver MD_InitInput%PtfmInit = 0.0 MD_InitInput%RootName = "MoorDyn.MD" + ! fill in the hydrodynamics data + ALLOCATE( MD_InitInput%WaveVel (2,200,3)) + ALLOCATE( MD_InitInput%WaveAcc (2,200,3)) + ALLOCATE( MD_InitInput%WavePDyn(2,200) ) + ALLOCATE( MD_InitInput%WaveElev(2,200) ) + ALLOCATE( MD_InitInput%WaveTime(2) ) + MD_InitInput%WaveVel = 0.0_ReKi + MD_InitInput%WaveAcc = 0.0_ReKi + MD_InitInput%WavePDyn = 0.0_ReKi + MD_InitInput%WaveElev = 0.0_ReKi + MD_InitInput%WaveTime = 0.0_ReKi + DO I = 1,SIZE(MD_InitInput%WaveTime) + MD_InitInput%WaveTime(I) = 600.0*I + END DO + CALL GetNewUnit( Un ) OPEN(Unit=Un,FILE='MD.out',STATUS='UNKNOWN') @@ -196,11 +212,15 @@ PROGRAM MoorDyn_Driver print *, "Read ", ntIn, " time steps from input file." print *, PtfmMotIn + + END IF ! ----------------------- specify stepping details ----------------------- + + IF (ntIn > 0) THEN tMax = PtfmMotIn(ntIn, 1) ! save last time step as total sim time ELSE @@ -251,18 +271,30 @@ PROGRAM MoorDyn_Driver ELSE PtfmMot = 0.0_Reki END IF - - + ! --------------------------------------------------------------- ! Set the initial input values ! --------------------------------------------------------------- - ! start with zeros >>> or should this be the initial row of DOFs? <<< + ! zero the displacements to start with MD_Input(1)%PtFairleadDisplacement%TranslationDisp = 0.0_ReKi + + ! zero the tension commands MD_Input(1)%DeltaL = 0.0_ReKi MD_Input(1)%DeltaLdot = 0.0_ReKi + +! ! zero water inputs (if passing wave info in from glue code) +! MD_Input(1)%U = 0.0 +! MD_Input(1)%Ud = 0.0 +! MD_Input(1)%zeta = 0.0 +! MD_Input(1)%PDyn = 0.0 +! ! now add some current in x for testing +! MD_Input(1)%U(1,:) = 1.0 + + + DO i = 2, MD_interp_order + 1 CALL MD_CopyInput( MD_Input(1), MD_Input(i), MESH_NEWCOPY, ErrStat, ErrMsg ) @@ -293,8 +325,10 @@ PROGRAM MoorDyn_Driver ! ------------------------------------------------------------------------- - ! BEGIN time marching >>> note that 3 rotational platform DOFs are currently neglected <<< + ! BEGIN time marching >>> note that 3 rotational platform DOFs are currently neglected <<< ! ------------------------------------------------------------------------- + + ! TODO: add rotational DOFs, update coupling points, add filtering, and add velocity and acceleration <<<< print *,"Doing time marching now..." @@ -304,14 +338,22 @@ PROGRAM MoorDyn_Driver t = dtC*(i-1) + print *, t + MD_InputTimes(1) = t + dtC !MD_InputTimes(2) = MD_InputTimes(1) - dtC !MD_InputTimes(3) = MD_InputTimes(2) - dtC ! apply platform translations (neglecting rotations for now) - MD_Input(1)%PtFairleadDisplacement%TranslationDisp(1,1) = PtfmMot(i, 1) - MD_Input(1)%PtFairleadDisplacement%TranslationDisp(1,2) = PtfmMot(i, 2) - MD_Input(1)%PtFairleadDisplacement%TranslationDisp(1,3) = PtfmMot(i, 3) + DO J = 1,MD_Parameter%nCpldCons + MD_Input(1)%PtFairleadDisplacement%TranslationDisp(1,J) = PtfmMot(i, 1) + MD_Input(1)%PtFairleadDisplacement%TranslationDisp(2,J) = PtfmMot(i, 2) + MD_Input(1)%PtFairleadDisplacement%TranslationDisp(3,J) = PtfmMot(i, 3) + END DO + + !MD_Input(1)%PtFairleadDisplacement%TranslationDisp(1,1) = PtfmMot(i, 1) + !MD_Input(1)%PtFairleadDisplacement%TranslationDisp(1,2) = PtfmMot(i, 2) + !MD_Input(1)%PtFairleadDisplacement%TranslationDisp(1,3) = PtfmMot(i, 3) !MD_Input(2)%PtFairleadDisplacement%TranslationDisp(1,1) = .001*n_t_global !MD_Input(3)%PtFairleadDisplacement%TranslationDisp(1,1) = .001*n_t_global @@ -413,4 +455,4 @@ PROGRAM MoorDyn_Driver 100 FORMAT(2(1X,F8.3),9(1X,E12.5)) - END PROGRAM \ No newline at end of file + END PROGRAM diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index a1c9feaf82..1a97939b7b 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -28,6 +28,10 @@ MODULE MoorDyn_IO PRIVATE + + INTEGER, PARAMETER :: nCoef = 30 ! maximum number of entries to allow in nonlinear coefficient lookup tables + ! it would be nice if the above worked for everything, but I think it needs to also be matched in the Registry + ! --------------------------- Output definitions ----------------------------------------- ! The following are some definitions for use with the output options in MoorDyn. @@ -41,8 +45,7 @@ MODULE MoorDyn_IO ! QType - (int) the type of quantity to output. 0=tension, 1=x pos, etc. see the parameters below ! NodeID - (int) the ID number of the node of the output quantity - ! These are the "OTypes": 0=Connect object, 1=Line Object - ! (will just use 0 and 1 rather than parameter names) + ! These are the "OTypes": 1=Line, 2=Connect, 3=Rod, 4=Body ! Indices for computing output channels: - customized for the MD_OutParmType approach ! these are the "QTypes" @@ -56,17 +59,21 @@ MODULE MoorDyn_IO INTEGER, PARAMETER :: AccX = 7 INTEGER, PARAMETER :: AccY = 8 INTEGER, PARAMETER :: AccZ = 9 - INTEGER, PARAMETER :: Ten = 10 - INTEGER, PARAMETER :: FX = 11 - INTEGER, PARAMETER :: FY = 12 - INTEGER, PARAMETER :: FZ = 13 + INTEGER, PARAMETER :: Ten = 10 + INTEGER, PARAMETER :: FX = 11 + INTEGER, PARAMETER :: FY = 12 + INTEGER, PARAMETER :: FZ = 13 + INTEGER, PARAMETER :: Pitch = 14 + INTEGER, PARAMETER :: Roll = 15 + INTEGER, PARAMETER :: Yaw = 16 ! List of units corresponding to the quantities parameters for QTypes - CHARACTER(ChanLen), PARAMETER :: UnitList(0:13) = (/ & + CHARACTER(ChanLen), PARAMETER :: UnitList(0:16) = (/ & "(s) ","(m) ","(m) ","(m) ", & "(m/s) ","(m/s) ","(m/s) ", & "(m/s2) ","(m/s2) ","(m/s2) ", & - "(N) ","(N) ","(N) ","(N) " /) + "(N) ","(N) ","(N) ","(N) ", & + "(deg) ","(deg) ","(deg) "/) CHARACTER(28), PARAMETER :: OutPFmt = "( I4, 3X,A 10,1 X, A10 )" ! Output format parameter output list. CHARACTER(28), PARAMETER :: OutSFmt = "ES10.3E2" @@ -84,7 +91,9 @@ MODULE MoorDyn_IO - PUBLIC :: MDIO_ReadInput + ! PUBLIC :: MDIO_ReadInput + PUBLIC :: getCoefficientOrCurve + PUBLIC :: DecomposeString PUBLIC :: MDIO_OpenOutput PUBLIC :: MDIO_CloseOutput PUBLIC :: MDIO_ProcessOutList @@ -95,524 +104,155 @@ MODULE MoorDyn_IO - - !==================================================================================================== - SUBROUTINE MDIO_ReadInput( InitInp, p, m, ErrStat, ErrMsg ) - - ! This subroutine reads the input required for MoorDyn from the file whose name is an - ! input parameter. It sets the size of p%NTypes, NConnects, and NLines, - ! allocates LineTypeList, ConnectList, and LineList, and puts all the read contents of - ! the input file into the respective slots in those lists of types. - - - ! Passed variables - - TYPE(MD_InitInputType), INTENT( INOUT ) :: InitInp ! the MoorDyn data - TYPE(MD_ParameterType), INTENT(INOUT) :: p ! Parameters - TYPE(MD_MiscVarType), INTENT( OUT) :: m ! INTENT( OUT) : Initial misc/optimization vars - INTEGER, INTENT( OUT ) :: ErrStat ! returns a non-zero value when an error occurs - CHARACTER(*), INTENT( OUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None - - - ! Local variables - - INTEGER :: I ! generic integer for counting - INTEGER :: J ! generic integer for counting - INTEGER :: UnIn ! Unit number for the input file - INTEGER :: UnEc ! The local unit number for this module's echo file - CHARACTER(1024) :: EchoFile ! Name of MoorDyn echo file - CHARACTER(1024) :: Line ! String to temporarially hold value of read line - CHARACTER(20) :: LineOutString ! String to temporarially hold characters specifying line output options - CHARACTER(20) :: OptString ! String to temporarially hold name of option variable - CHARACTER(20) :: OptValue ! String to temporarially hold value of options variable input - CHARACTER(1024) :: FileName ! - - INTEGER(IntKi) :: ErrStat2 - CHARACTER(ErrMsgLen) :: ErrMsg2 - CHARACTER(*), PARAMETER :: RoutineName = 'MDIO_ReadInput' - - - ! - UnEc = -1 - - ! Initialize ErrStat - ErrStat = ErrID_None - ErrMsg = "" - - !------------------------------------------------------------------------------------------------- - ! Open the file - !------------------------------------------------------------------------------------------------- - FileName = TRIM(InitInp%FileName) - - CALL GetNewUnit( UnIn ) - CALL OpenFInpFile( UnIn, FileName, ErrStat2, ErrMsg2 ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - - - CALL WrScr( ' MD_Init: Opening MoorDyn input file: '//FileName ) - - - !------------------------------------------------------------------------------------------------- - ! File header - !------------------------------------------------------------------------------------------------- - - CALL ReadCom( UnIn, FileName, 'MoorDyn input file header line 1', ErrStat2, ErrMsg2 ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - - - CALL ReadCom( UnIn, FileName, 'MoorDyn input file header line 2', ErrStat2, ErrMsg2 ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - - - ! Echo Input Files. - CALL ReadVar ( UnIn, FileName, InitInp%Echo, 'Echo', 'Echo Input', ErrStat2, ErrMsg2 ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - - - ! If we are Echoing the input then we should re-read the first three lines so that we can echo them - ! using the NWTC_Library routines. The echoing is done inside those routines via a global variable - ! which we must store, set, and then replace on error or completion. - - IF ( InitInp%Echo ) THEN - - !print *, 'gonna try to open echo file' - - EchoFile = TRIM(p%RootName)//'.ech' ! open an echo file for writing - - !print *, 'name is ', EchoFile - - CALL GetNewUnit( UnEc ) - CALL OpenEcho ( UnEc, EchoFile, ErrStat2, ErrMsg2 ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - - REWIND(UnIn) ! rewind to start of input file to re-read the first few lines - - - - - CALL ReadCom( UnIn, FileName, 'MoorDyn input file header line 1', ErrStat2, ErrMsg2, UnEc ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - - CALL ReadCom( UnIn, FileName, 'MoorDyn input file header line 2', ErrStat2, ErrMsg2, UnEc ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - - - ! Echo Input Files. Note this line is prevented from being echoed by the ReadVar routine. - CALL ReadVar ( UnIn, FileName, InitInp%Echo, 'Echo', 'Echo the input file data', ErrStat2, ErrMsg2, UnEc ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - - !print *, 'at end of echo if statement' - - END IF - - - !------------------------------------------------------------------------------------------------- - ! Line Types Properties Section - !------------------------------------------------------------------------------------------------- - - CALL ReadCom( UnIn, FileName, 'Line types header', ErrStat2, ErrMsg2, UnEc ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - - - CALL ReadVar ( UnIn, FileName, p%NTypes, 'NTypes', 'Number of line types', ErrStat2, ErrMsg2, UnEc ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - - - ! Table header - DO I = 1, 2 - CALL ReadCom( UnIn, FileName, 'Line types table header', ErrStat2, ErrMsg2, UnEc ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - END DO - - ! make sure NTypes isn't zero - IF ( p%NTypes < 1 ) THEN - CALL SetErrStat( ErrID_Fatal, 'NTypes parameter must be greater than zero.', ErrStat, ErrMsg, RoutineName ) - CALL CleanUp() - RETURN - END IF - - ! Allocate memory for LineTypeList array to hold line type properties - ALLOCATE ( m%LineTypeList(p%NTypes), STAT = ErrStat2 ) - IF ( ErrStat2 /= 0 ) THEN - CALL SetErrStat( ErrID_Fatal, 'Error allocating space for LineTypeList array.', ErrStat, ErrMsg, RoutineName ) - CALL CleanUp() - RETURN - END IF - - ! read each line - DO I = 1,p%NTypes - ! read the table entries Name Diam MassDenInAir EA cIntDamp Can Cat Cdn Cdt in the MoorDyn input file - READ(UnIn,'(A)',IOSTAT=ErrStat2) Line !read into a line - - IF (ErrStat2 == 0) THEN - READ(Line,*,IOSTAT=ErrStat2) m%LineTypeList(I)%name, m%LineTypeList(I)%d, & - m%LineTypeList(I)%w, m%LineTypeList(I)%EA, m%LineTypeList(I)%BA, & - m%LineTypeList(I)%Can, m%LineTypeList(I)%Cat, m%LineTypeList(I)%Cdn, m%LineTypeList(I)%Cdt - END IF - - m%LineTypeList(I)%IdNum = I ! specify IdNum of line type for error checking - - - IF ( ErrStat2 /= ErrID_None ) THEN - CALL SetErrStat( ErrID_Fatal, 'Failed to read line type properties for line '//trim(Num2LStr(I)), ErrStat, ErrMsg, RoutineName ) - CALL CleanUp() - RETURN - END IF - - IF ( InitInp%Echo ) THEN - WRITE( UnEc, '(A)' ) TRIM(Line) - END IF - - END DO - - - - !------------------------------------------------------------------------------------------------- - ! Connections Section - !------------------------------------------------------------------------------------------------- - - CALL ReadCom( UnIn, FileName, 'Connections header', ErrStat2, ErrMsg2, UnEc ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - - - CALL ReadVar ( UnIn, FileName, p%NConnects, 'NConnects', 'Number of Connects', ErrStat2, ErrMsg2, UnEc ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - - - ! Table header - DO I = 1, 2 - CALL ReadCom( UnIn, FileName, 'Connects header', ErrStat2, ErrMsg2, UnEc ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - END DO - - ! make sure NConnects is at least two - IF ( p%NConnects < 2 ) THEN - ErrMsg = ' NConnects parameter must be at least 2.' - CALL CleanUp() - RETURN - END IF - - ! allocate ConnectList - ALLOCATE ( m%ConnectList(p%NConnects), STAT = ErrStat2 ) - IF ( ErrStat2 /= 0 ) THEN - CALL SetErrStat( ErrID_Fatal, 'Error allocating space for ConnectList array.', ErrStat, ErrMsg, RoutineName ) - CALL CleanUp() - RETURN - END IF - - - ! read each line - DO I = 1,p%NConnects - ! read the table entries Node Type X Y Z M V FX FY FZ Cda Ca - READ(UnIn,'(A)',IOSTAT=ErrStat2) Line !read into a line - - IF (ErrStat2 == 0) THEN - READ(Line,*,IOSTAT=ErrStat2) m%ConnectList(I)%IdNum, m%ConnectList(I)%type, m%ConnectList(I)%conX, & - m%ConnectList(I)%conY, m%ConnectList(I)%conZ, m%ConnectList(I)%conM, & - m%ConnectList(I)%conV, m%ConnectList(I)%conFX, m%ConnectList(I)%conFY, & - m%ConnectList(I)%conFZ, m%ConnectList(I)%conCdA, m%ConnectList(I)%conCa - END IF - - IF ( ErrStat2 /= 0 ) THEN - CALL WrScr(' Unable to parse Connection '//trim(Num2LStr(I))//' row in input file.') ! Specific screen output because errors likely - CALL WrScr(' Ensure row has all 12 columns, including CdA and Ca.') ! to be caused by non-updated input file formats. - CALL SetErrStat( ErrID_Fatal, 'Failed to read connects.' , ErrStat, ErrMsg, RoutineName ) ! would be nice to specify which line <<<<<<<<< - CALL CleanUp() - RETURN - END IF - - ! check for sequential IdNums - IF ( m%ConnectList(I)%IdNum .NE. I ) THEN - CALL SetErrStat( ErrID_Fatal, 'Node numbers must be sequential starting from 1.', ErrStat, ErrMsg, RoutineName ) - CALL CleanUp() - RETURN - END IF - - - - - IF ( InitInp%Echo ) THEN - WRITE( UnEc, '(A)' ) TRIM(Line) - END IF - - END DO - - - !------------------------------------------------------------------------------------------------- - ! Lines Section - !------------------------------------------------------------------------------------------------- - - CALL ReadCom( UnIn, FileName, 'Lines header', ErrStat2, ErrMsg2, UnEc ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - - - CALL ReadVar ( UnIn, FileName, p%NLines, 'NLines', 'Number of Lines', ErrStat2, ErrMsg2, UnEc ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - - - ! Table header - DO I = 1, 2 - CALL ReadCom( UnIn, FileName, 'Lines header', ErrStat2, ErrMsg2, UnEc ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - END DO - - ! make sure NLines is at least one - IF ( p%NLines < 1 ) THEN - CALL SetErrStat( ErrID_Fatal, 'NLines parameter must be at least 1.', ErrStat, ErrMsg, RoutineName ) - CALL CleanUp() - RETURN - END IF - - ! allocate LineList - ALLOCATE ( m%LineList(p%NLines), STAT = ErrStat2 ) - IF ( ErrStat2 /= 0 ) THEN - CALL SetErrStat( ErrID_Fatal, 'Error allocating space for LineList array.', ErrStat, ErrMsg, RoutineName ) - CALL CleanUp() - RETURN - END IF - - ! read each line - DO I = 1,p%NLines - ! read the table entries Line LineType UnstrLen NumSegs NodeAnch NodeFair Flags/Outputs - READ(UnIn,'(A)',IOSTAT=ErrStat2) Line !read into a line - - - IF (ErrStat2 == 0) THEN - READ(Line,*,IOSTAT=ErrStat2) m%LineList(I)%IdNum, m%LineList(I)%type, m%LineList(I)%UnstrLen, & - m%LineList(I)%N, m%LineList(I)%AnchConnect, m%LineList(I)%FairConnect, LineOutString, m%LineList(I)%CtrlChan - END IF - - IF ( ErrStat2 /= 0 ) THEN - CALL SetErrStat( ErrID_Fatal, 'Failed to read line data for Line '//trim(Num2LStr(I)), ErrStat, ErrMsg, RoutineName ) - CALL CleanUp() - RETURN - END IF - - - ! check for sequential IdNums - IF ( m%LineList(I)%IdNum .NE. I ) THEN - CALL SetErrStat( ErrID_Fatal, 'Line numbers must be sequential starting from 1.', ErrStat, ErrMsg, RoutineName ) - CALL CleanUp() - RETURN - END IF - - ! identify index of line type - DO J = 1,p%NTypes - IF (trim(m%LineList(I)%type) == trim(m%LineTypeList(J)%name)) THEN - m%LineList(I)%PropsIdNum = J - EXIT - IF (J == p%NTypes) THEN ! call an error if there is no match - CALL SetErrStat( ErrID_Severe, 'Unable to find matching line type name for Line '//trim(Num2LStr(I)), ErrStat, ErrMsg, RoutineName ) - END IF - END IF - END DO - - ! process output flag characters (LineOutString) and set line output flag array (OutFlagList) - m%LineList(I)%OutFlagList = 0 ! first set array all to zero - IF ( scan( LineOutString, 'p') > 0 ) m%LineList(I)%OutFlagList(2) = 1 - IF ( scan( LineOutString, 'v') > 0 ) m%LineList(I)%OutFlagList(3) = 1 - IF ( scan( LineOutString, 'U') > 0 ) m%LineList(I)%OutFlagList(4) = 1 - IF ( scan( LineOutString, 'D') > 0 ) m%LineList(I)%OutFlagList(5) = 1 - IF ( scan( LineOutString, 't') > 0 ) m%LineList(I)%OutFlagList(6) = 1 - IF ( scan( LineOutString, 'c') > 0 ) m%LineList(I)%OutFlagList(7) = 1 - IF ( scan( LineOutString, 's') > 0 ) m%LineList(I)%OutFlagList(8) = 1 - IF ( scan( LineOutString, 'd') > 0 ) m%LineList(I)%OutFlagList(9) = 1 - IF ( scan( LineOutString, 'l') > 0 ) m%LineList(I)%OutFlagList(10)= 1 - IF (SUM(m%LineList(I)%OutFlagList) > 0) m%LineList(I)%OutFlagList(1) = 1 ! this first entry signals whether to create any output file at all - ! the above letter-index combinations define which OutFlagList entry corresponds to which output type - + ! read in stiffness/damping coefficient or load nonlinear data file if applicable + SUBROUTINE getCoefficientOrCurve(inputString, LineProp_c, LineProp_npoints, LineProp_Xs, LineProp_Ys, ErrStat3, ErrMsg3) - ! check errors - IF ( ErrStat /= ErrID_None ) THEN - ErrMsg = ' Failed to read line data for Line '//trim(Num2LStr(I)) - CALL CleanUp() - RETURN - END IF - - - IF ( InitInp%Echo ) THEN - WRITE( UnEc, '(A)' ) TRIM(Line) - END IF - - END DO ! I - - - !------------------------------------------------------------------------------------------------- - ! Read any options lines - !------------------------------------------------------------------------------------------------- + CHARACTER(40), INTENT(IN ) :: inputString + REAL(DbKi), INTENT(INOUT) :: LineProp_c + INTEGER(IntKi), INTENT( OUT) :: LineProp_nPoints + REAL(DbKi), INTENT( OUT) :: LineProp_Xs (nCoef) + REAL(DbKi), INTENT( OUT) :: LineProp_Ys (nCoef) + + INTEGER(IntKi), INTENT( OUT) :: ErrStat3 ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg3 ! Error message if ErrStat /= ErrID_None - CALL ReadCom( UnIn, FileName, 'Options header', ErrStat2, ErrMsg2, UnEc ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF + INTEGER(IntKi) :: nC, I + INTEGER(IntKi) :: UnCoef ! unit number for coefficient input file + + + INTEGER(IntKi) :: ErrStat4 + CHARACTER(120) :: ErrMsg4 + CHARACTER(120) :: Line2 + + + if (SCAN(inputString, "abcdfghijklmnopqrstuvwxyzABCDFGHIJKLMNOPQRSTUVWXYZ") == 0) then ! "eE" are exluded as they're used for scientific notation! + + ! "found NO letter in the line coefficient value so treating it as a number." + READ(inputString, *, IOSTAT=ErrStat4) LineProp_c ! convert the entry string into a real number + LineProp_npoints = 0; + + else ! otherwise interpet the input as a file name to load stress-strain lookup data from + + print *, "found A letter in the line coefficient value so will try to load the filename." + + LineProp_c = 0.0 + + ! load lookup table data from file + + CALL GetNewUnit( UnCoef ) + CALL OpenFInpFile( UnCoef, TRIM(inputString), ErrStat4, ErrMsg4 ) ! add error handling? + + READ(UnCoef,'(A)',IOSTAT=ErrStat4) Line2 ! skip the first three lines (title, names, and units) then parse + READ(UnCoef,'(A)',IOSTAT=ErrStat4) Line2 + READ(UnCoef,'(A)',IOSTAT=ErrStat4) Line2 + + DO I = 1, nCoef + + READ(UnCoef,'(A)',IOSTAT=ErrStat4) Line2 !read into a line - ! loop through any remaining input lines, and use them to set options (overwriting default values in many cases). - ! doing this manually since I'm not sure that there is a built in subroutine for reading any input value on any line number. - DO - - READ(UnIn,'(A)',IOSTAT=ErrStat2) Line !read into a line - - IF (ErrStat2 == 0) THEN - IF (( Line(1:3) == '---' ) .OR. ( Line(1:3) == 'END' ) .OR. ( Line(1:3) == 'end' )) EXIT ! check if it's the end line - - READ(Line,*,IOSTAT=ErrStat2) OptValue, OptString ! look at first two entries, ignore remaining words in line, which should be comments - END IF - - IF ( ErrStat2 /= 0 ) THEN - CALL SetErrStat( ErrID_Fatal, 'Failed to read options.', ErrStat, ErrMsg, RoutineName ) ! would be nice to specify which line had the error - CALL CleanUp() - RETURN - END IF - - CALL Conv2UC(OptString) - - ! check all possible options types and see if OptString is one of them, in which case set the variable. - if ( OptString == 'DTM') THEN - read (OptValue,*) p%dtM0 ! InitInp%DTmooring - else if ( OptString == 'G') then - read (OptValue,*) p%G - else if ( OptString == 'RHOW') then - read (OptValue,*) p%rhoW - else if ( OptString == 'WTRDPTH') then - read (OptValue,*) p%WtrDpth - else if ( OptString == 'KBOT') then - read (OptValue,*) p%kBot - else if ( OptString == 'CBOT') then - read (OptValue,*) p%cBot - else if ( OptString == 'DTIC') then - read (OptValue,*) InitInp%dtIC - else if ( OptString == 'TMAXIC') then - read (OptValue,*) InitInp%TMaxIC - else if ( OptString == 'CDSCALEIC') then - read (OptValue,*) InitInp%CdScaleIC - else if ( OptString == 'THRESHIC') then - read (OptValue,*) InitInp%threshIC - else - CALL SetErrStat( ErrID_Warn, 'unable to interpret input '//trim(OptString), ErrStat, ErrMsg, RoutineName ) - end if - - IF ( InitInp%Echo ) THEN - WRITE( UnEc, '(A)' ) TRIM(Line) - END IF - - END DO - - - !------------------------------------------------------------------------------------------------- - ! Read the FAST-style outputs list in the final section, if there is one - !------------------------------------------------------------------------------------------------- - ! we don't read in the outputs header line because it's already been read in for detecting the end of the variable-length options section - ! CALL ReadCom( UnIn, FileName, 'Outputs header', ErrStat, ErrMsg, UnEc ) - - ! allocate InitInp%Outliest (to a really big number for now...) - CALL AllocAry( InitInp%OutList, 1000, "MoorDyn Input File's Outlist", ErrStat2, ErrMsg2 ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() + IF (ErrStat4 > 0) EXIT + + READ(Line2,*,IOSTAT=ErrStat4) LineProp_Xs(I), LineProp_Ys(I) + + END DO + + if (I < 2) then + ErrStat3 = ErrID_Fatal + ErrMsg3 = "Less than the minimum of 2 data lines found in file "//TRIM(inputString)//" (first 3 lines are headers)." + Close (UnCoef) RETURN - END IF - - ! OutList - List of user-requested output channels (-): - CALL ReadOutputList ( UnIn, FileName, InitInp%OutList, p%NumOuts, 'OutList', "List of user-requested output channels", ErrStat2, ErrMsg2, UnEc ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - - !print *, 'NumOuts is ', p%NumOuts - !print *, ' OutList is ', InitInp%OutList(1:p%NumOuts) - - - !------------------------------------------------------------------------------------------------- - ! This is the end of the input file - !------------------------------------------------------------------------------------------------- - - CALL CleanUp() - - CONTAINS - ! subroutine to set ErrState and close the files if an error occurs - SUBROUTINE CleanUp() + else + LineProp_npoints = I; + Close (UnCoef) + end if + + END IF + + END SUBROUTINE getCoefficientOrCurve + + + - ! ErrStat = ErrID_Fatal - CLOSE( UnIn ) - IF (InitInp%Echo) CLOSE( UnEc ) + ! Split a string into separate letter strings and integers. Letters are converted to uppercase. + SUBROUTINE DecomposeString(outWord, let1, num1, let2, num2, let3) + + CHARACTER(*), INTENT(INOUT) :: outWord + CHARACTER(25), INTENT( OUT) :: let1 + ! INTEGER(IntKi), INTENT( OUT) :: num1 + CHARACTER(25), INTENT( OUT) :: num1 + CHARACTER(25), INTENT( OUT) :: let2 + CHARACTER(25), INTENT( OUT) :: num2 +! INTEGER(IntKi), INTENT( OUT) :: num2 + CHARACTER(25), INTENT( OUT) :: let3 + + INTEGER(IntKi) :: I ! Generic loop-counting index + + CHARACTER(ChanLen) :: OutListTmp ! A string to temporarily hold OutList(I), the name of each output channel + CHARACTER(ChanLen) :: qVal ! quantity type string to match to list of valid options + + INTEGER :: oID ! ID number of connect or line object + INTEGER :: nID ! ID number of node object + INTEGER :: i1 = 0 ! indices of start of numbers or letters in OutListTmp string, for parsing + INTEGER :: i2 = 0 + INTEGER :: i3 = 0 + INTEGER :: i4 = 0 - END SUBROUTINE + + CALL Conv2UC(outWord) ! convert to all uppercase for string matching purposes - END SUBROUTINE MDIO_ReadInput - ! ==================================================================================================== + ! start these strings as empty, and fill in only if used + let1 = '' + num1 = '' + let2 = '' + num2 = '' + let3 = '' + ! find indicies of changes in number-vs-letter in characters of outWord and split into segments accordingly + + i1 = scan( outWord , '1234567890' ) ! find index of first number in the string + if (i1 > 0) then ! if there is a number + let1 = TRIM(outWord( 1:i1-1)) + i2 = i1+verify( outWord(i1+1:) , '1234567890' ) ! find starting index of second set of letters (if first character is a letter, i.e. i1>1), otherwise index of first letter + if (i2 > i1) then ! if there is a second letter/word + num1 = TRIM(outWord(i1:i2-1)) + i3 = i2+scan( outWord(i2+1:) , '1234567890' ) ! find starting index of second set of numbers <<<< + if (i3 > i2) then ! if there is a second number + let2 = TRIM(outWord(i2:i3-1)) + i4 = i3+verify( outWord(i3+1:) , '1234567890' ) ! third letter start + if (i4 > i3) then ! if there is a third letter/word + num2 = TRIM(outWord(i3:i4-1)) + let3 = TRIM(outWord(i4: )) + else + num2 = TRIM(outWord(i3:)) + end if + else + let2 = TRIM(outWord(i2:)) + end if + else + num1 = TRIM(outWord(i1:)) + end if + else + let1 = TRIM(outWord) + end if + + + !READ(outWord(i1:i2-1)) num1 + !READ(outWord(i3:i4-1)) num2 + + ! print *, "Decomposed string ", outWord, " into:" + ! print *, let1 + ! print *, num1 + ! print *, let2 + ! print *, num2 + ! print *, let3 + ! print *, "based on indices (i1-i4):" + ! print *, i1 + ! print *, i2 + ! print *, i3 + ! print *, i4 + + END SUBROUTINE DecomposeString + ! ==================================================================================================== @@ -645,7 +285,16 @@ SUBROUTINE MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat, ErrMsg ) INTEGER :: oID ! ID number of connect or line object INTEGER :: nID ! ID number of node object INTEGER :: i1,i2,i3,i4 ! indices of start of numbers or letters in OutListTmp string, for parsing - + + CHARACTER(25) :: let1 ! strings used for splitting and parsing identifiers + CHARACTER(25) :: num1 + CHARACTER(25) :: let2 + CHARACTER(25) :: num2 + CHARACTER(25) :: let3 + + INTEGER(IntKi) :: LineNumOuts ! number of entries in LineWrOutput for each line + INTEGER(IntKi) :: RodNumOuts ! same for Rods + ! see the top of the module for info on the output labelling types @@ -680,74 +329,100 @@ SUBROUTINE MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat, ErrMsg ) DO I = 1,p%NumOuts OutListTmp = OutList(I) ! current requested output name + + call DecomposeString(OutListTmp, let1, num1, let2, num2, let3) + + + !p%OutParam(I)%Name = OutListTmp CALL Conv2UC(OutListTmp) ! convert to all uppercase for string matching purposes - ! find indicies of changes in number-vs-letter in characters of OutListTmp - i1 = scan( OutListTmp , '1234567890' ) ! first number in the string - i2 = i1+verify( OutListTmp(i1+1:) , '1234567890' ) ! second letter start (assuming first character is a letter, i.e. i1>1) - i3 = i2+scan( OutListTmp(i2+1:) , '1234567890' ) ! second number start - i4 = i3+verify( OutListTmp(i3+1:) , '1234567890' ) ! third letter start - !i5 = scan( OutListTmp(i1:) , '1234567890' ) ! find first letter after first number - + ! ! find indicies of changes in number-vs-letter in characters of OutListTmp + ! i1 = scan( OutListTmp , '1234567890' ) ! first number in the string + ! i2 = i1+verify( OutListTmp(i1+1:) , '1234567890' ) ! second letter start (assuming first character is a letter, i.e. i1>1) + ! i3 = i2+scan( OutListTmp(i2+1:) , '1234567890' ) ! second number start + ! i4 = i3+verify( OutListTmp(i3+1:) , '1234567890' ) ! third letter start + ! error check - IF (i1 <= 1) THEN - CALL DenoteInvalidOutput(p%OutParam(I)) ! flag as invalid - CALL WrScr('Warning: invalid output specifier '//trim(OutListTmp)//'. Starting character must be C or L.') - CYCLE ! <<<<<<<<<<< check correct usage - END IF + ! IF (i1 <= 1) THEN + ! CALL DenoteInvalidOutput(p%OutParam(I)) ! flag as invalid + ! CALL WrScr('Warning: invalid output specifier '//trim(OutListTmp)//'. Starting character must be C or L.') + ! CYCLE ! <<<<<<<<<<< check correct usage + ! END IF p%OutParam(I)%Name = OutListTmp ! label channel with whatever name was inputted, for now ! figure out what type of output it is and process accordingly - ! fairlead tension case (updated) - IF (OutListTmp(1:i1-1) == 'FAIRTEN') THEN + ! fairlead tension case (updated) <<<<<<<<<<<<<<<<<<<<<<<<<<< these are not currently working - need new way to find ObjID + IF (let1 == 'FAIRTEN') THEN p%OutParam(I)%OType = 2 ! connection object type p%OutParam(I)%QType = Ten ! tension quantity type p%OutParam(I)%Units = UnitList(Ten) ! set units according to QType - READ (OutListTmp(i1:),*) oID ! this is the line number + READ (num1,*) oID ! this is the line number p%OutParam(I)%ObjID = m%LineList(oID)%FairConnect ! get the connection ID of the fairlead p%OutParam(I)%NodeID = -1 ! not used. m%LineList(oID)%N ! specify node N (fairlead) + print *, "WARNING - FAIRTEN and ANCHTEN results aren't supported yet in MD v2" ! achor tension case - ELSE IF (OutListTmp(1:i1-1) == 'ANCHTEN') THEN + ELSE IF (let1 == 'ANCHTEN') THEN p%OutParam(I)%OType = 2 ! connectoin object type p%OutParam(I)%QType = Ten ! tension quantity type p%OutParam(I)%Units = UnitList(Ten) ! set units according to QType - READ (OutListTmp(i1:),*) oID ! this is the line number + READ (num1,*) oID ! this is the line number p%OutParam(I)%ObjID = m%LineList(oID)%AnchConnect ! get the connection ID of the fairlead p%OutParam(I)%NodeID = -1 ! not used. m%LineList(oID)%0 ! specify node 0 (anchor) + print *, "WARNING - FAIRTEN and ANCHTEN results aren't supported yet in MD v2" ! more general case ELSE ! what object type? - ! Line case ... L?N?xxxx - IF (OutListTmp(1:i1-1) == 'L') THEN + + ! Line case + IF (let1(1:1) == 'L') THEN ! Look for L?N?xxxx p%OutParam(I)%OType = 1 ! Line object type - ! for now we'll just assume the next character(s) are "n" to represent node number: - READ (OutListTmp(i3:i4-1),*) nID + ! for now we'll just assume the next character(s) are "n" to represent node number or "s" to represent segment number + READ (num2,*) nID ! node or segment ID p%OutParam%NodeID = nID - qVal = OutListTmp(i4:) ! isolate quantity type string - ! Connect case ... C?xxx or Con?xxx - ELSE IF (OutListTmp(1:1) == 'C') THEN + + qVal = let3 ! quantity type string + + ! Connect case + ELSE IF (let1(1:1) == 'C') THEN ! Look for C?xxx or Con?xxx p%OutParam(I)%OType = 2 ! Connect object type - qVal = OutListTmp(i2:) ! isolate quantity type string + qVal = let2 ! quantity type string + + ! Rod case + ELSE IF (let1(1:1) == 'R') THEN ! Look for R?xxx or Rod?xxx + p%OutParam(I)%OType = 3 ! Rod object type + IF (LEN_TRIM(let3)== 0) THEN ! No third character cluster indicates this is a whole-rod channel + p%OutParam%NodeID = 0 + qVal = let2 ! quantity type string + ELSE + READ (num2,*) nID ! rod node ID + p%OutParam%NodeID = nID + qVal = let3 ! quantity type string + END IF + + ! Body case + ELSE IF (Let1(1:1) == 'B') THEN ! Look for B?xxx or Body?xxx + p%OutParam(I)%OType = 4 ! Body object type + qVal = let2 ! quantity type string ! should do fairlead option also! ! error ELSE CALL DenoteInvalidOutput(p%OutParam(I)) ! flag as invalid - CALL WrScr('Warning: invalid output specifier '//trim(OutListTmp)//'. Type must be L or C.') + CALL WrScr('Warning: invalid output specifier '//trim(OutListTmp)//'. Must start with L, C, R, or B') CYCLE END IF ! object number - READ (OutListTmp(i1:i2-1),*) oID - p%OutParam(I)%ObjID = oID ! line or connect ID number + READ (num1,*) oID + p%OutParam(I)%ObjID = oID ! line or connect ID number ! which kind of quantity? IF (qVal == 'PX') THEN @@ -789,6 +464,15 @@ SUBROUTINE MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat, ErrMsg ) ELSE IF (qVal == 'FZ') THEN p%OutParam(I)%QType = FZ p%OutParam(I)%Units = UnitList(FZ) + ELSE IF (qVal == 'ROLL') THEN + p%OutParam(I)%QType = Roll + p%OutParam(I)%Units = UnitList(Roll) + ELSE IF (qVal == 'PITCH') THEN + p%OutParam(I)%QType = Pitch + p%OutParam(I)%Units = UnitList(Pitch) + ELSE IF (qVal == 'YAW') THEN + p%OutParam(I)%QType = Yaw + p%OutParam(I)%Units = UnitList(Yaw) ELSE CALL DenoteInvalidOutput(p%OutParam(I)) ! flag as invalid CALL WrScr('Warning: invalid output specifier '//trim(OutListTmp)//'. Quantity type not recognized.') @@ -798,23 +482,44 @@ SUBROUTINE MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat, ErrMsg ) END IF ! also check whether each object index and node index (if applicable) is in range - IF (p%OutParam(I)%OType==2) THEN + + IF (p%OutParam(I)%OType==1) THEN ! Line + IF (p%OutParam(I)%ObjID > p%NLines) THEN + CALL WrScr('Warning: output Line index excedes number of Lines in requested output '//trim(OutListTmp)//'.') + CALL DenoteInvalidOutput(p%OutParam(I)) ! flag as invalid + END IF + IF (p%OutParam(I)%NodeID > m%LineList(p%OutParam(I)%ObjID)%N) THEN + CALL WrScr('Warning: output node index excedes number of nodes in requested output '//trim(OutListTmp)//'.') + CALL DenoteInvalidOutput(p%OutParam(I)) ! flag as invalid + ELSE IF (p%OutParam(I)%NodeID < 0) THEN + CALL WrScr('Warning: output node index is less than zero in requested output '//trim(OutListTmp)//'.') + CALL DenoteInvalidOutput(p%OutParam(I)) ! flag as invalid + END IF + + ELSE IF (p%OutParam(I)%OType==2) THEN ! Connect IF (p%OutParam(I)%ObjID > p%NConnects) THEN CALL WrScr('Warning: output Connect index excedes number of Connects in requested output '//trim(OutListTmp)//'.') CALL DenoteInvalidOutput(p%OutParam(I)) ! flag as invalid END IF - ELSE IF (p%OutParam(I)%OType==1) THEN - IF (p%OutParam(I)%ObjID > p%NLines) THEN - CALL WrScr('Warning: output Line index excedes number of Lines in requested output '//trim(OutListTmp)//'.') + + ELSE IF (p%OutParam(I)%OType==3) THEN ! Rod + IF (p%OutParam(I)%ObjID > p%NRods) THEN + CALL WrScr('Warning: output Rod index excedes number of Rods in requested output '//trim(OutListTmp)//'.') CALL DenoteInvalidOutput(p%OutParam(I)) ! flag as invalid END IF - IF (p%OutParam(I)%NodeID > m%LineList(p%OutParam(I)%ObjID)%N) THEN + IF (p%OutParam(I)%NodeID > m%RodList(p%OutParam(I)%ObjID)%N) THEN CALL WrScr('Warning: output node index excedes number of nodes in requested output '//trim(OutListTmp)//'.') CALL DenoteInvalidOutput(p%OutParam(I)) ! flag as invalid ELSE IF (p%OutParam(I)%NodeID < 0) THEN CALL WrScr('Warning: output node index is less than zero in requested output '//trim(OutListTmp)//'.') CALL DenoteInvalidOutput(p%OutParam(I)) ! flag as invalid END IF + + ELSE IF (p%OutParam(I)%OType==4) THEN ! Body + IF (p%OutParam(I)%ObjID > p%NBodies) THEN + CALL WrScr('Warning: output Body index excedes number of Bodies in requested output '//trim(OutListTmp)//'.') + CALL DenoteInvalidOutput(p%OutParam(I)) ! flag as invalid + END IF END IF @@ -855,13 +560,36 @@ SUBROUTINE MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat, ErrMsg ) ! allocate output array in each Line DO I=1,p%NLines - ALLOCATE(m%LineList(I)%LineWrOutput( 1 + 3*(m%LineList(I)%N + 1)*SUM(m%LineList(I)%OutFlagList(2:5)) + m%LineList(I)%N*SUM(m%LineList(I)%OutFlagList(6:10)) ), STAT = ErrStat) + + + ! calculate number of output entries (excluding time) to write for this line + LineNumOuts = 3*(m%LineList(I)%N + 1)*SUM(m%LineList(I)%OutFlagList(2:6)) & + + (m%LineList(I)%N + 1)*SUM(m%LineList(I)%OutFlagList(7:9)) & + + m%LineList(I)%N*SUM(m%LineList(I)%OutFlagList(10:18)) + + ALLOCATE(m%LineList(I)%LineWrOutput( 1 + LineNumOuts), STAT = ErrStat) IF ( ErrStat /= ErrID_None ) THEN ErrMsg = ' Error allocating space for a LineWrOutput array' ErrStat = ErrID_Fatal RETURN END IF END DO ! I + + ! allocate output array in each Rod + DO I=1,p%NRods + + ! calculate number of output entries (excluding time) to write for this Rod + RodNumOuts = 3*(m%RodList(I)%N + 1)*SUM(m%RodList(I)%OutFlagList(2:9)) & + + (m%RodList(I)%N + 1)*SUM(m%RodList(I)%OutFlagList(10:11)) & + + m%RodList(I)%N*SUM(m%RodList(I)%OutFlagList(12:18)) + + ALLOCATE(m%RodList(I)%RodWrOutput( 1 + RodNumOuts), STAT = ErrStat) + IF ( ErrStat /= ErrID_None ) THEN + ErrMsg = ' Error allocating space for a RodWrOutput array' + ErrStat = ErrID_Fatal + RETURN + END IF + END DO ! I !print *, "y%WriteOutput allocated to size ", size(y%WriteOutput) @@ -888,13 +616,13 @@ SUBROUTINE DenoteInvalidOutput( OutParm ) END SUBROUTINE DenoteInvalidOutput END SUBROUTINE MDIO_ProcessOutList - !==================================================================================================== + !----------------------------------------------------------------------------------------============ - !==================================================================================================== + !----------------------------------------------------------------------------------------============ SUBROUTINE MDIO_OpenOutput( OutRootName, p, m, InitOut, ErrStat, ErrMsg ) !---------------------------------------------------------------------------------------------------- @@ -908,8 +636,9 @@ SUBROUTINE MDIO_OpenOutput( OutRootName, p, m, InitOut, ErrStat, ErrMsg ) INTEGER :: I ! Generic loop counter INTEGER :: J ! Generic loop counter CHARACTER(1024) :: OutFileName ! The name of the output file including the full path. -! INTEGER :: L ! counter for index in LineWrOutput - INTEGER :: LineNumOuts ! number of entries in LineWrOutput for each line + INTEGER :: L ! counter for index in LineWrOutput + INTEGER :: LineNumOuts ! number of entries in LineWrOutput for each line + INTEGER :: RodNumOuts ! for Rods ... redundant <<< CHARACTER(200) :: Frmt ! a string to hold a format statement INTEGER :: ErrStat2 @@ -917,7 +646,7 @@ SUBROUTINE MDIO_OpenOutput( OutRootName, p, m, InitOut, ErrStat, ErrMsg ) ErrStat = ErrID_None ErrMsg = "" - p%Delim = ' ' ! for now + p%Delim = ' ' ! for now !------------------------------------------------------------------------------------------------- ! Open the output file, if necessary, and write the header @@ -939,7 +668,7 @@ SUBROUTINE MDIO_OpenOutput( OutRootName, p, m, InitOut, ErrStat, ErrMsg ) !Write the names of the output parameters: - Frmt = '(A10,'//TRIM(Int2LStr(p%NumOuts))//'(A1,A10))' + Frmt = '(A10,'//TRIM(Int2LStr(p%NumOuts))//'(A1,A12))' WRITE(p%MDUnOut,Frmt, IOSTAT=ErrStat2) TRIM( 'Time' ), ( p%Delim, TRIM( p%OutParam(I)%Name), I=1,p%NumOuts ) @@ -975,90 +704,123 @@ SUBROUTINE MDIO_OpenOutput( OutRootName, p, m, InitOut, ErrStat, ErrMsg ) END IF - ! calculate number of output entries (including time) to write for this line - LineNumOuts = 1 + 3*(m%LineList(I)%N + 1)*SUM(m%LineList(I)%OutFlagList(2:5)) + m%LineList(I)%N*SUM(m%LineList(I)%OutFlagList(6:10)) - - Frmt = '(A10,'//TRIM(Int2LStr(LineNumOuts))//'(A1,A10))' ! should evenutally use user specified format? - !Frmt = '(A10,'//TRIM(Int2LStr(3+3*m%LineList(I)%N))//'(A1,A10))' + ! calculate number of output entries (excluding time) to write for this line + LineNumOuts = 3*(m%LineList(I)%N + 1)*SUM(m%LineList(I)%OutFlagList(2:6)) & + + (m%LineList(I)%N + 1)*SUM(m%LineList(I)%OutFlagList(7:9)) & + + m%LineList(I)%N*SUM(m%LineList(I)%OutFlagList(10:18)) + + PRINT *, LineNumOuts, " output channels" + + Frmt = '(A10,'//TRIM(Int2LStr(1 + LineNumOuts))//'(A1,A12))' ! should evenutally use user specified format? + !Frmt = '(A10,'//TRIM(Int2LStr(3+3*m%LineList(I)%N))//'(A1,A12))' ! Write the names of the output parameters: (these use "implied DO" loops) WRITE(m%LineList(I)%LineUnOut,'(A10)', advance='no', IOSTAT=ErrStat2) TRIM( 'Time' ) IF (m%LineList(I)%OutFlagList(2) == 1) THEN - WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & ( p%Delim, 'Node'//TRIM(Int2Lstr(J))//'px', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'py', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'pz', J=0,(m%LineList(I)%N) ) END IF IF (m%LineList(I)%OutFlagList(3) == 1) THEN - WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & ( p%Delim, 'Node'//TRIM(Int2Lstr(J))//'vx', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'vy', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'vz', J=0,(m%LineList(I)%N) ) END IF IF (m%LineList(I)%OutFlagList(4) == 1) THEN - WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & ( p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Ux', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Uy', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Uz', J=0,(m%LineList(I)%N) ) END IF IF (m%LineList(I)%OutFlagList(5) == 1) THEN - WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & ( p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Dx', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Dy', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Dz', J=0,(m%LineList(I)%N) ) END IF IF (m%LineList(I)%OutFlagList(6) == 1) THEN - WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & - ( p%Delim, 'Seg'//TRIM(Int2Lstr(J))//'Ten', J=1,(m%LineList(I)%N) ) + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Node'//TRIM(Int2Lstr(J))//'bx', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'by', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'bz', J=0,(m%LineList(I)%N) ) END IF + IF (m%LineList(I)%OutFlagList(7) == 1) THEN - WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & - ( p%Delim, 'Seg'//TRIM(Int2Lstr(J))//'Dmp', J=1,(m%LineList(I)%N) ) + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Wz', J=0,(m%LineList(I)%N) ) END IF IF (m%LineList(I)%OutFlagList(8) == 1) THEN - WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Kurv', J=0,(m%LineList(I)%N) ) + END IF + + IF (m%LineList(I)%OutFlagList(10) == 1) THEN + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Seg'//TRIM(Int2Lstr(J))//'Ten', J=1,(m%LineList(I)%N) ) + END IF + IF (m%LineList(I)%OutFlagList(11) == 1) THEN + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Seg'//TRIM(Int2Lstr(J))//'Dmp', J=1,(m%LineList(I)%N) ) + END IF + IF (m%LineList(I)%OutFlagList(12) == 1) THEN + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & ( p%Delim, 'Seg'//TRIM(Int2Lstr(J))//'Str', J=1,(m%LineList(I)%N) ) END IF - IF (m%LineList(I)%OutFlagList(9) == 1) THEN - WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & + IF (m%LineList(I)%OutFlagList(13) == 1) THEN + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & ( p%Delim, 'Seg'//TRIM(Int2Lstr(J))//'SRt', J=1,(m%LineList(I)%N) ) END IF - IF (m%LineList(I)%OutFlagList(10)== 1) THEN + IF (m%LineList(I)%OutFlagList(14)== 1) THEN WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & ( p%Delim, 'Seg'//TRIM(Int2Lstr(J))//'Lst', J=1,(m%LineList(I)%N) ) END IF WRITE(m%LineList(I)%LineUnOut,'(A1)', IOSTAT=ErrStat2) ' ' ! make line break at the end + ! Now write the units line WRITE(m%LineList(I)%LineUnOut,'(A10)', advance='no', IOSTAT=ErrStat2) TRIM( '(s)' ) IF (m%LineList(I)%OutFlagList(2) == 1) THEN - WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & ( p%Delim, '(m)', p%Delim, '(m)', p%Delim, '(m)', J=0,(m%LineList(I)%N) ) END IF IF (m%LineList(I)%OutFlagList(3) == 1) THEN - WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & ( p%Delim, '(m/s)', p%Delim, '(m/s)', p%Delim, '(m/s)', J=0,(m%LineList(I)%N) ) END IF IF (m%LineList(I)%OutFlagList(4) == 1) THEN - WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & ( p%Delim, '(m/s)', p%Delim, '(m/s)', p%Delim, '(m/s)', J=0,(m%LineList(I)%N) ) END IF IF (m%LineList(I)%OutFlagList(5) == 1) THEN - WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & ( p%Delim, '(N)', p%Delim, '(N)', p%Delim, '(N)', J=0,(m%LineList(I)%N) ) END IF IF (m%LineList(I)%OutFlagList(6) == 1) THEN - WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & - ( p%Delim, '(N)', J=1,(m%LineList(I)%N) ) + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((3+3*m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(N)', p%Delim, '(N)', p%Delim, '(N)', J=0,(m%LineList(I)%N) ) END IF + IF (m%LineList(I)%OutFlagList(7) == 1) THEN - WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & - ( p%Delim, '(N)', J=1,(m%LineList(I)%N) ) + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(Nup)', J=0,(m%LineList(I)%N) ) END IF IF (m%LineList(I)%OutFlagList(8) == 1) THEN - WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(1/m)', J=0,(m%LineList(I)%N) ) + END IF + + IF (m%LineList(I)%OutFlagList(10) == 1) THEN + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(N)', J=1,(m%LineList(I)%N) ) + END IF + IF (m%LineList(I)%OutFlagList(11) == 1) THEN + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(N)', J=1,(m%LineList(I)%N) ) + END IF + IF (m%LineList(I)%OutFlagList(12) == 1) THEN + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & ( p%Delim, '(-)', J=1,(m%LineList(I)%N) ) END IF - IF (m%LineList(I)%OutFlagList(9) == 1) THEN - WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & + IF (m%LineList(I)%OutFlagList(13) == 1) THEN + WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & ( p%Delim, '(1/s)', J=1,(m%LineList(I)%N) ) END IF - IF (m%LineList(I)%OutFlagList(10)== 1) THEN + IF (m%LineList(I)%OutFlagList(14)== 1) THEN WRITE(m%LineList(I)%LineUnOut,'('//TRIM(Int2LStr((m%LineList(I)%N)))//'(A1,A10))', advance='no', IOSTAT=ErrStat2) & ( p%Delim, '(m)', J=1,(m%LineList(I)%N) ) END IF @@ -1070,13 +832,185 @@ SUBROUTINE MDIO_OpenOutput( OutRootName, p, m, InitOut, ErrStat, ErrMsg ) END DO ! I - line number + + + !-------------------------------------------------------------------------- + ! now do the same for rod output files + !-------------------------------------------------------------------------- + + !! allocate UnLineOuts + !ALLOCATE(UnLineOuts(p%NLines)) ! should add error checking + + DO I = 1,p%NRods + + + IF (m%RodList(I)%OutFlagList(1) == 1) THEN ! only proceed if the Rod is flagged to output a file + + ! Open the file for output + OutFileName = TRIM(p%RootName)//'.Rod'//TRIM(Int2LStr(I))//'.out' + CALL GetNewUnit( m%RodList(I)%RodUnOut ) + + CALL OpenFOutFile ( m%RodList(I)%RodUnOut, OutFileName, ErrStat, ErrMsg ) + IF ( ErrStat > ErrID_None ) THEN + ErrMsg = ' Error opening Rod output file '//TRIM(ErrMsg) + ErrStat = ErrID_Fatal + RETURN + END IF + + + ! calculate number of output entries (excluding time) to write for this Rod + RodNumOuts = 3*(m%RodList(I)%N + 1)*SUM(m%RodList(I)%OutFlagList(2:9)) & + + (m%RodList(I)%N + 1)*SUM(m%RodList(I)%OutFlagList(10:11)) & + + m%RodList(I)%N*SUM(m%RodList(I)%OutFlagList(12:18)) + + PRINT *, RodNumOuts, " output channels" + + Frmt = '(A10,'//TRIM(Int2LStr(1 + RodNumOuts))//'(A1,A12))' ! should evenutally use user specified format? + !Frmt = '(A10,'//TRIM(Int2LStr(3+3*m%RodList(I)%N))//'(A1,A12))' + + ! >>> should functionalize the below <<< + + + ! Write the names of the output parameters: (these use "implied DO" loops) + + WRITE(m%RodList(I)%RodUnOut,'(A10)', advance='no', IOSTAT=ErrStat2) TRIM( 'Time' ) + IF (m%RodList(I)%OutFlagList(2) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((3+3*m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Node'//TRIM(Int2Lstr(J))//'px', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'py', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'pz', J=0,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(3) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((3+3*m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Node'//TRIM(Int2Lstr(J))//'vx', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'vy', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'vz', J=0,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(4) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((3+3*m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Ux', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Uy', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Uz', J=0,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(5) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((3+3*m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Box', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Boy', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Boz', J=0,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(6) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((3+3*m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Dx', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Dy', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Dz', J=0,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(7) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((3+3*m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Fix', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Fiy', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Fiz', J=0,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(8) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((3+3*m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Pdx', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Pdy', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Pdz', J=0,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(9) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((3+3*m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Node'//TRIM(Int2Lstr(J))//'bx', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'by', p%Delim, 'Node'//TRIM(Int2Lstr(J))//'bz', J=0,(m%RodList(I)%N) ) + END IF + + IF (m%RodList(I)%OutFlagList(10) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Wz', J=0,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(11) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Node'//TRIM(Int2Lstr(J))//'Kurv', J=0,(m%RodList(I)%N) ) + END IF + + IF (m%RodList(I)%OutFlagList(12) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Seg'//TRIM(Int2Lstr(J))//'Ten', J=1,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(13) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Seg'//TRIM(Int2Lstr(J))//'Dmp', J=1,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(14) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Seg'//TRIM(Int2Lstr(J))//'Str', J=1,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(15) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, 'Seg'//TRIM(Int2Lstr(J))//'SRt', J=1,(m%RodList(I)%N) ) + END IF + + WRITE(m%RodList(I)%RodUnOut,'(A1)', IOSTAT=ErrStat2) ' ' ! make line break at the end + + + ! Now write the units line + + WRITE(m%RodList(I)%RodUnOut,'(A10)', advance='no', IOSTAT=ErrStat2) TRIM( '(s)' ) + IF (m%RodList(I)%OutFlagList(2) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((3+3*m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(m)', p%Delim, '(m)', p%Delim, '(m)', J=0,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(3) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((3+3*m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(m/s)', p%Delim, '(m/s)', p%Delim, '(m/s)', J=0,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(4) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((3+3*m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(m/s)', p%Delim, '(m/s)', p%Delim, '(m/s)', J=0,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(5) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((3+3*m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(N)', p%Delim, '(N)', p%Delim, '(N)', J=0,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(6) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((3+3*m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(N)', p%Delim, '(N)', p%Delim, '(N)', J=0,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(7) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((3+3*m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(N)', p%Delim, '(N)', p%Delim, '(N)', J=0,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(8) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((3+3*m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(N)', p%Delim, '(N)', p%Delim, '(N)', J=0,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(9) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((3+3*m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(N)', p%Delim, '(N)', p%Delim, '(N)', J=0,(m%RodList(I)%N) ) + END IF + + IF (m%RodList(I)%OutFlagList(10) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(Nup)', J=0,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(11) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(1/m)', J=0,(m%RodList(I)%N) ) + END IF + + IF (m%RodList(I)%OutFlagList(12) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(N)', J=1,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(13) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(N)', J=1,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(14) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(-)', J=1,(m%RodList(I)%N) ) + END IF + IF (m%RodList(I)%OutFlagList(15) == 1) THEN + WRITE(m%RodList(I)%RodUnOut,'('//TRIM(Int2LStr((m%RodList(I)%N)))//'(A1,A12))', advance='no', IOSTAT=ErrStat2) & + ( p%Delim, '(1/s)', J=1,(m%RodList(I)%N) ) + END IF + + WRITE(m%RodList(I)%RodUnOut,'(A1)', IOSTAT=ErrStat2) ' ' ! make Rod break at the end + + END IF ! if rod is flagged for output file + + END DO ! I - rod number + ! need to fix error handling in this sub END SUBROUTINE MDIO_OpenOutput - !==================================================================================================== + !----------------------------------------------------------------------------------------============ - !==================================================================================================== + !----------------------------------------------------------------------------------------============ SUBROUTINE MDIO_CloseOutput ( p, m, ErrStat, ErrMsg ) ! This function cleans up after running the MoorDyn output module. ! It closes the output files and releases memory. @@ -1118,10 +1052,10 @@ SUBROUTINE MDIO_CloseOutput ( p, m, ErrStat, ErrMsg ) END DO END SUBROUTINE MDIO_CloseOutput - !==================================================================================================== + !----------------------------------------------------------------------------------------============ - !==================================================================================================== + !----------------------------------------------------------------------------------------============ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) ! This subroutine gathers the output data defined by the OutParams list and ! writes it to the output file opened in MDIO_OutInit() @@ -1138,6 +1072,7 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) INTEGER :: K ! Generic loop counter INTEGER :: L ! counter for index in LineWrOutput INTEGER :: LineNumOuts ! number of entries in LineWrOutput for each line + INTEGER :: RodNumOuts ! same for Rods CHARACTER(200) :: Frmt ! a string to hold a format statement @@ -1149,80 +1084,146 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) ErrStat = ErrID_None ErrMsg = '' END IF + + ! -------------------------------- main output file -------------------------------- + + if ( p%NumOuts > 0_IntKi ) then + + ! gather the required output quantities (INCOMPLETE!) + DO I = 1,p%NumOuts + + + IF (p%OutParam(I)%OType == 1) THEN ! if dealing with a Line output + + SELECT CASE (p%OutParam(I)%QType) + CASE (PosX) + y%WriteOutput(I) = m%LineList(p%OutParam(I)%ObjID)%r(1,p%OutParam(I)%NodeID) ! x position + CASE (PosY) + y%WriteOutput(I) = m%LineList(p%OutParam(I)%ObjID)%r(2,p%OutParam(I)%NodeID) ! y position + CASE (PosZ) + y%WriteOutput(I) = m%LineList(p%OutParam(I)%ObjID)%r(3,p%OutParam(I)%NodeID) ! z position + CASE (VelX) + y%WriteOutput(I) = m%LineList(p%OutParam(I)%ObjID)%rd(1,p%OutParam(I)%NodeID) ! x velocity + CASE (VelY) + y%WriteOutput(I) = m%LineList(p%OutParam(I)%ObjID)%rd(2,p%OutParam(I)%NodeID) ! y velocity + CASE (VelZ) + y%WriteOutput(I) = m%LineList(p%OutParam(I)%ObjID)%rd(3,p%OutParam(I)%NodeID) ! z velocity + CASE (Ten) + y%WriteOutput(I) = TwoNorm(m%LineList(p%OutParam(I)%ObjID)%T(:,p%OutParam(I)%NodeID)) ! this is actually the segment tension ( 1 < NodeID < N ) Should deal with properly! + ! ^^^^^^^^^^^^^^^^^^^^^^^^ The above should be changed to give a node-specific output including weight, as is done in the C version <<<< + CASE DEFAULT + y%WriteOutput(I) = 0.0_ReKi + ErrStat = ErrID_Warn + ErrMsg = ' Unsupported output quantity '//TRIM(Num2Lstr(p%OutParam(I)%QType))//' requested from Line '//TRIM(Num2Lstr(p%OutParam(I)%ObjID))//'.' + END SELECT + + ELSE IF (p%OutParam(I)%OType == 2) THEN ! if dealing with a Connect output + SELECT CASE (p%OutParam(I)%QType) + CASE (PosX) + y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%r(1) ! x position + CASE (PosY) + y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%r(2) ! y position + CASE (PosZ) + y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%r(3) ! z position + CASE (VelX) + y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%rd(1) ! x velocity + CASE (VelY) + y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%rd(2) ! y velocity + CASE (VelZ) + y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%rd(3) ! z velocity + CASE (Ten) + y%WriteOutput(I) = TwoNorm(m%ConnectList(p%OutParam(I)%ObjID)%Fnet) ! total force magnitude on a connect (used eg. for fairlead and anchor tensions) + CASE (FX) + y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%Fnet(1) ! total force in x - added Nov 24 + CASE (FY) + y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%Fnet(2) ! total force in y + CASE (FZ) + y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%Fnet(3) ! total force in z + CASE DEFAULT + y%WriteOutput(I) = 0.0_ReKi + ErrStat = ErrID_Warn + ErrMsg = ' Unsupported output quantity '//TRIM(Num2Lstr(p%OutParam(I)%QType))//' requested from Connection '//TRIM(Num2Lstr(p%OutParam(I)%ObjID))//'.' + END SELECT + + ELSE IF (p%OutParam(I)%OType == 3) THEN ! if dealing with a Rod output + + SELECT CASE (p%OutParam(I)%QType) + CASE (PosX) + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%r(1,p%OutParam(I)%NodeID) ! x position + CASE (PosY) + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%r(2,p%OutParam(I)%NodeID) ! y position + CASE (PosZ) + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%r(3,p%OutParam(I)%NodeID) ! z position + CASE (VelX) + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%rd(1,p%OutParam(I)%NodeID) ! x velocity + CASE (VelY) + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%rd(2,p%OutParam(I)%NodeID) ! y velocity + CASE (VelZ) + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%rd(3,p%OutParam(I)%NodeID) ! z velocity + CASE (FX) + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%F6net(1) ! total force in x - added Nov 24 + CASE (FY) + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%F6net(2) ! total force in y + CASE (FZ) + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%F6net(3) ! total force in z + CASE (Roll) + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%roll ! rod roll + CASE (Pitch) + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%pitch ! rod pitch + CASE DEFAULT + y%WriteOutput(I) = 0.0_ReKi + ErrStat = ErrID_Warn + ErrMsg = ' Unsupported output quantity '//TRIM(Num2Lstr(p%OutParam(I)%QType))//' requested from Rod '//TRIM(Num2Lstr(p%OutParam(I)%ObjID))//'.' + END SELECT + + ELSE IF (p%OutParam(I)%OType == 4) THEN ! if dealing with a Body output + SELECT CASE (p%OutParam(I)%QType) + CASE (PosX) + y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%r6(1) ! x position + CASE (PosY) + y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%r6(2) ! y position + CASE (PosZ) + y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%r6(3) ! z position + CASE (VelX) + y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%v6(1) ! x velocity + CASE (VelY) + y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%v6(2) ! y velocity + CASE (VelZ) + y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%v6(3) ! z velocity + CASE (FX) + y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%F6net(1) ! total force in x - added Nov 24 + CASE (FY) + y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%F6net(2) ! total force in y + CASE (FZ) + y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%F6net(3) ! total force in z + CASE (Roll) + y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%r6(4) ! roll + CASE (Pitch) + y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%r6(5) ! pitch + CASE (Yaw) + y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%r6(6) ! yaw + CASE DEFAULT + y%WriteOutput(I) = 0.0_ReKi + ErrStat = ErrID_Warn + ErrMsg = ' Unsupported output quantity '//TRIM(Num2Lstr(p%OutParam(I)%QType))//' requested from Body '//TRIM(Num2Lstr(p%OutParam(I)%ObjID))//'.' + END SELECT + + + ELSE ! it must be an invalid output, so write zero + y%WriteOutput(I) = 0.0_ReKi - ! Return if there are no outputs - if ( p%NumOuts < 1_IntKi ) return - - - ! gather the required output quantities (INCOMPLETE!) - DO I = 1,p%NumOuts - - IF (p%OutParam(I)%OType == 2) THEN ! if dealing with a Connect output - SELECT CASE (p%OutParam(I)%QType) - CASE (PosX) - y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%r(1) ! x position - CASE (PosY) - y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%r(2) ! y position - CASE (PosZ) - y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%r(3) ! z position - CASE (VelX) - y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%rd(1) ! x velocity - CASE (VelY) - y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%rd(2) ! y velocity - CASE (VelZ) - y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%rd(3) ! z velocity - CASE (Ten) - y%WriteOutput(I) = TwoNorm(m%ConnectList(p%OutParam(I)%ObjID)%Ftot) ! total force magnitude on a connect (used eg. for fairlead and anchor tensions) - CASE (FX) - y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%Ftot(1) ! total force in x - added Nov 24 - CASE (FY) - y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%Ftot(2) ! total force in y - CASE (FZ) - y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%Ftot(3) ! total force in z - CASE DEFAULT - y%WriteOutput(I) = 0.0_DbKi - ErrStat = ErrID_Warn - ErrMsg = ' Unsupported output quantity '//TRIM(Num2Lstr(p%OutParam(I)%QType))//' requested from Connection '//TRIM(Num2Lstr(p%OutParam(I)%ObjID))//'.' - END SELECT - - ELSE IF (p%OutParam(I)%OType == 1) THEN ! if dealing with a Line output - - SELECT CASE (p%OutParam(I)%QType) - CASE (PosX) - y%WriteOutput(I) = m%LineList(p%OutParam(I)%ObjID)%r(1,p%OutParam(I)%NodeID) ! x position - CASE (PosY) - y%WriteOutput(I) = m%LineList(p%OutParam(I)%ObjID)%r(2,p%OutParam(I)%NodeID) ! y position - CASE (PosZ) - y%WriteOutput(I) = m%LineList(p%OutParam(I)%ObjID)%r(3,p%OutParam(I)%NodeID) ! z position - CASE (VelX) - y%WriteOutput(I) = m%LineList(p%OutParam(I)%ObjID)%rd(1,p%OutParam(I)%NodeID) ! x velocity - CASE (VelY) - y%WriteOutput(I) = m%LineList(p%OutParam(I)%ObjID)%rd(2,p%OutParam(I)%NodeID) ! y velocity - CASE (VelZ) - y%WriteOutput(I) = m%LineList(p%OutParam(I)%ObjID)%rd(3,p%OutParam(I)%NodeID) ! z velocity - CASE (Ten) - y%WriteOutput(I) = TwoNorm(m%LineList(p%OutParam(I)%ObjID)%T(:,p%OutParam(I)%NodeID)) ! this is actually the segment tension ( 1 < NodeID < N ) Should deal with properly! - CASE DEFAULT - y%WriteOutput(I) = 0.0_DbKi - ErrStat = ErrID_Warn - ErrMsg = ' Unsupported output quantity '//TRIM(Num2Lstr(p%OutParam(I)%QType))//' requested from Line '//TRIM(Num2Lstr(p%OutParam(I)%ObjID))//'.' - END SELECT - - ELSE ! it must be an invalid output, so write zero - y%WriteOutput(I) = 0.0_DbKi - - END IF - - END DO ! I, loop through OutParam + END IF + END DO ! I, loop through OutParam - ! Write the output parameters to the file - Frmt = '(F10.4,'//TRIM(Int2LStr(p%NumOuts))//'(A1,e12.6))' ! should evenutally use user specified format? + ! Write the output parameters to the file - WRITE(p%MDUnOut,Frmt) Time, ( p%Delim, y%WriteOutput(I), I=1,p%NumOuts ) + Frmt = '(F10.4,'//TRIM(Int2LStr(p%NumOuts))//'(A1,e12.5))' ! should evenutally use user specified format? + WRITE(p%MDUnOut,Frmt) Time, ( p%Delim, y%WriteOutput(I), I=1,p%NumOuts ) + END IF @@ -1234,10 +1235,14 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) IF (m%LineList(I)%OutFlagList(1) == 1) THEN ! only proceed if the line is flagged to output a file ! calculate number of output entries to write for this line - LineNumOuts = 3*(m%LineList(I)%N + 1)*SUM(m%LineList(I)%OutFlagList(2:5)) + m%LineList(I)%N*SUM(m%LineList(I)%OutFlagList(6:10)) + !LineNumOuts = 3*(m%LineList(I)%N + 1)*SUM(m%LineList(I)%OutFlagList(2:5)) + m%LineList(I)%N*SUM(m%LineList(I)%OutFlagList(6:9)) + + LineNumOuts = 3*(m%LineList(I)%N + 1)*SUM(m%LineList(I)%OutFlagList(2:6)) & + + (m%LineList(I)%N + 1)*SUM(m%LineList(I)%OutFlagList(7:9)) & + + m%LineList(I)%N*SUM(m%LineList(I)%OutFlagList(10:18)) - Frmt = '(F10.4,'//TRIM(Int2LStr(LineNumOuts))//'(A1,e12.6))' ! should evenutally use user specified format? + Frmt = '(F10.4,'//TRIM(Int2LStr(LineNumOuts))//'(A1,e12.5))' ! should evenutally use user specified format? L = 1 ! start of index of line output file at first entry @@ -1270,7 +1275,7 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) IF (m%LineList(I)%OutFlagList(4) == 1) THEN DO J = 0,m%LineList(I)%N ! note index starts at zero because these are nodes DO K = 1,3 - m%LineList(I)%LineWrOutput(L) = 0.0 + m%LineList(I)%LineWrOutput(L) = m%LineList(I)%U(K,J) L = L+1 END DO END DO @@ -1288,8 +1293,36 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) END IF - ! Segment tension force (excludes damping term, just EA) + ! Node seabed contact force IF (m%LineList(I)%OutFlagList(6) == 1) THEN + DO J = 0,m%LineList(I)%N + DO K = 1,3 + m%LineList(I)%LineWrOutput(L) = m%LineList(I)%B(K,J) + L = L+1 + END DO + END DO + END IF + + + ! Node weights + IF (m%LineList(I)%OutFlagList(7) == 1) THEN + DO J = 0,m%LineList(I)%N + m%LineList(I)%LineWrOutput(L) = m%LineList(I)%W(3,J) + L = L+1 + END DO + END IF + + ! ! Node curvatures + ! IF (m%LineList(I)%OutFlagList(8) == 1) THEN + ! DO J = 0,m%LineList(I)%N + ! m%LineList(I)%LineWrOutput(L) = m%LineList(I)%W(3,J) + ! L = L+1 + ! END DO + ! END IF + + + ! Segment tension force (excludes damping term, just EA) + IF (m%LineList(I)%OutFlagList(10) == 1) THEN DO J = 1,m%LineList(I)%N m%LineList(I)%LineWrOutput(L) = TwoNorm(m%LineList(I)%T(:,J) ) L = L+1 @@ -1297,7 +1330,7 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) END IF ! Segment internal damping force - IF (m%LineList(I)%OutFlagList(7) == 1) THEN + IF (m%LineList(I)%OutFlagList(11) == 1) THEN DO J = 1,m%LineList(I)%N IF (( m%LineList(I)%Td(3,J)*m%LineList(I)%T(3,J) ) > 0) THEN ! if statement for handling sign (positive = tension) m%LineList(I)%LineWrOutput(L) = TwoNorm(m%LineList(I)%Td(:,J) ) @@ -1309,7 +1342,7 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) END IF ! Segment strain - IF (m%LineList(I)%OutFlagList(8) == 1) THEN + IF (m%LineList(I)%OutFlagList(12) == 1) THEN DO J = 1,m%LineList(I)%N m%LineList(I)%LineWrOutput(L) = m%LineList(I)%lstr(J)/m%LineList(I)%l(J) - 1.0 L = L+1 @@ -1317,7 +1350,7 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) END IF ! Segment strain rate - IF (m%LineList(I)%OutFlagList(9) == 1) THEN + IF (m%LineList(I)%OutFlagList(13) == 1) THEN DO J = 1,m%LineList(I)%N m%LineList(I)%LineWrOutput(L) = m%LineList(I)%lstrd(J)/m%LineList(I)%l(J) L = L+1 @@ -1325,13 +1358,14 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) END IF ! Segment length - IF (m%LineList(I)%OutFlagList(10) == 1) THEN + IF (m%LineList(I)%OutFlagList(14) == 1) THEN DO J = 1,m%LineList(I)%N m%LineList(I)%LineWrOutput(L) = m%LineList(I)%lstr(J) L = L+1 END DO END IF + WRITE(m%LineList(I)%LineUnOut,Frmt) Time, ( p%Delim, m%LineList(I)%LineWrOutput(J), J=1,(LineNumOuts) ) !WRITE(m%LineList(I)%LineUnOut,Frmt) Time, ( p%Delim, m%LineList(I)%LineWrOutput(J), J=1,(3+3*m%LineList(I)%N) ) @@ -1339,9 +1373,150 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) END IF ! if line output file flag is on END DO ! I + + + + !------------------------------------------------------------------------ + ! now do the outputs for each Rod! + + DO I=1,p%NRods + + IF (m%RodList(I)%OutFlagList(1) == 1) THEN ! only proceed if the line is flagged to output a file + + ! calculate number of output entries to write for this Rod + RodNumOuts = 3*(m%RodList(I)%N + 1)*SUM(m%RodList(I)%OutFlagList(2:9)) & + + (m%RodList(I)%N + 1)*SUM(m%RodList(I)%OutFlagList(10:11)) & + + m%RodList(I)%N*SUM(m%RodList(I)%OutFlagList(12:18)) + + + Frmt = '(F10.4,'//TRIM(Int2LStr(RodNumOuts))//'(A1,e12.5))' ! should evenutally use user specified format? + + L = 1 ! start of index of line output file at first entry + + ! Time + ! m%RodList(I)%RodWrOutput(L) = Time + ! L = L+1 + + ! Node positions + IF (m%RodList(I)%OutFlagList(2) == 1) THEN + DO J = 0,m%RodList(I)%N ! note index starts at zero because these are nodes + DO K = 1,3 + m%RodList(I)%RodWrOutput(L) = m%RodList(I)%r(K,J) + L = L+1 + END DO + END DO + END IF + + ! Node velocities + IF (m%RodList(I)%OutFlagList(3) == 1) THEN + DO J = 0,m%RodList(I)%N ! note index starts at zero because these are nodes + DO K = 1,3 + m%RodList(I)%RodWrOutput(L) = m%RodList(I)%rd(K,J) + L = L+1 + END DO + END DO + END IF + + + ! Node wave velocities (not implemented yet) + IF (m%RodList(I)%OutFlagList(4) == 1) THEN + DO J = 0,m%RodList(I)%N ! note index starts at zero because these are nodes + DO K = 1,3 + m%RodList(I)%RodWrOutput(L) = m%RodList(I)%U(K,J) + L = L+1 + END DO + END DO + END IF + + ! Node buoyancy forces + IF (m%RodList(I)%OutFlagList(5) == 1) THEN + DO J = 0,m%RodList(I)%N ! note index starts at zero because these are nodes + DO K = 1,3 + m%RodList(I)%RodWrOutput(L) = m%RodList(I)%Bo(K,J) + L = L+1 + END DO + END DO + END IF + + ! Node drag forces + IF (m%RodList(I)%OutFlagList(6) == 1) THEN + DO J = 0,m%RodList(I)%N ! note index starts at zero because these are nodes + DO K = 1,3 + m%RodList(I)%RodWrOutput(L) = m%RodList(I)%Dp(K,J) + m%RodList(I)%Dq(K,J) + L = L+1 + END DO + END DO + END IF + + ! Node inertia forces + IF (m%RodList(I)%OutFlagList(7) == 1) THEN + DO J = 0,m%RodList(I)%N ! note index starts at zero because these are nodes + DO K = 1,3 + m%RodList(I)%RodWrOutput(L) = m%RodList(I)%Ap(K,J) + m%RodList(I)%Aq(K,J) + L = L+1 + END DO + END DO + END IF + + ! Node dynamic pressure forces + IF (m%RodList(I)%OutFlagList(8) == 1) THEN + DO J = 0,m%RodList(I)%N ! note index starts at zero because these are nodes + DO K = 1,3 + m%RodList(I)%RodWrOutput(L) = m%RodList(I)%Pd(K,J) + L = L+1 + END DO + END DO + END IF + + ! Node seabed contact force + IF (m%RodList(I)%OutFlagList(9) == 1) THEN + DO J = 0,m%RodList(I)%N + DO K = 1,3 + m%RodList(I)%RodWrOutput(L) = m%RodList(I)%B(K,J) + L = L+1 + END DO + END DO + END IF + + + ! Node weights + IF (m%RodList(I)%OutFlagList(10) == 1) THEN + DO J = 0,m%RodList(I)%N + m%RodList(I)%RodWrOutput(L) = m%RodList(I)%W(3,J) + L = L+1 + END DO + END IF + + ! ! Node curvatures + ! IF (m%RodList(I)%OutFlagList(8) == 1) THEN + ! DO J = 0,m%RodList(I)%N + ! m%RodList(I)%RodWrOutput(L) = m%RodList(I)%W(3,J) + ! L = L+1 + ! END DO + ! END IF + + + ! Segment tension force (excludes damping term, just EA) + ! N/A + + ! Segment internal damping force + ! N/A + + ! Segment strain + ! N/A + + ! Segment strain rate + ! N/A + + + WRITE(m%RodList(I)%RodUnOut,Frmt) Time, ( p%Delim, m%RodList(I)%RodWrOutput(J), J=1,(RodNumOuts) ) + + END IF ! if line output file flag is on + + END DO ! I END SUBROUTINE MDIO_WriteOutputs - !==================================================================================================== + !----------------------------------------------------------------------------------------============ END MODULE MoorDyn_IO diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index a0716a3b61..5da24a0159 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -22,11 +22,23 @@ typedef ^ ^ ReKi PtfmInit {6} - typedef ^ ^ CHARACTER(1024) FileName - "" - "MoorDyn input file" typedef ^ ^ CHARACTER(1024) RootName - - - "RootName for writing output files" - typedef ^ ^ LOGICAL Echo - "" - "echo parameter - do we want to echo the header line describing the input file?" -typedef ^ ^ ReKi DTIC - - - "convergence check time step for IC generation" "[s]" -typedef ^ ^ ReKi TMaxIC - 120 - "maximum time to allow for getting converged ICs" "[s]" +typedef ^ ^ DbKi DTIC - - - "convergence check time step for IC generation" "[s]" +typedef ^ ^ DbKi TMaxIC - 120 - "maximum time to allow for getting converged ICs" "[s]" typedef ^ ^ ReKi CdScaleIC - 1 - "factor to scale drag coefficients by during dynamic relaxation" "[]" typedef ^ ^ ReKi threshIC - 0.01 - "convergence tolerance for ICs (0.01 means 1%)" "[]" -typedef ^ ^ CHARACTER(ChanLen) OutList {:} "" - "string containing list of output channels requested in input file" +#typedef ^ ^ CHARACTER(ChanLen) OutList {:} "" - "string containing list of output channels requested in input file" + +#typedef ^ ^ DbKi UGrid {:}{:}{:} - - "water velocities time series at each grid point" - +#typedef ^ ^ DbKi UdGrid {:}{:}{:} - - "water accelerations time series at each grid point" - +#typedef ^ ^ DbKi zetaGrid {:}{:} - - "water surface elevations time series at each grid point" - +#typedef ^ ^ DbKi PDynGrid {:}{:} - - "water dynamic pressure time series at each grid point" - + +typedef ^ ^ ReKi WaveVel {:}{:}{:} - - "" - +typedef ^ ^ ReKi WaveAcc {:}{:}{:} - - "" - +typedef ^ ^ ReKi WavePDyn {:}{:} - - "" - +typedef ^ ^ ReKi WaveElev {:}{:} - - "" - +typedef ^ ^ DbKi WaveTime {:} - - "Should this be double precision?" - + # ====================================== Internal data types ======================================================================== @@ -35,19 +47,70 @@ typedef ^ MD_LineProp IntKi IdNum - typedef ^ ^ CHARACTER(10) name - - - "name/identifier of this set of line properties" typedef ^ ^ DbKi d - - - "volume-equivalent diameter" "[m]" typedef ^ ^ DbKi w - - - "per-length weight in air" "[kg/m]" -typedef ^ ^ DbKi EA - - - "stiffness" "[N]" +typedef ^ ^ DbKi EA - - - "axial stiffness" "[N]" typedef ^ ^ DbKi BA - - - "internal damping coefficient times area" "[N-s]" +typedef ^ ^ DbKi EI - - - "bending stiffness" "[N-m]" +typedef ^ ^ DbKi Can - - - "transverse added mass coefficient" +typedef ^ ^ DbKi Cat - - - "tangential added mass coefficient" +typedef ^ ^ DbKi Cdn - - - "transverse drag coefficient" +typedef ^ ^ DbKi Cdt - - - "tangential drag coefficient" +typedef ^ ^ IntKi nEApoints - 0 - "number of values in stress-strain lookup table (0 means using constant E)" +typedef ^ ^ DbKi stiffXs {30} - - "x array for stress-strain lookup table (up to nCoef)" +typedef ^ ^ DbKi stiffYs {30} - - "y array for stress-strain lookup table" +typedef ^ ^ IntKi nBpoints - 0 - "number of values in stress-strainrate lookup table (0 means using constant c)" +typedef ^ ^ DbKi dampXs {30} - - "x array for stress-strainrate lookup table (up to nCoef)" +typedef ^ ^ DbKi dampYs {30} - - "y array for stress-strainrate lookup table " +typedef ^ ^ IntKi nEIpoints - 0 - "number of values in bending stress-strain lookup table (0 means using constant E)" +typedef ^ ^ DbKi bstiffXs {30} - - "x array for stress-strain lookup table (up to nCoef)" +typedef ^ ^ DbKi bstiffYs {30} - - "y array for stress-strain lookup table" + +# rod properties from rod dictionary input +typedef ^ MD_RodProp IntKi IdNum - - - "integer identifier of this set of rod properties" +typedef ^ ^ CHARACTER(10) name - - - "name/identifier of this set of rod properties" +typedef ^ ^ DbKi d - - - "volume-equivalent diameter" "[m]" +typedef ^ ^ DbKi w - - - "per-length weight in air" "[kg/m]" typedef ^ ^ DbKi Can - - - "transverse added mass coefficient" typedef ^ ^ DbKi Cat - - - "tangential added mass coefficient" typedef ^ ^ DbKi Cdn - - - "transverse drag coefficient" typedef ^ ^ DbKi Cdt - - - "tangential drag coefficient" +typedef ^ ^ DbKi CdEnd - - - "drag coefficient for rod end" "[-]" +typedef ^ ^ DbKi CaEnd - - - "added mass coefficient for rod end" "[-]" + +# this is the Body type, which holds data for each body object +typedef ^ MD_Body IntKi IdNum - - - "integer identifier of this Connection" +typedef ^ ^ IntKi typeNum - - - "integer identifying the type. 0=fixed, 1=vessel, 2=connect" +typedef ^ ^ IntKi AttachedC {30} - - "list of IdNums of connections attached to this body" +typedef ^ ^ IntKi AttachedR {30} - - "list of IdNums of rods attached to this body" +typedef ^ ^ IntKi nAttachedC - 0 - "number of attached connections" +typedef ^ ^ IntKi nAttachedR - 0 - "number of attached rods" +typedef ^ ^ DbKi rConnectRel {3}{30} - - "relative position of connection on body" +typedef ^ ^ DbKi r6RodRel {6}{30} - - "relative position and orientation of rod on body" +typedef ^ ^ DbKi bodyM - - - "" +typedef ^ ^ DbKi bodyV - - - "" +typedef ^ ^ DbKi bodyI {3} - - "" +typedef ^ ^ DbKi bodyCdA {6} - - "product of drag force and frontal area of connection point" "[m^2]" +typedef ^ ^ DbKi bodyCa {6} - - "added mass coefficient of connection point" "-" +typedef ^ ^ DbKi time - - - "current time" "[s]" +typedef ^ ^ DbKi r6 {6} - - "position" +typedef ^ ^ DbKi v6 {6} - - "velocity" +typedef ^ ^ DbKi a6 {6} - - "acceleration (only used for coupled bodies)" +typedef ^ ^ DbKi U {3} - - "water velocity at ref point" "[m/s]" +typedef ^ ^ DbKi Ud {3} - - "water acceleration at ref point" "[m/s^2]" +typedef ^ ^ DbKi zeta - - - "water surface elevation above ref point" "[m]" +typedef ^ ^ DbKi F6net {6} - - "total force and moment on body (excluding inertial loads)" +typedef ^ ^ DbKi M6net {6}{6} - - "total mass matrix of Body and any attached objects" +typedef ^ ^ DbKi M {6}{6} - - "rotated body 6-dof mass and inertia matrix in global orientation" +typedef ^ ^ DbKi M0 {6}{6} - - "body 6-dof mass and inertia matrix in its own frame" +typedef ^ ^ DbKi OrMat {3}{3} - - "DCM for body orientation" +typedef ^ ^ DbKi rCG {3} - - "vector in body frame from ref point to CG (before rods etc..)" # this is the Connection type, which holds data for each connection object typedef ^ MD_Connect IntKi IdNum - - - "integer identifier of this Connection" typedef ^ ^ CHARACTER(10) type - - - "type of Connect: fix, vessel, connect" -typedef ^ ^ IntKi TypeNum - - - "integer identifying the type. 0=fixed, 1=vessel, 2=connect" -typedef ^ ^ IntKi AttachedFairs {:} - - "list of IdNums of connected Line tops" -typedef ^ ^ IntKi AttachedAnchs {:} - - "list of IdNums of connected Line bottoms" +typedef ^ ^ IntKi typeNum - - - "integer identifying the type. 0=fixed, 1=vessel, 2=connect" +typedef ^ ^ IntKi Attached {10} - - "list of IdNums of lines attached to this connection node" +typedef ^ ^ IntKi Top {10} - - "list of ints specifying whether each line is attached at 1 = top/fairlead(end B), 0 = bottom/anchor(end A)" +typedef ^ ^ IntKi nAttached - 0 - "number of attached lines" typedef ^ ^ DbKi conX - - - "" typedef ^ ^ DbKi conY - - - "" typedef ^ ^ DbKi conZ - - - "" @@ -58,30 +121,109 @@ typedef ^ ^ DbKi conFY - typedef ^ ^ DbKi conFZ - - - "" typedef ^ ^ DbKi conCa - - - "added mass coefficient of connection point" "-" typedef ^ ^ DbKi conCdA - - - "product of drag force and frontal area of connection point" "[m^2]" -typedef ^ ^ DbKi Ftot {3} - - "total force on node" -typedef ^ ^ DbKi Mtot {3}{3} - - "node mass matrix, from attached lines" -typedef ^ ^ DbKi S {3}{3} - - "inverse mass matrix" "[kg]" +typedef ^ ^ DbKi time - - - "current time" "[s]" typedef ^ ^ DbKi r {3} - - "position" typedef ^ ^ DbKi rd {3} - - "velocity" +typedef ^ ^ DbKi a {3} - - "acceleration (only used for coupled points)" +typedef ^ ^ DbKi U {3} - - "water velocity at node" "[m/s]" +typedef ^ ^ DbKi Ud {3} - - "water acceleration at node" "[m/s^2]" +typedef ^ ^ DbKi zeta - - - "water surface elevation above node" "[m]" +typedef ^ ^ DbKi PDyn {:} - - "water dynamic pressure at node" "[Pa]" +typedef ^ ^ DbKi Fnet {3} - - "total force on node (excluding inertial loads)" +typedef ^ ^ DbKi M {3}{3} - - "node mass matrix, from attached lines" + +# this is the Rod type, which holds data for each Rod object +typedef ^ MD_Rod IntKi IdNum - - - "integer identifier of this Line" +typedef ^ ^ CHARACTER(10) type - - - "type of Rod. should match one of RodProp names" +typedef ^ ^ IntKi PropsIdNum - - - "the IdNum of the associated rod properties" - +typedef ^ ^ IntKi typeNum - - - "integer identifying the type. 0=fixed, 1=vessel, 2=connect" +typedef ^ ^ IntKi AttachedA {10} - - "list of IdNums of lines attached to end A" +typedef ^ ^ IntKi AttachedB {10} - - "list of IdNums of lines attached to end B" +typedef ^ ^ IntKi TopA {10} - - "list of ints specifying whether each line is attached at 1 = top/fairlead(end B), 0 = bottom/anchor(end A)" +typedef ^ ^ IntKi TopB {10} - - "list of ints specifying whether each line is attached at 1 = top/fairlead(end B), 0 = bottom/anchor(end A)" +typedef ^ ^ IntKi nAttachedA - 0 - "number of attached lines to Rod end A" +typedef ^ ^ IntKi nAttachedB - 0 - "number of attached lines to Rod end B" +typedef ^ ^ IntKi OutFlagList {20} - - "array specifying what line quantities should be output (1 vs 0)" - +typedef ^ ^ IntKi N - - - "The number of elements in the line" - +typedef ^ ^ IntKi endTypeA - - - "type of connection at end A: 0=pinned to Connection, 1=cantilevered to Rod." - +typedef ^ ^ IntKi endTypeB - - - "type of connection at end B: 0=pinned to Connection, 1=cantilevered to Rod." - +typedef ^ ^ DbKi UnstrLen - - - "length of the rod" "[m]" +typedef ^ ^ DbKi mass - - - "mass of the rod" "[kg]" +typedef ^ ^ DbKi rho - - - "density" "[kg/m3]" +typedef ^ ^ DbKi d - - - "volume-equivalent diameter" "[m]" +typedef ^ ^ DbKi Can - - - "" "[-]" +typedef ^ ^ DbKi Cat - - - "" "[-]" +typedef ^ ^ DbKi Cdn - - - "" "[-]" +typedef ^ ^ DbKi Cdt - - - "" "[-]" +typedef ^ ^ DbKi CdEnd - - - "drag coefficient for rod end" "[-]" +typedef ^ ^ DbKi CaEnd - - - "added mass coefficient for rod end" "[-]" +typedef ^ ^ DbKi time - - - "current time" "[s]" +typedef ^ ^ DbKi roll - - - "roll relative to vertical" "deg" +typedef ^ ^ DbKi pitch - - - "pitch relative to vertical" "deg" +typedef ^ ^ DbKi r {:}{:} - - "node positions" - +typedef ^ ^ DbKi rd {:}{:} - - "node velocities" - +typedef ^ ^ DbKi q {3} - - "tangent vector for rod as a whole" - +typedef ^ ^ DbKi l {:} - - "segment unstretched length" "[m]" +typedef ^ ^ DbKi V {:} - - "segment volume" "[m^3]" +typedef ^ ^ DbKi U {:}{:} - - "water velocity at node" "[m/s]" +typedef ^ ^ DbKi Ud {:}{:} - - "water acceleration at node" "[m/s^2]" +typedef ^ ^ DbKi zeta {:} - - "water surface elevation above node" "[m]" +typedef ^ ^ DbKi PDyn {:} - - "water dynamic pressure at node" "[Pa]" +typedef ^ ^ DbKi W {:}{:} - - "weight vectors" "[N]" +typedef ^ ^ DbKi Bo {:}{:} - - "buoyancy force vectors" "[N]" +typedef ^ ^ DbKi Pd {:}{:} - - "dynamic pressure force vectors" "[N]" +typedef ^ ^ DbKi Dp {:}{:} - - "node drag (transverse)" "[N]" +typedef ^ ^ DbKi Dq {:}{:} - - "node drag (axial)" "[N]" +typedef ^ ^ DbKi Ap {:}{:} - - "node added mass forcing (transverse)" "[N]" +typedef ^ ^ DbKi Aq {:}{:} - - "node added mass forcing (axial)" "[N]" +typedef ^ ^ DbKi B {:}{:} - - "node bottom contact force" "[N]" +typedef ^ ^ DbKi Fnet {:}{:} - - "total force on node" "[N]" +typedef ^ ^ DbKi M {:}{:}{:} - - "node mass matrix" "[kg]" +typedef ^ ^ DbKi Mext {3} - - "external moment vector holding sum of any externally applied moments i.e. bending lines" - +typedef ^ ^ DbKi r6 {6} - - "6 DOF position vector" - +typedef ^ ^ DbKi v6 {6} - - "6 DOF velocity vector" - +typedef ^ ^ DbKi a6 {6} - - "6 DOF acceleration vector (only used for coupled Rods)" - +typedef ^ ^ DbKi F6net {6} - - "total force and moment about end A (excluding inertial loads) that Rod may exert on whatever it's attached to" +typedef ^ ^ DbKi M6net {6}{6} - - "total mass matrix about end A of Rod and any attached Points" +typedef ^ ^ DbKi OrMat {3}{3} - - "DCM for body orientation" +typedef ^ ^ IntKi RodUnOut - - - "unit number of rod output file" +typedef ^ ^ DbKi RodWrOutput {:} - - "one row of output data for this rod" + # this is the Line type, which holds data for each line object typedef ^ MD_Line IntKi IdNum - - - "integer identifier of this Line" -typedef ^ ^ CHARACTER(10) type - - - "type of line. should match one of LineProp names" +#typedef ^ ^ CHARACTER(10) type - - - "type of line. should match one of LineProp names" +typedef ^ ^ IntKi PropsIdNum - - - "the IdNum of the associated line properties" - typedef ^ ^ IntKi OutFlagList {20} - - "array specifying what line quantities should be output (1 vs 0)" - typedef ^ ^ IntKi CtrlChan - - - "index of control channel that will drive line active tensioning (0 for none)" - typedef ^ ^ IntKi FairConnect - - - "IdNum of Connection at fairlead" typedef ^ ^ IntKi AnchConnect - - - "IdNum of Connection at anchor" -typedef ^ ^ IntKi PropsIdNum - - - "the IdNum of the associated line properties" - typedef ^ ^ IntKi N - - - "The number of elements in the line" - +typedef ^ ^ IntKi endTypeA - - - "type of connection at end A: 0=pinned to Connection, 1=cantilevered to Rod." - +typedef ^ ^ IntKi endTypeB - - - "type of connection at end B: 0=pinned to Connection, 1=cantilevered to Rod." - typedef ^ ^ DbKi UnstrLen - - - "unstretched length of the line" - -typedef ^ ^ DbKi BA - - - "internal damping coefficient times area for this line only" "[N-s]" +typedef ^ ^ DbKi rho - - - "density" "[kg/m3]" +typedef ^ ^ DbKi d - - - "volume-equivalent diameter" "[m]" +typedef ^ ^ DbKi EA - - - "stiffness" "[N]" +typedef ^ ^ DbKi EI - - - "bending stiffness" "[N-m]" +typedef ^ ^ DbKi BA - - - "internal damping coefficient times area for this line only" "[N-s]" +typedef ^ ^ DbKi Can - - - "" "[-]" +typedef ^ ^ DbKi Cat - - - "" "[-]" +typedef ^ ^ DbKi Cdn - - - "" "[-]" +typedef ^ ^ DbKi Cdt - - - "" "[-]" +typedef ^ ^ DbKi time - - - "current time" "[s]" typedef ^ ^ DbKi r {:}{:} - - "node positions" - typedef ^ ^ DbKi rd {:}{:} - - "node velocities" - typedef ^ ^ DbKi q {:}{:} - - "node tangent vectors" - typedef ^ ^ DbKi l {:} - - "segment unstretched length" "[m]" +typedef ^ ^ ReKi ld {:} - - "segment unstretched length rate of change (used in active tensioning)" "[m]" typedef ^ ^ DbKi lstr {:} - - "segment stretched length" "[m]" typedef ^ ^ DbKi lstrd {:} - - "segment change in stretched length" "[m/s]" typedef ^ ^ DbKi V {:} - - "segment volume" "[m^3]" +typedef ^ ^ DbKi U {:}{:} - - "water velocity at node" "[m/s]" +typedef ^ ^ DbKi Ud {:}{:} - - "water acceleration at node" "[m/s^2]" +typedef ^ ^ DbKi zeta {:} - - "water surface elevation above node" "[m]" +typedef ^ ^ DbKi PDyn {:} - - "water dynamic pressure at node" "[Pa]" typedef ^ ^ DbKi T {:}{:} - - "segment tension vectors" "[N]" typedef ^ ^ DbKi Td {:}{:} - - "segment internal damping force vectors" "[N]" typedef ^ ^ DbKi W {:}{:} - - "weight/buoyancy vectors" "[N]" @@ -90,17 +232,21 @@ typedef ^ ^ DbKi Dq {:}{:} typedef ^ ^ DbKi Ap {:}{:} - - "node added mass forcing (transverse)" "[N]" typedef ^ ^ DbKi Aq {:}{:} - - "node added mass forcing (axial)" "[N]" typedef ^ ^ DbKi B {:}{:} - - "node bottom contact force" "[N]" -typedef ^ ^ DbKi F {:}{:} - - "total force on node" "[N]" +typedef ^ ^ DbKi Fnet {:}{:} - - "total force on node" "[N]" typedef ^ ^ DbKi S {:}{:}{:} - - "node inverse mass matrix" "[kg]" typedef ^ ^ DbKi M {:}{:}{:} - - "node mass matrix" "[kg]" +typedef ^ ^ DbKi EndMomentA {3} - - "vector of end moments due to bending at line end A" "[N-m]" +typedef ^ ^ DbKi EndMomentB {3} - - "vector of end moments due to bending at line end B" "[N-m]" typedef ^ ^ IntKi LineUnOut - - - "unit number of line output file" -typedef ^ ^ ReKi LineWrOutput {:} - - "one row of output data for this line" +typedef ^ ^ DbKi LineWrOutput {:} - - "one row of output data for this line" +# this is the Fail type, which holds data for possible line failure descriptors TO BE FILLED IN LATER +typedef ^ MD_Fail IntKi IdNum - - - "integer identifier of this failure" # this is the MDOutParmType - a less literal alternative of the NWTC OutParmType for MoorDyn (to avoid huge lists of possible output channel permutations) -typedef ^ MD_OutParmType CHARACTER(ChanLen) Name - - - "name of output channel" -typedef ^ ^ CHARACTER(ChanLen) Units - - - "units string" +typedef ^ MD_OutParmType CHARACTER(10) Name - - - "name of output channel" +typedef ^ ^ CHARACTER(10) Units - - - "units string" typedef ^ ^ IntKi QType - - - "type of quantity - 0=tension, 1=x, 2=y, 3=z..." typedef ^ ^ IntKi OType - - - "type of object - 0=line, 1=connect" typedef ^ ^ IntKi NodeID - - - "node number if OType=0. 0=anchor, -1=N=Fairlead" @@ -114,7 +260,7 @@ typedef ^ ^ ProgDesc Ver - " ## ============================== Define Continuous states here: ===================================================================================================================================== -typedef ^ ContinuousStateType DbKi states {:} "" - "full list of node coordinates and velocities" "[m] or [m/s]" +typedef ^ ContinuousStateType DbKi states {:} "" - "state vector of mooring system, e.g. node coordinates and velocities" "" ## ============================== Define Discrete states here: ===================================================================================================================================== @@ -131,43 +277,90 @@ typedef ^ OtherStateType SiKi dummy - ## ============================== Define Misc variables here: ===================================================================================================================================== typedef ^ MiscVarType MD_LineProp LineTypeList {:} - - "array of properties for each line type" - -typedef ^ ^ MD_Connect ConnectList {:} - - "array of connection properties" - -typedef ^ ^ MD_Line LineList {:} - - "array of line properties" - -typedef ^ ^ IntKi FairIdList {:} - - "array of size NFairs listing the ID of each fairlead (index of ConnectList)" "" -typedef ^ ^ IntKi ConnIdList {:} - - "array of size NConnss listing the ID of each connect type connection (index of ConnectList)" "" -typedef ^ ^ IntKi LineStateIndList {:} - - "starting index of each line's states in state vector" "" -typedef ^ ^ ReKi MDWrOutput {:} - - "Data from time step to be written to a MoorDyn output file" +typedef ^ ^ MD_RodProp RodTypeList {:} - - "array of properties for each rod type" - +typedef ^ ^ MD_Body GroundBody - - - "the single ground body which is the parent of all stationary connections" - +typedef ^ ^ MD_Body BodyList {:} - - "array of body objects" - +typedef ^ ^ MD_Rod RodList {:} - - "array of rod objects" - +typedef ^ ^ MD_Connect ConnectList {:} - - "array of connection objects" - +typedef ^ ^ MD_Line LineList {:} - - "array of line objects" - +typedef ^ ^ MD_Fail FailList {:} - - "array of line objects" - +typedef ^ ^ IntKi FreeConIs {:} - - "array of free connection indices in ConnectList vector" "" +typedef ^ ^ IntKi CpldConIs {:} - - "array of coupled/fairlead connection indices in ConnectList vector" "" +typedef ^ ^ IntKi FreeRodIs {:} - - "array of free rod indices in RodList vector" "" +typedef ^ ^ IntKi CpldRodIs {:} - - "array of coupled/fairlead rod indices in RodList vector" "" +typedef ^ ^ IntKi FreeBodyIs {:} - - "array of free body indices in BodyList vector" "" +typedef ^ ^ IntKi CpldBodyIs {:} - - "array of coupled body indices in BodyList vector" "" +typedef ^ ^ IntKi LineStateIs1 {:} - - "starting index of each line's states in state vector" "" +typedef ^ ^ IntKi LineStateIsN {:} - - "ending index of each line's states in state vector" "" +typedef ^ ^ IntKi ConStateIs1 {:} - - "starting index of each line's states in state vector" "" +typedef ^ ^ IntKi ConStateIsN {:} - - "ending index of each line's states in state vector" "" +typedef ^ ^ IntKi RodStateIs1 {:} - - "starting index of each rod's states in state vector" "" +typedef ^ ^ IntKi RodStateIsN {:} - - "ending index of each rod's states in state vector" "" +typedef ^ ^ IntKi BodyStateIs1 {:} - - "starting index of each body's states in state vector" "" +typedef ^ ^ IntKi BodyStateIsN {:} - - "ending index of each body's states in state vector" "" +typedef ^ ^ IntKi Nx - - - "number of states and size of state vector" "" +typedef ^ ^ MD_ContinuousStateType xTemp - - - "contains temporary state vector used in integration (put here so it's only allocated once)" +typedef ^ ^ MD_ContinuousStateType xdTemp - - - "contains temporary state derivative vector used in integration (put here so it's only allocated once)" +typedef ^ ^ DbKi zeros6 {6} - - "array of zeros for convenience" +typedef ^ ^ DbKi MDWrOutput {:} - - "Data from time step to be written to a MoorDyn output file" ## ============================== Parameters ============================================================================================================================================ -typedef ^ ParameterType IntKi NTypes - - - "number of line types" "" -typedef ^ ^ IntKi NConnects - - - "number of Connection objects" "" -typedef ^ ^ IntKi NFairs - - - "number of Fairlead Connections" "" -typedef ^ ^ IntKi NConns - - - "number of Connect type Connections - not to be confused with NConnects" "" -typedef ^ ^ IntKi NAnchs - - - "number of Anchor type Connections" "" -typedef ^ ^ IntKi NLines - - - "number of Line objects" "" -typedef ^ ^ ReKi g - 9.81 - "gravitational constant" "[kg/m^2]" -typedef ^ ^ ReKi rhoW - - - "density of seawater" "[m]" -typedef ^ ^ ReKi WtrDpth - - - "water depth" "[m]" -typedef ^ ^ ReKi kBot - - - "bottom stiffness" "[Pa/m]" -typedef ^ ^ ReKi cBot - - - "bottom damping" "[Pa-s/m]" -typedef ^ ^ ReKi dtM0 - - - "desired mooring model time step" "[s]" -typedef ^ ^ ReKi dtCoupling - - - "coupling time step that MoorDyn should expect" "[s]" +typedef ^ ParameterType IntKi nLineTypes - 0 - "number of line types" "" +typedef ^ ^ IntKi nRodTypes - 0 - "number of rod types" "" +typedef ^ ^ IntKi nConnects - 0 - "number of Connection objects" "" +typedef ^ ^ IntKi nConnectsExtra - 0 - "number of Connection objects including space for extra ones that could arise from line failures" "" +typedef ^ ^ IntKi nBodies - 0 - "number of Body objects" "" +typedef ^ ^ IntKi nRods - 0 - "number of Rod objects" "" +typedef ^ ^ IntKi nLines - 0 - "number of Line objects" "" +typedef ^ ^ IntKi nFails - 0 - "number of failure conditions" "" +typedef ^ ^ IntKi nFreeBodies - 0 - "" "" +typedef ^ ^ IntKi nFreeRods - 0 - "" "" +typedef ^ ^ IntKi nFreeCons - 0 - "" "" +typedef ^ ^ IntKi nCpldBodies - 0 - "" "" +typedef ^ ^ IntKi nCpldRods - 0 - "" "" +typedef ^ ^ IntKi nCpldCons - 0 - "number of Fairlead Connections" "" +typedef ^ ^ IntKi NConns - 0 - "number of Connect type Connections - not to be confused with NConnects" "" +typedef ^ ^ IntKi NAnchs - 0 - "number of Anchor type Connections" "" +typedef ^ ^ DbKi g - 9.81 - "gravitational constant (positive)" "[m/s^2]" +typedef ^ ^ DbKi rhoW - 1025 - "density of seawater" "[kg/m^3]" +typedef ^ ^ DbKi WtrDpth - - - "water depth" "[m]" +typedef ^ ^ DbKi kBot - - - "bottom stiffness" "[Pa/m]" +typedef ^ ^ DbKi cBot - - - "bottom damping" "[Pa-s/m]" +typedef ^ ^ DbKi dtM0 - - - "desired mooring model time step" "[s]" +typedef ^ ^ DbKi dtCoupling - - - "coupling time step that MoorDyn should expect" "[s]" typedef ^ ^ IntKi NumOuts - - - "Number of parameters in the output list (number of outputs requested)" - typedef ^ ^ CHARACTER(1024) RootName - - - "RootName for writing output files" - typedef ^ ^ MD_OutParmType OutParam {:} - - "Names and units (and other characteristics) of all requested output parameters" - typedef ^ ^ CHARACTER(1) Delim - - - "Column delimiter for output text files" - typedef ^ ^ IntKi MDUnOut - - - "Unit number of main output file" +typedef ^ ^ IntKi WaterKin - - - "Flag for whether or how to consider water kinematics" + +typedef ^ ^ DbKi ux {:}{:}{:}{:} - - "water velocities time series at each grid point" - +typedef ^ ^ DbKi uy {:}{:}{:}{:} - - "water velocities time series at each grid point" - +typedef ^ ^ DbKi uz {:}{:}{:}{:} - - "water velocities time series at each grid point" - +typedef ^ ^ DbKi ax {:}{:}{:}{:} - - "water accelerations time series at each grid point" - +typedef ^ ^ DbKi ay {:}{:}{:}{:} - - "water accelerations time series at each grid point" - +typedef ^ ^ DbKi az {:}{:}{:}{:} - - "water accelerations time series at each grid point" - +typedef ^ ^ DbKi PDyn {:}{:}{:}{:} - - "water dynamic pressure time series at each grid point" - +typedef ^ ^ DbKi zeta {:}{:}{:} - - "water surface elevations time series at each surface grid point" - +typedef ^ ^ DbKi px {:} - - "" - +typedef ^ ^ DbKi py {:} - - "" - +typedef ^ ^ DbKi pz {:} - - "" - +typedef ^ ^ DbKi dtWave - - - "" - # ============================== Inputs ============================================================================================================================================ -typedef ^ InputType MeshType PtFairleadDisplacement - - - "mesh for position AND VELOCITY of each fairlead in X,Y,Z" "[m, m/s]" -# typedef ^ ^ MeshType HydroForceLineMesh - - - "Meshed input data" - -typedef ^ ^ ReKi DeltaL {:} - - "change in line length command for each channel" "[m]" -typedef ^ ^ ReKi DeltaLdot {:} - - "rate of change of line length command for each channel" "[m]" +typedef ^ InputType MeshType CoupledKinematics - - - "mesh for position AND VELOCITY of each fairlead in X,Y,Z" "[m, m/s]" +typedef ^ ^ ReKi DeltaL {:} - - "change in line length command for each channel" "[m]" +typedef ^ ^ ReKi DeltaLdot {:} - - "rate of change of line length command for each channel" "[m]" +#typedef ^ ^ DbKi U {:}{:} - - "water velocities at each node" - +#typedef ^ ^ DbKi Ud {:}{:} - - "water accelerations at each node" - +#typedef ^ ^ DbKi zeta {:} - - "water surface elevations above each node" - +#typedef ^ ^ DbKi PDyn {:} - - "water dynamic pressure at each node" - ## ============================== Outputs ============================================================================================================================================ -typedef ^ OutputType MeshType PtFairleadLoad - - - "point mesh for fairlead forces in X,Y,Z" "[N]" +typedef ^ OutputType MeshType CoupledLoads - - - "point mesh for fairlead forces in X,Y,Z" "[N]" +#typedef ^ ^ DbKi rAll {:}{:} - - "Mesh of all point positions: bodies, rods, points, line internal nodes" - typedef ^ ^ ReKi WriteOutput {:} - - "output vector returned to glue code" "" -# typedef ^ ^ MeshType LineMeshPosition - - - "Meshed output data" - diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index b51407552d..3e42839d91 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -42,11 +42,15 @@ MODULE MoorDyn_Types CHARACTER(1024) :: FileName !< MoorDyn input file [-] CHARACTER(1024) :: RootName !< RootName for writing output files [-] LOGICAL :: Echo !< echo parameter - do we want to echo the header line describing the input file? [-] - REAL(ReKi) :: DTIC !< convergence check time step for IC generation [[s]] - REAL(ReKi) :: TMaxIC = 120 !< maximum time to allow for getting converged ICs [[s]] + REAL(DbKi) :: DTIC !< convergence check time step for IC generation [[s]] + REAL(DbKi) :: TMaxIC = 120 !< maximum time to allow for getting converged ICs [[s]] REAL(ReKi) :: CdScaleIC = 1 !< factor to scale drag coefficients by during dynamic relaxation [[]] REAL(ReKi) :: threshIC = 0.01 !< convergence tolerance for ICs (0.01 means 1%) [[]] - CHARACTER(ChanLen) , DIMENSION(:), ALLOCATABLE :: OutList !< string containing list of output channels requested in input file [-] + REAL(ReKi) , DIMENSION(:,:,:), ALLOCATABLE :: WaveVel !< [-] + REAL(ReKi) , DIMENSION(:,:,:), ALLOCATABLE :: WaveAcc !< [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: WavePDyn !< [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: WaveElev !< [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: WaveTime !< Should this be double precision? [-] END TYPE MD_InitInputType ! ======================= ! ========= MD_LineProp ======= @@ -55,21 +59,76 @@ MODULE MoorDyn_Types CHARACTER(10) :: name !< name/identifier of this set of line properties [-] REAL(DbKi) :: d !< volume-equivalent diameter [[m]] REAL(DbKi) :: w !< per-length weight in air [[kg/m]] - REAL(DbKi) :: EA !< stiffness [[N]] + REAL(DbKi) :: EA !< axial stiffness [[N]] REAL(DbKi) :: BA !< internal damping coefficient times area [[N-s]] + REAL(DbKi) :: EI !< bending stiffness [[N-m]] REAL(DbKi) :: Can !< transverse added mass coefficient [-] REAL(DbKi) :: Cat !< tangential added mass coefficient [-] REAL(DbKi) :: Cdn !< transverse drag coefficient [-] REAL(DbKi) :: Cdt !< tangential drag coefficient [-] + INTEGER(IntKi) :: nEApoints = 0 !< number of values in stress-strain lookup table (0 means using constant E) [-] + REAL(DbKi) , DIMENSION(1:30) :: stiffXs !< x array for stress-strain lookup table (up to nCoef) [-] + REAL(DbKi) , DIMENSION(1:30) :: stiffYs !< y array for stress-strain lookup table [-] + INTEGER(IntKi) :: nBpoints = 0 !< number of values in stress-strainrate lookup table (0 means using constant c) [-] + REAL(DbKi) , DIMENSION(1:30) :: dampXs !< x array for stress-strainrate lookup table (up to nCoef) [-] + REAL(DbKi) , DIMENSION(1:30) :: dampYs !< y array for stress-strainrate lookup table [-] + INTEGER(IntKi) :: nEIpoints = 0 !< number of values in bending stress-strain lookup table (0 means using constant E) [-] + REAL(DbKi) , DIMENSION(1:30) :: bstiffXs !< x array for stress-strain lookup table (up to nCoef) [-] + REAL(DbKi) , DIMENSION(1:30) :: bstiffYs !< y array for stress-strain lookup table [-] END TYPE MD_LineProp ! ======================= +! ========= MD_RodProp ======= + TYPE, PUBLIC :: MD_RodProp + INTEGER(IntKi) :: IdNum !< integer identifier of this set of rod properties [-] + CHARACTER(10) :: name !< name/identifier of this set of rod properties [-] + REAL(DbKi) :: d !< volume-equivalent diameter [[m]] + REAL(DbKi) :: w !< per-length weight in air [[kg/m]] + REAL(DbKi) :: Can !< transverse added mass coefficient [-] + REAL(DbKi) :: Cat !< tangential added mass coefficient [-] + REAL(DbKi) :: Cdn !< transverse drag coefficient [-] + REAL(DbKi) :: Cdt !< tangential drag coefficient [-] + REAL(DbKi) :: CdEnd !< drag coefficient for rod end [[-]] + REAL(DbKi) :: CaEnd !< added mass coefficient for rod end [[-]] + END TYPE MD_RodProp +! ======================= +! ========= MD_Body ======= + TYPE, PUBLIC :: MD_Body + INTEGER(IntKi) :: IdNum !< integer identifier of this Connection [-] + INTEGER(IntKi) :: typeNum !< integer identifying the type. 0=fixed, 1=vessel, 2=connect [-] + INTEGER(IntKi) , DIMENSION(1:30) :: AttachedC !< list of IdNums of connections attached to this body [-] + INTEGER(IntKi) , DIMENSION(1:30) :: AttachedR !< list of IdNums of rods attached to this body [-] + INTEGER(IntKi) :: nAttachedC = 0 !< number of attached connections [-] + INTEGER(IntKi) :: nAttachedR = 0 !< number of attached rods [-] + REAL(DbKi) , DIMENSION(1:3,1:30) :: rConnectRel !< relative position of connection on body [-] + REAL(DbKi) , DIMENSION(1:6,1:30) :: r6RodRel !< relative position and orientation of rod on body [-] + REAL(DbKi) :: bodyM !< [-] + REAL(DbKi) :: bodyV !< [-] + REAL(DbKi) , DIMENSION(1:3) :: bodyI !< [-] + REAL(DbKi) , DIMENSION(1:6) :: bodyCdA !< product of drag force and frontal area of connection point [[m^2]] + REAL(DbKi) , DIMENSION(1:6) :: bodyCa !< added mass coefficient of connection point [-] + REAL(DbKi) :: time !< current time [[s]] + REAL(DbKi) , DIMENSION(1:6) :: r6 !< position [-] + REAL(DbKi) , DIMENSION(1:6) :: v6 !< velocity [-] + REAL(DbKi) , DIMENSION(1:6) :: a6 !< acceleration (only used for coupled bodies) [-] + REAL(DbKi) , DIMENSION(1:3) :: U !< water velocity at ref point [[m/s]] + REAL(DbKi) , DIMENSION(1:3) :: Ud !< water acceleration at ref point [[m/s^2]] + REAL(DbKi) :: zeta !< water surface elevation above ref point [[m]] + REAL(DbKi) , DIMENSION(1:6) :: F6net !< total force and moment on body (excluding inertial loads) [-] + REAL(DbKi) , DIMENSION(1:6,1:6) :: M6net !< total mass matrix of Body and any attached objects [-] + REAL(DbKi) , DIMENSION(1:6,1:6) :: M !< rotated body 6-dof mass and inertia matrix in global orientation [-] + REAL(DbKi) , DIMENSION(1:6,1:6) :: M0 !< body 6-dof mass and inertia matrix in its own frame [-] + REAL(DbKi) , DIMENSION(1:3,1:3) :: OrMat !< DCM for body orientation [-] + REAL(DbKi) , DIMENSION(1:3) :: rCG !< vector in body frame from ref point to CG (before rods etc..) [-] + END TYPE MD_Body +! ======================= ! ========= MD_Connect ======= TYPE, PUBLIC :: MD_Connect INTEGER(IntKi) :: IdNum !< integer identifier of this Connection [-] CHARACTER(10) :: type !< type of Connect: fix, vessel, connect [-] - INTEGER(IntKi) :: TypeNum !< integer identifying the type. 0=fixed, 1=vessel, 2=connect [-] - INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: AttachedFairs !< list of IdNums of connected Line tops [-] - INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: AttachedAnchs !< list of IdNums of connected Line bottoms [-] + INTEGER(IntKi) :: typeNum !< integer identifying the type. 0=fixed, 1=vessel, 2=connect [-] + INTEGER(IntKi) , DIMENSION(1:10) :: Attached !< list of IdNums of lines attached to this connection node [-] + INTEGER(IntKi) , DIMENSION(1:10) :: Top !< list of ints specifying whether each line is attached at 1 = top/fairlead(end B), 0 = bottom/anchor(end A) [-] + INTEGER(IntKi) :: nAttached = 0 !< number of attached lines [-] REAL(DbKi) :: conX !< [-] REAL(DbKi) :: conY !< [-] REAL(DbKi) :: conZ !< [-] @@ -80,32 +139,111 @@ MODULE MoorDyn_Types REAL(DbKi) :: conFZ !< [-] REAL(DbKi) :: conCa !< added mass coefficient of connection point [-] REAL(DbKi) :: conCdA !< product of drag force and frontal area of connection point [[m^2]] - REAL(DbKi) , DIMENSION(1:3) :: Ftot !< total force on node [-] - REAL(DbKi) , DIMENSION(1:3,1:3) :: Mtot !< node mass matrix, from attached lines [-] - REAL(DbKi) , DIMENSION(1:3,1:3) :: S !< inverse mass matrix [[kg]] + REAL(DbKi) :: time !< current time [[s]] REAL(DbKi) , DIMENSION(1:3) :: r !< position [-] REAL(DbKi) , DIMENSION(1:3) :: rd !< velocity [-] + REAL(DbKi) , DIMENSION(1:3) :: a !< acceleration (only used for coupled points) [-] + REAL(DbKi) , DIMENSION(1:3) :: U !< water velocity at node [[m/s]] + REAL(DbKi) , DIMENSION(1:3) :: Ud !< water acceleration at node [[m/s^2]] + REAL(DbKi) :: zeta !< water surface elevation above node [[m]] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: PDyn !< water dynamic pressure at node [[Pa]] + REAL(DbKi) , DIMENSION(1:3) :: Fnet !< total force on node (excluding inertial loads) [-] + REAL(DbKi) , DIMENSION(1:3,1:3) :: M !< node mass matrix, from attached lines [-] END TYPE MD_Connect ! ======================= +! ========= MD_Rod ======= + TYPE, PUBLIC :: MD_Rod + INTEGER(IntKi) :: IdNum !< integer identifier of this Line [-] + CHARACTER(10) :: type !< type of Rod. should match one of RodProp names [-] + INTEGER(IntKi) :: PropsIdNum !< the IdNum of the associated rod properties [-] + INTEGER(IntKi) :: typeNum !< integer identifying the type. 0=fixed, 1=vessel, 2=connect [-] + INTEGER(IntKi) , DIMENSION(1:10) :: AttachedA !< list of IdNums of lines attached to end A [-] + INTEGER(IntKi) , DIMENSION(1:10) :: AttachedB !< list of IdNums of lines attached to end B [-] + INTEGER(IntKi) , DIMENSION(1:10) :: TopA !< list of ints specifying whether each line is attached at 1 = top/fairlead(end B), 0 = bottom/anchor(end A) [-] + INTEGER(IntKi) , DIMENSION(1:10) :: TopB !< list of ints specifying whether each line is attached at 1 = top/fairlead(end B), 0 = bottom/anchor(end A) [-] + INTEGER(IntKi) :: nAttachedA = 0 !< number of attached lines to Rod end A [-] + INTEGER(IntKi) :: nAttachedB = 0 !< number of attached lines to Rod end B [-] + INTEGER(IntKi) , DIMENSION(1:20) :: OutFlagList !< array specifying what line quantities should be output (1 vs 0) [-] + INTEGER(IntKi) :: N !< The number of elements in the line [-] + INTEGER(IntKi) :: endTypeA !< type of connection at end A: 0=pinned to Connection, 1=cantilevered to Rod. [-] + INTEGER(IntKi) :: endTypeB !< type of connection at end B: 0=pinned to Connection, 1=cantilevered to Rod. [-] + REAL(DbKi) :: UnstrLen !< length of the rod [[m]] + REAL(DbKi) :: mass !< mass of the rod [[kg]] + REAL(DbKi) :: rho !< density [[kg/m3]] + REAL(DbKi) :: d !< volume-equivalent diameter [[m]] + REAL(DbKi) :: Can !< [[-]] + REAL(DbKi) :: Cat !< [[-]] + REAL(DbKi) :: Cdn !< [[-]] + REAL(DbKi) :: Cdt !< [[-]] + REAL(DbKi) :: CdEnd !< drag coefficient for rod end [[-]] + REAL(DbKi) :: CaEnd !< added mass coefficient for rod end [[-]] + REAL(DbKi) :: time !< current time [[s]] + REAL(DbKi) :: roll !< roll relative to vertical [deg] + REAL(DbKi) :: pitch !< pitch relative to vertical [deg] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: r !< node positions [-] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: rd !< node velocities [-] + REAL(DbKi) , DIMENSION(1:3) :: q !< tangent vector for rod as a whole [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: l !< segment unstretched length [[m]] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: V !< segment volume [[m^3]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: U !< water velocity at node [[m/s]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Ud !< water acceleration at node [[m/s^2]] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: zeta !< water surface elevation above node [[m]] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: PDyn !< water dynamic pressure at node [[Pa]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: W !< weight vectors [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Bo !< buoyancy force vectors [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Pd !< dynamic pressure force vectors [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Dp !< node drag (transverse) [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Dq !< node drag (axial) [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Ap !< node added mass forcing (transverse) [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Aq !< node added mass forcing (axial) [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: B !< node bottom contact force [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Fnet !< total force on node [[N]] + REAL(DbKi) , DIMENSION(:,:,:), ALLOCATABLE :: M !< node mass matrix [[kg]] + REAL(DbKi) , DIMENSION(1:3) :: Mext !< external moment vector holding sum of any externally applied moments i.e. bending lines [-] + REAL(DbKi) , DIMENSION(1:6) :: r6 !< 6 DOF position vector [-] + REAL(DbKi) , DIMENSION(1:6) :: v6 !< 6 DOF velocity vector [-] + REAL(DbKi) , DIMENSION(1:6) :: a6 !< 6 DOF acceleration vector (only used for coupled Rods) [-] + REAL(DbKi) , DIMENSION(1:6) :: F6net !< total force and moment about end A (excluding inertial loads) that Rod may exert on whatever it's attached to [-] + REAL(DbKi) , DIMENSION(1:6,1:6) :: M6net !< total mass matrix about end A of Rod and any attached Points [-] + REAL(DbKi) , DIMENSION(1:3,1:3) :: OrMat !< DCM for body orientation [-] + INTEGER(IntKi) :: RodUnOut !< unit number of rod output file [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: RodWrOutput !< one row of output data for this rod [-] + END TYPE MD_Rod +! ======================= ! ========= MD_Line ======= TYPE, PUBLIC :: MD_Line INTEGER(IntKi) :: IdNum !< integer identifier of this Line [-] - CHARACTER(10) :: type !< type of line. should match one of LineProp names [-] + INTEGER(IntKi) :: PropsIdNum !< the IdNum of the associated line properties [-] INTEGER(IntKi) , DIMENSION(1:20) :: OutFlagList !< array specifying what line quantities should be output (1 vs 0) [-] INTEGER(IntKi) :: CtrlChan !< index of control channel that will drive line active tensioning (0 for none) [-] INTEGER(IntKi) :: FairConnect !< IdNum of Connection at fairlead [-] INTEGER(IntKi) :: AnchConnect !< IdNum of Connection at anchor [-] - INTEGER(IntKi) :: PropsIdNum !< the IdNum of the associated line properties [-] INTEGER(IntKi) :: N !< The number of elements in the line [-] + INTEGER(IntKi) :: endTypeA !< type of connection at end A: 0=pinned to Connection, 1=cantilevered to Rod. [-] + INTEGER(IntKi) :: endTypeB !< type of connection at end B: 0=pinned to Connection, 1=cantilevered to Rod. [-] REAL(DbKi) :: UnstrLen !< unstretched length of the line [-] + REAL(DbKi) :: rho !< density [[kg/m3]] + REAL(DbKi) :: d !< volume-equivalent diameter [[m]] + REAL(DbKi) :: EA !< stiffness [[N]] + REAL(DbKi) :: EI !< bending stiffness [[N-m]] REAL(DbKi) :: BA !< internal damping coefficient times area for this line only [[N-s]] + REAL(DbKi) :: Can !< [[-]] + REAL(DbKi) :: Cat !< [[-]] + REAL(DbKi) :: Cdn !< [[-]] + REAL(DbKi) :: Cdt !< [[-]] + REAL(DbKi) :: time !< current time [[s]] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: r !< node positions [-] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: rd !< node velocities [-] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: q !< node tangent vectors [-] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: l !< segment unstretched length [[m]] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: ld !< segment unstretched length rate of change (used in active tensioning) [[m]] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: lstr !< segment stretched length [[m]] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: lstrd !< segment change in stretched length [[m/s]] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: V !< segment volume [[m^3]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: U !< water velocity at node [[m/s]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Ud !< water acceleration at node [[m/s^2]] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: zeta !< water surface elevation above node [[m]] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: PDyn !< water dynamic pressure at node [[Pa]] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: T !< segment tension vectors [[N]] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Td !< segment internal damping force vectors [[N]] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: W !< weight/buoyancy vectors [[N]] @@ -114,17 +252,24 @@ MODULE MoorDyn_Types REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Ap !< node added mass forcing (transverse) [[N]] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Aq !< node added mass forcing (axial) [[N]] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: B !< node bottom contact force [[N]] - REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: F !< total force on node [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Fnet !< total force on node [[N]] REAL(DbKi) , DIMENSION(:,:,:), ALLOCATABLE :: S !< node inverse mass matrix [[kg]] REAL(DbKi) , DIMENSION(:,:,:), ALLOCATABLE :: M !< node mass matrix [[kg]] + REAL(DbKi) , DIMENSION(1:3) :: EndMomentA !< vector of end moments due to bending at line end A [[N-m]] + REAL(DbKi) , DIMENSION(1:3) :: EndMomentB !< vector of end moments due to bending at line end B [[N-m]] INTEGER(IntKi) :: LineUnOut !< unit number of line output file [-] - REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: LineWrOutput !< one row of output data for this line [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: LineWrOutput !< one row of output data for this line [-] END TYPE MD_Line ! ======================= +! ========= MD_Fail ======= + TYPE, PUBLIC :: MD_Fail + INTEGER(IntKi) :: IdNum !< integer identifier of this failure [-] + END TYPE MD_Fail +! ======================= ! ========= MD_OutParmType ======= TYPE, PUBLIC :: MD_OutParmType - CHARACTER(ChanLen) :: Name !< name of output channel [-] - CHARACTER(ChanLen) :: Units !< units string [-] + CHARACTER(10) :: Name !< name of output channel [-] + CHARACTER(10) :: Units !< units string [-] INTEGER(IntKi) :: QType !< type of quantity - 0=tension, 1=x, 2=y, 3=z... [-] INTEGER(IntKi) :: OType !< type of object - 0=line, 1=connect [-] INTEGER(IntKi) :: NodeID !< node number if OType=0. 0=anchor, -1=N=Fairlead [-] @@ -140,7 +285,7 @@ MODULE MoorDyn_Types ! ======================= ! ========= MD_ContinuousStateType ======= TYPE, PUBLIC :: MD_ContinuousStateType - REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: states !< full list of node coordinates and velocities [[m] or [m/s]] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: states !< state vector of mooring system, e.g. node coordinates and velocities [] END TYPE MD_ContinuousStateType ! ======================= ! ========= MD_DiscreteStateType ======= @@ -161,46 +306,89 @@ MODULE MoorDyn_Types ! ========= MD_MiscVarType ======= TYPE, PUBLIC :: MD_MiscVarType TYPE(MD_LineProp) , DIMENSION(:), ALLOCATABLE :: LineTypeList !< array of properties for each line type [-] - TYPE(MD_Connect) , DIMENSION(:), ALLOCATABLE :: ConnectList !< array of connection properties [-] - TYPE(MD_Line) , DIMENSION(:), ALLOCATABLE :: LineList !< array of line properties [-] - INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: FairIdList !< array of size NFairs listing the ID of each fairlead (index of ConnectList) [] - INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: ConnIdList !< array of size NConnss listing the ID of each connect type connection (index of ConnectList) [] - INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: LineStateIndList !< starting index of each line's states in state vector [] - REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: MDWrOutput !< Data from time step to be written to a MoorDyn output file [-] + TYPE(MD_RodProp) , DIMENSION(:), ALLOCATABLE :: RodTypeList !< array of properties for each rod type [-] + TYPE(MD_Body) :: GroundBody !< the single ground body which is the parent of all stationary connections [-] + TYPE(MD_Body) , DIMENSION(:), ALLOCATABLE :: BodyList !< array of body objects [-] + TYPE(MD_Rod) , DIMENSION(:), ALLOCATABLE :: RodList !< array of rod objects [-] + TYPE(MD_Connect) , DIMENSION(:), ALLOCATABLE :: ConnectList !< array of connection objects [-] + TYPE(MD_Line) , DIMENSION(:), ALLOCATABLE :: LineList !< array of line objects [-] + TYPE(MD_Fail) , DIMENSION(:), ALLOCATABLE :: FailList !< array of line objects [-] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: FreeConIs !< array of free connection indices in ConnectList vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: CpldConIs !< array of coupled/fairlead connection indices in ConnectList vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: FreeRodIs !< array of free rod indices in RodList vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: CpldRodIs !< array of coupled/fairlead rod indices in RodList vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: FreeBodyIs !< array of free body indices in BodyList vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: CpldBodyIs !< array of coupled body indices in BodyList vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: LineStateIs1 !< starting index of each line's states in state vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: LineStateIsN !< ending index of each line's states in state vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: ConStateIs1 !< starting index of each line's states in state vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: ConStateIsN !< ending index of each line's states in state vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: RodStateIs1 !< starting index of each rod's states in state vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: RodStateIsN !< ending index of each rod's states in state vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: BodyStateIs1 !< starting index of each body's states in state vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: BodyStateIsN !< ending index of each body's states in state vector [] + INTEGER(IntKi) :: Nx !< number of states and size of state vector [] + TYPE(MD_ContinuousStateType) :: xTemp !< contains temporary state vector used in integration (put here so it's only allocated once) [-] + TYPE(MD_ContinuousStateType) :: xdTemp !< contains temporary state derivative vector used in integration (put here so it's only allocated once) [-] + REAL(DbKi) , DIMENSION(1:6) :: zeros6 !< array of zeros for convenience [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: MDWrOutput !< Data from time step to be written to a MoorDyn output file [-] END TYPE MD_MiscVarType ! ======================= ! ========= MD_ParameterType ======= TYPE, PUBLIC :: MD_ParameterType - INTEGER(IntKi) :: NTypes !< number of line types [] - INTEGER(IntKi) :: NConnects !< number of Connection objects [] - INTEGER(IntKi) :: NFairs !< number of Fairlead Connections [] - INTEGER(IntKi) :: NConns !< number of Connect type Connections - not to be confused with NConnects [] - INTEGER(IntKi) :: NAnchs !< number of Anchor type Connections [] - INTEGER(IntKi) :: NLines !< number of Line objects [] - REAL(ReKi) :: g = 9.81 !< gravitational constant [[kg/m^2]] - REAL(ReKi) :: rhoW !< density of seawater [[m]] - REAL(ReKi) :: WtrDpth !< water depth [[m]] - REAL(ReKi) :: kBot !< bottom stiffness [[Pa/m]] - REAL(ReKi) :: cBot !< bottom damping [[Pa-s/m]] - REAL(ReKi) :: dtM0 !< desired mooring model time step [[s]] - REAL(ReKi) :: dtCoupling !< coupling time step that MoorDyn should expect [[s]] + INTEGER(IntKi) :: nLineTypes = 0 !< number of line types [] + INTEGER(IntKi) :: nRodTypes = 0 !< number of rod types [] + INTEGER(IntKi) :: nConnects = 0 !< number of Connection objects [] + INTEGER(IntKi) :: nConnectsExtra = 0 !< number of Connection objects including space for extra ones that could arise from line failures [] + INTEGER(IntKi) :: nBodies = 0 !< number of Body objects [] + INTEGER(IntKi) :: nRods = 0 !< number of Rod objects [] + INTEGER(IntKi) :: nLines = 0 !< number of Line objects [] + INTEGER(IntKi) :: nFails = 0 !< number of failure conditions [] + INTEGER(IntKi) :: nFreeBodies = 0 !< [] + INTEGER(IntKi) :: nFreeRods = 0 !< [] + INTEGER(IntKi) :: nFreeCons = 0 !< [] + INTEGER(IntKi) :: nCpldBodies = 0 !< [] + INTEGER(IntKi) :: nCpldRods = 0 !< [] + INTEGER(IntKi) :: nCpldCons = 0 !< number of Fairlead Connections [] + INTEGER(IntKi) :: NConns = 0 !< number of Connect type Connections - not to be confused with NConnects [] + INTEGER(IntKi) :: NAnchs = 0 !< number of Anchor type Connections [] + REAL(DbKi) :: g = 9.81 !< gravitational constant (positive) [[m/s^2]] + REAL(DbKi) :: rhoW = 1025 !< density of seawater [[kg/m^3]] + REAL(DbKi) :: WtrDpth !< water depth [[m]] + REAL(DbKi) :: kBot !< bottom stiffness [[Pa/m]] + REAL(DbKi) :: cBot !< bottom damping [[Pa-s/m]] + REAL(DbKi) :: dtM0 !< desired mooring model time step [[s]] + REAL(DbKi) :: dtCoupling !< coupling time step that MoorDyn should expect [[s]] INTEGER(IntKi) :: NumOuts !< Number of parameters in the output list (number of outputs requested) [-] CHARACTER(1024) :: RootName !< RootName for writing output files [-] TYPE(MD_OutParmType) , DIMENSION(:), ALLOCATABLE :: OutParam !< Names and units (and other characteristics) of all requested output parameters [-] CHARACTER(1) :: Delim !< Column delimiter for output text files [-] INTEGER(IntKi) :: MDUnOut !< Unit number of main output file [-] + INTEGER(IntKi) :: WaterKin !< Flag for whether or how to consider water kinematics [-] + REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: ux !< water velocities time series at each grid point [-] + REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: uy !< water velocities time series at each grid point [-] + REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: uz !< water velocities time series at each grid point [-] + REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: ax !< water accelerations time series at each grid point [-] + REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: ay !< water accelerations time series at each grid point [-] + REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: az !< water accelerations time series at each grid point [-] + REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: PDyn !< water dynamic pressure time series at each grid point [-] + REAL(DbKi) , DIMENSION(:,:,:), ALLOCATABLE :: zeta !< water surface elevations time series at each surface grid point [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: px !< [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: py !< [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: pz !< [-] + REAL(DbKi) :: dtWave !< [-] END TYPE MD_ParameterType ! ======================= ! ========= MD_InputType ======= TYPE, PUBLIC :: MD_InputType - TYPE(MeshType) :: PtFairleadDisplacement !< mesh for position AND VELOCITY of each fairlead in X,Y,Z [[m, m/s]] + TYPE(MeshType) :: CoupledKinematics !< mesh for position AND VELOCITY of each fairlead in X,Y,Z [[m, m/s]] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: DeltaL !< change in line length command for each channel [[m]] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: DeltaLdot !< rate of change of line length command for each channel [[m]] END TYPE MD_InputType ! ======================= ! ========= MD_OutputType ======= TYPE, PUBLIC :: MD_OutputType - TYPE(MeshType) :: PtFairleadLoad !< point mesh for fairlead forces in X,Y,Z [[N]] + TYPE(MeshType) :: CoupledLoads !< point mesh for fairlead forces in X,Y,Z [[N]] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: WriteOutput !< output vector returned to glue code [] END TYPE MD_OutputType ! ======================= @@ -216,6 +404,7 @@ SUBROUTINE MD_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, ErrSt INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: i4, i4_l, i4_u ! bounds (upper/lower) for an array dimension 4 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyInitInput' @@ -233,17 +422,77 @@ SUBROUTINE MD_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, ErrSt DstInitInputData%TMaxIC = SrcInitInputData%TMaxIC DstInitInputData%CdScaleIC = SrcInitInputData%CdScaleIC DstInitInputData%threshIC = SrcInitInputData%threshIC -IF (ALLOCATED(SrcInitInputData%OutList)) THEN - i1_l = LBOUND(SrcInitInputData%OutList,1) - i1_u = UBOUND(SrcInitInputData%OutList,1) - IF (.NOT. ALLOCATED(DstInitInputData%OutList)) THEN - ALLOCATE(DstInitInputData%OutList(i1_l:i1_u),STAT=ErrStat2) +IF (ALLOCATED(SrcInitInputData%WaveVel)) THEN + i1_l = LBOUND(SrcInitInputData%WaveVel,1) + i1_u = UBOUND(SrcInitInputData%WaveVel,1) + i2_l = LBOUND(SrcInitInputData%WaveVel,2) + i2_u = UBOUND(SrcInitInputData%WaveVel,2) + i3_l = LBOUND(SrcInitInputData%WaveVel,3) + i3_u = UBOUND(SrcInitInputData%WaveVel,3) + IF (.NOT. ALLOCATED(DstInitInputData%WaveVel)) THEN + ALLOCATE(DstInitInputData%WaveVel(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitInputData%WaveVel.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitInputData%WaveVel = SrcInitInputData%WaveVel +ENDIF +IF (ALLOCATED(SrcInitInputData%WaveAcc)) THEN + i1_l = LBOUND(SrcInitInputData%WaveAcc,1) + i1_u = UBOUND(SrcInitInputData%WaveAcc,1) + i2_l = LBOUND(SrcInitInputData%WaveAcc,2) + i2_u = UBOUND(SrcInitInputData%WaveAcc,2) + i3_l = LBOUND(SrcInitInputData%WaveAcc,3) + i3_u = UBOUND(SrcInitInputData%WaveAcc,3) + IF (.NOT. ALLOCATED(DstInitInputData%WaveAcc)) THEN + ALLOCATE(DstInitInputData%WaveAcc(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitInputData%WaveAcc.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitInputData%WaveAcc = SrcInitInputData%WaveAcc +ENDIF +IF (ALLOCATED(SrcInitInputData%WavePDyn)) THEN + i1_l = LBOUND(SrcInitInputData%WavePDyn,1) + i1_u = UBOUND(SrcInitInputData%WavePDyn,1) + i2_l = LBOUND(SrcInitInputData%WavePDyn,2) + i2_u = UBOUND(SrcInitInputData%WavePDyn,2) + IF (.NOT. ALLOCATED(DstInitInputData%WavePDyn)) THEN + ALLOCATE(DstInitInputData%WavePDyn(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitInputData%WavePDyn.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitInputData%WavePDyn = SrcInitInputData%WavePDyn +ENDIF +IF (ALLOCATED(SrcInitInputData%WaveElev)) THEN + i1_l = LBOUND(SrcInitInputData%WaveElev,1) + i1_u = UBOUND(SrcInitInputData%WaveElev,1) + i2_l = LBOUND(SrcInitInputData%WaveElev,2) + i2_u = UBOUND(SrcInitInputData%WaveElev,2) + IF (.NOT. ALLOCATED(DstInitInputData%WaveElev)) THEN + ALLOCATE(DstInitInputData%WaveElev(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitInputData%WaveElev.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitInputData%WaveElev = SrcInitInputData%WaveElev +ENDIF +IF (ALLOCATED(SrcInitInputData%WaveTime)) THEN + i1_l = LBOUND(SrcInitInputData%WaveTime,1) + i1_u = UBOUND(SrcInitInputData%WaveTime,1) + IF (.NOT. ALLOCATED(DstInitInputData%WaveTime)) THEN + ALLOCATE(DstInitInputData%WaveTime(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitInputData%OutList.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitInputData%WaveTime.', ErrStat, ErrMsg,RoutineName) RETURN END IF END IF - DstInitInputData%OutList = SrcInitInputData%OutList + DstInitInputData%WaveTime = SrcInitInputData%WaveTime ENDIF END SUBROUTINE MD_CopyInitInput @@ -256,8 +505,20 @@ SUBROUTINE MD_DestroyInitInput( InitInputData, ErrStat, ErrMsg ) ! ErrStat = ErrID_None ErrMsg = "" -IF (ALLOCATED(InitInputData%OutList)) THEN - DEALLOCATE(InitInputData%OutList) +IF (ALLOCATED(InitInputData%WaveVel)) THEN + DEALLOCATE(InitInputData%WaveVel) +ENDIF +IF (ALLOCATED(InitInputData%WaveAcc)) THEN + DEALLOCATE(InitInputData%WaveAcc) +ENDIF +IF (ALLOCATED(InitInputData%WavePDyn)) THEN + DEALLOCATE(InitInputData%WavePDyn) +ENDIF +IF (ALLOCATED(InitInputData%WaveElev)) THEN + DEALLOCATE(InitInputData%WaveElev) +ENDIF +IF (ALLOCATED(InitInputData%WaveTime)) THEN + DEALLOCATE(InitInputData%WaveTime) ENDIF END SUBROUTINE MD_DestroyInitInput @@ -303,14 +564,34 @@ SUBROUTINE MD_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg Int_BufSz = Int_BufSz + 1*LEN(InData%FileName) ! FileName Int_BufSz = Int_BufSz + 1*LEN(InData%RootName) ! RootName Int_BufSz = Int_BufSz + 1 ! Echo - Re_BufSz = Re_BufSz + 1 ! DTIC - Re_BufSz = Re_BufSz + 1 ! TMaxIC + Db_BufSz = Db_BufSz + 1 ! DTIC + Db_BufSz = Db_BufSz + 1 ! TMaxIC Re_BufSz = Re_BufSz + 1 ! CdScaleIC Re_BufSz = Re_BufSz + 1 ! threshIC - Int_BufSz = Int_BufSz + 1 ! OutList allocated yes/no - IF ( ALLOCATED(InData%OutList) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! OutList upper/lower bounds for each dimension - Int_BufSz = Int_BufSz + SIZE(InData%OutList)*LEN(InData%OutList) ! OutList + Int_BufSz = Int_BufSz + 1 ! WaveVel allocated yes/no + IF ( ALLOCATED(InData%WaveVel) ) THEN + Int_BufSz = Int_BufSz + 2*3 ! WaveVel upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%WaveVel) ! WaveVel + END IF + Int_BufSz = Int_BufSz + 1 ! WaveAcc allocated yes/no + IF ( ALLOCATED(InData%WaveAcc) ) THEN + Int_BufSz = Int_BufSz + 2*3 ! WaveAcc upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%WaveAcc) ! WaveAcc + END IF + Int_BufSz = Int_BufSz + 1 ! WavePDyn allocated yes/no + IF ( ALLOCATED(InData%WavePDyn) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! WavePDyn upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%WavePDyn) ! WavePDyn + END IF + Int_BufSz = Int_BufSz + 1 ! WaveElev allocated yes/no + IF ( ALLOCATED(InData%WaveElev) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! WaveElev upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%WaveElev) ! WaveElev + END IF + Int_BufSz = Int_BufSz + 1 ! WaveTime allocated yes/no + IF ( ALLOCATED(InData%WaveTime) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! WaveTime upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%WaveTime) ! WaveTime END IF IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) @@ -359,29 +640,117 @@ SUBROUTINE MD_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg END DO ! I IntKiBuf(Int_Xferred) = TRANSFER(InData%Echo, IntKiBuf(1)) Int_Xferred = Int_Xferred + 1 - ReKiBuf(Re_Xferred) = InData%DTIC - Re_Xferred = Re_Xferred + 1 - ReKiBuf(Re_Xferred) = InData%TMaxIC - Re_Xferred = Re_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%DTIC + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%TMaxIC + Db_Xferred = Db_Xferred + 1 ReKiBuf(Re_Xferred) = InData%CdScaleIC Re_Xferred = Re_Xferred + 1 ReKiBuf(Re_Xferred) = InData%threshIC Re_Xferred = Re_Xferred + 1 - IF ( .NOT. ALLOCATED(InData%OutList) ) THEN + IF ( .NOT. ALLOCATED(InData%WaveVel) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%OutList,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%OutList,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveVel,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveVel,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveVel,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveVel,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveVel,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveVel,3) Int_Xferred = Int_Xferred + 2 - DO i1 = LBOUND(InData%OutList,1), UBOUND(InData%OutList,1) - DO I = 1, LEN(InData%OutList) - IntKiBuf(Int_Xferred) = ICHAR(InData%OutList(i1)(I:I), IntKi) - Int_Xferred = Int_Xferred + 1 - END DO ! I + DO i3 = LBOUND(InData%WaveVel,3), UBOUND(InData%WaveVel,3) + DO i2 = LBOUND(InData%WaveVel,2), UBOUND(InData%WaveVel,2) + DO i1 = LBOUND(InData%WaveVel,1), UBOUND(InData%WaveVel,1) + ReKiBuf(Re_Xferred) = InData%WaveVel(i1,i2,i3) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%WaveAcc) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveAcc,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveAcc,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveAcc,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveAcc,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveAcc,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveAcc,3) + Int_Xferred = Int_Xferred + 2 + + DO i3 = LBOUND(InData%WaveAcc,3), UBOUND(InData%WaveAcc,3) + DO i2 = LBOUND(InData%WaveAcc,2), UBOUND(InData%WaveAcc,2) + DO i1 = LBOUND(InData%WaveAcc,1), UBOUND(InData%WaveAcc,1) + ReKiBuf(Re_Xferred) = InData%WaveAcc(i1,i2,i3) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%WavePDyn) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WavePDyn,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WavePDyn,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WavePDyn,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WavePDyn,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%WavePDyn,2), UBOUND(InData%WavePDyn,2) + DO i1 = LBOUND(InData%WavePDyn,1), UBOUND(InData%WavePDyn,1) + ReKiBuf(Re_Xferred) = InData%WavePDyn(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%WaveElev) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveElev,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveElev,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveElev,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveElev,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%WaveElev,2), UBOUND(InData%WaveElev,2) + DO i1 = LBOUND(InData%WaveElev,1), UBOUND(InData%WaveElev,1) + ReKiBuf(Re_Xferred) = InData%WaveElev(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%WaveTime) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WaveTime,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WaveTime,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%WaveTime,1), UBOUND(InData%WaveTime,1) + DbKiBuf(Db_Xferred) = InData%WaveTime(i1) + Db_Xferred = Db_Xferred + 1 END DO END IF END SUBROUTINE MD_PackInitInput @@ -402,6 +771,7 @@ SUBROUTINE MD_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Err INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: i4, i4_l, i4_u ! bounds (upper/lower) for an array dimension 4 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackInitInput' @@ -437,32 +807,132 @@ SUBROUTINE MD_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Err END DO ! I OutData%Echo = TRANSFER(IntKiBuf(Int_Xferred), OutData%Echo) Int_Xferred = Int_Xferred + 1 - OutData%DTIC = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 - OutData%TMaxIC = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 + OutData%DTIC = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%TMaxIC = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 OutData%CdScaleIC = ReKiBuf(Re_Xferred) Re_Xferred = Re_Xferred + 1 OutData%threshIC = ReKiBuf(Re_Xferred) Re_Xferred = Re_Xferred + 1 - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! OutList not allocated + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WaveVel not allocated Int_Xferred = Int_Xferred + 1 ELSE Int_Xferred = Int_Xferred + 1 i1_l = IntKiBuf( Int_Xferred ) i1_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%OutList)) DEALLOCATE(OutData%OutList) - ALLOCATE(OutData%OutList(i1_l:i1_u),STAT=ErrStat2) + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%WaveVel)) DEALLOCATE(OutData%WaveVel) + ALLOCATE(OutData%WaveVel(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%OutList.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%WaveVel.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i1 = LBOUND(OutData%OutList,1), UBOUND(OutData%OutList,1) - DO I = 1, LEN(OutData%OutList) - OutData%OutList(i1)(I:I) = CHAR(IntKiBuf(Int_Xferred)) - Int_Xferred = Int_Xferred + 1 - END DO ! I + DO i3 = LBOUND(OutData%WaveVel,3), UBOUND(OutData%WaveVel,3) + DO i2 = LBOUND(OutData%WaveVel,2), UBOUND(OutData%WaveVel,2) + DO i1 = LBOUND(OutData%WaveVel,1), UBOUND(OutData%WaveVel,1) + OutData%WaveVel(i1,i2,i3) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WaveAcc not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%WaveAcc)) DEALLOCATE(OutData%WaveAcc) + ALLOCATE(OutData%WaveAcc(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%WaveAcc.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i3 = LBOUND(OutData%WaveAcc,3), UBOUND(OutData%WaveAcc,3) + DO i2 = LBOUND(OutData%WaveAcc,2), UBOUND(OutData%WaveAcc,2) + DO i1 = LBOUND(OutData%WaveAcc,1), UBOUND(OutData%WaveAcc,1) + OutData%WaveAcc(i1,i2,i3) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WavePDyn not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%WavePDyn)) DEALLOCATE(OutData%WavePDyn) + ALLOCATE(OutData%WavePDyn(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%WavePDyn.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%WavePDyn,2), UBOUND(OutData%WavePDyn,2) + DO i1 = LBOUND(OutData%WavePDyn,1), UBOUND(OutData%WavePDyn,1) + OutData%WavePDyn(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WaveElev not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%WaveElev)) DEALLOCATE(OutData%WaveElev) + ALLOCATE(OutData%WaveElev(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%WaveElev.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%WaveElev,2), UBOUND(OutData%WaveElev,2) + DO i1 = LBOUND(OutData%WaveElev,1), UBOUND(OutData%WaveElev,1) + OutData%WaveElev(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WaveTime not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%WaveTime)) DEALLOCATE(OutData%WaveTime) + ALLOCATE(OutData%WaveTime(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%WaveTime.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%WaveTime,1), UBOUND(OutData%WaveTime,1) + OutData%WaveTime(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 END DO END IF END SUBROUTINE MD_UnPackInitInput @@ -475,6 +945,7 @@ SUBROUTINE MD_CopyLineProp( SrcLinePropData, DstLinePropData, CtrlCode, ErrStat, CHARACTER(*), INTENT( OUT) :: ErrMsg ! Local INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyLineProp' @@ -487,10 +958,20 @@ SUBROUTINE MD_CopyLineProp( SrcLinePropData, DstLinePropData, CtrlCode, ErrStat, DstLinePropData%w = SrcLinePropData%w DstLinePropData%EA = SrcLinePropData%EA DstLinePropData%BA = SrcLinePropData%BA + DstLinePropData%EI = SrcLinePropData%EI DstLinePropData%Can = SrcLinePropData%Can DstLinePropData%Cat = SrcLinePropData%Cat DstLinePropData%Cdn = SrcLinePropData%Cdn DstLinePropData%Cdt = SrcLinePropData%Cdt + DstLinePropData%nEApoints = SrcLinePropData%nEApoints + DstLinePropData%stiffXs = SrcLinePropData%stiffXs + DstLinePropData%stiffYs = SrcLinePropData%stiffYs + DstLinePropData%nBpoints = SrcLinePropData%nBpoints + DstLinePropData%dampXs = SrcLinePropData%dampXs + DstLinePropData%dampYs = SrcLinePropData%dampYs + DstLinePropData%nEIpoints = SrcLinePropData%nEIpoints + DstLinePropData%bstiffXs = SrcLinePropData%bstiffXs + DstLinePropData%bstiffYs = SrcLinePropData%bstiffYs END SUBROUTINE MD_CopyLineProp SUBROUTINE MD_DestroyLineProp( LinePropData, ErrStat, ErrMsg ) @@ -545,10 +1026,20 @@ SUBROUTINE MD_PackLineProp( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Db_BufSz = Db_BufSz + 1 ! w Db_BufSz = Db_BufSz + 1 ! EA Db_BufSz = Db_BufSz + 1 ! BA + Db_BufSz = Db_BufSz + 1 ! EI Db_BufSz = Db_BufSz + 1 ! Can Db_BufSz = Db_BufSz + 1 ! Cat Db_BufSz = Db_BufSz + 1 ! Cdn Db_BufSz = Db_BufSz + 1 ! Cdt + Int_BufSz = Int_BufSz + 1 ! nEApoints + Db_BufSz = Db_BufSz + SIZE(InData%stiffXs) ! stiffXs + Db_BufSz = Db_BufSz + SIZE(InData%stiffYs) ! stiffYs + Int_BufSz = Int_BufSz + 1 ! nBpoints + Db_BufSz = Db_BufSz + SIZE(InData%dampXs) ! dampXs + Db_BufSz = Db_BufSz + SIZE(InData%dampYs) ! dampYs + Int_BufSz = Int_BufSz + 1 ! nEIpoints + Db_BufSz = Db_BufSz + SIZE(InData%bstiffXs) ! bstiffXs + Db_BufSz = Db_BufSz + SIZE(InData%bstiffYs) ! bstiffYs IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) IF (ErrStat2 /= 0) THEN @@ -590,6 +1081,8 @@ SUBROUTINE MD_PackLineProp( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%BA Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%EI + Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%Can Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%Cat @@ -598,6 +1091,36 @@ SUBROUTINE MD_PackLineProp( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%Cdt Db_Xferred = Db_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nEApoints + Int_Xferred = Int_Xferred + 1 + DO i1 = LBOUND(InData%stiffXs,1), UBOUND(InData%stiffXs,1) + DbKiBuf(Db_Xferred) = InData%stiffXs(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%stiffYs,1), UBOUND(InData%stiffYs,1) + DbKiBuf(Db_Xferred) = InData%stiffYs(i1) + Db_Xferred = Db_Xferred + 1 + END DO + IntKiBuf(Int_Xferred) = InData%nBpoints + Int_Xferred = Int_Xferred + 1 + DO i1 = LBOUND(InData%dampXs,1), UBOUND(InData%dampXs,1) + DbKiBuf(Db_Xferred) = InData%dampXs(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%dampYs,1), UBOUND(InData%dampYs,1) + DbKiBuf(Db_Xferred) = InData%dampYs(i1) + Db_Xferred = Db_Xferred + 1 + END DO + IntKiBuf(Int_Xferred) = InData%nEIpoints + Int_Xferred = Int_Xferred + 1 + DO i1 = LBOUND(InData%bstiffXs,1), UBOUND(InData%bstiffXs,1) + DbKiBuf(Db_Xferred) = InData%bstiffXs(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%bstiffYs,1), UBOUND(InData%bstiffYs,1) + DbKiBuf(Db_Xferred) = InData%bstiffYs(i1) + Db_Xferred = Db_Xferred + 1 + END DO END SUBROUTINE MD_PackLineProp SUBROUTINE MD_UnPackLineProp( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) @@ -613,6 +1136,7 @@ SUBROUTINE MD_UnPackLineProp( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrM INTEGER(IntKi) :: Db_Xferred INTEGER(IntKi) :: Int_Xferred INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackLineProp' @@ -640,6 +1164,8 @@ SUBROUTINE MD_UnPackLineProp( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrM Db_Xferred = Db_Xferred + 1 OutData%BA = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 + OutData%EI = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 OutData%Can = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 OutData%Cat = DbKiBuf(Db_Xferred) @@ -648,90 +1174,92 @@ SUBROUTINE MD_UnPackLineProp( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrM Db_Xferred = Db_Xferred + 1 OutData%Cdt = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 + OutData%nEApoints = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + i1_l = LBOUND(OutData%stiffXs,1) + i1_u = UBOUND(OutData%stiffXs,1) + DO i1 = LBOUND(OutData%stiffXs,1), UBOUND(OutData%stiffXs,1) + OutData%stiffXs(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%stiffYs,1) + i1_u = UBOUND(OutData%stiffYs,1) + DO i1 = LBOUND(OutData%stiffYs,1), UBOUND(OutData%stiffYs,1) + OutData%stiffYs(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + OutData%nBpoints = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + i1_l = LBOUND(OutData%dampXs,1) + i1_u = UBOUND(OutData%dampXs,1) + DO i1 = LBOUND(OutData%dampXs,1), UBOUND(OutData%dampXs,1) + OutData%dampXs(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%dampYs,1) + i1_u = UBOUND(OutData%dampYs,1) + DO i1 = LBOUND(OutData%dampYs,1), UBOUND(OutData%dampYs,1) + OutData%dampYs(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + OutData%nEIpoints = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + i1_l = LBOUND(OutData%bstiffXs,1) + i1_u = UBOUND(OutData%bstiffXs,1) + DO i1 = LBOUND(OutData%bstiffXs,1), UBOUND(OutData%bstiffXs,1) + OutData%bstiffXs(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%bstiffYs,1) + i1_u = UBOUND(OutData%bstiffYs,1) + DO i1 = LBOUND(OutData%bstiffYs,1), UBOUND(OutData%bstiffYs,1) + OutData%bstiffYs(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO END SUBROUTINE MD_UnPackLineProp - SUBROUTINE MD_CopyConnect( SrcConnectData, DstConnectData, CtrlCode, ErrStat, ErrMsg ) - TYPE(MD_Connect), INTENT(IN) :: SrcConnectData - TYPE(MD_Connect), INTENT(INOUT) :: DstConnectData + SUBROUTINE MD_CopyRodProp( SrcRodPropData, DstRodPropData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_RodProp), INTENT(IN) :: SrcRodPropData + TYPE(MD_RodProp), INTENT(INOUT) :: DstRodPropData INTEGER(IntKi), INTENT(IN ) :: CtrlCode INTEGER(IntKi), INTENT( OUT) :: ErrStat CHARACTER(*), INTENT( OUT) :: ErrMsg ! Local INTEGER(IntKi) :: i,j,k - INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 - INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 - CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyConnect' + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyRodProp' ! ErrStat = ErrID_None ErrMsg = "" - DstConnectData%IdNum = SrcConnectData%IdNum - DstConnectData%type = SrcConnectData%type - DstConnectData%TypeNum = SrcConnectData%TypeNum -IF (ALLOCATED(SrcConnectData%AttachedFairs)) THEN - i1_l = LBOUND(SrcConnectData%AttachedFairs,1) - i1_u = UBOUND(SrcConnectData%AttachedFairs,1) - IF (.NOT. ALLOCATED(DstConnectData%AttachedFairs)) THEN - ALLOCATE(DstConnectData%AttachedFairs(i1_l:i1_u),STAT=ErrStat2) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstConnectData%AttachedFairs.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF - END IF - DstConnectData%AttachedFairs = SrcConnectData%AttachedFairs -ENDIF -IF (ALLOCATED(SrcConnectData%AttachedAnchs)) THEN - i1_l = LBOUND(SrcConnectData%AttachedAnchs,1) - i1_u = UBOUND(SrcConnectData%AttachedAnchs,1) - IF (.NOT. ALLOCATED(DstConnectData%AttachedAnchs)) THEN - ALLOCATE(DstConnectData%AttachedAnchs(i1_l:i1_u),STAT=ErrStat2) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstConnectData%AttachedAnchs.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF - END IF - DstConnectData%AttachedAnchs = SrcConnectData%AttachedAnchs -ENDIF - DstConnectData%conX = SrcConnectData%conX - DstConnectData%conY = SrcConnectData%conY - DstConnectData%conZ = SrcConnectData%conZ - DstConnectData%conM = SrcConnectData%conM - DstConnectData%conV = SrcConnectData%conV - DstConnectData%conFX = SrcConnectData%conFX - DstConnectData%conFY = SrcConnectData%conFY - DstConnectData%conFZ = SrcConnectData%conFZ - DstConnectData%conCa = SrcConnectData%conCa - DstConnectData%conCdA = SrcConnectData%conCdA - DstConnectData%Ftot = SrcConnectData%Ftot - DstConnectData%Mtot = SrcConnectData%Mtot - DstConnectData%S = SrcConnectData%S - DstConnectData%r = SrcConnectData%r - DstConnectData%rd = SrcConnectData%rd - END SUBROUTINE MD_CopyConnect - - SUBROUTINE MD_DestroyConnect( ConnectData, ErrStat, ErrMsg ) - TYPE(MD_Connect), INTENT(INOUT) :: ConnectData + DstRodPropData%IdNum = SrcRodPropData%IdNum + DstRodPropData%name = SrcRodPropData%name + DstRodPropData%d = SrcRodPropData%d + DstRodPropData%w = SrcRodPropData%w + DstRodPropData%Can = SrcRodPropData%Can + DstRodPropData%Cat = SrcRodPropData%Cat + DstRodPropData%Cdn = SrcRodPropData%Cdn + DstRodPropData%Cdt = SrcRodPropData%Cdt + DstRodPropData%CdEnd = SrcRodPropData%CdEnd + DstRodPropData%CaEnd = SrcRodPropData%CaEnd + END SUBROUTINE MD_CopyRodProp + + SUBROUTINE MD_DestroyRodProp( RodPropData, ErrStat, ErrMsg ) + TYPE(MD_RodProp), INTENT(INOUT) :: RodPropData INTEGER(IntKi), INTENT( OUT) :: ErrStat CHARACTER(*), INTENT( OUT) :: ErrMsg - CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyConnect' + CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyRodProp' INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 ! ErrStat = ErrID_None ErrMsg = "" -IF (ALLOCATED(ConnectData%AttachedFairs)) THEN - DEALLOCATE(ConnectData%AttachedFairs) -ENDIF -IF (ALLOCATED(ConnectData%AttachedAnchs)) THEN - DEALLOCATE(ConnectData%AttachedAnchs) -ENDIF - END SUBROUTINE MD_DestroyConnect + END SUBROUTINE MD_DestroyRodProp - SUBROUTINE MD_PackConnect( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + SUBROUTINE MD_PackRodProp( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) - TYPE(MD_Connect), INTENT(IN) :: InData + TYPE(MD_RodProp), INTENT(IN) :: InData INTEGER(IntKi), INTENT( OUT) :: ErrStat CHARACTER(*), INTENT( OUT) :: ErrMsg LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly @@ -746,7 +1274,7 @@ SUBROUTINE MD_PackConnect( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 - CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackConnect' + CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackRodProp' ! buffers to store subtypes, if any REAL(ReKi), ALLOCATABLE :: Re_Buf(:) REAL(DbKi), ALLOCATABLE :: Db_Buf(:) @@ -763,33 +1291,15 @@ SUBROUTINE MD_PackConnect( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Db_BufSz = 0 Int_BufSz = 0 Int_BufSz = Int_BufSz + 1 ! IdNum - Int_BufSz = Int_BufSz + 1*LEN(InData%type) ! type - Int_BufSz = Int_BufSz + 1 ! TypeNum - Int_BufSz = Int_BufSz + 1 ! AttachedFairs allocated yes/no - IF ( ALLOCATED(InData%AttachedFairs) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! AttachedFairs upper/lower bounds for each dimension - Int_BufSz = Int_BufSz + SIZE(InData%AttachedFairs) ! AttachedFairs - END IF - Int_BufSz = Int_BufSz + 1 ! AttachedAnchs allocated yes/no - IF ( ALLOCATED(InData%AttachedAnchs) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! AttachedAnchs upper/lower bounds for each dimension - Int_BufSz = Int_BufSz + SIZE(InData%AttachedAnchs) ! AttachedAnchs - END IF - Db_BufSz = Db_BufSz + 1 ! conX - Db_BufSz = Db_BufSz + 1 ! conY - Db_BufSz = Db_BufSz + 1 ! conZ - Db_BufSz = Db_BufSz + 1 ! conM - Db_BufSz = Db_BufSz + 1 ! conV - Db_BufSz = Db_BufSz + 1 ! conFX - Db_BufSz = Db_BufSz + 1 ! conFY - Db_BufSz = Db_BufSz + 1 ! conFZ - Db_BufSz = Db_BufSz + 1 ! conCa - Db_BufSz = Db_BufSz + 1 ! conCdA - Db_BufSz = Db_BufSz + SIZE(InData%Ftot) ! Ftot - Db_BufSz = Db_BufSz + SIZE(InData%Mtot) ! Mtot - Db_BufSz = Db_BufSz + SIZE(InData%S) ! S - Db_BufSz = Db_BufSz + SIZE(InData%r) ! r - Db_BufSz = Db_BufSz + SIZE(InData%rd) ! rd + Int_BufSz = Int_BufSz + 1*LEN(InData%name) ! name + Db_BufSz = Db_BufSz + 1 ! d + Db_BufSz = Db_BufSz + 1 ! w + Db_BufSz = Db_BufSz + 1 ! Can + Db_BufSz = Db_BufSz + 1 ! Cat + Db_BufSz = Db_BufSz + 1 ! Cdn + Db_BufSz = Db_BufSz + 1 ! Cdt + Db_BufSz = Db_BufSz + 1 ! CdEnd + Db_BufSz = Db_BufSz + 1 ! CaEnd IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) IF (ErrStat2 /= 0) THEN @@ -819,93 +1329,33 @@ SUBROUTINE MD_PackConnect( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, IntKiBuf(Int_Xferred) = InData%IdNum Int_Xferred = Int_Xferred + 1 - DO I = 1, LEN(InData%type) - IntKiBuf(Int_Xferred) = ICHAR(InData%type(I:I), IntKi) + DO I = 1, LEN(InData%name) + IntKiBuf(Int_Xferred) = ICHAR(InData%name(I:I), IntKi) Int_Xferred = Int_Xferred + 1 END DO ! I - IntKiBuf(Int_Xferred) = InData%TypeNum - Int_Xferred = Int_Xferred + 1 - IF ( .NOT. ALLOCATED(InData%AttachedFairs) ) THEN - IntKiBuf( Int_Xferred ) = 0 - Int_Xferred = Int_Xferred + 1 - ELSE - IntKiBuf( Int_Xferred ) = 1 - Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%AttachedFairs,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%AttachedFairs,1) - Int_Xferred = Int_Xferred + 2 - - DO i1 = LBOUND(InData%AttachedFairs,1), UBOUND(InData%AttachedFairs,1) - IntKiBuf(Int_Xferred) = InData%AttachedFairs(i1) - Int_Xferred = Int_Xferred + 1 - END DO - END IF - IF ( .NOT. ALLOCATED(InData%AttachedAnchs) ) THEN - IntKiBuf( Int_Xferred ) = 0 - Int_Xferred = Int_Xferred + 1 - ELSE - IntKiBuf( Int_Xferred ) = 1 - Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%AttachedAnchs,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%AttachedAnchs,1) - Int_Xferred = Int_Xferred + 2 - - DO i1 = LBOUND(InData%AttachedAnchs,1), UBOUND(InData%AttachedAnchs,1) - IntKiBuf(Int_Xferred) = InData%AttachedAnchs(i1) - Int_Xferred = Int_Xferred + 1 - END DO - END IF - DbKiBuf(Db_Xferred) = InData%conX - Db_Xferred = Db_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%conY - Db_Xferred = Db_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%conZ + DbKiBuf(Db_Xferred) = InData%d Db_Xferred = Db_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%conM + DbKiBuf(Db_Xferred) = InData%w Db_Xferred = Db_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%conV + DbKiBuf(Db_Xferred) = InData%Can Db_Xferred = Db_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%conFX + DbKiBuf(Db_Xferred) = InData%Cat Db_Xferred = Db_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%conFY + DbKiBuf(Db_Xferred) = InData%Cdn Db_Xferred = Db_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%conFZ + DbKiBuf(Db_Xferred) = InData%Cdt Db_Xferred = Db_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%conCa + DbKiBuf(Db_Xferred) = InData%CdEnd Db_Xferred = Db_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%conCdA + DbKiBuf(Db_Xferred) = InData%CaEnd Db_Xferred = Db_Xferred + 1 - DO i1 = LBOUND(InData%Ftot,1), UBOUND(InData%Ftot,1) - DbKiBuf(Db_Xferred) = InData%Ftot(i1) - Db_Xferred = Db_Xferred + 1 - END DO - DO i2 = LBOUND(InData%Mtot,2), UBOUND(InData%Mtot,2) - DO i1 = LBOUND(InData%Mtot,1), UBOUND(InData%Mtot,1) - DbKiBuf(Db_Xferred) = InData%Mtot(i1,i2) - Db_Xferred = Db_Xferred + 1 - END DO - END DO - DO i2 = LBOUND(InData%S,2), UBOUND(InData%S,2) - DO i1 = LBOUND(InData%S,1), UBOUND(InData%S,1) - DbKiBuf(Db_Xferred) = InData%S(i1,i2) - Db_Xferred = Db_Xferred + 1 - END DO - END DO - DO i1 = LBOUND(InData%r,1), UBOUND(InData%r,1) - DbKiBuf(Db_Xferred) = InData%r(i1) - Db_Xferred = Db_Xferred + 1 - END DO - DO i1 = LBOUND(InData%rd,1), UBOUND(InData%rd,1) - DbKiBuf(Db_Xferred) = InData%rd(i1) - Db_Xferred = Db_Xferred + 1 - END DO - END SUBROUTINE MD_PackConnect + END SUBROUTINE MD_PackRodProp - SUBROUTINE MD_UnPackConnect( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + SUBROUTINE MD_UnPackRodProp( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) - TYPE(MD_Connect), INTENT(INOUT) :: OutData + TYPE(MD_RodProp), INTENT(INOUT) :: OutData INTEGER(IntKi), INTENT( OUT) :: ErrStat CHARACTER(*), INTENT( OUT) :: ErrMsg ! Local variables @@ -914,11 +1364,9 @@ SUBROUTINE MD_UnPackConnect( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMs INTEGER(IntKi) :: Db_Xferred INTEGER(IntKi) :: Int_Xferred INTEGER(IntKi) :: i - INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 - INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 - CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackConnect' + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackRodProp' ! buffers to store meshes, if any REAL(ReKi), ALLOCATABLE :: Re_Buf(:) REAL(DbKi), ALLOCATABLE :: Db_Buf(:) @@ -931,111 +1379,31 @@ SUBROUTINE MD_UnPackConnect( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMs Int_Xferred = 1 OutData%IdNum = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 - DO I = 1, LEN(OutData%type) - OutData%type(I:I) = CHAR(IntKiBuf(Int_Xferred)) + DO I = 1, LEN(OutData%name) + OutData%name(I:I) = CHAR(IntKiBuf(Int_Xferred)) Int_Xferred = Int_Xferred + 1 END DO ! I - OutData%TypeNum = IntKiBuf(Int_Xferred) - Int_Xferred = Int_Xferred + 1 - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! AttachedFairs not allocated - Int_Xferred = Int_Xferred + 1 - ELSE - Int_Xferred = Int_Xferred + 1 - i1_l = IntKiBuf( Int_Xferred ) - i1_u = IntKiBuf( Int_Xferred + 1) - Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%AttachedFairs)) DEALLOCATE(OutData%AttachedFairs) - ALLOCATE(OutData%AttachedFairs(i1_l:i1_u),STAT=ErrStat2) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%AttachedFairs.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF - DO i1 = LBOUND(OutData%AttachedFairs,1), UBOUND(OutData%AttachedFairs,1) - OutData%AttachedFairs(i1) = IntKiBuf(Int_Xferred) - Int_Xferred = Int_Xferred + 1 - END DO - END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! AttachedAnchs not allocated - Int_Xferred = Int_Xferred + 1 - ELSE - Int_Xferred = Int_Xferred + 1 - i1_l = IntKiBuf( Int_Xferred ) - i1_u = IntKiBuf( Int_Xferred + 1) - Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%AttachedAnchs)) DEALLOCATE(OutData%AttachedAnchs) - ALLOCATE(OutData%AttachedAnchs(i1_l:i1_u),STAT=ErrStat2) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%AttachedAnchs.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF - DO i1 = LBOUND(OutData%AttachedAnchs,1), UBOUND(OutData%AttachedAnchs,1) - OutData%AttachedAnchs(i1) = IntKiBuf(Int_Xferred) - Int_Xferred = Int_Xferred + 1 - END DO - END IF - OutData%conX = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 - OutData%conY = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 - OutData%conZ = DbKiBuf(Db_Xferred) + OutData%d = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 - OutData%conM = DbKiBuf(Db_Xferred) + OutData%w = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 - OutData%conV = DbKiBuf(Db_Xferred) + OutData%Can = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 - OutData%conFX = DbKiBuf(Db_Xferred) + OutData%Cat = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 - OutData%conFY = DbKiBuf(Db_Xferred) + OutData%Cdn = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 - OutData%conFZ = DbKiBuf(Db_Xferred) + OutData%Cdt = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 - OutData%conCa = DbKiBuf(Db_Xferred) + OutData%CdEnd = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 - OutData%conCdA = DbKiBuf(Db_Xferred) + OutData%CaEnd = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 - i1_l = LBOUND(OutData%Ftot,1) - i1_u = UBOUND(OutData%Ftot,1) - DO i1 = LBOUND(OutData%Ftot,1), UBOUND(OutData%Ftot,1) - OutData%Ftot(i1) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 - END DO - i1_l = LBOUND(OutData%Mtot,1) - i1_u = UBOUND(OutData%Mtot,1) - i2_l = LBOUND(OutData%Mtot,2) - i2_u = UBOUND(OutData%Mtot,2) - DO i2 = LBOUND(OutData%Mtot,2), UBOUND(OutData%Mtot,2) - DO i1 = LBOUND(OutData%Mtot,1), UBOUND(OutData%Mtot,1) - OutData%Mtot(i1,i2) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 - END DO - END DO - i1_l = LBOUND(OutData%S,1) - i1_u = UBOUND(OutData%S,1) - i2_l = LBOUND(OutData%S,2) - i2_u = UBOUND(OutData%S,2) - DO i2 = LBOUND(OutData%S,2), UBOUND(OutData%S,2) - DO i1 = LBOUND(OutData%S,1), UBOUND(OutData%S,1) - OutData%S(i1,i2) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 - END DO - END DO - i1_l = LBOUND(OutData%r,1) - i1_u = UBOUND(OutData%r,1) - DO i1 = LBOUND(OutData%r,1), UBOUND(OutData%r,1) - OutData%r(i1) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 - END DO - i1_l = LBOUND(OutData%rd,1) - i1_u = UBOUND(OutData%rd,1) - DO i1 = LBOUND(OutData%rd,1), UBOUND(OutData%rd,1) - OutData%rd(i1) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 - END DO - END SUBROUTINE MD_UnPackConnect + END SUBROUTINE MD_UnPackRodProp - SUBROUTINE MD_CopyLine( SrcLineData, DstLineData, CtrlCode, ErrStat, ErrMsg ) - TYPE(MD_Line), INTENT(IN) :: SrcLineData - TYPE(MD_Line), INTENT(INOUT) :: DstLineData + SUBROUTINE MD_CopyBody( SrcBodyData, DstBodyData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_Body), INTENT(IN) :: SrcBodyData + TYPE(MD_Body), INTENT(INOUT) :: DstBodyData INTEGER(IntKi), INTENT(IN ) :: CtrlCode INTEGER(IntKi), INTENT( OUT) :: ErrStat CHARACTER(*), INTENT( OUT) :: ErrMsg @@ -1043,56 +1411,2501 @@ SUBROUTINE MD_CopyLine( SrcLineData, DstLineData, CtrlCode, ErrStat, ErrMsg ) INTEGER(IntKi) :: i,j,k INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 - INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 - CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyLine' + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyBody' ! ErrStat = ErrID_None ErrMsg = "" - DstLineData%IdNum = SrcLineData%IdNum - DstLineData%type = SrcLineData%type - DstLineData%OutFlagList = SrcLineData%OutFlagList - DstLineData%CtrlChan = SrcLineData%CtrlChan - DstLineData%FairConnect = SrcLineData%FairConnect - DstLineData%AnchConnect = SrcLineData%AnchConnect - DstLineData%PropsIdNum = SrcLineData%PropsIdNum - DstLineData%N = SrcLineData%N - DstLineData%UnstrLen = SrcLineData%UnstrLen - DstLineData%BA = SrcLineData%BA -IF (ALLOCATED(SrcLineData%r)) THEN - i1_l = LBOUND(SrcLineData%r,1) - i1_u = UBOUND(SrcLineData%r,1) - i2_l = LBOUND(SrcLineData%r,2) - i2_u = UBOUND(SrcLineData%r,2) - IF (.NOT. ALLOCATED(DstLineData%r)) THEN - ALLOCATE(DstLineData%r(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%r.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF + DstBodyData%IdNum = SrcBodyData%IdNum + DstBodyData%typeNum = SrcBodyData%typeNum + DstBodyData%AttachedC = SrcBodyData%AttachedC + DstBodyData%AttachedR = SrcBodyData%AttachedR + DstBodyData%nAttachedC = SrcBodyData%nAttachedC + DstBodyData%nAttachedR = SrcBodyData%nAttachedR + DstBodyData%rConnectRel = SrcBodyData%rConnectRel + DstBodyData%r6RodRel = SrcBodyData%r6RodRel + DstBodyData%bodyM = SrcBodyData%bodyM + DstBodyData%bodyV = SrcBodyData%bodyV + DstBodyData%bodyI = SrcBodyData%bodyI + DstBodyData%bodyCdA = SrcBodyData%bodyCdA + DstBodyData%bodyCa = SrcBodyData%bodyCa + DstBodyData%time = SrcBodyData%time + DstBodyData%r6 = SrcBodyData%r6 + DstBodyData%v6 = SrcBodyData%v6 + DstBodyData%a6 = SrcBodyData%a6 + DstBodyData%U = SrcBodyData%U + DstBodyData%Ud = SrcBodyData%Ud + DstBodyData%zeta = SrcBodyData%zeta + DstBodyData%F6net = SrcBodyData%F6net + DstBodyData%M6net = SrcBodyData%M6net + DstBodyData%M = SrcBodyData%M + DstBodyData%M0 = SrcBodyData%M0 + DstBodyData%OrMat = SrcBodyData%OrMat + DstBodyData%rCG = SrcBodyData%rCG + END SUBROUTINE MD_CopyBody + + SUBROUTINE MD_DestroyBody( BodyData, ErrStat, ErrMsg ) + TYPE(MD_Body), INTENT(INOUT) :: BodyData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyBody' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE MD_DestroyBody + + SUBROUTINE MD_PackBody( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(MD_Body), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackBody' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! IdNum + Int_BufSz = Int_BufSz + 1 ! typeNum + Int_BufSz = Int_BufSz + SIZE(InData%AttachedC) ! AttachedC + Int_BufSz = Int_BufSz + SIZE(InData%AttachedR) ! AttachedR + Int_BufSz = Int_BufSz + 1 ! nAttachedC + Int_BufSz = Int_BufSz + 1 ! nAttachedR + Db_BufSz = Db_BufSz + SIZE(InData%rConnectRel) ! rConnectRel + Db_BufSz = Db_BufSz + SIZE(InData%r6RodRel) ! r6RodRel + Db_BufSz = Db_BufSz + 1 ! bodyM + Db_BufSz = Db_BufSz + 1 ! bodyV + Db_BufSz = Db_BufSz + SIZE(InData%bodyI) ! bodyI + Db_BufSz = Db_BufSz + SIZE(InData%bodyCdA) ! bodyCdA + Db_BufSz = Db_BufSz + SIZE(InData%bodyCa) ! bodyCa + Db_BufSz = Db_BufSz + 1 ! time + Db_BufSz = Db_BufSz + SIZE(InData%r6) ! r6 + Db_BufSz = Db_BufSz + SIZE(InData%v6) ! v6 + Db_BufSz = Db_BufSz + SIZE(InData%a6) ! a6 + Db_BufSz = Db_BufSz + SIZE(InData%U) ! U + Db_BufSz = Db_BufSz + SIZE(InData%Ud) ! Ud + Db_BufSz = Db_BufSz + 1 ! zeta + Db_BufSz = Db_BufSz + SIZE(InData%F6net) ! F6net + Db_BufSz = Db_BufSz + SIZE(InData%M6net) ! M6net + Db_BufSz = Db_BufSz + SIZE(InData%M) ! M + Db_BufSz = Db_BufSz + SIZE(InData%M0) ! M0 + Db_BufSz = Db_BufSz + SIZE(InData%OrMat) ! OrMat + Db_BufSz = Db_BufSz + SIZE(InData%rCG) ! rCG + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF END IF - DstLineData%r = SrcLineData%r -ENDIF -IF (ALLOCATED(SrcLineData%rd)) THEN - i1_l = LBOUND(SrcLineData%rd,1) - i1_u = UBOUND(SrcLineData%rd,1) - i2_l = LBOUND(SrcLineData%rd,2) - i2_u = UBOUND(SrcLineData%rd,2) - IF (.NOT. ALLOCATED(DstLineData%rd)) THEN - ALLOCATE(DstLineData%rd(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%rd.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF END IF - DstLineData%rd = SrcLineData%rd -ENDIF -IF (ALLOCATED(SrcLineData%q)) THEN - i1_l = LBOUND(SrcLineData%q,1) - i1_u = UBOUND(SrcLineData%q,1) - i2_l = LBOUND(SrcLineData%q,2) - i2_u = UBOUND(SrcLineData%q,2) + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IntKiBuf(Int_Xferred) = InData%IdNum + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%typeNum + Int_Xferred = Int_Xferred + 1 + DO i1 = LBOUND(InData%AttachedC,1), UBOUND(InData%AttachedC,1) + IntKiBuf(Int_Xferred) = InData%AttachedC(i1) + Int_Xferred = Int_Xferred + 1 + END DO + DO i1 = LBOUND(InData%AttachedR,1), UBOUND(InData%AttachedR,1) + IntKiBuf(Int_Xferred) = InData%AttachedR(i1) + Int_Xferred = Int_Xferred + 1 + END DO + IntKiBuf(Int_Xferred) = InData%nAttachedC + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nAttachedR + Int_Xferred = Int_Xferred + 1 + DO i2 = LBOUND(InData%rConnectRel,2), UBOUND(InData%rConnectRel,2) + DO i1 = LBOUND(InData%rConnectRel,1), UBOUND(InData%rConnectRel,1) + DbKiBuf(Db_Xferred) = InData%rConnectRel(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + DO i2 = LBOUND(InData%r6RodRel,2), UBOUND(InData%r6RodRel,2) + DO i1 = LBOUND(InData%r6RodRel,1), UBOUND(InData%r6RodRel,1) + DbKiBuf(Db_Xferred) = InData%r6RodRel(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + DbKiBuf(Db_Xferred) = InData%bodyM + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%bodyV + Db_Xferred = Db_Xferred + 1 + DO i1 = LBOUND(InData%bodyI,1), UBOUND(InData%bodyI,1) + DbKiBuf(Db_Xferred) = InData%bodyI(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%bodyCdA,1), UBOUND(InData%bodyCdA,1) + DbKiBuf(Db_Xferred) = InData%bodyCdA(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%bodyCa,1), UBOUND(InData%bodyCa,1) + DbKiBuf(Db_Xferred) = InData%bodyCa(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DbKiBuf(Db_Xferred) = InData%time + Db_Xferred = Db_Xferred + 1 + DO i1 = LBOUND(InData%r6,1), UBOUND(InData%r6,1) + DbKiBuf(Db_Xferred) = InData%r6(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%v6,1), UBOUND(InData%v6,1) + DbKiBuf(Db_Xferred) = InData%v6(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%a6,1), UBOUND(InData%a6,1) + DbKiBuf(Db_Xferred) = InData%a6(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%U,1), UBOUND(InData%U,1) + DbKiBuf(Db_Xferred) = InData%U(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%Ud,1), UBOUND(InData%Ud,1) + DbKiBuf(Db_Xferred) = InData%Ud(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DbKiBuf(Db_Xferred) = InData%zeta + Db_Xferred = Db_Xferred + 1 + DO i1 = LBOUND(InData%F6net,1), UBOUND(InData%F6net,1) + DbKiBuf(Db_Xferred) = InData%F6net(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i2 = LBOUND(InData%M6net,2), UBOUND(InData%M6net,2) + DO i1 = LBOUND(InData%M6net,1), UBOUND(InData%M6net,1) + DbKiBuf(Db_Xferred) = InData%M6net(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + DO i2 = LBOUND(InData%M,2), UBOUND(InData%M,2) + DO i1 = LBOUND(InData%M,1), UBOUND(InData%M,1) + DbKiBuf(Db_Xferred) = InData%M(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + DO i2 = LBOUND(InData%M0,2), UBOUND(InData%M0,2) + DO i1 = LBOUND(InData%M0,1), UBOUND(InData%M0,1) + DbKiBuf(Db_Xferred) = InData%M0(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + DO i2 = LBOUND(InData%OrMat,2), UBOUND(InData%OrMat,2) + DO i1 = LBOUND(InData%OrMat,1), UBOUND(InData%OrMat,1) + DbKiBuf(Db_Xferred) = InData%OrMat(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + DO i1 = LBOUND(InData%rCG,1), UBOUND(InData%rCG,1) + DbKiBuf(Db_Xferred) = InData%rCG(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END SUBROUTINE MD_PackBody + + SUBROUTINE MD_UnPackBody( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_Body), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackBody' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%IdNum = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%typeNum = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + i1_l = LBOUND(OutData%AttachedC,1) + i1_u = UBOUND(OutData%AttachedC,1) + DO i1 = LBOUND(OutData%AttachedC,1), UBOUND(OutData%AttachedC,1) + OutData%AttachedC(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + i1_l = LBOUND(OutData%AttachedR,1) + i1_u = UBOUND(OutData%AttachedR,1) + DO i1 = LBOUND(OutData%AttachedR,1), UBOUND(OutData%AttachedR,1) + OutData%AttachedR(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + OutData%nAttachedC = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nAttachedR = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + i1_l = LBOUND(OutData%rConnectRel,1) + i1_u = UBOUND(OutData%rConnectRel,1) + i2_l = LBOUND(OutData%rConnectRel,2) + i2_u = UBOUND(OutData%rConnectRel,2) + DO i2 = LBOUND(OutData%rConnectRel,2), UBOUND(OutData%rConnectRel,2) + DO i1 = LBOUND(OutData%rConnectRel,1), UBOUND(OutData%rConnectRel,1) + OutData%rConnectRel(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + i1_l = LBOUND(OutData%r6RodRel,1) + i1_u = UBOUND(OutData%r6RodRel,1) + i2_l = LBOUND(OutData%r6RodRel,2) + i2_u = UBOUND(OutData%r6RodRel,2) + DO i2 = LBOUND(OutData%r6RodRel,2), UBOUND(OutData%r6RodRel,2) + DO i1 = LBOUND(OutData%r6RodRel,1), UBOUND(OutData%r6RodRel,1) + OutData%r6RodRel(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + OutData%bodyM = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%bodyV = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + i1_l = LBOUND(OutData%bodyI,1) + i1_u = UBOUND(OutData%bodyI,1) + DO i1 = LBOUND(OutData%bodyI,1), UBOUND(OutData%bodyI,1) + OutData%bodyI(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%bodyCdA,1) + i1_u = UBOUND(OutData%bodyCdA,1) + DO i1 = LBOUND(OutData%bodyCdA,1), UBOUND(OutData%bodyCdA,1) + OutData%bodyCdA(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%bodyCa,1) + i1_u = UBOUND(OutData%bodyCa,1) + DO i1 = LBOUND(OutData%bodyCa,1), UBOUND(OutData%bodyCa,1) + OutData%bodyCa(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + OutData%time = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + i1_l = LBOUND(OutData%r6,1) + i1_u = UBOUND(OutData%r6,1) + DO i1 = LBOUND(OutData%r6,1), UBOUND(OutData%r6,1) + OutData%r6(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%v6,1) + i1_u = UBOUND(OutData%v6,1) + DO i1 = LBOUND(OutData%v6,1), UBOUND(OutData%v6,1) + OutData%v6(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%a6,1) + i1_u = UBOUND(OutData%a6,1) + DO i1 = LBOUND(OutData%a6,1), UBOUND(OutData%a6,1) + OutData%a6(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%U,1) + i1_u = UBOUND(OutData%U,1) + DO i1 = LBOUND(OutData%U,1), UBOUND(OutData%U,1) + OutData%U(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%Ud,1) + i1_u = UBOUND(OutData%Ud,1) + DO i1 = LBOUND(OutData%Ud,1), UBOUND(OutData%Ud,1) + OutData%Ud(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + OutData%zeta = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + i1_l = LBOUND(OutData%F6net,1) + i1_u = UBOUND(OutData%F6net,1) + DO i1 = LBOUND(OutData%F6net,1), UBOUND(OutData%F6net,1) + OutData%F6net(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%M6net,1) + i1_u = UBOUND(OutData%M6net,1) + i2_l = LBOUND(OutData%M6net,2) + i2_u = UBOUND(OutData%M6net,2) + DO i2 = LBOUND(OutData%M6net,2), UBOUND(OutData%M6net,2) + DO i1 = LBOUND(OutData%M6net,1), UBOUND(OutData%M6net,1) + OutData%M6net(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + i1_l = LBOUND(OutData%M,1) + i1_u = UBOUND(OutData%M,1) + i2_l = LBOUND(OutData%M,2) + i2_u = UBOUND(OutData%M,2) + DO i2 = LBOUND(OutData%M,2), UBOUND(OutData%M,2) + DO i1 = LBOUND(OutData%M,1), UBOUND(OutData%M,1) + OutData%M(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + i1_l = LBOUND(OutData%M0,1) + i1_u = UBOUND(OutData%M0,1) + i2_l = LBOUND(OutData%M0,2) + i2_u = UBOUND(OutData%M0,2) + DO i2 = LBOUND(OutData%M0,2), UBOUND(OutData%M0,2) + DO i1 = LBOUND(OutData%M0,1), UBOUND(OutData%M0,1) + OutData%M0(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + i1_l = LBOUND(OutData%OrMat,1) + i1_u = UBOUND(OutData%OrMat,1) + i2_l = LBOUND(OutData%OrMat,2) + i2_u = UBOUND(OutData%OrMat,2) + DO i2 = LBOUND(OutData%OrMat,2), UBOUND(OutData%OrMat,2) + DO i1 = LBOUND(OutData%OrMat,1), UBOUND(OutData%OrMat,1) + OutData%OrMat(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + i1_l = LBOUND(OutData%rCG,1) + i1_u = UBOUND(OutData%rCG,1) + DO i1 = LBOUND(OutData%rCG,1), UBOUND(OutData%rCG,1) + OutData%rCG(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END SUBROUTINE MD_UnPackBody + + SUBROUTINE MD_CopyConnect( SrcConnectData, DstConnectData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_Connect), INTENT(IN) :: SrcConnectData + TYPE(MD_Connect), INTENT(INOUT) :: DstConnectData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyConnect' +! + ErrStat = ErrID_None + ErrMsg = "" + DstConnectData%IdNum = SrcConnectData%IdNum + DstConnectData%type = SrcConnectData%type + DstConnectData%typeNum = SrcConnectData%typeNum + DstConnectData%Attached = SrcConnectData%Attached + DstConnectData%Top = SrcConnectData%Top + DstConnectData%nAttached = SrcConnectData%nAttached + DstConnectData%conX = SrcConnectData%conX + DstConnectData%conY = SrcConnectData%conY + DstConnectData%conZ = SrcConnectData%conZ + DstConnectData%conM = SrcConnectData%conM + DstConnectData%conV = SrcConnectData%conV + DstConnectData%conFX = SrcConnectData%conFX + DstConnectData%conFY = SrcConnectData%conFY + DstConnectData%conFZ = SrcConnectData%conFZ + DstConnectData%conCa = SrcConnectData%conCa + DstConnectData%conCdA = SrcConnectData%conCdA + DstConnectData%time = SrcConnectData%time + DstConnectData%r = SrcConnectData%r + DstConnectData%rd = SrcConnectData%rd + DstConnectData%a = SrcConnectData%a + DstConnectData%U = SrcConnectData%U + DstConnectData%Ud = SrcConnectData%Ud + DstConnectData%zeta = SrcConnectData%zeta +IF (ALLOCATED(SrcConnectData%PDyn)) THEN + i1_l = LBOUND(SrcConnectData%PDyn,1) + i1_u = UBOUND(SrcConnectData%PDyn,1) + IF (.NOT. ALLOCATED(DstConnectData%PDyn)) THEN + ALLOCATE(DstConnectData%PDyn(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstConnectData%PDyn.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstConnectData%PDyn = SrcConnectData%PDyn +ENDIF + DstConnectData%Fnet = SrcConnectData%Fnet + DstConnectData%M = SrcConnectData%M + END SUBROUTINE MD_CopyConnect + + SUBROUTINE MD_DestroyConnect( ConnectData, ErrStat, ErrMsg ) + TYPE(MD_Connect), INTENT(INOUT) :: ConnectData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyConnect' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(ConnectData%PDyn)) THEN + DEALLOCATE(ConnectData%PDyn) +ENDIF + END SUBROUTINE MD_DestroyConnect + + SUBROUTINE MD_PackConnect( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(MD_Connect), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackConnect' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! IdNum + Int_BufSz = Int_BufSz + 1*LEN(InData%type) ! type + Int_BufSz = Int_BufSz + 1 ! typeNum + Int_BufSz = Int_BufSz + SIZE(InData%Attached) ! Attached + Int_BufSz = Int_BufSz + SIZE(InData%Top) ! Top + Int_BufSz = Int_BufSz + 1 ! nAttached + Db_BufSz = Db_BufSz + 1 ! conX + Db_BufSz = Db_BufSz + 1 ! conY + Db_BufSz = Db_BufSz + 1 ! conZ + Db_BufSz = Db_BufSz + 1 ! conM + Db_BufSz = Db_BufSz + 1 ! conV + Db_BufSz = Db_BufSz + 1 ! conFX + Db_BufSz = Db_BufSz + 1 ! conFY + Db_BufSz = Db_BufSz + 1 ! conFZ + Db_BufSz = Db_BufSz + 1 ! conCa + Db_BufSz = Db_BufSz + 1 ! conCdA + Db_BufSz = Db_BufSz + 1 ! time + Db_BufSz = Db_BufSz + SIZE(InData%r) ! r + Db_BufSz = Db_BufSz + SIZE(InData%rd) ! rd + Db_BufSz = Db_BufSz + SIZE(InData%a) ! a + Db_BufSz = Db_BufSz + SIZE(InData%U) ! U + Db_BufSz = Db_BufSz + SIZE(InData%Ud) ! Ud + Db_BufSz = Db_BufSz + 1 ! zeta + Int_BufSz = Int_BufSz + 1 ! PDyn allocated yes/no + IF ( ALLOCATED(InData%PDyn) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! PDyn upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%PDyn) ! PDyn + END IF + Db_BufSz = Db_BufSz + SIZE(InData%Fnet) ! Fnet + Db_BufSz = Db_BufSz + SIZE(InData%M) ! M + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IntKiBuf(Int_Xferred) = InData%IdNum + Int_Xferred = Int_Xferred + 1 + DO I = 1, LEN(InData%type) + IntKiBuf(Int_Xferred) = ICHAR(InData%type(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + IntKiBuf(Int_Xferred) = InData%typeNum + Int_Xferred = Int_Xferred + 1 + DO i1 = LBOUND(InData%Attached,1), UBOUND(InData%Attached,1) + IntKiBuf(Int_Xferred) = InData%Attached(i1) + Int_Xferred = Int_Xferred + 1 + END DO + DO i1 = LBOUND(InData%Top,1), UBOUND(InData%Top,1) + IntKiBuf(Int_Xferred) = InData%Top(i1) + Int_Xferred = Int_Xferred + 1 + END DO + IntKiBuf(Int_Xferred) = InData%nAttached + Int_Xferred = Int_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%conX + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%conY + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%conZ + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%conM + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%conV + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%conFX + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%conFY + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%conFZ + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%conCa + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%conCdA + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%time + Db_Xferred = Db_Xferred + 1 + DO i1 = LBOUND(InData%r,1), UBOUND(InData%r,1) + DbKiBuf(Db_Xferred) = InData%r(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%rd,1), UBOUND(InData%rd,1) + DbKiBuf(Db_Xferred) = InData%rd(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%a,1), UBOUND(InData%a,1) + DbKiBuf(Db_Xferred) = InData%a(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%U,1), UBOUND(InData%U,1) + DbKiBuf(Db_Xferred) = InData%U(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%Ud,1), UBOUND(InData%Ud,1) + DbKiBuf(Db_Xferred) = InData%Ud(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DbKiBuf(Db_Xferred) = InData%zeta + Db_Xferred = Db_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%PDyn) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%PDyn,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PDyn,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%PDyn,1), UBOUND(InData%PDyn,1) + DbKiBuf(Db_Xferred) = InData%PDyn(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + DO i1 = LBOUND(InData%Fnet,1), UBOUND(InData%Fnet,1) + DbKiBuf(Db_Xferred) = InData%Fnet(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i2 = LBOUND(InData%M,2), UBOUND(InData%M,2) + DO i1 = LBOUND(InData%M,1), UBOUND(InData%M,1) + DbKiBuf(Db_Xferred) = InData%M(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END SUBROUTINE MD_PackConnect + + SUBROUTINE MD_UnPackConnect( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_Connect), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackConnect' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%IdNum = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + DO I = 1, LEN(OutData%type) + OutData%type(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + OutData%typeNum = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + i1_l = LBOUND(OutData%Attached,1) + i1_u = UBOUND(OutData%Attached,1) + DO i1 = LBOUND(OutData%Attached,1), UBOUND(OutData%Attached,1) + OutData%Attached(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + i1_l = LBOUND(OutData%Top,1) + i1_u = UBOUND(OutData%Top,1) + DO i1 = LBOUND(OutData%Top,1), UBOUND(OutData%Top,1) + OutData%Top(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + OutData%nAttached = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%conX = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%conY = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%conZ = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%conM = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%conV = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%conFX = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%conFY = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%conFZ = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%conCa = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%conCdA = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%time = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + i1_l = LBOUND(OutData%r,1) + i1_u = UBOUND(OutData%r,1) + DO i1 = LBOUND(OutData%r,1), UBOUND(OutData%r,1) + OutData%r(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%rd,1) + i1_u = UBOUND(OutData%rd,1) + DO i1 = LBOUND(OutData%rd,1), UBOUND(OutData%rd,1) + OutData%rd(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%a,1) + i1_u = UBOUND(OutData%a,1) + DO i1 = LBOUND(OutData%a,1), UBOUND(OutData%a,1) + OutData%a(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%U,1) + i1_u = UBOUND(OutData%U,1) + DO i1 = LBOUND(OutData%U,1), UBOUND(OutData%U,1) + OutData%U(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%Ud,1) + i1_u = UBOUND(OutData%Ud,1) + DO i1 = LBOUND(OutData%Ud,1), UBOUND(OutData%Ud,1) + OutData%Ud(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + OutData%zeta = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! PDyn not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%PDyn)) DEALLOCATE(OutData%PDyn) + ALLOCATE(OutData%PDyn(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%PDyn.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%PDyn,1), UBOUND(OutData%PDyn,1) + OutData%PDyn(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + i1_l = LBOUND(OutData%Fnet,1) + i1_u = UBOUND(OutData%Fnet,1) + DO i1 = LBOUND(OutData%Fnet,1), UBOUND(OutData%Fnet,1) + OutData%Fnet(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%M,1) + i1_u = UBOUND(OutData%M,1) + i2_l = LBOUND(OutData%M,2) + i2_u = UBOUND(OutData%M,2) + DO i2 = LBOUND(OutData%M,2), UBOUND(OutData%M,2) + DO i1 = LBOUND(OutData%M,1), UBOUND(OutData%M,1) + OutData%M(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END SUBROUTINE MD_UnPackConnect + + SUBROUTINE MD_CopyRod( SrcRodData, DstRodData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_Rod), INTENT(IN) :: SrcRodData + TYPE(MD_Rod), INTENT(INOUT) :: DstRodData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyRod' +! + ErrStat = ErrID_None + ErrMsg = "" + DstRodData%IdNum = SrcRodData%IdNum + DstRodData%type = SrcRodData%type + DstRodData%PropsIdNum = SrcRodData%PropsIdNum + DstRodData%typeNum = SrcRodData%typeNum + DstRodData%AttachedA = SrcRodData%AttachedA + DstRodData%AttachedB = SrcRodData%AttachedB + DstRodData%TopA = SrcRodData%TopA + DstRodData%TopB = SrcRodData%TopB + DstRodData%nAttachedA = SrcRodData%nAttachedA + DstRodData%nAttachedB = SrcRodData%nAttachedB + DstRodData%OutFlagList = SrcRodData%OutFlagList + DstRodData%N = SrcRodData%N + DstRodData%endTypeA = SrcRodData%endTypeA + DstRodData%endTypeB = SrcRodData%endTypeB + DstRodData%UnstrLen = SrcRodData%UnstrLen + DstRodData%mass = SrcRodData%mass + DstRodData%rho = SrcRodData%rho + DstRodData%d = SrcRodData%d + DstRodData%Can = SrcRodData%Can + DstRodData%Cat = SrcRodData%Cat + DstRodData%Cdn = SrcRodData%Cdn + DstRodData%Cdt = SrcRodData%Cdt + DstRodData%CdEnd = SrcRodData%CdEnd + DstRodData%CaEnd = SrcRodData%CaEnd + DstRodData%time = SrcRodData%time + DstRodData%roll = SrcRodData%roll + DstRodData%pitch = SrcRodData%pitch +IF (ALLOCATED(SrcRodData%r)) THEN + i1_l = LBOUND(SrcRodData%r,1) + i1_u = UBOUND(SrcRodData%r,1) + i2_l = LBOUND(SrcRodData%r,2) + i2_u = UBOUND(SrcRodData%r,2) + IF (.NOT. ALLOCATED(DstRodData%r)) THEN + ALLOCATE(DstRodData%r(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%r.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%r = SrcRodData%r +ENDIF +IF (ALLOCATED(SrcRodData%rd)) THEN + i1_l = LBOUND(SrcRodData%rd,1) + i1_u = UBOUND(SrcRodData%rd,1) + i2_l = LBOUND(SrcRodData%rd,2) + i2_u = UBOUND(SrcRodData%rd,2) + IF (.NOT. ALLOCATED(DstRodData%rd)) THEN + ALLOCATE(DstRodData%rd(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%rd.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%rd = SrcRodData%rd +ENDIF + DstRodData%q = SrcRodData%q +IF (ALLOCATED(SrcRodData%l)) THEN + i1_l = LBOUND(SrcRodData%l,1) + i1_u = UBOUND(SrcRodData%l,1) + IF (.NOT. ALLOCATED(DstRodData%l)) THEN + ALLOCATE(DstRodData%l(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%l.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%l = SrcRodData%l +ENDIF +IF (ALLOCATED(SrcRodData%V)) THEN + i1_l = LBOUND(SrcRodData%V,1) + i1_u = UBOUND(SrcRodData%V,1) + IF (.NOT. ALLOCATED(DstRodData%V)) THEN + ALLOCATE(DstRodData%V(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%V.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%V = SrcRodData%V +ENDIF +IF (ALLOCATED(SrcRodData%U)) THEN + i1_l = LBOUND(SrcRodData%U,1) + i1_u = UBOUND(SrcRodData%U,1) + i2_l = LBOUND(SrcRodData%U,2) + i2_u = UBOUND(SrcRodData%U,2) + IF (.NOT. ALLOCATED(DstRodData%U)) THEN + ALLOCATE(DstRodData%U(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%U.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%U = SrcRodData%U +ENDIF +IF (ALLOCATED(SrcRodData%Ud)) THEN + i1_l = LBOUND(SrcRodData%Ud,1) + i1_u = UBOUND(SrcRodData%Ud,1) + i2_l = LBOUND(SrcRodData%Ud,2) + i2_u = UBOUND(SrcRodData%Ud,2) + IF (.NOT. ALLOCATED(DstRodData%Ud)) THEN + ALLOCATE(DstRodData%Ud(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%Ud.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%Ud = SrcRodData%Ud +ENDIF +IF (ALLOCATED(SrcRodData%zeta)) THEN + i1_l = LBOUND(SrcRodData%zeta,1) + i1_u = UBOUND(SrcRodData%zeta,1) + IF (.NOT. ALLOCATED(DstRodData%zeta)) THEN + ALLOCATE(DstRodData%zeta(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%zeta.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%zeta = SrcRodData%zeta +ENDIF +IF (ALLOCATED(SrcRodData%PDyn)) THEN + i1_l = LBOUND(SrcRodData%PDyn,1) + i1_u = UBOUND(SrcRodData%PDyn,1) + IF (.NOT. ALLOCATED(DstRodData%PDyn)) THEN + ALLOCATE(DstRodData%PDyn(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%PDyn.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%PDyn = SrcRodData%PDyn +ENDIF +IF (ALLOCATED(SrcRodData%W)) THEN + i1_l = LBOUND(SrcRodData%W,1) + i1_u = UBOUND(SrcRodData%W,1) + i2_l = LBOUND(SrcRodData%W,2) + i2_u = UBOUND(SrcRodData%W,2) + IF (.NOT. ALLOCATED(DstRodData%W)) THEN + ALLOCATE(DstRodData%W(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%W.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%W = SrcRodData%W +ENDIF +IF (ALLOCATED(SrcRodData%Bo)) THEN + i1_l = LBOUND(SrcRodData%Bo,1) + i1_u = UBOUND(SrcRodData%Bo,1) + i2_l = LBOUND(SrcRodData%Bo,2) + i2_u = UBOUND(SrcRodData%Bo,2) + IF (.NOT. ALLOCATED(DstRodData%Bo)) THEN + ALLOCATE(DstRodData%Bo(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%Bo.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%Bo = SrcRodData%Bo +ENDIF +IF (ALLOCATED(SrcRodData%Pd)) THEN + i1_l = LBOUND(SrcRodData%Pd,1) + i1_u = UBOUND(SrcRodData%Pd,1) + i2_l = LBOUND(SrcRodData%Pd,2) + i2_u = UBOUND(SrcRodData%Pd,2) + IF (.NOT. ALLOCATED(DstRodData%Pd)) THEN + ALLOCATE(DstRodData%Pd(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%Pd.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%Pd = SrcRodData%Pd +ENDIF +IF (ALLOCATED(SrcRodData%Dp)) THEN + i1_l = LBOUND(SrcRodData%Dp,1) + i1_u = UBOUND(SrcRodData%Dp,1) + i2_l = LBOUND(SrcRodData%Dp,2) + i2_u = UBOUND(SrcRodData%Dp,2) + IF (.NOT. ALLOCATED(DstRodData%Dp)) THEN + ALLOCATE(DstRodData%Dp(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%Dp.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%Dp = SrcRodData%Dp +ENDIF +IF (ALLOCATED(SrcRodData%Dq)) THEN + i1_l = LBOUND(SrcRodData%Dq,1) + i1_u = UBOUND(SrcRodData%Dq,1) + i2_l = LBOUND(SrcRodData%Dq,2) + i2_u = UBOUND(SrcRodData%Dq,2) + IF (.NOT. ALLOCATED(DstRodData%Dq)) THEN + ALLOCATE(DstRodData%Dq(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%Dq.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%Dq = SrcRodData%Dq +ENDIF +IF (ALLOCATED(SrcRodData%Ap)) THEN + i1_l = LBOUND(SrcRodData%Ap,1) + i1_u = UBOUND(SrcRodData%Ap,1) + i2_l = LBOUND(SrcRodData%Ap,2) + i2_u = UBOUND(SrcRodData%Ap,2) + IF (.NOT. ALLOCATED(DstRodData%Ap)) THEN + ALLOCATE(DstRodData%Ap(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%Ap.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%Ap = SrcRodData%Ap +ENDIF +IF (ALLOCATED(SrcRodData%Aq)) THEN + i1_l = LBOUND(SrcRodData%Aq,1) + i1_u = UBOUND(SrcRodData%Aq,1) + i2_l = LBOUND(SrcRodData%Aq,2) + i2_u = UBOUND(SrcRodData%Aq,2) + IF (.NOT. ALLOCATED(DstRodData%Aq)) THEN + ALLOCATE(DstRodData%Aq(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%Aq.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%Aq = SrcRodData%Aq +ENDIF +IF (ALLOCATED(SrcRodData%B)) THEN + i1_l = LBOUND(SrcRodData%B,1) + i1_u = UBOUND(SrcRodData%B,1) + i2_l = LBOUND(SrcRodData%B,2) + i2_u = UBOUND(SrcRodData%B,2) + IF (.NOT. ALLOCATED(DstRodData%B)) THEN + ALLOCATE(DstRodData%B(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%B.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%B = SrcRodData%B +ENDIF +IF (ALLOCATED(SrcRodData%Fnet)) THEN + i1_l = LBOUND(SrcRodData%Fnet,1) + i1_u = UBOUND(SrcRodData%Fnet,1) + i2_l = LBOUND(SrcRodData%Fnet,2) + i2_u = UBOUND(SrcRodData%Fnet,2) + IF (.NOT. ALLOCATED(DstRodData%Fnet)) THEN + ALLOCATE(DstRodData%Fnet(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%Fnet.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%Fnet = SrcRodData%Fnet +ENDIF +IF (ALLOCATED(SrcRodData%M)) THEN + i1_l = LBOUND(SrcRodData%M,1) + i1_u = UBOUND(SrcRodData%M,1) + i2_l = LBOUND(SrcRodData%M,2) + i2_u = UBOUND(SrcRodData%M,2) + i3_l = LBOUND(SrcRodData%M,3) + i3_u = UBOUND(SrcRodData%M,3) + IF (.NOT. ALLOCATED(DstRodData%M)) THEN + ALLOCATE(DstRodData%M(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%M.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%M = SrcRodData%M +ENDIF + DstRodData%Mext = SrcRodData%Mext + DstRodData%r6 = SrcRodData%r6 + DstRodData%v6 = SrcRodData%v6 + DstRodData%a6 = SrcRodData%a6 + DstRodData%F6net = SrcRodData%F6net + DstRodData%M6net = SrcRodData%M6net + DstRodData%OrMat = SrcRodData%OrMat + DstRodData%RodUnOut = SrcRodData%RodUnOut +IF (ALLOCATED(SrcRodData%RodWrOutput)) THEN + i1_l = LBOUND(SrcRodData%RodWrOutput,1) + i1_u = UBOUND(SrcRodData%RodWrOutput,1) + IF (.NOT. ALLOCATED(DstRodData%RodWrOutput)) THEN + ALLOCATE(DstRodData%RodWrOutput(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstRodData%RodWrOutput.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstRodData%RodWrOutput = SrcRodData%RodWrOutput +ENDIF + END SUBROUTINE MD_CopyRod + + SUBROUTINE MD_DestroyRod( RodData, ErrStat, ErrMsg ) + TYPE(MD_Rod), INTENT(INOUT) :: RodData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyRod' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(RodData%r)) THEN + DEALLOCATE(RodData%r) +ENDIF +IF (ALLOCATED(RodData%rd)) THEN + DEALLOCATE(RodData%rd) +ENDIF +IF (ALLOCATED(RodData%l)) THEN + DEALLOCATE(RodData%l) +ENDIF +IF (ALLOCATED(RodData%V)) THEN + DEALLOCATE(RodData%V) +ENDIF +IF (ALLOCATED(RodData%U)) THEN + DEALLOCATE(RodData%U) +ENDIF +IF (ALLOCATED(RodData%Ud)) THEN + DEALLOCATE(RodData%Ud) +ENDIF +IF (ALLOCATED(RodData%zeta)) THEN + DEALLOCATE(RodData%zeta) +ENDIF +IF (ALLOCATED(RodData%PDyn)) THEN + DEALLOCATE(RodData%PDyn) +ENDIF +IF (ALLOCATED(RodData%W)) THEN + DEALLOCATE(RodData%W) +ENDIF +IF (ALLOCATED(RodData%Bo)) THEN + DEALLOCATE(RodData%Bo) +ENDIF +IF (ALLOCATED(RodData%Pd)) THEN + DEALLOCATE(RodData%Pd) +ENDIF +IF (ALLOCATED(RodData%Dp)) THEN + DEALLOCATE(RodData%Dp) +ENDIF +IF (ALLOCATED(RodData%Dq)) THEN + DEALLOCATE(RodData%Dq) +ENDIF +IF (ALLOCATED(RodData%Ap)) THEN + DEALLOCATE(RodData%Ap) +ENDIF +IF (ALLOCATED(RodData%Aq)) THEN + DEALLOCATE(RodData%Aq) +ENDIF +IF (ALLOCATED(RodData%B)) THEN + DEALLOCATE(RodData%B) +ENDIF +IF (ALLOCATED(RodData%Fnet)) THEN + DEALLOCATE(RodData%Fnet) +ENDIF +IF (ALLOCATED(RodData%M)) THEN + DEALLOCATE(RodData%M) +ENDIF +IF (ALLOCATED(RodData%RodWrOutput)) THEN + DEALLOCATE(RodData%RodWrOutput) +ENDIF + END SUBROUTINE MD_DestroyRod + + SUBROUTINE MD_PackRod( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(MD_Rod), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackRod' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! IdNum + Int_BufSz = Int_BufSz + 1*LEN(InData%type) ! type + Int_BufSz = Int_BufSz + 1 ! PropsIdNum + Int_BufSz = Int_BufSz + 1 ! typeNum + Int_BufSz = Int_BufSz + SIZE(InData%AttachedA) ! AttachedA + Int_BufSz = Int_BufSz + SIZE(InData%AttachedB) ! AttachedB + Int_BufSz = Int_BufSz + SIZE(InData%TopA) ! TopA + Int_BufSz = Int_BufSz + SIZE(InData%TopB) ! TopB + Int_BufSz = Int_BufSz + 1 ! nAttachedA + Int_BufSz = Int_BufSz + 1 ! nAttachedB + Int_BufSz = Int_BufSz + SIZE(InData%OutFlagList) ! OutFlagList + Int_BufSz = Int_BufSz + 1 ! N + Int_BufSz = Int_BufSz + 1 ! endTypeA + Int_BufSz = Int_BufSz + 1 ! endTypeB + Db_BufSz = Db_BufSz + 1 ! UnstrLen + Db_BufSz = Db_BufSz + 1 ! mass + Db_BufSz = Db_BufSz + 1 ! rho + Db_BufSz = Db_BufSz + 1 ! d + Db_BufSz = Db_BufSz + 1 ! Can + Db_BufSz = Db_BufSz + 1 ! Cat + Db_BufSz = Db_BufSz + 1 ! Cdn + Db_BufSz = Db_BufSz + 1 ! Cdt + Db_BufSz = Db_BufSz + 1 ! CdEnd + Db_BufSz = Db_BufSz + 1 ! CaEnd + Db_BufSz = Db_BufSz + 1 ! time + Db_BufSz = Db_BufSz + 1 ! roll + Db_BufSz = Db_BufSz + 1 ! pitch + Int_BufSz = Int_BufSz + 1 ! r allocated yes/no + IF ( ALLOCATED(InData%r) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! r upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%r) ! r + END IF + Int_BufSz = Int_BufSz + 1 ! rd allocated yes/no + IF ( ALLOCATED(InData%rd) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! rd upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%rd) ! rd + END IF + Db_BufSz = Db_BufSz + SIZE(InData%q) ! q + Int_BufSz = Int_BufSz + 1 ! l allocated yes/no + IF ( ALLOCATED(InData%l) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! l upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%l) ! l + END IF + Int_BufSz = Int_BufSz + 1 ! V allocated yes/no + IF ( ALLOCATED(InData%V) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! V upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%V) ! V + END IF + Int_BufSz = Int_BufSz + 1 ! U allocated yes/no + IF ( ALLOCATED(InData%U) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! U upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%U) ! U + END IF + Int_BufSz = Int_BufSz + 1 ! Ud allocated yes/no + IF ( ALLOCATED(InData%Ud) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Ud upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Ud) ! Ud + END IF + Int_BufSz = Int_BufSz + 1 ! zeta allocated yes/no + IF ( ALLOCATED(InData%zeta) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! zeta upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%zeta) ! zeta + END IF + Int_BufSz = Int_BufSz + 1 ! PDyn allocated yes/no + IF ( ALLOCATED(InData%PDyn) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! PDyn upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%PDyn) ! PDyn + END IF + Int_BufSz = Int_BufSz + 1 ! W allocated yes/no + IF ( ALLOCATED(InData%W) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! W upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%W) ! W + END IF + Int_BufSz = Int_BufSz + 1 ! Bo allocated yes/no + IF ( ALLOCATED(InData%Bo) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Bo upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Bo) ! Bo + END IF + Int_BufSz = Int_BufSz + 1 ! Pd allocated yes/no + IF ( ALLOCATED(InData%Pd) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Pd upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Pd) ! Pd + END IF + Int_BufSz = Int_BufSz + 1 ! Dp allocated yes/no + IF ( ALLOCATED(InData%Dp) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Dp upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Dp) ! Dp + END IF + Int_BufSz = Int_BufSz + 1 ! Dq allocated yes/no + IF ( ALLOCATED(InData%Dq) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Dq upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Dq) ! Dq + END IF + Int_BufSz = Int_BufSz + 1 ! Ap allocated yes/no + IF ( ALLOCATED(InData%Ap) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Ap upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Ap) ! Ap + END IF + Int_BufSz = Int_BufSz + 1 ! Aq allocated yes/no + IF ( ALLOCATED(InData%Aq) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Aq upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Aq) ! Aq + END IF + Int_BufSz = Int_BufSz + 1 ! B allocated yes/no + IF ( ALLOCATED(InData%B) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! B upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%B) ! B + END IF + Int_BufSz = Int_BufSz + 1 ! Fnet allocated yes/no + IF ( ALLOCATED(InData%Fnet) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Fnet upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Fnet) ! Fnet + END IF + Int_BufSz = Int_BufSz + 1 ! M allocated yes/no + IF ( ALLOCATED(InData%M) ) THEN + Int_BufSz = Int_BufSz + 2*3 ! M upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%M) ! M + END IF + Db_BufSz = Db_BufSz + SIZE(InData%Mext) ! Mext + Db_BufSz = Db_BufSz + SIZE(InData%r6) ! r6 + Db_BufSz = Db_BufSz + SIZE(InData%v6) ! v6 + Db_BufSz = Db_BufSz + SIZE(InData%a6) ! a6 + Db_BufSz = Db_BufSz + SIZE(InData%F6net) ! F6net + Db_BufSz = Db_BufSz + SIZE(InData%M6net) ! M6net + Db_BufSz = Db_BufSz + SIZE(InData%OrMat) ! OrMat + Int_BufSz = Int_BufSz + 1 ! RodUnOut + Int_BufSz = Int_BufSz + 1 ! RodWrOutput allocated yes/no + IF ( ALLOCATED(InData%RodWrOutput) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! RodWrOutput upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%RodWrOutput) ! RodWrOutput + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IntKiBuf(Int_Xferred) = InData%IdNum + Int_Xferred = Int_Xferred + 1 + DO I = 1, LEN(InData%type) + IntKiBuf(Int_Xferred) = ICHAR(InData%type(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + IntKiBuf(Int_Xferred) = InData%PropsIdNum + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%typeNum + Int_Xferred = Int_Xferred + 1 + DO i1 = LBOUND(InData%AttachedA,1), UBOUND(InData%AttachedA,1) + IntKiBuf(Int_Xferred) = InData%AttachedA(i1) + Int_Xferred = Int_Xferred + 1 + END DO + DO i1 = LBOUND(InData%AttachedB,1), UBOUND(InData%AttachedB,1) + IntKiBuf(Int_Xferred) = InData%AttachedB(i1) + Int_Xferred = Int_Xferred + 1 + END DO + DO i1 = LBOUND(InData%TopA,1), UBOUND(InData%TopA,1) + IntKiBuf(Int_Xferred) = InData%TopA(i1) + Int_Xferred = Int_Xferred + 1 + END DO + DO i1 = LBOUND(InData%TopB,1), UBOUND(InData%TopB,1) + IntKiBuf(Int_Xferred) = InData%TopB(i1) + Int_Xferred = Int_Xferred + 1 + END DO + IntKiBuf(Int_Xferred) = InData%nAttachedA + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nAttachedB + Int_Xferred = Int_Xferred + 1 + DO i1 = LBOUND(InData%OutFlagList,1), UBOUND(InData%OutFlagList,1) + IntKiBuf(Int_Xferred) = InData%OutFlagList(i1) + Int_Xferred = Int_Xferred + 1 + END DO + IntKiBuf(Int_Xferred) = InData%N + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%endTypeA + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%endTypeB + Int_Xferred = Int_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%UnstrLen + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%mass + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%rho + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%d + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%Can + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%Cat + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%Cdn + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%Cdt + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%CdEnd + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%CaEnd + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%time + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%roll + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%pitch + Db_Xferred = Db_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%r) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%r,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%r,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%r,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%r,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%r,2), UBOUND(InData%r,2) + DO i1 = LBOUND(InData%r,1), UBOUND(InData%r,1) + DbKiBuf(Db_Xferred) = InData%r(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%rd) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%rd,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%rd,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%rd,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%rd,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%rd,2), UBOUND(InData%rd,2) + DO i1 = LBOUND(InData%rd,1), UBOUND(InData%rd,1) + DbKiBuf(Db_Xferred) = InData%rd(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + DO i1 = LBOUND(InData%q,1), UBOUND(InData%q,1) + DbKiBuf(Db_Xferred) = InData%q(i1) + Db_Xferred = Db_Xferred + 1 + END DO + IF ( .NOT. ALLOCATED(InData%l) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%l,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%l,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%l,1), UBOUND(InData%l,1) + DbKiBuf(Db_Xferred) = InData%l(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%V) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%V,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%V,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%V,1), UBOUND(InData%V,1) + DbKiBuf(Db_Xferred) = InData%V(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%U) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%U,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%U,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%U,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%U,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%U,2), UBOUND(InData%U,2) + DO i1 = LBOUND(InData%U,1), UBOUND(InData%U,1) + DbKiBuf(Db_Xferred) = InData%U(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%Ud) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Ud,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Ud,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Ud,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Ud,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Ud,2), UBOUND(InData%Ud,2) + DO i1 = LBOUND(InData%Ud,1), UBOUND(InData%Ud,1) + DbKiBuf(Db_Xferred) = InData%Ud(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%zeta) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%zeta,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%zeta,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%zeta,1), UBOUND(InData%zeta,1) + DbKiBuf(Db_Xferred) = InData%zeta(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%PDyn) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%PDyn,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PDyn,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%PDyn,1), UBOUND(InData%PDyn,1) + DbKiBuf(Db_Xferred) = InData%PDyn(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%W) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%W,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%W,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%W,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%W,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%W,2), UBOUND(InData%W,2) + DO i1 = LBOUND(InData%W,1), UBOUND(InData%W,1) + DbKiBuf(Db_Xferred) = InData%W(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%Bo) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Bo,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Bo,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Bo,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Bo,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Bo,2), UBOUND(InData%Bo,2) + DO i1 = LBOUND(InData%Bo,1), UBOUND(InData%Bo,1) + DbKiBuf(Db_Xferred) = InData%Bo(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%Pd) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Pd,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Pd,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Pd,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Pd,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Pd,2), UBOUND(InData%Pd,2) + DO i1 = LBOUND(InData%Pd,1), UBOUND(InData%Pd,1) + DbKiBuf(Db_Xferred) = InData%Pd(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%Dp) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Dp,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Dp,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Dp,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Dp,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Dp,2), UBOUND(InData%Dp,2) + DO i1 = LBOUND(InData%Dp,1), UBOUND(InData%Dp,1) + DbKiBuf(Db_Xferred) = InData%Dp(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%Dq) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Dq,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Dq,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Dq,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Dq,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Dq,2), UBOUND(InData%Dq,2) + DO i1 = LBOUND(InData%Dq,1), UBOUND(InData%Dq,1) + DbKiBuf(Db_Xferred) = InData%Dq(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%Ap) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Ap,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Ap,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Ap,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Ap,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Ap,2), UBOUND(InData%Ap,2) + DO i1 = LBOUND(InData%Ap,1), UBOUND(InData%Ap,1) + DbKiBuf(Db_Xferred) = InData%Ap(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%Aq) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Aq,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Aq,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Aq,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Aq,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Aq,2), UBOUND(InData%Aq,2) + DO i1 = LBOUND(InData%Aq,1), UBOUND(InData%Aq,1) + DbKiBuf(Db_Xferred) = InData%Aq(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%B) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%B,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%B,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%B,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%B,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%B,2), UBOUND(InData%B,2) + DO i1 = LBOUND(InData%B,1), UBOUND(InData%B,1) + DbKiBuf(Db_Xferred) = InData%B(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%Fnet) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Fnet,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Fnet,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Fnet,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Fnet,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Fnet,2), UBOUND(InData%Fnet,2) + DO i1 = LBOUND(InData%Fnet,1), UBOUND(InData%Fnet,1) + DbKiBuf(Db_Xferred) = InData%Fnet(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%M) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%M,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%M,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%M,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%M,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%M,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%M,3) + Int_Xferred = Int_Xferred + 2 + + DO i3 = LBOUND(InData%M,3), UBOUND(InData%M,3) + DO i2 = LBOUND(InData%M,2), UBOUND(InData%M,2) + DO i1 = LBOUND(InData%M,1), UBOUND(InData%M,1) + DbKiBuf(Db_Xferred) = InData%M(i1,i2,i3) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END IF + DO i1 = LBOUND(InData%Mext,1), UBOUND(InData%Mext,1) + DbKiBuf(Db_Xferred) = InData%Mext(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%r6,1), UBOUND(InData%r6,1) + DbKiBuf(Db_Xferred) = InData%r6(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%v6,1), UBOUND(InData%v6,1) + DbKiBuf(Db_Xferred) = InData%v6(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%a6,1), UBOUND(InData%a6,1) + DbKiBuf(Db_Xferred) = InData%a6(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%F6net,1), UBOUND(InData%F6net,1) + DbKiBuf(Db_Xferred) = InData%F6net(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i2 = LBOUND(InData%M6net,2), UBOUND(InData%M6net,2) + DO i1 = LBOUND(InData%M6net,1), UBOUND(InData%M6net,1) + DbKiBuf(Db_Xferred) = InData%M6net(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + DO i2 = LBOUND(InData%OrMat,2), UBOUND(InData%OrMat,2) + DO i1 = LBOUND(InData%OrMat,1), UBOUND(InData%OrMat,1) + DbKiBuf(Db_Xferred) = InData%OrMat(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + IntKiBuf(Int_Xferred) = InData%RodUnOut + Int_Xferred = Int_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%RodWrOutput) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%RodWrOutput,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%RodWrOutput,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%RodWrOutput,1), UBOUND(InData%RodWrOutput,1) + DbKiBuf(Db_Xferred) = InData%RodWrOutput(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + END SUBROUTINE MD_PackRod + + SUBROUTINE MD_UnPackRod( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_Rod), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackRod' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%IdNum = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + DO I = 1, LEN(OutData%type) + OutData%type(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + OutData%PropsIdNum = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%typeNum = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + i1_l = LBOUND(OutData%AttachedA,1) + i1_u = UBOUND(OutData%AttachedA,1) + DO i1 = LBOUND(OutData%AttachedA,1), UBOUND(OutData%AttachedA,1) + OutData%AttachedA(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + i1_l = LBOUND(OutData%AttachedB,1) + i1_u = UBOUND(OutData%AttachedB,1) + DO i1 = LBOUND(OutData%AttachedB,1), UBOUND(OutData%AttachedB,1) + OutData%AttachedB(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + i1_l = LBOUND(OutData%TopA,1) + i1_u = UBOUND(OutData%TopA,1) + DO i1 = LBOUND(OutData%TopA,1), UBOUND(OutData%TopA,1) + OutData%TopA(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + i1_l = LBOUND(OutData%TopB,1) + i1_u = UBOUND(OutData%TopB,1) + DO i1 = LBOUND(OutData%TopB,1), UBOUND(OutData%TopB,1) + OutData%TopB(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + OutData%nAttachedA = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nAttachedB = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + i1_l = LBOUND(OutData%OutFlagList,1) + i1_u = UBOUND(OutData%OutFlagList,1) + DO i1 = LBOUND(OutData%OutFlagList,1), UBOUND(OutData%OutFlagList,1) + OutData%OutFlagList(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + OutData%N = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%endTypeA = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%endTypeB = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%UnstrLen = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%mass = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%rho = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%d = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%Can = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%Cat = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%Cdn = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%Cdt = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%CdEnd = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%CaEnd = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%time = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%roll = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%pitch = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! r not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%r)) DEALLOCATE(OutData%r) + ALLOCATE(OutData%r(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%r.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%r,2), UBOUND(OutData%r,2) + DO i1 = LBOUND(OutData%r,1), UBOUND(OutData%r,1) + OutData%r(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! rd not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%rd)) DEALLOCATE(OutData%rd) + ALLOCATE(OutData%rd(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%rd.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%rd,2), UBOUND(OutData%rd,2) + DO i1 = LBOUND(OutData%rd,1), UBOUND(OutData%rd,1) + OutData%rd(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + i1_l = LBOUND(OutData%q,1) + i1_u = UBOUND(OutData%q,1) + DO i1 = LBOUND(OutData%q,1), UBOUND(OutData%q,1) + OutData%q(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! l not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%l)) DEALLOCATE(OutData%l) + ALLOCATE(OutData%l(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%l.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%l,1), UBOUND(OutData%l,1) + OutData%l(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! V not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%V)) DEALLOCATE(OutData%V) + ALLOCATE(OutData%V(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%V.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%V,1), UBOUND(OutData%V,1) + OutData%V(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! U not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%U)) DEALLOCATE(OutData%U) + ALLOCATE(OutData%U(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%U.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%U,2), UBOUND(OutData%U,2) + DO i1 = LBOUND(OutData%U,1), UBOUND(OutData%U,1) + OutData%U(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Ud not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Ud)) DEALLOCATE(OutData%Ud) + ALLOCATE(OutData%Ud(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Ud.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Ud,2), UBOUND(OutData%Ud,2) + DO i1 = LBOUND(OutData%Ud,1), UBOUND(OutData%Ud,1) + OutData%Ud(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! zeta not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%zeta)) DEALLOCATE(OutData%zeta) + ALLOCATE(OutData%zeta(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%zeta.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%zeta,1), UBOUND(OutData%zeta,1) + OutData%zeta(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! PDyn not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%PDyn)) DEALLOCATE(OutData%PDyn) + ALLOCATE(OutData%PDyn(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%PDyn.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%PDyn,1), UBOUND(OutData%PDyn,1) + OutData%PDyn(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! W not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%W)) DEALLOCATE(OutData%W) + ALLOCATE(OutData%W(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%W.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%W,2), UBOUND(OutData%W,2) + DO i1 = LBOUND(OutData%W,1), UBOUND(OutData%W,1) + OutData%W(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Bo not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Bo)) DEALLOCATE(OutData%Bo) + ALLOCATE(OutData%Bo(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Bo.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Bo,2), UBOUND(OutData%Bo,2) + DO i1 = LBOUND(OutData%Bo,1), UBOUND(OutData%Bo,1) + OutData%Bo(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Pd not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Pd)) DEALLOCATE(OutData%Pd) + ALLOCATE(OutData%Pd(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Pd.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Pd,2), UBOUND(OutData%Pd,2) + DO i1 = LBOUND(OutData%Pd,1), UBOUND(OutData%Pd,1) + OutData%Pd(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Dp not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Dp)) DEALLOCATE(OutData%Dp) + ALLOCATE(OutData%Dp(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Dp.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Dp,2), UBOUND(OutData%Dp,2) + DO i1 = LBOUND(OutData%Dp,1), UBOUND(OutData%Dp,1) + OutData%Dp(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Dq not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Dq)) DEALLOCATE(OutData%Dq) + ALLOCATE(OutData%Dq(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Dq.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Dq,2), UBOUND(OutData%Dq,2) + DO i1 = LBOUND(OutData%Dq,1), UBOUND(OutData%Dq,1) + OutData%Dq(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Ap not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Ap)) DEALLOCATE(OutData%Ap) + ALLOCATE(OutData%Ap(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Ap.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Ap,2), UBOUND(OutData%Ap,2) + DO i1 = LBOUND(OutData%Ap,1), UBOUND(OutData%Ap,1) + OutData%Ap(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Aq not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Aq)) DEALLOCATE(OutData%Aq) + ALLOCATE(OutData%Aq(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Aq.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Aq,2), UBOUND(OutData%Aq,2) + DO i1 = LBOUND(OutData%Aq,1), UBOUND(OutData%Aq,1) + OutData%Aq(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! B not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%B)) DEALLOCATE(OutData%B) + ALLOCATE(OutData%B(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%B.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%B,2), UBOUND(OutData%B,2) + DO i1 = LBOUND(OutData%B,1), UBOUND(OutData%B,1) + OutData%B(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Fnet not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Fnet)) DEALLOCATE(OutData%Fnet) + ALLOCATE(OutData%Fnet(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Fnet.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Fnet,2), UBOUND(OutData%Fnet,2) + DO i1 = LBOUND(OutData%Fnet,1), UBOUND(OutData%Fnet,1) + OutData%Fnet(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! M not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%M)) DEALLOCATE(OutData%M) + ALLOCATE(OutData%M(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%M.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i3 = LBOUND(OutData%M,3), UBOUND(OutData%M,3) + DO i2 = LBOUND(OutData%M,2), UBOUND(OutData%M,2) + DO i1 = LBOUND(OutData%M,1), UBOUND(OutData%M,1) + OutData%M(i1,i2,i3) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END IF + i1_l = LBOUND(OutData%Mext,1) + i1_u = UBOUND(OutData%Mext,1) + DO i1 = LBOUND(OutData%Mext,1), UBOUND(OutData%Mext,1) + OutData%Mext(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%r6,1) + i1_u = UBOUND(OutData%r6,1) + DO i1 = LBOUND(OutData%r6,1), UBOUND(OutData%r6,1) + OutData%r6(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%v6,1) + i1_u = UBOUND(OutData%v6,1) + DO i1 = LBOUND(OutData%v6,1), UBOUND(OutData%v6,1) + OutData%v6(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%a6,1) + i1_u = UBOUND(OutData%a6,1) + DO i1 = LBOUND(OutData%a6,1), UBOUND(OutData%a6,1) + OutData%a6(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%F6net,1) + i1_u = UBOUND(OutData%F6net,1) + DO i1 = LBOUND(OutData%F6net,1), UBOUND(OutData%F6net,1) + OutData%F6net(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%M6net,1) + i1_u = UBOUND(OutData%M6net,1) + i2_l = LBOUND(OutData%M6net,2) + i2_u = UBOUND(OutData%M6net,2) + DO i2 = LBOUND(OutData%M6net,2), UBOUND(OutData%M6net,2) + DO i1 = LBOUND(OutData%M6net,1), UBOUND(OutData%M6net,1) + OutData%M6net(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + i1_l = LBOUND(OutData%OrMat,1) + i1_u = UBOUND(OutData%OrMat,1) + i2_l = LBOUND(OutData%OrMat,2) + i2_u = UBOUND(OutData%OrMat,2) + DO i2 = LBOUND(OutData%OrMat,2), UBOUND(OutData%OrMat,2) + DO i1 = LBOUND(OutData%OrMat,1), UBOUND(OutData%OrMat,1) + OutData%OrMat(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + OutData%RodUnOut = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! RodWrOutput not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%RodWrOutput)) DEALLOCATE(OutData%RodWrOutput) + ALLOCATE(OutData%RodWrOutput(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%RodWrOutput.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%RodWrOutput,1), UBOUND(OutData%RodWrOutput,1) + OutData%RodWrOutput(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + END SUBROUTINE MD_UnPackRod + + SUBROUTINE MD_CopyLine( SrcLineData, DstLineData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_Line), INTENT(IN) :: SrcLineData + TYPE(MD_Line), INTENT(INOUT) :: DstLineData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyLine' +! + ErrStat = ErrID_None + ErrMsg = "" + DstLineData%IdNum = SrcLineData%IdNum + DstLineData%PropsIdNum = SrcLineData%PropsIdNum + DstLineData%OutFlagList = SrcLineData%OutFlagList + DstLineData%CtrlChan = SrcLineData%CtrlChan + DstLineData%FairConnect = SrcLineData%FairConnect + DstLineData%AnchConnect = SrcLineData%AnchConnect + DstLineData%N = SrcLineData%N + DstLineData%endTypeA = SrcLineData%endTypeA + DstLineData%endTypeB = SrcLineData%endTypeB + DstLineData%UnstrLen = SrcLineData%UnstrLen + DstLineData%rho = SrcLineData%rho + DstLineData%d = SrcLineData%d + DstLineData%EA = SrcLineData%EA + DstLineData%EI = SrcLineData%EI + DstLineData%BA = SrcLineData%BA + DstLineData%Can = SrcLineData%Can + DstLineData%Cat = SrcLineData%Cat + DstLineData%Cdn = SrcLineData%Cdn + DstLineData%Cdt = SrcLineData%Cdt + DstLineData%time = SrcLineData%time +IF (ALLOCATED(SrcLineData%r)) THEN + i1_l = LBOUND(SrcLineData%r,1) + i1_u = UBOUND(SrcLineData%r,1) + i2_l = LBOUND(SrcLineData%r,2) + i2_u = UBOUND(SrcLineData%r,2) + IF (.NOT. ALLOCATED(DstLineData%r)) THEN + ALLOCATE(DstLineData%r(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%r.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%r = SrcLineData%r +ENDIF +IF (ALLOCATED(SrcLineData%rd)) THEN + i1_l = LBOUND(SrcLineData%rd,1) + i1_u = UBOUND(SrcLineData%rd,1) + i2_l = LBOUND(SrcLineData%rd,2) + i2_u = UBOUND(SrcLineData%rd,2) + IF (.NOT. ALLOCATED(DstLineData%rd)) THEN + ALLOCATE(DstLineData%rd(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%rd.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%rd = SrcLineData%rd +ENDIF +IF (ALLOCATED(SrcLineData%q)) THEN + i1_l = LBOUND(SrcLineData%q,1) + i1_u = UBOUND(SrcLineData%q,1) + i2_l = LBOUND(SrcLineData%q,2) + i2_u = UBOUND(SrcLineData%q,2) IF (.NOT. ALLOCATED(DstLineData%q)) THEN ALLOCATE(DstLineData%q(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN @@ -1114,6 +3927,18 @@ SUBROUTINE MD_CopyLine( SrcLineData, DstLineData, CtrlCode, ErrStat, ErrMsg ) END IF DstLineData%l = SrcLineData%l ENDIF +IF (ALLOCATED(SrcLineData%ld)) THEN + i1_l = LBOUND(SrcLineData%ld,1) + i1_u = UBOUND(SrcLineData%ld,1) + IF (.NOT. ALLOCATED(DstLineData%ld)) THEN + ALLOCATE(DstLineData%ld(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%ld.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%ld = SrcLineData%ld +ENDIF IF (ALLOCATED(SrcLineData%lstr)) THEN i1_l = LBOUND(SrcLineData%lstr,1) i1_u = UBOUND(SrcLineData%lstr,1) @@ -1150,6 +3975,58 @@ SUBROUTINE MD_CopyLine( SrcLineData, DstLineData, CtrlCode, ErrStat, ErrMsg ) END IF DstLineData%V = SrcLineData%V ENDIF +IF (ALLOCATED(SrcLineData%U)) THEN + i1_l = LBOUND(SrcLineData%U,1) + i1_u = UBOUND(SrcLineData%U,1) + i2_l = LBOUND(SrcLineData%U,2) + i2_u = UBOUND(SrcLineData%U,2) + IF (.NOT. ALLOCATED(DstLineData%U)) THEN + ALLOCATE(DstLineData%U(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%U.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%U = SrcLineData%U +ENDIF +IF (ALLOCATED(SrcLineData%Ud)) THEN + i1_l = LBOUND(SrcLineData%Ud,1) + i1_u = UBOUND(SrcLineData%Ud,1) + i2_l = LBOUND(SrcLineData%Ud,2) + i2_u = UBOUND(SrcLineData%Ud,2) + IF (.NOT. ALLOCATED(DstLineData%Ud)) THEN + ALLOCATE(DstLineData%Ud(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%Ud.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%Ud = SrcLineData%Ud +ENDIF +IF (ALLOCATED(SrcLineData%zeta)) THEN + i1_l = LBOUND(SrcLineData%zeta,1) + i1_u = UBOUND(SrcLineData%zeta,1) + IF (.NOT. ALLOCATED(DstLineData%zeta)) THEN + ALLOCATE(DstLineData%zeta(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%zeta.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%zeta = SrcLineData%zeta +ENDIF +IF (ALLOCATED(SrcLineData%PDyn)) THEN + i1_l = LBOUND(SrcLineData%PDyn,1) + i1_u = UBOUND(SrcLineData%PDyn,1) + IF (.NOT. ALLOCATED(DstLineData%PDyn)) THEN + ALLOCATE(DstLineData%PDyn(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%PDyn.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%PDyn = SrcLineData%PDyn +ENDIF IF (ALLOCATED(SrcLineData%T)) THEN i1_l = LBOUND(SrcLineData%T,1) i1_u = UBOUND(SrcLineData%T,1) @@ -1262,19 +4139,19 @@ SUBROUTINE MD_CopyLine( SrcLineData, DstLineData, CtrlCode, ErrStat, ErrMsg ) END IF DstLineData%B = SrcLineData%B ENDIF -IF (ALLOCATED(SrcLineData%F)) THEN - i1_l = LBOUND(SrcLineData%F,1) - i1_u = UBOUND(SrcLineData%F,1) - i2_l = LBOUND(SrcLineData%F,2) - i2_u = UBOUND(SrcLineData%F,2) - IF (.NOT. ALLOCATED(DstLineData%F)) THEN - ALLOCATE(DstLineData%F(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) +IF (ALLOCATED(SrcLineData%Fnet)) THEN + i1_l = LBOUND(SrcLineData%Fnet,1) + i1_u = UBOUND(SrcLineData%Fnet,1) + i2_l = LBOUND(SrcLineData%Fnet,2) + i2_u = UBOUND(SrcLineData%Fnet,2) + IF (.NOT. ALLOCATED(DstLineData%Fnet)) THEN + ALLOCATE(DstLineData%Fnet(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%F.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%Fnet.', ErrStat, ErrMsg,RoutineName) RETURN END IF END IF - DstLineData%F = SrcLineData%F + DstLineData%Fnet = SrcLineData%Fnet ENDIF IF (ALLOCATED(SrcLineData%S)) THEN i1_l = LBOUND(SrcLineData%S,1) @@ -1308,6 +4185,8 @@ SUBROUTINE MD_CopyLine( SrcLineData, DstLineData, CtrlCode, ErrStat, ErrMsg ) END IF DstLineData%M = SrcLineData%M ENDIF + DstLineData%EndMomentA = SrcLineData%EndMomentA + DstLineData%EndMomentB = SrcLineData%EndMomentB DstLineData%LineUnOut = SrcLineData%LineUnOut IF (ALLOCATED(SrcLineData%LineWrOutput)) THEN i1_l = LBOUND(SrcLineData%LineWrOutput,1) @@ -1344,6 +4223,9 @@ SUBROUTINE MD_DestroyLine( LineData, ErrStat, ErrMsg ) IF (ALLOCATED(LineData%l)) THEN DEALLOCATE(LineData%l) ENDIF +IF (ALLOCATED(LineData%ld)) THEN + DEALLOCATE(LineData%ld) +ENDIF IF (ALLOCATED(LineData%lstr)) THEN DEALLOCATE(LineData%lstr) ENDIF @@ -1353,6 +4235,18 @@ SUBROUTINE MD_DestroyLine( LineData, ErrStat, ErrMsg ) IF (ALLOCATED(LineData%V)) THEN DEALLOCATE(LineData%V) ENDIF +IF (ALLOCATED(LineData%U)) THEN + DEALLOCATE(LineData%U) +ENDIF +IF (ALLOCATED(LineData%Ud)) THEN + DEALLOCATE(LineData%Ud) +ENDIF +IF (ALLOCATED(LineData%zeta)) THEN + DEALLOCATE(LineData%zeta) +ENDIF +IF (ALLOCATED(LineData%PDyn)) THEN + DEALLOCATE(LineData%PDyn) +ENDIF IF (ALLOCATED(LineData%T)) THEN DEALLOCATE(LineData%T) ENDIF @@ -1377,8 +4271,8 @@ SUBROUTINE MD_DestroyLine( LineData, ErrStat, ErrMsg ) IF (ALLOCATED(LineData%B)) THEN DEALLOCATE(LineData%B) ENDIF -IF (ALLOCATED(LineData%F)) THEN - DEALLOCATE(LineData%F) +IF (ALLOCATED(LineData%Fnet)) THEN + DEALLOCATE(LineData%Fnet) ENDIF IF (ALLOCATED(LineData%S)) THEN DEALLOCATE(LineData%S) @@ -1427,15 +4321,25 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Db_BufSz = 0 Int_BufSz = 0 Int_BufSz = Int_BufSz + 1 ! IdNum - Int_BufSz = Int_BufSz + 1*LEN(InData%type) ! type + Int_BufSz = Int_BufSz + 1 ! PropsIdNum Int_BufSz = Int_BufSz + SIZE(InData%OutFlagList) ! OutFlagList Int_BufSz = Int_BufSz + 1 ! CtrlChan Int_BufSz = Int_BufSz + 1 ! FairConnect Int_BufSz = Int_BufSz + 1 ! AnchConnect - Int_BufSz = Int_BufSz + 1 ! PropsIdNum Int_BufSz = Int_BufSz + 1 ! N + Int_BufSz = Int_BufSz + 1 ! endTypeA + Int_BufSz = Int_BufSz + 1 ! endTypeB Db_BufSz = Db_BufSz + 1 ! UnstrLen + Db_BufSz = Db_BufSz + 1 ! rho + Db_BufSz = Db_BufSz + 1 ! d + Db_BufSz = Db_BufSz + 1 ! EA + Db_BufSz = Db_BufSz + 1 ! EI Db_BufSz = Db_BufSz + 1 ! BA + Db_BufSz = Db_BufSz + 1 ! Can + Db_BufSz = Db_BufSz + 1 ! Cat + Db_BufSz = Db_BufSz + 1 ! Cdn + Db_BufSz = Db_BufSz + 1 ! Cdt + Db_BufSz = Db_BufSz + 1 ! time Int_BufSz = Int_BufSz + 1 ! r allocated yes/no IF ( ALLOCATED(InData%r) ) THEN Int_BufSz = Int_BufSz + 2*2 ! r upper/lower bounds for each dimension @@ -1456,6 +4360,11 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_BufSz = Int_BufSz + 2*1 ! l upper/lower bounds for each dimension Db_BufSz = Db_BufSz + SIZE(InData%l) ! l END IF + Int_BufSz = Int_BufSz + 1 ! ld allocated yes/no + IF ( ALLOCATED(InData%ld) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! ld upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%ld) ! ld + END IF Int_BufSz = Int_BufSz + 1 ! lstr allocated yes/no IF ( ALLOCATED(InData%lstr) ) THEN Int_BufSz = Int_BufSz + 2*1 ! lstr upper/lower bounds for each dimension @@ -1471,6 +4380,26 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_BufSz = Int_BufSz + 2*1 ! V upper/lower bounds for each dimension Db_BufSz = Db_BufSz + SIZE(InData%V) ! V END IF + Int_BufSz = Int_BufSz + 1 ! U allocated yes/no + IF ( ALLOCATED(InData%U) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! U upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%U) ! U + END IF + Int_BufSz = Int_BufSz + 1 ! Ud allocated yes/no + IF ( ALLOCATED(InData%Ud) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Ud upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Ud) ! Ud + END IF + Int_BufSz = Int_BufSz + 1 ! zeta allocated yes/no + IF ( ALLOCATED(InData%zeta) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! zeta upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%zeta) ! zeta + END IF + Int_BufSz = Int_BufSz + 1 ! PDyn allocated yes/no + IF ( ALLOCATED(InData%PDyn) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! PDyn upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%PDyn) ! PDyn + END IF Int_BufSz = Int_BufSz + 1 ! T allocated yes/no IF ( ALLOCATED(InData%T) ) THEN Int_BufSz = Int_BufSz + 2*2 ! T upper/lower bounds for each dimension @@ -1511,10 +4440,10 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_BufSz = Int_BufSz + 2*2 ! B upper/lower bounds for each dimension Db_BufSz = Db_BufSz + SIZE(InData%B) ! B END IF - Int_BufSz = Int_BufSz + 1 ! F allocated yes/no - IF ( ALLOCATED(InData%F) ) THEN - Int_BufSz = Int_BufSz + 2*2 ! F upper/lower bounds for each dimension - Db_BufSz = Db_BufSz + SIZE(InData%F) ! F + Int_BufSz = Int_BufSz + 1 ! Fnet allocated yes/no + IF ( ALLOCATED(InData%Fnet) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Fnet upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Fnet) ! Fnet END IF Int_BufSz = Int_BufSz + 1 ! S allocated yes/no IF ( ALLOCATED(InData%S) ) THEN @@ -1526,11 +4455,13 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_BufSz = Int_BufSz + 2*3 ! M upper/lower bounds for each dimension Db_BufSz = Db_BufSz + SIZE(InData%M) ! M END IF + Db_BufSz = Db_BufSz + SIZE(InData%EndMomentA) ! EndMomentA + Db_BufSz = Db_BufSz + SIZE(InData%EndMomentB) ! EndMomentB Int_BufSz = Int_BufSz + 1 ! LineUnOut Int_BufSz = Int_BufSz + 1 ! LineWrOutput allocated yes/no IF ( ALLOCATED(InData%LineWrOutput) ) THEN Int_BufSz = Int_BufSz + 2*1 ! LineWrOutput upper/lower bounds for each dimension - Re_BufSz = Re_BufSz + SIZE(InData%LineWrOutput) ! LineWrOutput + Db_BufSz = Db_BufSz + SIZE(InData%LineWrOutput) ! LineWrOutput END IF IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) @@ -1561,10 +4492,8 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz IntKiBuf(Int_Xferred) = InData%IdNum Int_Xferred = Int_Xferred + 1 - DO I = 1, LEN(InData%type) - IntKiBuf(Int_Xferred) = ICHAR(InData%type(I:I), IntKi) - Int_Xferred = Int_Xferred + 1 - END DO ! I + IntKiBuf(Int_Xferred) = InData%PropsIdNum + Int_Xferred = Int_Xferred + 1 DO i1 = LBOUND(InData%OutFlagList,1), UBOUND(InData%OutFlagList,1) IntKiBuf(Int_Xferred) = InData%OutFlagList(i1) Int_Xferred = Int_Xferred + 1 @@ -1575,14 +4504,34 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%AnchConnect Int_Xferred = Int_Xferred + 1 - IntKiBuf(Int_Xferred) = InData%PropsIdNum - Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%N Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%endTypeA + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%endTypeB + Int_Xferred = Int_Xferred + 1 DbKiBuf(Db_Xferred) = InData%UnstrLen Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%rho + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%d + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%EA + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%EI + Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%BA Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%Can + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%Cat + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%Cdn + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%Cdt + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%time + Db_Xferred = Db_Xferred + 1 IF ( .NOT. ALLOCATED(InData%r) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -1636,70 +4585,155 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz IntKiBuf( Int_Xferred + 1) = UBOUND(InData%q,2) Int_Xferred = Int_Xferred + 2 - DO i2 = LBOUND(InData%q,2), UBOUND(InData%q,2) - DO i1 = LBOUND(InData%q,1), UBOUND(InData%q,1) - DbKiBuf(Db_Xferred) = InData%q(i1,i2) - Db_Xferred = Db_Xferred + 1 - END DO + DO i2 = LBOUND(InData%q,2), UBOUND(InData%q,2) + DO i1 = LBOUND(InData%q,1), UBOUND(InData%q,1) + DbKiBuf(Db_Xferred) = InData%q(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%l) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%l,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%l,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%l,1), UBOUND(InData%l,1) + DbKiBuf(Db_Xferred) = InData%l(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%ld) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ld,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ld,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%ld,1), UBOUND(InData%ld,1) + ReKiBuf(Re_Xferred) = InData%ld(i1) + Re_Xferred = Re_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%lstr) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%lstr,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%lstr,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%lstr,1), UBOUND(InData%lstr,1) + DbKiBuf(Db_Xferred) = InData%lstr(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%lstrd) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%lstrd,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%lstrd,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%lstrd,1), UBOUND(InData%lstrd,1) + DbKiBuf(Db_Xferred) = InData%lstrd(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%V) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%V,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%V,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%V,1), UBOUND(InData%V,1) + DbKiBuf(Db_Xferred) = InData%V(i1) + Db_Xferred = Db_Xferred + 1 END DO END IF - IF ( .NOT. ALLOCATED(InData%l) ) THEN + IF ( .NOT. ALLOCATED(InData%U) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%l,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%l,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%U,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%U,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%U,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%U,2) Int_Xferred = Int_Xferred + 2 - DO i1 = LBOUND(InData%l,1), UBOUND(InData%l,1) - DbKiBuf(Db_Xferred) = InData%l(i1) - Db_Xferred = Db_Xferred + 1 + DO i2 = LBOUND(InData%U,2), UBOUND(InData%U,2) + DO i1 = LBOUND(InData%U,1), UBOUND(InData%U,1) + DbKiBuf(Db_Xferred) = InData%U(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO END DO END IF - IF ( .NOT. ALLOCATED(InData%lstr) ) THEN + IF ( .NOT. ALLOCATED(InData%Ud) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%lstr,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%lstr,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%Ud,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Ud,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Ud,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Ud,2) Int_Xferred = Int_Xferred + 2 - DO i1 = LBOUND(InData%lstr,1), UBOUND(InData%lstr,1) - DbKiBuf(Db_Xferred) = InData%lstr(i1) - Db_Xferred = Db_Xferred + 1 + DO i2 = LBOUND(InData%Ud,2), UBOUND(InData%Ud,2) + DO i1 = LBOUND(InData%Ud,1), UBOUND(InData%Ud,1) + DbKiBuf(Db_Xferred) = InData%Ud(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO END DO END IF - IF ( .NOT. ALLOCATED(InData%lstrd) ) THEN + IF ( .NOT. ALLOCATED(InData%zeta) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%lstrd,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%lstrd,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%zeta,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%zeta,1) Int_Xferred = Int_Xferred + 2 - DO i1 = LBOUND(InData%lstrd,1), UBOUND(InData%lstrd,1) - DbKiBuf(Db_Xferred) = InData%lstrd(i1) + DO i1 = LBOUND(InData%zeta,1), UBOUND(InData%zeta,1) + DbKiBuf(Db_Xferred) = InData%zeta(i1) Db_Xferred = Db_Xferred + 1 END DO END IF - IF ( .NOT. ALLOCATED(InData%V) ) THEN + IF ( .NOT. ALLOCATED(InData%PDyn) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%V,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%V,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%PDyn,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PDyn,1) Int_Xferred = Int_Xferred + 2 - DO i1 = LBOUND(InData%V,1), UBOUND(InData%V,1) - DbKiBuf(Db_Xferred) = InData%V(i1) + DO i1 = LBOUND(InData%PDyn,1), UBOUND(InData%PDyn,1) + DbKiBuf(Db_Xferred) = InData%PDyn(i1) Db_Xferred = Db_Xferred + 1 END DO END IF @@ -1863,22 +4897,22 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz END DO END DO END IF - IF ( .NOT. ALLOCATED(InData%F) ) THEN + IF ( .NOT. ALLOCATED(InData%Fnet) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%F,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%Fnet,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Fnet,1) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%F,2) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F,2) + IntKiBuf( Int_Xferred ) = LBOUND(InData%Fnet,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Fnet,2) Int_Xferred = Int_Xferred + 2 - DO i2 = LBOUND(InData%F,2), UBOUND(InData%F,2) - DO i1 = LBOUND(InData%F,1), UBOUND(InData%F,1) - DbKiBuf(Db_Xferred) = InData%F(i1,i2) + DO i2 = LBOUND(InData%Fnet,2), UBOUND(InData%Fnet,2) + DO i1 = LBOUND(InData%Fnet,1), UBOUND(InData%Fnet,1) + DbKiBuf(Db_Xferred) = InData%Fnet(i1,i2) Db_Xferred = Db_Xferred + 1 END DO END DO @@ -1933,6 +4967,14 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz END DO END DO END IF + DO i1 = LBOUND(InData%EndMomentA,1), UBOUND(InData%EndMomentA,1) + DbKiBuf(Db_Xferred) = InData%EndMomentA(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%EndMomentB,1), UBOUND(InData%EndMomentB,1) + DbKiBuf(Db_Xferred) = InData%EndMomentB(i1) + Db_Xferred = Db_Xferred + 1 + END DO IntKiBuf(Int_Xferred) = InData%LineUnOut Int_Xferred = Int_Xferred + 1 IF ( .NOT. ALLOCATED(InData%LineWrOutput) ) THEN @@ -1946,8 +4988,8 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_Xferred = Int_Xferred + 2 DO i1 = LBOUND(InData%LineWrOutput,1), UBOUND(InData%LineWrOutput,1) - ReKiBuf(Re_Xferred) = InData%LineWrOutput(i1) - Re_Xferred = Re_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%LineWrOutput(i1) + Db_Xferred = Db_Xferred + 1 END DO END IF END SUBROUTINE MD_PackLine @@ -1983,10 +5025,8 @@ SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) Int_Xferred = 1 OutData%IdNum = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 - DO I = 1, LEN(OutData%type) - OutData%type(I:I) = CHAR(IntKiBuf(Int_Xferred)) - Int_Xferred = Int_Xferred + 1 - END DO ! I + OutData%PropsIdNum = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 i1_l = LBOUND(OutData%OutFlagList,1) i1_u = UBOUND(OutData%OutFlagList,1) DO i1 = LBOUND(OutData%OutFlagList,1), UBOUND(OutData%OutFlagList,1) @@ -1999,14 +5039,34 @@ SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) Int_Xferred = Int_Xferred + 1 OutData%AnchConnect = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 - OutData%PropsIdNum = IntKiBuf(Int_Xferred) - Int_Xferred = Int_Xferred + 1 OutData%N = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 + OutData%endTypeA = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%endTypeB = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 OutData%UnstrLen = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 + OutData%rho = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%d = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%EA = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%EI = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 OutData%BA = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 + OutData%Can = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%Cat = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%Cdn = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%Cdt = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%time = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! r not allocated Int_Xferred = Int_Xferred + 1 ELSE @@ -2094,6 +5154,24 @@ SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) Db_Xferred = Db_Xferred + 1 END DO END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ld not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%ld)) DEALLOCATE(OutData%ld) + ALLOCATE(OutData%ld(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ld.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%ld,1), UBOUND(OutData%ld,1) + OutData%ld(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END IF IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! lstr not allocated Int_Xferred = Int_Xferred + 1 ELSE @@ -2148,6 +5226,88 @@ SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) Db_Xferred = Db_Xferred + 1 END DO END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! U not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%U)) DEALLOCATE(OutData%U) + ALLOCATE(OutData%U(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%U.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%U,2), UBOUND(OutData%U,2) + DO i1 = LBOUND(OutData%U,1), UBOUND(OutData%U,1) + OutData%U(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Ud not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Ud)) DEALLOCATE(OutData%Ud) + ALLOCATE(OutData%Ud(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Ud.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Ud,2), UBOUND(OutData%Ud,2) + DO i1 = LBOUND(OutData%Ud,1), UBOUND(OutData%Ud,1) + OutData%Ud(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! zeta not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%zeta)) DEALLOCATE(OutData%zeta) + ALLOCATE(OutData%zeta(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%zeta.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%zeta,1), UBOUND(OutData%zeta,1) + OutData%zeta(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! PDyn not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%PDyn)) DEALLOCATE(OutData%PDyn) + ALLOCATE(OutData%PDyn(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%PDyn.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%PDyn,1), UBOUND(OutData%PDyn,1) + OutData%PDyn(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! T not allocated Int_Xferred = Int_Xferred + 1 ELSE @@ -2332,7 +5492,7 @@ SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) END DO END DO END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! F not allocated + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Fnet not allocated Int_Xferred = Int_Xferred + 1 ELSE Int_Xferred = Int_Xferred + 1 @@ -2342,15 +5502,15 @@ SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) i2_l = IntKiBuf( Int_Xferred ) i2_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%F)) DEALLOCATE(OutData%F) - ALLOCATE(OutData%F(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ALLOCATED(OutData%Fnet)) DEALLOCATE(OutData%Fnet) + ALLOCATE(OutData%Fnet(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%F.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Fnet.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i2 = LBOUND(OutData%F,2), UBOUND(OutData%F,2) - DO i1 = LBOUND(OutData%F,1), UBOUND(OutData%F,1) - OutData%F(i1,i2) = DbKiBuf(Db_Xferred) + DO i2 = LBOUND(OutData%Fnet,2), UBOUND(OutData%Fnet,2) + DO i1 = LBOUND(OutData%Fnet,1), UBOUND(OutData%Fnet,1) + OutData%Fnet(i1,i2) = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 END DO END DO @@ -2411,27 +5571,164 @@ SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) END DO END DO END IF + i1_l = LBOUND(OutData%EndMomentA,1) + i1_u = UBOUND(OutData%EndMomentA,1) + DO i1 = LBOUND(OutData%EndMomentA,1), UBOUND(OutData%EndMomentA,1) + OutData%EndMomentA(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%EndMomentB,1) + i1_u = UBOUND(OutData%EndMomentB,1) + DO i1 = LBOUND(OutData%EndMomentB,1), UBOUND(OutData%EndMomentB,1) + OutData%EndMomentB(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO OutData%LineUnOut = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! LineWrOutput not allocated Int_Xferred = Int_Xferred + 1 ELSE Int_Xferred = Int_Xferred + 1 - i1_l = IntKiBuf( Int_Xferred ) - i1_u = IntKiBuf( Int_Xferred + 1) - Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%LineWrOutput)) DEALLOCATE(OutData%LineWrOutput) - ALLOCATE(OutData%LineWrOutput(i1_l:i1_u),STAT=ErrStat2) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%LineWrOutput.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF - DO i1 = LBOUND(OutData%LineWrOutput,1), UBOUND(OutData%LineWrOutput,1) - OutData%LineWrOutput(i1) = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 - END DO - END IF - END SUBROUTINE MD_UnPackLine + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%LineWrOutput)) DEALLOCATE(OutData%LineWrOutput) + ALLOCATE(OutData%LineWrOutput(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%LineWrOutput.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%LineWrOutput,1), UBOUND(OutData%LineWrOutput,1) + OutData%LineWrOutput(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + END SUBROUTINE MD_UnPackLine + + SUBROUTINE MD_CopyFail( SrcFailData, DstFailData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_Fail), INTENT(IN) :: SrcFailData + TYPE(MD_Fail), INTENT(INOUT) :: DstFailData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyFail' +! + ErrStat = ErrID_None + ErrMsg = "" + DstFailData%IdNum = SrcFailData%IdNum + END SUBROUTINE MD_CopyFail + + SUBROUTINE MD_DestroyFail( FailData, ErrStat, ErrMsg ) + TYPE(MD_Fail), INTENT(INOUT) :: FailData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyFail' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE MD_DestroyFail + + SUBROUTINE MD_PackFail( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(MD_Fail), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackFail' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! IdNum + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IntKiBuf(Int_Xferred) = InData%IdNum + Int_Xferred = Int_Xferred + 1 + END SUBROUTINE MD_PackFail + + SUBROUTINE MD_UnPackFail( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_Fail), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackFail' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%IdNum = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END SUBROUTINE MD_UnPackFail SUBROUTINE MD_CopyOutParmType( SrcOutParmTypeData, DstOutParmTypeData, CtrlCode, ErrStat, ErrMsg ) TYPE(MD_OutParmType), INTENT(IN) :: SrcOutParmTypeData @@ -3501,6 +6798,57 @@ SUBROUTINE MD_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) IF (ErrStat>=AbortErrLev) RETURN ENDDO ENDIF +IF (ALLOCATED(SrcMiscData%RodTypeList)) THEN + i1_l = LBOUND(SrcMiscData%RodTypeList,1) + i1_u = UBOUND(SrcMiscData%RodTypeList,1) + IF (.NOT. ALLOCATED(DstMiscData%RodTypeList)) THEN + ALLOCATE(DstMiscData%RodTypeList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%RodTypeList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcMiscData%RodTypeList,1), UBOUND(SrcMiscData%RodTypeList,1) + CALL MD_Copyrodprop( SrcMiscData%RodTypeList(i1), DstMiscData%RodTypeList(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF + CALL MD_Copybody( SrcMiscData%GroundBody, DstMiscData%GroundBody, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN +IF (ALLOCATED(SrcMiscData%BodyList)) THEN + i1_l = LBOUND(SrcMiscData%BodyList,1) + i1_u = UBOUND(SrcMiscData%BodyList,1) + IF (.NOT. ALLOCATED(DstMiscData%BodyList)) THEN + ALLOCATE(DstMiscData%BodyList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%BodyList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcMiscData%BodyList,1), UBOUND(SrcMiscData%BodyList,1) + CALL MD_Copybody( SrcMiscData%BodyList(i1), DstMiscData%BodyList(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF +IF (ALLOCATED(SrcMiscData%RodList)) THEN + i1_l = LBOUND(SrcMiscData%RodList,1) + i1_u = UBOUND(SrcMiscData%RodList,1) + IF (.NOT. ALLOCATED(DstMiscData%RodList)) THEN + ALLOCATE(DstMiscData%RodList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%RodList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcMiscData%RodList,1), UBOUND(SrcMiscData%RodList,1) + CALL MD_Copyrod( SrcMiscData%RodList(i1), DstMiscData%RodList(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF IF (ALLOCATED(SrcMiscData%ConnectList)) THEN i1_l = LBOUND(SrcMiscData%ConnectList,1) i1_u = UBOUND(SrcMiscData%ConnectList,1) @@ -3533,42 +6881,198 @@ SUBROUTINE MD_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) IF (ErrStat>=AbortErrLev) RETURN ENDDO ENDIF -IF (ALLOCATED(SrcMiscData%FairIdList)) THEN - i1_l = LBOUND(SrcMiscData%FairIdList,1) - i1_u = UBOUND(SrcMiscData%FairIdList,1) - IF (.NOT. ALLOCATED(DstMiscData%FairIdList)) THEN - ALLOCATE(DstMiscData%FairIdList(i1_l:i1_u),STAT=ErrStat2) +IF (ALLOCATED(SrcMiscData%FailList)) THEN + i1_l = LBOUND(SrcMiscData%FailList,1) + i1_u = UBOUND(SrcMiscData%FailList,1) + IF (.NOT. ALLOCATED(DstMiscData%FailList)) THEN + ALLOCATE(DstMiscData%FailList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%FailList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcMiscData%FailList,1), UBOUND(SrcMiscData%FailList,1) + CALL MD_Copyfail( SrcMiscData%FailList(i1), DstMiscData%FailList(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF +IF (ALLOCATED(SrcMiscData%FreeConIs)) THEN + i1_l = LBOUND(SrcMiscData%FreeConIs,1) + i1_u = UBOUND(SrcMiscData%FreeConIs,1) + IF (.NOT. ALLOCATED(DstMiscData%FreeConIs)) THEN + ALLOCATE(DstMiscData%FreeConIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%FreeConIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%FreeConIs = SrcMiscData%FreeConIs +ENDIF +IF (ALLOCATED(SrcMiscData%CpldConIs)) THEN + i1_l = LBOUND(SrcMiscData%CpldConIs,1) + i1_u = UBOUND(SrcMiscData%CpldConIs,1) + IF (.NOT. ALLOCATED(DstMiscData%CpldConIs)) THEN + ALLOCATE(DstMiscData%CpldConIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%CpldConIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%CpldConIs = SrcMiscData%CpldConIs +ENDIF +IF (ALLOCATED(SrcMiscData%FreeRodIs)) THEN + i1_l = LBOUND(SrcMiscData%FreeRodIs,1) + i1_u = UBOUND(SrcMiscData%FreeRodIs,1) + IF (.NOT. ALLOCATED(DstMiscData%FreeRodIs)) THEN + ALLOCATE(DstMiscData%FreeRodIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%FreeRodIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%FreeRodIs = SrcMiscData%FreeRodIs +ENDIF +IF (ALLOCATED(SrcMiscData%CpldRodIs)) THEN + i1_l = LBOUND(SrcMiscData%CpldRodIs,1) + i1_u = UBOUND(SrcMiscData%CpldRodIs,1) + IF (.NOT. ALLOCATED(DstMiscData%CpldRodIs)) THEN + ALLOCATE(DstMiscData%CpldRodIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%CpldRodIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%CpldRodIs = SrcMiscData%CpldRodIs +ENDIF +IF (ALLOCATED(SrcMiscData%FreeBodyIs)) THEN + i1_l = LBOUND(SrcMiscData%FreeBodyIs,1) + i1_u = UBOUND(SrcMiscData%FreeBodyIs,1) + IF (.NOT. ALLOCATED(DstMiscData%FreeBodyIs)) THEN + ALLOCATE(DstMiscData%FreeBodyIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%FreeBodyIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%FreeBodyIs = SrcMiscData%FreeBodyIs +ENDIF +IF (ALLOCATED(SrcMiscData%CpldBodyIs)) THEN + i1_l = LBOUND(SrcMiscData%CpldBodyIs,1) + i1_u = UBOUND(SrcMiscData%CpldBodyIs,1) + IF (.NOT. ALLOCATED(DstMiscData%CpldBodyIs)) THEN + ALLOCATE(DstMiscData%CpldBodyIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%CpldBodyIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%CpldBodyIs = SrcMiscData%CpldBodyIs +ENDIF +IF (ALLOCATED(SrcMiscData%LineStateIs1)) THEN + i1_l = LBOUND(SrcMiscData%LineStateIs1,1) + i1_u = UBOUND(SrcMiscData%LineStateIs1,1) + IF (.NOT. ALLOCATED(DstMiscData%LineStateIs1)) THEN + ALLOCATE(DstMiscData%LineStateIs1(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%LineStateIs1.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%LineStateIs1 = SrcMiscData%LineStateIs1 +ENDIF +IF (ALLOCATED(SrcMiscData%LineStateIsN)) THEN + i1_l = LBOUND(SrcMiscData%LineStateIsN,1) + i1_u = UBOUND(SrcMiscData%LineStateIsN,1) + IF (.NOT. ALLOCATED(DstMiscData%LineStateIsN)) THEN + ALLOCATE(DstMiscData%LineStateIsN(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%LineStateIsN.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%LineStateIsN = SrcMiscData%LineStateIsN +ENDIF +IF (ALLOCATED(SrcMiscData%ConStateIs1)) THEN + i1_l = LBOUND(SrcMiscData%ConStateIs1,1) + i1_u = UBOUND(SrcMiscData%ConStateIs1,1) + IF (.NOT. ALLOCATED(DstMiscData%ConStateIs1)) THEN + ALLOCATE(DstMiscData%ConStateIs1(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%ConStateIs1.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%ConStateIs1 = SrcMiscData%ConStateIs1 +ENDIF +IF (ALLOCATED(SrcMiscData%ConStateIsN)) THEN + i1_l = LBOUND(SrcMiscData%ConStateIsN,1) + i1_u = UBOUND(SrcMiscData%ConStateIsN,1) + IF (.NOT. ALLOCATED(DstMiscData%ConStateIsN)) THEN + ALLOCATE(DstMiscData%ConStateIsN(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%ConStateIsN.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%ConStateIsN = SrcMiscData%ConStateIsN +ENDIF +IF (ALLOCATED(SrcMiscData%RodStateIs1)) THEN + i1_l = LBOUND(SrcMiscData%RodStateIs1,1) + i1_u = UBOUND(SrcMiscData%RodStateIs1,1) + IF (.NOT. ALLOCATED(DstMiscData%RodStateIs1)) THEN + ALLOCATE(DstMiscData%RodStateIs1(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%RodStateIs1.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%RodStateIs1 = SrcMiscData%RodStateIs1 +ENDIF +IF (ALLOCATED(SrcMiscData%RodStateIsN)) THEN + i1_l = LBOUND(SrcMiscData%RodStateIsN,1) + i1_u = UBOUND(SrcMiscData%RodStateIsN,1) + IF (.NOT. ALLOCATED(DstMiscData%RodStateIsN)) THEN + ALLOCATE(DstMiscData%RodStateIsN(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%FairIdList.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%RodStateIsN.', ErrStat, ErrMsg,RoutineName) RETURN END IF END IF - DstMiscData%FairIdList = SrcMiscData%FairIdList + DstMiscData%RodStateIsN = SrcMiscData%RodStateIsN ENDIF -IF (ALLOCATED(SrcMiscData%ConnIdList)) THEN - i1_l = LBOUND(SrcMiscData%ConnIdList,1) - i1_u = UBOUND(SrcMiscData%ConnIdList,1) - IF (.NOT. ALLOCATED(DstMiscData%ConnIdList)) THEN - ALLOCATE(DstMiscData%ConnIdList(i1_l:i1_u),STAT=ErrStat2) +IF (ALLOCATED(SrcMiscData%BodyStateIs1)) THEN + i1_l = LBOUND(SrcMiscData%BodyStateIs1,1) + i1_u = UBOUND(SrcMiscData%BodyStateIs1,1) + IF (.NOT. ALLOCATED(DstMiscData%BodyStateIs1)) THEN + ALLOCATE(DstMiscData%BodyStateIs1(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%ConnIdList.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%BodyStateIs1.', ErrStat, ErrMsg,RoutineName) RETURN END IF END IF - DstMiscData%ConnIdList = SrcMiscData%ConnIdList + DstMiscData%BodyStateIs1 = SrcMiscData%BodyStateIs1 ENDIF -IF (ALLOCATED(SrcMiscData%LineStateIndList)) THEN - i1_l = LBOUND(SrcMiscData%LineStateIndList,1) - i1_u = UBOUND(SrcMiscData%LineStateIndList,1) - IF (.NOT. ALLOCATED(DstMiscData%LineStateIndList)) THEN - ALLOCATE(DstMiscData%LineStateIndList(i1_l:i1_u),STAT=ErrStat2) +IF (ALLOCATED(SrcMiscData%BodyStateIsN)) THEN + i1_l = LBOUND(SrcMiscData%BodyStateIsN,1) + i1_u = UBOUND(SrcMiscData%BodyStateIsN,1) + IF (.NOT. ALLOCATED(DstMiscData%BodyStateIsN)) THEN + ALLOCATE(DstMiscData%BodyStateIsN(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%LineStateIndList.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%BodyStateIsN.', ErrStat, ErrMsg,RoutineName) RETURN END IF END IF - DstMiscData%LineStateIndList = SrcMiscData%LineStateIndList + DstMiscData%BodyStateIsN = SrcMiscData%BodyStateIsN ENDIF + DstMiscData%Nx = SrcMiscData%Nx + CALL MD_CopyContState( SrcMiscData%xTemp, DstMiscData%xTemp, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + CALL MD_CopyContState( SrcMiscData%xdTemp, DstMiscData%xdTemp, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + DstMiscData%zeros6 = SrcMiscData%zeros6 IF (ALLOCATED(SrcMiscData%MDWrOutput)) THEN i1_l = LBOUND(SrcMiscData%MDWrOutput,1) i1_u = UBOUND(SrcMiscData%MDWrOutput,1) @@ -3598,6 +7102,25 @@ SUBROUTINE MD_DestroyMisc( MiscData, ErrStat, ErrMsg ) ENDDO DEALLOCATE(MiscData%LineTypeList) ENDIF +IF (ALLOCATED(MiscData%RodTypeList)) THEN +DO i1 = LBOUND(MiscData%RodTypeList,1), UBOUND(MiscData%RodTypeList,1) + CALL MD_Destroyrodprop( MiscData%RodTypeList(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(MiscData%RodTypeList) +ENDIF + CALL MD_Destroybody( MiscData%GroundBody, ErrStat, ErrMsg ) +IF (ALLOCATED(MiscData%BodyList)) THEN +DO i1 = LBOUND(MiscData%BodyList,1), UBOUND(MiscData%BodyList,1) + CALL MD_Destroybody( MiscData%BodyList(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(MiscData%BodyList) +ENDIF +IF (ALLOCATED(MiscData%RodList)) THEN +DO i1 = LBOUND(MiscData%RodList,1), UBOUND(MiscData%RodList,1) + CALL MD_Destroyrod( MiscData%RodList(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(MiscData%RodList) +ENDIF IF (ALLOCATED(MiscData%ConnectList)) THEN DO i1 = LBOUND(MiscData%ConnectList,1), UBOUND(MiscData%ConnectList,1) CALL MD_Destroyconnect( MiscData%ConnectList(i1), ErrStat, ErrMsg ) @@ -3610,15 +7133,56 @@ SUBROUTINE MD_DestroyMisc( MiscData, ErrStat, ErrMsg ) ENDDO DEALLOCATE(MiscData%LineList) ENDIF -IF (ALLOCATED(MiscData%FairIdList)) THEN - DEALLOCATE(MiscData%FairIdList) +IF (ALLOCATED(MiscData%FailList)) THEN +DO i1 = LBOUND(MiscData%FailList,1), UBOUND(MiscData%FailList,1) + CALL MD_Destroyfail( MiscData%FailList(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(MiscData%FailList) +ENDIF +IF (ALLOCATED(MiscData%FreeConIs)) THEN + DEALLOCATE(MiscData%FreeConIs) +ENDIF +IF (ALLOCATED(MiscData%CpldConIs)) THEN + DEALLOCATE(MiscData%CpldConIs) +ENDIF +IF (ALLOCATED(MiscData%FreeRodIs)) THEN + DEALLOCATE(MiscData%FreeRodIs) +ENDIF +IF (ALLOCATED(MiscData%CpldRodIs)) THEN + DEALLOCATE(MiscData%CpldRodIs) +ENDIF +IF (ALLOCATED(MiscData%FreeBodyIs)) THEN + DEALLOCATE(MiscData%FreeBodyIs) +ENDIF +IF (ALLOCATED(MiscData%CpldBodyIs)) THEN + DEALLOCATE(MiscData%CpldBodyIs) +ENDIF +IF (ALLOCATED(MiscData%LineStateIs1)) THEN + DEALLOCATE(MiscData%LineStateIs1) ENDIF -IF (ALLOCATED(MiscData%ConnIdList)) THEN - DEALLOCATE(MiscData%ConnIdList) +IF (ALLOCATED(MiscData%LineStateIsN)) THEN + DEALLOCATE(MiscData%LineStateIsN) ENDIF -IF (ALLOCATED(MiscData%LineStateIndList)) THEN - DEALLOCATE(MiscData%LineStateIndList) +IF (ALLOCATED(MiscData%ConStateIs1)) THEN + DEALLOCATE(MiscData%ConStateIs1) ENDIF +IF (ALLOCATED(MiscData%ConStateIsN)) THEN + DEALLOCATE(MiscData%ConStateIsN) +ENDIF +IF (ALLOCATED(MiscData%RodStateIs1)) THEN + DEALLOCATE(MiscData%RodStateIs1) +ENDIF +IF (ALLOCATED(MiscData%RodStateIsN)) THEN + DEALLOCATE(MiscData%RodStateIsN) +ENDIF +IF (ALLOCATED(MiscData%BodyStateIs1)) THEN + DEALLOCATE(MiscData%BodyStateIs1) +ENDIF +IF (ALLOCATED(MiscData%BodyStateIsN)) THEN + DEALLOCATE(MiscData%BodyStateIsN) +ENDIF + CALL MD_DestroyContState( MiscData%xTemp, ErrStat, ErrMsg ) + CALL MD_DestroyContState( MiscData%xdTemp, ErrStat, ErrMsg ) IF (ALLOCATED(MiscData%MDWrOutput)) THEN DEALLOCATE(MiscData%MDWrOutput) ENDIF @@ -3649,145 +7213,619 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz REAL(DbKi), ALLOCATABLE :: Db_Buf(:) INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) - OnlySize = .FALSE. - IF ( PRESENT(SizeOnly) ) THEN - OnlySize = SizeOnly - ENDIF - ! - ErrStat = ErrID_None - ErrMsg = "" - Re_BufSz = 0 - Db_BufSz = 0 - Int_BufSz = 0 - Int_BufSz = Int_BufSz + 1 ! LineTypeList allocated yes/no - IF ( ALLOCATED(InData%LineTypeList) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! LineTypeList upper/lower bounds for each dimension - ! Allocate buffers for subtypes, if any (we'll get sizes from these) - DO i1 = LBOUND(InData%LineTypeList,1), UBOUND(InData%LineTypeList,1) - Int_BufSz = Int_BufSz + 3 ! LineTypeList: size of buffers for each call to pack subtype - CALL MD_Packlineprop( Re_Buf, Db_Buf, Int_Buf, InData%LineTypeList(i1), ErrStat2, ErrMsg2, .TRUE. ) ! LineTypeList + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! LineTypeList allocated yes/no + IF ( ALLOCATED(InData%LineTypeList) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! LineTypeList upper/lower bounds for each dimension + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + DO i1 = LBOUND(InData%LineTypeList,1), UBOUND(InData%LineTypeList,1) + Int_BufSz = Int_BufSz + 3 ! LineTypeList: size of buffers for each call to pack subtype + CALL MD_Packlineprop( Re_Buf, Db_Buf, Int_Buf, InData%LineTypeList(i1), ErrStat2, ErrMsg2, .TRUE. ) ! LineTypeList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! LineTypeList + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! LineTypeList + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! LineTypeList + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 1 ! RodTypeList allocated yes/no + IF ( ALLOCATED(InData%RodTypeList) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! RodTypeList upper/lower bounds for each dimension + DO i1 = LBOUND(InData%RodTypeList,1), UBOUND(InData%RodTypeList,1) + Int_BufSz = Int_BufSz + 3 ! RodTypeList: size of buffers for each call to pack subtype + CALL MD_Packrodprop( Re_Buf, Db_Buf, Int_Buf, InData%RodTypeList(i1), ErrStat2, ErrMsg2, .TRUE. ) ! RodTypeList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! RodTypeList + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! RodTypeList + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! RodTypeList + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 3 ! GroundBody: size of buffers for each call to pack subtype + CALL MD_Packbody( Re_Buf, Db_Buf, Int_Buf, InData%GroundBody, ErrStat2, ErrMsg2, .TRUE. ) ! GroundBody + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! GroundBody + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! GroundBody + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! GroundBody + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + Int_BufSz = Int_BufSz + 1 ! BodyList allocated yes/no + IF ( ALLOCATED(InData%BodyList) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! BodyList upper/lower bounds for each dimension + DO i1 = LBOUND(InData%BodyList,1), UBOUND(InData%BodyList,1) + Int_BufSz = Int_BufSz + 3 ! BodyList: size of buffers for each call to pack subtype + CALL MD_Packbody( Re_Buf, Db_Buf, Int_Buf, InData%BodyList(i1), ErrStat2, ErrMsg2, .TRUE. ) ! BodyList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! BodyList + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! BodyList + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! BodyList + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 1 ! RodList allocated yes/no + IF ( ALLOCATED(InData%RodList) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! RodList upper/lower bounds for each dimension + DO i1 = LBOUND(InData%RodList,1), UBOUND(InData%RodList,1) + Int_BufSz = Int_BufSz + 3 ! RodList: size of buffers for each call to pack subtype + CALL MD_Packrod( Re_Buf, Db_Buf, Int_Buf, InData%RodList(i1), ErrStat2, ErrMsg2, .TRUE. ) ! RodList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! RodList + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! RodList + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! RodList + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 1 ! ConnectList allocated yes/no + IF ( ALLOCATED(InData%ConnectList) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! ConnectList upper/lower bounds for each dimension + DO i1 = LBOUND(InData%ConnectList,1), UBOUND(InData%ConnectList,1) + Int_BufSz = Int_BufSz + 3 ! ConnectList: size of buffers for each call to pack subtype + CALL MD_Packconnect( Re_Buf, Db_Buf, Int_Buf, InData%ConnectList(i1), ErrStat2, ErrMsg2, .TRUE. ) ! ConnectList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! ConnectList + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! ConnectList + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! ConnectList + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 1 ! LineList allocated yes/no + IF ( ALLOCATED(InData%LineList) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! LineList upper/lower bounds for each dimension + DO i1 = LBOUND(InData%LineList,1), UBOUND(InData%LineList,1) + Int_BufSz = Int_BufSz + 3 ! LineList: size of buffers for each call to pack subtype + CALL MD_Packline( Re_Buf, Db_Buf, Int_Buf, InData%LineList(i1), ErrStat2, ErrMsg2, .TRUE. ) ! LineList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! LineList + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! LineList + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! LineList + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 1 ! FailList allocated yes/no + IF ( ALLOCATED(InData%FailList) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! FailList upper/lower bounds for each dimension + DO i1 = LBOUND(InData%FailList,1), UBOUND(InData%FailList,1) + Int_BufSz = Int_BufSz + 3 ! FailList: size of buffers for each call to pack subtype + CALL MD_Packfail( Re_Buf, Db_Buf, Int_Buf, InData%FailList(i1), ErrStat2, ErrMsg2, .TRUE. ) ! FailList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! FailList + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! FailList + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! FailList + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 1 ! FreeConIs allocated yes/no + IF ( ALLOCATED(InData%FreeConIs) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! FreeConIs upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%FreeConIs) ! FreeConIs + END IF + Int_BufSz = Int_BufSz + 1 ! CpldConIs allocated yes/no + IF ( ALLOCATED(InData%CpldConIs) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! CpldConIs upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%CpldConIs) ! CpldConIs + END IF + Int_BufSz = Int_BufSz + 1 ! FreeRodIs allocated yes/no + IF ( ALLOCATED(InData%FreeRodIs) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! FreeRodIs upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%FreeRodIs) ! FreeRodIs + END IF + Int_BufSz = Int_BufSz + 1 ! CpldRodIs allocated yes/no + IF ( ALLOCATED(InData%CpldRodIs) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! CpldRodIs upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%CpldRodIs) ! CpldRodIs + END IF + Int_BufSz = Int_BufSz + 1 ! FreeBodyIs allocated yes/no + IF ( ALLOCATED(InData%FreeBodyIs) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! FreeBodyIs upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%FreeBodyIs) ! FreeBodyIs + END IF + Int_BufSz = Int_BufSz + 1 ! CpldBodyIs allocated yes/no + IF ( ALLOCATED(InData%CpldBodyIs) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! CpldBodyIs upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%CpldBodyIs) ! CpldBodyIs + END IF + Int_BufSz = Int_BufSz + 1 ! LineStateIs1 allocated yes/no + IF ( ALLOCATED(InData%LineStateIs1) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! LineStateIs1 upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%LineStateIs1) ! LineStateIs1 + END IF + Int_BufSz = Int_BufSz + 1 ! LineStateIsN allocated yes/no + IF ( ALLOCATED(InData%LineStateIsN) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! LineStateIsN upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%LineStateIsN) ! LineStateIsN + END IF + Int_BufSz = Int_BufSz + 1 ! ConStateIs1 allocated yes/no + IF ( ALLOCATED(InData%ConStateIs1) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! ConStateIs1 upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%ConStateIs1) ! ConStateIs1 + END IF + Int_BufSz = Int_BufSz + 1 ! ConStateIsN allocated yes/no + IF ( ALLOCATED(InData%ConStateIsN) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! ConStateIsN upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%ConStateIsN) ! ConStateIsN + END IF + Int_BufSz = Int_BufSz + 1 ! RodStateIs1 allocated yes/no + IF ( ALLOCATED(InData%RodStateIs1) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! RodStateIs1 upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%RodStateIs1) ! RodStateIs1 + END IF + Int_BufSz = Int_BufSz + 1 ! RodStateIsN allocated yes/no + IF ( ALLOCATED(InData%RodStateIsN) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! RodStateIsN upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%RodStateIsN) ! RodStateIsN + END IF + Int_BufSz = Int_BufSz + 1 ! BodyStateIs1 allocated yes/no + IF ( ALLOCATED(InData%BodyStateIs1) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! BodyStateIs1 upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%BodyStateIs1) ! BodyStateIs1 + END IF + Int_BufSz = Int_BufSz + 1 ! BodyStateIsN allocated yes/no + IF ( ALLOCATED(InData%BodyStateIsN) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! BodyStateIsN upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%BodyStateIsN) ! BodyStateIsN + END IF + Int_BufSz = Int_BufSz + 1 ! Nx + Int_BufSz = Int_BufSz + 3 ! xTemp: size of buffers for each call to pack subtype + CALL MD_PackContState( Re_Buf, Db_Buf, Int_Buf, InData%xTemp, ErrStat2, ErrMsg2, .TRUE. ) ! xTemp CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN - IF(ALLOCATED(Re_Buf)) THEN ! LineTypeList + IF(ALLOCATED(Re_Buf)) THEN ! xTemp Re_BufSz = Re_BufSz + SIZE( Re_Buf ) DEALLOCATE(Re_Buf) END IF - IF(ALLOCATED(Db_Buf)) THEN ! LineTypeList + IF(ALLOCATED(Db_Buf)) THEN ! xTemp Db_BufSz = Db_BufSz + SIZE( Db_Buf ) DEALLOCATE(Db_Buf) END IF - IF(ALLOCATED(Int_Buf)) THEN ! LineTypeList + IF(ALLOCATED(Int_Buf)) THEN ! xTemp + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + Int_BufSz = Int_BufSz + 3 ! xdTemp: size of buffers for each call to pack subtype + CALL MD_PackContState( Re_Buf, Db_Buf, Int_Buf, InData%xdTemp, ErrStat2, ErrMsg2, .TRUE. ) ! xdTemp + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! xdTemp + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! xdTemp + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! xdTemp Int_BufSz = Int_BufSz + SIZE( Int_Buf ) DEALLOCATE(Int_Buf) END IF + Db_BufSz = Db_BufSz + SIZE(InData%zeros6) ! zeros6 + Int_BufSz = Int_BufSz + 1 ! MDWrOutput allocated yes/no + IF ( ALLOCATED(InData%MDWrOutput) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! MDWrOutput upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%MDWrOutput) ! MDWrOutput + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IF ( .NOT. ALLOCATED(InData%LineTypeList) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%LineTypeList,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%LineTypeList,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%LineTypeList,1), UBOUND(InData%LineTypeList,1) + CALL MD_Packlineprop( Re_Buf, Db_Buf, Int_Buf, InData%LineTypeList(i1), ErrStat2, ErrMsg2, OnlySize ) ! LineTypeList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF + IF ( .NOT. ALLOCATED(InData%RodTypeList) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%RodTypeList,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%RodTypeList,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%RodTypeList,1), UBOUND(InData%RodTypeList,1) + CALL MD_Packrodprop( Re_Buf, Db_Buf, Int_Buf, InData%RodTypeList(i1), ErrStat2, ErrMsg2, OnlySize ) ! RodTypeList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF + CALL MD_Packbody( Re_Buf, Db_Buf, Int_Buf, InData%GroundBody, ErrStat2, ErrMsg2, OnlySize ) ! GroundBody + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF ( .NOT. ALLOCATED(InData%BodyList) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%BodyList,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%BodyList,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%BodyList,1), UBOUND(InData%BodyList,1) + CALL MD_Packbody( Re_Buf, Db_Buf, Int_Buf, InData%BodyList(i1), ErrStat2, ErrMsg2, OnlySize ) ! BodyList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF + IF ( .NOT. ALLOCATED(InData%RodList) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%RodList,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%RodList,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%RodList,1), UBOUND(InData%RodList,1) + CALL MD_Packrod( Re_Buf, Db_Buf, Int_Buf, InData%RodList(i1), ErrStat2, ErrMsg2, OnlySize ) ! RodList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF END DO END IF - Int_BufSz = Int_BufSz + 1 ! ConnectList allocated yes/no - IF ( ALLOCATED(InData%ConnectList) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! ConnectList upper/lower bounds for each dimension + IF ( .NOT. ALLOCATED(InData%ConnectList) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ConnectList,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ConnectList,1) + Int_Xferred = Int_Xferred + 2 + DO i1 = LBOUND(InData%ConnectList,1), UBOUND(InData%ConnectList,1) - Int_BufSz = Int_BufSz + 3 ! ConnectList: size of buffers for each call to pack subtype - CALL MD_Packconnect( Re_Buf, Db_Buf, Int_Buf, InData%ConnectList(i1), ErrStat2, ErrMsg2, .TRUE. ) ! ConnectList + CALL MD_Packconnect( Re_Buf, Db_Buf, Int_Buf, InData%ConnectList(i1), ErrStat2, ErrMsg2, OnlySize ) ! ConnectList CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN - IF(ALLOCATED(Re_Buf)) THEN ! ConnectList - Re_BufSz = Re_BufSz + SIZE( Re_Buf ) - DEALLOCATE(Re_Buf) - END IF - IF(ALLOCATED(Db_Buf)) THEN ! ConnectList - Db_BufSz = Db_BufSz + SIZE( Db_Buf ) - DEALLOCATE(Db_Buf) - END IF - IF(ALLOCATED(Int_Buf)) THEN ! ConnectList - Int_BufSz = Int_BufSz + SIZE( Int_Buf ) - DEALLOCATE(Int_Buf) - END IF + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF END DO END IF - Int_BufSz = Int_BufSz + 1 ! LineList allocated yes/no - IF ( ALLOCATED(InData%LineList) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! LineList upper/lower bounds for each dimension + IF ( .NOT. ALLOCATED(InData%LineList) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%LineList,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%LineList,1) + Int_Xferred = Int_Xferred + 2 + DO i1 = LBOUND(InData%LineList,1), UBOUND(InData%LineList,1) - Int_BufSz = Int_BufSz + 3 ! LineList: size of buffers for each call to pack subtype - CALL MD_Packline( Re_Buf, Db_Buf, Int_Buf, InData%LineList(i1), ErrStat2, ErrMsg2, .TRUE. ) ! LineList + CALL MD_Packline( Re_Buf, Db_Buf, Int_Buf, InData%LineList(i1), ErrStat2, ErrMsg2, OnlySize ) ! LineList CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN - IF(ALLOCATED(Re_Buf)) THEN ! LineList - Re_BufSz = Re_BufSz + SIZE( Re_Buf ) - DEALLOCATE(Re_Buf) - END IF - IF(ALLOCATED(Db_Buf)) THEN ! LineList - Db_BufSz = Db_BufSz + SIZE( Db_Buf ) - DEALLOCATE(Db_Buf) - END IF - IF(ALLOCATED(Int_Buf)) THEN ! LineList - Int_BufSz = Int_BufSz + SIZE( Int_Buf ) - DEALLOCATE(Int_Buf) - END IF + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF END DO END IF - Int_BufSz = Int_BufSz + 1 ! FairIdList allocated yes/no - IF ( ALLOCATED(InData%FairIdList) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! FairIdList upper/lower bounds for each dimension - Int_BufSz = Int_BufSz + SIZE(InData%FairIdList) ! FairIdList - END IF - Int_BufSz = Int_BufSz + 1 ! ConnIdList allocated yes/no - IF ( ALLOCATED(InData%ConnIdList) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! ConnIdList upper/lower bounds for each dimension - Int_BufSz = Int_BufSz + SIZE(InData%ConnIdList) ! ConnIdList - END IF - Int_BufSz = Int_BufSz + 1 ! LineStateIndList allocated yes/no - IF ( ALLOCATED(InData%LineStateIndList) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! LineStateIndList upper/lower bounds for each dimension - Int_BufSz = Int_BufSz + SIZE(InData%LineStateIndList) ! LineStateIndList - END IF - Int_BufSz = Int_BufSz + 1 ! MDWrOutput allocated yes/no - IF ( ALLOCATED(InData%MDWrOutput) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! MDWrOutput upper/lower bounds for each dimension - Re_BufSz = Re_BufSz + SIZE(InData%MDWrOutput) ! MDWrOutput - END IF - IF ( Re_BufSz .GT. 0 ) THEN - ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF - END IF - IF ( Db_BufSz .GT. 0 ) THEN - ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF - END IF - IF ( Int_BufSz .GT. 0 ) THEN - ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF - END IF - IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) - - Re_Xferred = 1 - Db_Xferred = 1 - Int_Xferred = 1 - - IF ( .NOT. ALLOCATED(InData%LineTypeList) ) THEN + IF ( .NOT. ALLOCATED(InData%FailList) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%LineTypeList,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%LineTypeList,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%FailList,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%FailList,1) Int_Xferred = Int_Xferred + 2 - DO i1 = LBOUND(InData%LineTypeList,1), UBOUND(InData%LineTypeList,1) - CALL MD_Packlineprop( Re_Buf, Db_Buf, Int_Buf, InData%LineTypeList(i1), ErrStat2, ErrMsg2, OnlySize ) ! LineTypeList + DO i1 = LBOUND(InData%FailList,1), UBOUND(InData%FailList,1) + CALL MD_Packfail( Re_Buf, Db_Buf, Int_Buf, InData%FailList(i1), ErrStat2, ErrMsg2, OnlySize ) ! FailList CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -3817,18 +7855,219 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz ENDIF END DO END IF - IF ( .NOT. ALLOCATED(InData%ConnectList) ) THEN + IF ( .NOT. ALLOCATED(InData%FreeConIs) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%FreeConIs,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%FreeConIs,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%FreeConIs,1), UBOUND(InData%FreeConIs,1) + IntKiBuf(Int_Xferred) = InData%FreeConIs(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%CpldConIs) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%CpldConIs,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%CpldConIs,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%CpldConIs,1), UBOUND(InData%CpldConIs,1) + IntKiBuf(Int_Xferred) = InData%CpldConIs(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%FreeRodIs) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%FreeRodIs,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%FreeRodIs,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%FreeRodIs,1), UBOUND(InData%FreeRodIs,1) + IntKiBuf(Int_Xferred) = InData%FreeRodIs(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%CpldRodIs) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%CpldRodIs,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%CpldRodIs,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%CpldRodIs,1), UBOUND(InData%CpldRodIs,1) + IntKiBuf(Int_Xferred) = InData%CpldRodIs(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%FreeBodyIs) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%FreeBodyIs,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%FreeBodyIs,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%FreeBodyIs,1), UBOUND(InData%FreeBodyIs,1) + IntKiBuf(Int_Xferred) = InData%FreeBodyIs(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%CpldBodyIs) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%CpldBodyIs,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%CpldBodyIs,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%CpldBodyIs,1), UBOUND(InData%CpldBodyIs,1) + IntKiBuf(Int_Xferred) = InData%CpldBodyIs(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%LineStateIs1) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%LineStateIs1,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%LineStateIs1,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%LineStateIs1,1), UBOUND(InData%LineStateIs1,1) + IntKiBuf(Int_Xferred) = InData%LineStateIs1(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%LineStateIsN) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%LineStateIsN,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%LineStateIsN,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%LineStateIsN,1), UBOUND(InData%LineStateIsN,1) + IntKiBuf(Int_Xferred) = InData%LineStateIsN(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%ConStateIs1) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ConStateIs1,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ConStateIs1,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%ConStateIs1,1), UBOUND(InData%ConStateIs1,1) + IntKiBuf(Int_Xferred) = InData%ConStateIs1(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%ConStateIsN) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ConStateIsN,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ConStateIsN,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%ConStateIsN,1), UBOUND(InData%ConStateIsN,1) + IntKiBuf(Int_Xferred) = InData%ConStateIsN(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%RodStateIs1) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%RodStateIs1,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%RodStateIs1,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%RodStateIs1,1), UBOUND(InData%RodStateIs1,1) + IntKiBuf(Int_Xferred) = InData%RodStateIs1(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%RodStateIsN) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%ConnectList,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ConnectList,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%RodStateIsN,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%RodStateIsN,1) Int_Xferred = Int_Xferred + 2 - DO i1 = LBOUND(InData%ConnectList,1), UBOUND(InData%ConnectList,1) - CALL MD_Packconnect( Re_Buf, Db_Buf, Int_Buf, InData%ConnectList(i1), ErrStat2, ErrMsg2, OnlySize ) ! ConnectList + DO i1 = LBOUND(InData%RodStateIsN,1), UBOUND(InData%RodStateIsN,1) + IntKiBuf(Int_Xferred) = InData%RodStateIsN(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%BodyStateIs1) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%BodyStateIs1,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%BodyStateIs1,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%BodyStateIs1,1), UBOUND(InData%BodyStateIs1,1) + IntKiBuf(Int_Xferred) = InData%BodyStateIs1(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%BodyStateIsN) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%BodyStateIsN,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%BodyStateIsN,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%BodyStateIsN,1), UBOUND(InData%BodyStateIsN,1) + IntKiBuf(Int_Xferred) = InData%BodyStateIsN(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IntKiBuf(Int_Xferred) = InData%Nx + Int_Xferred = Int_Xferred + 1 + CALL MD_PackContState( Re_Buf, Db_Buf, Int_Buf, InData%xTemp, ErrStat2, ErrMsg2, OnlySize ) ! xTemp CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -3856,20 +8095,7 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz ELSE IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 ENDIF - END DO - END IF - IF ( .NOT. ALLOCATED(InData%LineList) ) THEN - IntKiBuf( Int_Xferred ) = 0 - Int_Xferred = Int_Xferred + 1 - ELSE - IntKiBuf( Int_Xferred ) = 1 - Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%LineList,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%LineList,1) - Int_Xferred = Int_Xferred + 2 - - DO i1 = LBOUND(InData%LineList,1), UBOUND(InData%LineList,1) - CALL MD_Packline( Re_Buf, Db_Buf, Int_Buf, InData%LineList(i1), ErrStat2, ErrMsg2, OnlySize ) ! LineList + CALL MD_PackContState( Re_Buf, Db_Buf, Int_Buf, InData%xdTemp, ErrStat2, ErrMsg2, OnlySize ) ! xdTemp CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -3897,53 +8123,10 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz ELSE IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 ENDIF + DO i1 = LBOUND(InData%zeros6,1), UBOUND(InData%zeros6,1) + DbKiBuf(Db_Xferred) = InData%zeros6(i1) + Db_Xferred = Db_Xferred + 1 END DO - END IF - IF ( .NOT. ALLOCATED(InData%FairIdList) ) THEN - IntKiBuf( Int_Xferred ) = 0 - Int_Xferred = Int_Xferred + 1 - ELSE - IntKiBuf( Int_Xferred ) = 1 - Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%FairIdList,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%FairIdList,1) - Int_Xferred = Int_Xferred + 2 - - DO i1 = LBOUND(InData%FairIdList,1), UBOUND(InData%FairIdList,1) - IntKiBuf(Int_Xferred) = InData%FairIdList(i1) - Int_Xferred = Int_Xferred + 1 - END DO - END IF - IF ( .NOT. ALLOCATED(InData%ConnIdList) ) THEN - IntKiBuf( Int_Xferred ) = 0 - Int_Xferred = Int_Xferred + 1 - ELSE - IntKiBuf( Int_Xferred ) = 1 - Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%ConnIdList,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ConnIdList,1) - Int_Xferred = Int_Xferred + 2 - - DO i1 = LBOUND(InData%ConnIdList,1), UBOUND(InData%ConnIdList,1) - IntKiBuf(Int_Xferred) = InData%ConnIdList(i1) - Int_Xferred = Int_Xferred + 1 - END DO - END IF - IF ( .NOT. ALLOCATED(InData%LineStateIndList) ) THEN - IntKiBuf( Int_Xferred ) = 0 - Int_Xferred = Int_Xferred + 1 - ELSE - IntKiBuf( Int_Xferred ) = 1 - Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%LineStateIndList,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%LineStateIndList,1) - Int_Xferred = Int_Xferred + 2 - - DO i1 = LBOUND(InData%LineStateIndList,1), UBOUND(InData%LineStateIndList,1) - IntKiBuf(Int_Xferred) = InData%LineStateIndList(i1) - Int_Xferred = Int_Xferred + 1 - END DO - END IF IF ( .NOT. ALLOCATED(InData%MDWrOutput) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -3955,8 +8138,8 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_Xferred = Int_Xferred + 2 DO i1 = LBOUND(InData%MDWrOutput,1), UBOUND(InData%MDWrOutput,1) - ReKiBuf(Re_Xferred) = InData%MDWrOutput(i1) - Re_Xferred = Re_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%MDWrOutput(i1) + Db_Xferred = Db_Xferred + 1 END DO END IF END SUBROUTINE MD_PackMisc @@ -3995,13 +8178,221 @@ SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) i1_l = IntKiBuf( Int_Xferred ) i1_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%LineTypeList)) DEALLOCATE(OutData%LineTypeList) - ALLOCATE(OutData%LineTypeList(i1_l:i1_u),STAT=ErrStat2) + IF (ALLOCATED(OutData%LineTypeList)) DEALLOCATE(OutData%LineTypeList) + ALLOCATE(OutData%LineTypeList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%LineTypeList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%LineTypeList,1), UBOUND(OutData%LineTypeList,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_Unpacklineprop( Re_Buf, Db_Buf, Int_Buf, OutData%LineTypeList(i1), ErrStat2, ErrMsg2 ) ! LineTypeList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! RodTypeList not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%RodTypeList)) DEALLOCATE(OutData%RodTypeList) + ALLOCATE(OutData%RodTypeList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%RodTypeList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%RodTypeList,1), UBOUND(OutData%RodTypeList,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_Unpackrodprop( Re_Buf, Db_Buf, Int_Buf, OutData%RodTypeList(i1), ErrStat2, ErrMsg2 ) ! RodTypeList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_Unpackbody( Re_Buf, Db_Buf, Int_Buf, OutData%GroundBody, ErrStat2, ErrMsg2 ) ! GroundBody + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! BodyList not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%BodyList)) DEALLOCATE(OutData%BodyList) + ALLOCATE(OutData%BodyList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%BodyList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%BodyList,1), UBOUND(OutData%BodyList,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_Unpackbody( Re_Buf, Db_Buf, Int_Buf, OutData%BodyList(i1), ErrStat2, ErrMsg2 ) ! BodyList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! RodList not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%RodList)) DEALLOCATE(OutData%RodList) + ALLOCATE(OutData%RodList(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%LineTypeList.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%RodList.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i1 = LBOUND(OutData%LineTypeList,1), UBOUND(OutData%LineTypeList,1) + DO i1 = LBOUND(OutData%RodList,1), UBOUND(OutData%RodList,1) Buf_size=IntKiBuf( Int_Xferred ) Int_Xferred = Int_Xferred + 1 IF(Buf_size > 0) THEN @@ -4035,7 +8426,7 @@ SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) Int_Xferred = Int_Xferred + Buf_size END IF - CALL MD_Unpacklineprop( Re_Buf, Db_Buf, Int_Buf, OutData%LineTypeList(i1), ErrStat2, ErrMsg2 ) ! LineTypeList + CALL MD_Unpackrod( Re_Buf, Db_Buf, Int_Buf, OutData%RodList(i1), ErrStat2, ErrMsg2 ) ! RodList CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -4156,60 +8547,402 @@ SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) END DO END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! FairIdList not allocated + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! FailList not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%FailList)) DEALLOCATE(OutData%FailList) + ALLOCATE(OutData%FailList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%FailList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%FailList,1), UBOUND(OutData%FailList,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_Unpackfail( Re_Buf, Db_Buf, Int_Buf, OutData%FailList(i1), ErrStat2, ErrMsg2 ) ! FailList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! FreeConIs not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%FreeConIs)) DEALLOCATE(OutData%FreeConIs) + ALLOCATE(OutData%FreeConIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%FreeConIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%FreeConIs,1), UBOUND(OutData%FreeConIs,1) + OutData%FreeConIs(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! CpldConIs not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%CpldConIs)) DEALLOCATE(OutData%CpldConIs) + ALLOCATE(OutData%CpldConIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%CpldConIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%CpldConIs,1), UBOUND(OutData%CpldConIs,1) + OutData%CpldConIs(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! FreeRodIs not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%FreeRodIs)) DEALLOCATE(OutData%FreeRodIs) + ALLOCATE(OutData%FreeRodIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%FreeRodIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%FreeRodIs,1), UBOUND(OutData%FreeRodIs,1) + OutData%FreeRodIs(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! CpldRodIs not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%CpldRodIs)) DEALLOCATE(OutData%CpldRodIs) + ALLOCATE(OutData%CpldRodIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%CpldRodIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%CpldRodIs,1), UBOUND(OutData%CpldRodIs,1) + OutData%CpldRodIs(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! FreeBodyIs not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%FreeBodyIs)) DEALLOCATE(OutData%FreeBodyIs) + ALLOCATE(OutData%FreeBodyIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%FreeBodyIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%FreeBodyIs,1), UBOUND(OutData%FreeBodyIs,1) + OutData%FreeBodyIs(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! CpldBodyIs not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%CpldBodyIs)) DEALLOCATE(OutData%CpldBodyIs) + ALLOCATE(OutData%CpldBodyIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%CpldBodyIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%CpldBodyIs,1), UBOUND(OutData%CpldBodyIs,1) + OutData%CpldBodyIs(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! LineStateIs1 not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%LineStateIs1)) DEALLOCATE(OutData%LineStateIs1) + ALLOCATE(OutData%LineStateIs1(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%LineStateIs1.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%LineStateIs1,1), UBOUND(OutData%LineStateIs1,1) + OutData%LineStateIs1(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! LineStateIsN not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%LineStateIsN)) DEALLOCATE(OutData%LineStateIsN) + ALLOCATE(OutData%LineStateIsN(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%LineStateIsN.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%LineStateIsN,1), UBOUND(OutData%LineStateIsN,1) + OutData%LineStateIsN(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ConStateIs1 not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%ConStateIs1)) DEALLOCATE(OutData%ConStateIs1) + ALLOCATE(OutData%ConStateIs1(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ConStateIs1.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%ConStateIs1,1), UBOUND(OutData%ConStateIs1,1) + OutData%ConStateIs1(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ConStateIsN not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%ConStateIsN)) DEALLOCATE(OutData%ConStateIsN) + ALLOCATE(OutData%ConStateIsN(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ConStateIsN.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%ConStateIsN,1), UBOUND(OutData%ConStateIsN,1) + OutData%ConStateIsN(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! RodStateIs1 not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%RodStateIs1)) DEALLOCATE(OutData%RodStateIs1) + ALLOCATE(OutData%RodStateIs1(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%RodStateIs1.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%RodStateIs1,1), UBOUND(OutData%RodStateIs1,1) + OutData%RodStateIs1(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! RodStateIsN not allocated Int_Xferred = Int_Xferred + 1 ELSE Int_Xferred = Int_Xferred + 1 i1_l = IntKiBuf( Int_Xferred ) i1_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%FairIdList)) DEALLOCATE(OutData%FairIdList) - ALLOCATE(OutData%FairIdList(i1_l:i1_u),STAT=ErrStat2) + IF (ALLOCATED(OutData%RodStateIsN)) DEALLOCATE(OutData%RodStateIsN) + ALLOCATE(OutData%RodStateIsN(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%FairIdList.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%RodStateIsN.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i1 = LBOUND(OutData%FairIdList,1), UBOUND(OutData%FairIdList,1) - OutData%FairIdList(i1) = IntKiBuf(Int_Xferred) + DO i1 = LBOUND(OutData%RodStateIsN,1), UBOUND(OutData%RodStateIsN,1) + OutData%RodStateIsN(i1) = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 END DO END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ConnIdList not allocated + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! BodyStateIs1 not allocated Int_Xferred = Int_Xferred + 1 ELSE Int_Xferred = Int_Xferred + 1 i1_l = IntKiBuf( Int_Xferred ) i1_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%ConnIdList)) DEALLOCATE(OutData%ConnIdList) - ALLOCATE(OutData%ConnIdList(i1_l:i1_u),STAT=ErrStat2) + IF (ALLOCATED(OutData%BodyStateIs1)) DEALLOCATE(OutData%BodyStateIs1) + ALLOCATE(OutData%BodyStateIs1(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ConnIdList.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%BodyStateIs1.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i1 = LBOUND(OutData%ConnIdList,1), UBOUND(OutData%ConnIdList,1) - OutData%ConnIdList(i1) = IntKiBuf(Int_Xferred) + DO i1 = LBOUND(OutData%BodyStateIs1,1), UBOUND(OutData%BodyStateIs1,1) + OutData%BodyStateIs1(i1) = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 END DO END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! LineStateIndList not allocated + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! BodyStateIsN not allocated Int_Xferred = Int_Xferred + 1 ELSE Int_Xferred = Int_Xferred + 1 i1_l = IntKiBuf( Int_Xferred ) i1_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%LineStateIndList)) DEALLOCATE(OutData%LineStateIndList) - ALLOCATE(OutData%LineStateIndList(i1_l:i1_u),STAT=ErrStat2) + IF (ALLOCATED(OutData%BodyStateIsN)) DEALLOCATE(OutData%BodyStateIsN) + ALLOCATE(OutData%BodyStateIsN(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%LineStateIndList.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%BodyStateIsN.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i1 = LBOUND(OutData%LineStateIndList,1), UBOUND(OutData%LineStateIndList,1) - OutData%LineStateIndList(i1) = IntKiBuf(Int_Xferred) + DO i1 = LBOUND(OutData%BodyStateIsN,1), UBOUND(OutData%BodyStateIsN,1) + OutData%BodyStateIsN(i1) = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 END DO END IF + OutData%Nx = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_UnpackContState( Re_Buf, Db_Buf, Int_Buf, OutData%xTemp, ErrStat2, ErrMsg2 ) ! xTemp + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_UnpackContState( Re_Buf, Db_Buf, Int_Buf, OutData%xdTemp, ErrStat2, ErrMsg2 ) ! xdTemp + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + i1_l = LBOUND(OutData%zeros6,1) + i1_u = UBOUND(OutData%zeros6,1) + DO i1 = LBOUND(OutData%zeros6,1), UBOUND(OutData%zeros6,1) + OutData%zeros6(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! MDWrOutput not allocated Int_Xferred = Int_Xferred + 1 ELSE @@ -4224,8 +8957,8 @@ SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) RETURN END IF DO i1 = LBOUND(OutData%MDWrOutput,1), UBOUND(OutData%MDWrOutput,1) - OutData%MDWrOutput(i1) = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 + OutData%MDWrOutput(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 END DO END IF END SUBROUTINE MD_UnPackMisc @@ -4239,18 +8972,31 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) ! Local INTEGER(IntKi) :: i,j,k INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: i4, i4_l, i4_u ! bounds (upper/lower) for an array dimension 4 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyParam' ! ErrStat = ErrID_None ErrMsg = "" - DstParamData%NTypes = SrcParamData%NTypes - DstParamData%NConnects = SrcParamData%NConnects - DstParamData%NFairs = SrcParamData%NFairs + DstParamData%nLineTypes = SrcParamData%nLineTypes + DstParamData%nRodTypes = SrcParamData%nRodTypes + DstParamData%nConnects = SrcParamData%nConnects + DstParamData%nConnectsExtra = SrcParamData%nConnectsExtra + DstParamData%nBodies = SrcParamData%nBodies + DstParamData%nRods = SrcParamData%nRods + DstParamData%nLines = SrcParamData%nLines + DstParamData%nFails = SrcParamData%nFails + DstParamData%nFreeBodies = SrcParamData%nFreeBodies + DstParamData%nFreeRods = SrcParamData%nFreeRods + DstParamData%nFreeCons = SrcParamData%nFreeCons + DstParamData%nCpldBodies = SrcParamData%nCpldBodies + DstParamData%nCpldRods = SrcParamData%nCpldRods + DstParamData%nCpldCons = SrcParamData%nCpldCons DstParamData%NConns = SrcParamData%NConns DstParamData%NAnchs = SrcParamData%NAnchs - DstParamData%NLines = SrcParamData%NLines DstParamData%g = SrcParamData%g DstParamData%rhoW = SrcParamData%rhoW DstParamData%WtrDpth = SrcParamData%WtrDpth @@ -4278,6 +9024,186 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) ENDIF DstParamData%Delim = SrcParamData%Delim DstParamData%MDUnOut = SrcParamData%MDUnOut + DstParamData%WaterKin = SrcParamData%WaterKin +IF (ALLOCATED(SrcParamData%ux)) THEN + i1_l = LBOUND(SrcParamData%ux,1) + i1_u = UBOUND(SrcParamData%ux,1) + i2_l = LBOUND(SrcParamData%ux,2) + i2_u = UBOUND(SrcParamData%ux,2) + i3_l = LBOUND(SrcParamData%ux,3) + i3_u = UBOUND(SrcParamData%ux,3) + i4_l = LBOUND(SrcParamData%ux,4) + i4_u = UBOUND(SrcParamData%ux,4) + IF (.NOT. ALLOCATED(DstParamData%ux)) THEN + ALLOCATE(DstParamData%ux(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%ux.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%ux = SrcParamData%ux +ENDIF +IF (ALLOCATED(SrcParamData%uy)) THEN + i1_l = LBOUND(SrcParamData%uy,1) + i1_u = UBOUND(SrcParamData%uy,1) + i2_l = LBOUND(SrcParamData%uy,2) + i2_u = UBOUND(SrcParamData%uy,2) + i3_l = LBOUND(SrcParamData%uy,3) + i3_u = UBOUND(SrcParamData%uy,3) + i4_l = LBOUND(SrcParamData%uy,4) + i4_u = UBOUND(SrcParamData%uy,4) + IF (.NOT. ALLOCATED(DstParamData%uy)) THEN + ALLOCATE(DstParamData%uy(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%uy.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%uy = SrcParamData%uy +ENDIF +IF (ALLOCATED(SrcParamData%uz)) THEN + i1_l = LBOUND(SrcParamData%uz,1) + i1_u = UBOUND(SrcParamData%uz,1) + i2_l = LBOUND(SrcParamData%uz,2) + i2_u = UBOUND(SrcParamData%uz,2) + i3_l = LBOUND(SrcParamData%uz,3) + i3_u = UBOUND(SrcParamData%uz,3) + i4_l = LBOUND(SrcParamData%uz,4) + i4_u = UBOUND(SrcParamData%uz,4) + IF (.NOT. ALLOCATED(DstParamData%uz)) THEN + ALLOCATE(DstParamData%uz(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%uz.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%uz = SrcParamData%uz +ENDIF +IF (ALLOCATED(SrcParamData%ax)) THEN + i1_l = LBOUND(SrcParamData%ax,1) + i1_u = UBOUND(SrcParamData%ax,1) + i2_l = LBOUND(SrcParamData%ax,2) + i2_u = UBOUND(SrcParamData%ax,2) + i3_l = LBOUND(SrcParamData%ax,3) + i3_u = UBOUND(SrcParamData%ax,3) + i4_l = LBOUND(SrcParamData%ax,4) + i4_u = UBOUND(SrcParamData%ax,4) + IF (.NOT. ALLOCATED(DstParamData%ax)) THEN + ALLOCATE(DstParamData%ax(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%ax.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%ax = SrcParamData%ax +ENDIF +IF (ALLOCATED(SrcParamData%ay)) THEN + i1_l = LBOUND(SrcParamData%ay,1) + i1_u = UBOUND(SrcParamData%ay,1) + i2_l = LBOUND(SrcParamData%ay,2) + i2_u = UBOUND(SrcParamData%ay,2) + i3_l = LBOUND(SrcParamData%ay,3) + i3_u = UBOUND(SrcParamData%ay,3) + i4_l = LBOUND(SrcParamData%ay,4) + i4_u = UBOUND(SrcParamData%ay,4) + IF (.NOT. ALLOCATED(DstParamData%ay)) THEN + ALLOCATE(DstParamData%ay(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%ay.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%ay = SrcParamData%ay +ENDIF +IF (ALLOCATED(SrcParamData%az)) THEN + i1_l = LBOUND(SrcParamData%az,1) + i1_u = UBOUND(SrcParamData%az,1) + i2_l = LBOUND(SrcParamData%az,2) + i2_u = UBOUND(SrcParamData%az,2) + i3_l = LBOUND(SrcParamData%az,3) + i3_u = UBOUND(SrcParamData%az,3) + i4_l = LBOUND(SrcParamData%az,4) + i4_u = UBOUND(SrcParamData%az,4) + IF (.NOT. ALLOCATED(DstParamData%az)) THEN + ALLOCATE(DstParamData%az(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%az.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%az = SrcParamData%az +ENDIF +IF (ALLOCATED(SrcParamData%PDyn)) THEN + i1_l = LBOUND(SrcParamData%PDyn,1) + i1_u = UBOUND(SrcParamData%PDyn,1) + i2_l = LBOUND(SrcParamData%PDyn,2) + i2_u = UBOUND(SrcParamData%PDyn,2) + i3_l = LBOUND(SrcParamData%PDyn,3) + i3_u = UBOUND(SrcParamData%PDyn,3) + i4_l = LBOUND(SrcParamData%PDyn,4) + i4_u = UBOUND(SrcParamData%PDyn,4) + IF (.NOT. ALLOCATED(DstParamData%PDyn)) THEN + ALLOCATE(DstParamData%PDyn(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%PDyn.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%PDyn = SrcParamData%PDyn +ENDIF +IF (ALLOCATED(SrcParamData%zeta)) THEN + i1_l = LBOUND(SrcParamData%zeta,1) + i1_u = UBOUND(SrcParamData%zeta,1) + i2_l = LBOUND(SrcParamData%zeta,2) + i2_u = UBOUND(SrcParamData%zeta,2) + i3_l = LBOUND(SrcParamData%zeta,3) + i3_u = UBOUND(SrcParamData%zeta,3) + IF (.NOT. ALLOCATED(DstParamData%zeta)) THEN + ALLOCATE(DstParamData%zeta(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%zeta.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%zeta = SrcParamData%zeta +ENDIF +IF (ALLOCATED(SrcParamData%px)) THEN + i1_l = LBOUND(SrcParamData%px,1) + i1_u = UBOUND(SrcParamData%px,1) + IF (.NOT. ALLOCATED(DstParamData%px)) THEN + ALLOCATE(DstParamData%px(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%px.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%px = SrcParamData%px +ENDIF +IF (ALLOCATED(SrcParamData%py)) THEN + i1_l = LBOUND(SrcParamData%py,1) + i1_u = UBOUND(SrcParamData%py,1) + IF (.NOT. ALLOCATED(DstParamData%py)) THEN + ALLOCATE(DstParamData%py(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%py.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%py = SrcParamData%py +ENDIF +IF (ALLOCATED(SrcParamData%pz)) THEN + i1_l = LBOUND(SrcParamData%pz,1) + i1_u = UBOUND(SrcParamData%pz,1) + IF (.NOT. ALLOCATED(DstParamData%pz)) THEN + ALLOCATE(DstParamData%pz(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%pz.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%pz = SrcParamData%pz +ENDIF + DstParamData%dtWave = SrcParamData%dtWave END SUBROUTINE MD_CopyParam SUBROUTINE MD_DestroyParam( ParamData, ErrStat, ErrMsg ) @@ -4294,6 +9220,39 @@ SUBROUTINE MD_DestroyParam( ParamData, ErrStat, ErrMsg ) CALL MD_Destroyoutparmtype( ParamData%OutParam(i1), ErrStat, ErrMsg ) ENDDO DEALLOCATE(ParamData%OutParam) +ENDIF +IF (ALLOCATED(ParamData%ux)) THEN + DEALLOCATE(ParamData%ux) +ENDIF +IF (ALLOCATED(ParamData%uy)) THEN + DEALLOCATE(ParamData%uy) +ENDIF +IF (ALLOCATED(ParamData%uz)) THEN + DEALLOCATE(ParamData%uz) +ENDIF +IF (ALLOCATED(ParamData%ax)) THEN + DEALLOCATE(ParamData%ax) +ENDIF +IF (ALLOCATED(ParamData%ay)) THEN + DEALLOCATE(ParamData%ay) +ENDIF +IF (ALLOCATED(ParamData%az)) THEN + DEALLOCATE(ParamData%az) +ENDIF +IF (ALLOCATED(ParamData%PDyn)) THEN + DEALLOCATE(ParamData%PDyn) +ENDIF +IF (ALLOCATED(ParamData%zeta)) THEN + DEALLOCATE(ParamData%zeta) +ENDIF +IF (ALLOCATED(ParamData%px)) THEN + DEALLOCATE(ParamData%px) +ENDIF +IF (ALLOCATED(ParamData%py)) THEN + DEALLOCATE(ParamData%py) +ENDIF +IF (ALLOCATED(ParamData%pz)) THEN + DEALLOCATE(ParamData%pz) ENDIF END SUBROUTINE MD_DestroyParam @@ -4332,19 +9291,29 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Re_BufSz = 0 Db_BufSz = 0 Int_BufSz = 0 - Int_BufSz = Int_BufSz + 1 ! NTypes - Int_BufSz = Int_BufSz + 1 ! NConnects - Int_BufSz = Int_BufSz + 1 ! NFairs + Int_BufSz = Int_BufSz + 1 ! nLineTypes + Int_BufSz = Int_BufSz + 1 ! nRodTypes + Int_BufSz = Int_BufSz + 1 ! nConnects + Int_BufSz = Int_BufSz + 1 ! nConnectsExtra + Int_BufSz = Int_BufSz + 1 ! nBodies + Int_BufSz = Int_BufSz + 1 ! nRods + Int_BufSz = Int_BufSz + 1 ! nLines + Int_BufSz = Int_BufSz + 1 ! nFails + Int_BufSz = Int_BufSz + 1 ! nFreeBodies + Int_BufSz = Int_BufSz + 1 ! nFreeRods + Int_BufSz = Int_BufSz + 1 ! nFreeCons + Int_BufSz = Int_BufSz + 1 ! nCpldBodies + Int_BufSz = Int_BufSz + 1 ! nCpldRods + Int_BufSz = Int_BufSz + 1 ! nCpldCons Int_BufSz = Int_BufSz + 1 ! NConns Int_BufSz = Int_BufSz + 1 ! NAnchs - Int_BufSz = Int_BufSz + 1 ! NLines - Re_BufSz = Re_BufSz + 1 ! g - Re_BufSz = Re_BufSz + 1 ! rhoW - Re_BufSz = Re_BufSz + 1 ! WtrDpth - Re_BufSz = Re_BufSz + 1 ! kBot - Re_BufSz = Re_BufSz + 1 ! cBot - Re_BufSz = Re_BufSz + 1 ! dtM0 - Re_BufSz = Re_BufSz + 1 ! dtCoupling + Db_BufSz = Db_BufSz + 1 ! g + Db_BufSz = Db_BufSz + 1 ! rhoW + Db_BufSz = Db_BufSz + 1 ! WtrDpth + Db_BufSz = Db_BufSz + 1 ! kBot + Db_BufSz = Db_BufSz + 1 ! cBot + Db_BufSz = Db_BufSz + 1 ! dtM0 + Db_BufSz = Db_BufSz + 1 ! dtCoupling Int_BufSz = Int_BufSz + 1 ! NumOuts Int_BufSz = Int_BufSz + 1*LEN(InData%RootName) ! RootName Int_BufSz = Int_BufSz + 1 ! OutParam allocated yes/no @@ -4373,6 +9342,63 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si END IF Int_BufSz = Int_BufSz + 1*LEN(InData%Delim) ! Delim Int_BufSz = Int_BufSz + 1 ! MDUnOut + Int_BufSz = Int_BufSz + 1 ! WaterKin + Int_BufSz = Int_BufSz + 1 ! ux allocated yes/no + IF ( ALLOCATED(InData%ux) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! ux upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%ux) ! ux + END IF + Int_BufSz = Int_BufSz + 1 ! uy allocated yes/no + IF ( ALLOCATED(InData%uy) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! uy upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%uy) ! uy + END IF + Int_BufSz = Int_BufSz + 1 ! uz allocated yes/no + IF ( ALLOCATED(InData%uz) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! uz upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%uz) ! uz + END IF + Int_BufSz = Int_BufSz + 1 ! ax allocated yes/no + IF ( ALLOCATED(InData%ax) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! ax upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%ax) ! ax + END IF + Int_BufSz = Int_BufSz + 1 ! ay allocated yes/no + IF ( ALLOCATED(InData%ay) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! ay upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%ay) ! ay + END IF + Int_BufSz = Int_BufSz + 1 ! az allocated yes/no + IF ( ALLOCATED(InData%az) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! az upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%az) ! az + END IF + Int_BufSz = Int_BufSz + 1 ! PDyn allocated yes/no + IF ( ALLOCATED(InData%PDyn) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! PDyn upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%PDyn) ! PDyn + END IF + Int_BufSz = Int_BufSz + 1 ! zeta allocated yes/no + IF ( ALLOCATED(InData%zeta) ) THEN + Int_BufSz = Int_BufSz + 2*3 ! zeta upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%zeta) ! zeta + END IF + Int_BufSz = Int_BufSz + 1 ! px allocated yes/no + IF ( ALLOCATED(InData%px) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! px upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%px) ! px + END IF + Int_BufSz = Int_BufSz + 1 ! py allocated yes/no + IF ( ALLOCATED(InData%py) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! py upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%py) ! py + END IF + Int_BufSz = Int_BufSz + 1 ! pz allocated yes/no + IF ( ALLOCATED(InData%pz) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! pz upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%pz) ! pz + END IF + Db_BufSz = Db_BufSz + 1 ! dtWave IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) IF (ErrStat2 /= 0) THEN @@ -4400,32 +9426,52 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Db_Xferred = 1 Int_Xferred = 1 - IntKiBuf(Int_Xferred) = InData%NTypes + IntKiBuf(Int_Xferred) = InData%nLineTypes + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nRodTypes + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nConnects + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nConnectsExtra + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nBodies + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nRods + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nLines + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nFails + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nFreeBodies + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nFreeRods + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nFreeCons Int_Xferred = Int_Xferred + 1 - IntKiBuf(Int_Xferred) = InData%NConnects + IntKiBuf(Int_Xferred) = InData%nCpldBodies Int_Xferred = Int_Xferred + 1 - IntKiBuf(Int_Xferred) = InData%NFairs + IntKiBuf(Int_Xferred) = InData%nCpldRods + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nCpldCons Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%NConns Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%NAnchs Int_Xferred = Int_Xferred + 1 - IntKiBuf(Int_Xferred) = InData%NLines - Int_Xferred = Int_Xferred + 1 - ReKiBuf(Re_Xferred) = InData%g - Re_Xferred = Re_Xferred + 1 - ReKiBuf(Re_Xferred) = InData%rhoW - Re_Xferred = Re_Xferred + 1 - ReKiBuf(Re_Xferred) = InData%WtrDpth - Re_Xferred = Re_Xferred + 1 - ReKiBuf(Re_Xferred) = InData%kBot - Re_Xferred = Re_Xferred + 1 - ReKiBuf(Re_Xferred) = InData%cBot - Re_Xferred = Re_Xferred + 1 - ReKiBuf(Re_Xferred) = InData%dtM0 - Re_Xferred = Re_Xferred + 1 - ReKiBuf(Re_Xferred) = InData%dtCoupling - Re_Xferred = Re_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%g + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%rhoW + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%WtrDpth + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%kBot + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%cBot + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%dtM0 + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%dtCoupling + Db_Xferred = Db_Xferred + 1 IntKiBuf(Int_Xferred) = InData%NumOuts Int_Xferred = Int_Xferred + 1 DO I = 1, LEN(InData%RootName) @@ -4473,12 +9519,296 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si ENDIF END DO END IF - DO I = 1, LEN(InData%Delim) - IntKiBuf(Int_Xferred) = ICHAR(InData%Delim(I:I), IntKi) - Int_Xferred = Int_Xferred + 1 - END DO ! I - IntKiBuf(Int_Xferred) = InData%MDUnOut + DO I = 1, LEN(InData%Delim) + IntKiBuf(Int_Xferred) = ICHAR(InData%Delim(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + IntKiBuf(Int_Xferred) = InData%MDUnOut + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%WaterKin + Int_Xferred = Int_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%ux) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ux,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ux,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ux,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ux,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ux,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ux,3) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ux,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ux,4) + Int_Xferred = Int_Xferred + 2 + + DO i4 = LBOUND(InData%ux,4), UBOUND(InData%ux,4) + DO i3 = LBOUND(InData%ux,3), UBOUND(InData%ux,3) + DO i2 = LBOUND(InData%ux,2), UBOUND(InData%ux,2) + DO i1 = LBOUND(InData%ux,1), UBOUND(InData%ux,1) + DbKiBuf(Db_Xferred) = InData%ux(i1,i2,i3,i4) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%uy) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uy,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uy,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uy,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uy,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uy,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uy,3) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uy,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uy,4) + Int_Xferred = Int_Xferred + 2 + + DO i4 = LBOUND(InData%uy,4), UBOUND(InData%uy,4) + DO i3 = LBOUND(InData%uy,3), UBOUND(InData%uy,3) + DO i2 = LBOUND(InData%uy,2), UBOUND(InData%uy,2) + DO i1 = LBOUND(InData%uy,1), UBOUND(InData%uy,1) + DbKiBuf(Db_Xferred) = InData%uy(i1,i2,i3,i4) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%uz) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uz,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uz,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uz,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uz,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uz,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uz,3) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uz,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uz,4) + Int_Xferred = Int_Xferred + 2 + + DO i4 = LBOUND(InData%uz,4), UBOUND(InData%uz,4) + DO i3 = LBOUND(InData%uz,3), UBOUND(InData%uz,3) + DO i2 = LBOUND(InData%uz,2), UBOUND(InData%uz,2) + DO i1 = LBOUND(InData%uz,1), UBOUND(InData%uz,1) + DbKiBuf(Db_Xferred) = InData%uz(i1,i2,i3,i4) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%ax) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ax,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ax,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ax,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ax,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ax,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ax,3) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ax,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ax,4) + Int_Xferred = Int_Xferred + 2 + + DO i4 = LBOUND(InData%ax,4), UBOUND(InData%ax,4) + DO i3 = LBOUND(InData%ax,3), UBOUND(InData%ax,3) + DO i2 = LBOUND(InData%ax,2), UBOUND(InData%ax,2) + DO i1 = LBOUND(InData%ax,1), UBOUND(InData%ax,1) + DbKiBuf(Db_Xferred) = InData%ax(i1,i2,i3,i4) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%ay) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ay,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ay,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ay,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ay,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ay,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ay,3) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ay,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ay,4) + Int_Xferred = Int_Xferred + 2 + + DO i4 = LBOUND(InData%ay,4), UBOUND(InData%ay,4) + DO i3 = LBOUND(InData%ay,3), UBOUND(InData%ay,3) + DO i2 = LBOUND(InData%ay,2), UBOUND(InData%ay,2) + DO i1 = LBOUND(InData%ay,1), UBOUND(InData%ay,1) + DbKiBuf(Db_Xferred) = InData%ay(i1,i2,i3,i4) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%az) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%az,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%az,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%az,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%az,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%az,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%az,3) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%az,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%az,4) + Int_Xferred = Int_Xferred + 2 + + DO i4 = LBOUND(InData%az,4), UBOUND(InData%az,4) + DO i3 = LBOUND(InData%az,3), UBOUND(InData%az,3) + DO i2 = LBOUND(InData%az,2), UBOUND(InData%az,2) + DO i1 = LBOUND(InData%az,1), UBOUND(InData%az,1) + DbKiBuf(Db_Xferred) = InData%az(i1,i2,i3,i4) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%PDyn) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%PDyn,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PDyn,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%PDyn,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PDyn,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%PDyn,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PDyn,3) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%PDyn,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PDyn,4) + Int_Xferred = Int_Xferred + 2 + + DO i4 = LBOUND(InData%PDyn,4), UBOUND(InData%PDyn,4) + DO i3 = LBOUND(InData%PDyn,3), UBOUND(InData%PDyn,3) + DO i2 = LBOUND(InData%PDyn,2), UBOUND(InData%PDyn,2) + DO i1 = LBOUND(InData%PDyn,1), UBOUND(InData%PDyn,1) + DbKiBuf(Db_Xferred) = InData%PDyn(i1,i2,i3,i4) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%zeta) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%zeta,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%zeta,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%zeta,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%zeta,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%zeta,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%zeta,3) + Int_Xferred = Int_Xferred + 2 + + DO i3 = LBOUND(InData%zeta,3), UBOUND(InData%zeta,3) + DO i2 = LBOUND(InData%zeta,2), UBOUND(InData%zeta,2) + DO i1 = LBOUND(InData%zeta,1), UBOUND(InData%zeta,1) + DbKiBuf(Db_Xferred) = InData%zeta(i1,i2,i3) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%px) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%px,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%px,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%px,1), UBOUND(InData%px,1) + DbKiBuf(Db_Xferred) = InData%px(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%py) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%py,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%py,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%py,1), UBOUND(InData%py,1) + DbKiBuf(Db_Xferred) = InData%py(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%pz) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%pz,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%pz,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%pz,1), UBOUND(InData%pz,1) + DbKiBuf(Db_Xferred) = InData%pz(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + DbKiBuf(Db_Xferred) = InData%dtWave + Db_Xferred = Db_Xferred + 1 END SUBROUTINE MD_PackParam SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) @@ -4495,6 +9825,9 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg INTEGER(IntKi) :: Int_Xferred INTEGER(IntKi) :: i INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: i4, i4_l, i4_u ! bounds (upper/lower) for an array dimension 4 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackParam' @@ -4508,32 +9841,52 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg Re_Xferred = 1 Db_Xferred = 1 Int_Xferred = 1 - OutData%NTypes = IntKiBuf(Int_Xferred) + OutData%nLineTypes = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nRodTypes = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nConnects = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 - OutData%NConnects = IntKiBuf(Int_Xferred) + OutData%nConnectsExtra = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 - OutData%NFairs = IntKiBuf(Int_Xferred) + OutData%nBodies = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nRods = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nLines = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nFails = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nFreeBodies = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nFreeRods = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nFreeCons = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nCpldBodies = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nCpldRods = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nCpldCons = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 OutData%NConns = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 OutData%NAnchs = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 - OutData%NLines = IntKiBuf(Int_Xferred) - Int_Xferred = Int_Xferred + 1 - OutData%g = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 - OutData%rhoW = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 - OutData%WtrDpth = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 - OutData%kBot = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 - OutData%cBot = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 - OutData%dtM0 = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 - OutData%dtCoupling = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 + OutData%g = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%rhoW = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%WtrDpth = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%kBot = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%cBot = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%dtM0 = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%dtCoupling = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 OutData%NumOuts = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 DO I = 1, LEN(OutData%RootName) @@ -4602,6 +9955,323 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg END DO ! I OutData%MDUnOut = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 + OutData%WaterKin = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ux not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i4_l = IntKiBuf( Int_Xferred ) + i4_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%ux)) DEALLOCATE(OutData%ux) + ALLOCATE(OutData%ux(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ux.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i4 = LBOUND(OutData%ux,4), UBOUND(OutData%ux,4) + DO i3 = LBOUND(OutData%ux,3), UBOUND(OutData%ux,3) + DO i2 = LBOUND(OutData%ux,2), UBOUND(OutData%ux,2) + DO i1 = LBOUND(OutData%ux,1), UBOUND(OutData%ux,1) + OutData%ux(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! uy not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i4_l = IntKiBuf( Int_Xferred ) + i4_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%uy)) DEALLOCATE(OutData%uy) + ALLOCATE(OutData%uy(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%uy.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i4 = LBOUND(OutData%uy,4), UBOUND(OutData%uy,4) + DO i3 = LBOUND(OutData%uy,3), UBOUND(OutData%uy,3) + DO i2 = LBOUND(OutData%uy,2), UBOUND(OutData%uy,2) + DO i1 = LBOUND(OutData%uy,1), UBOUND(OutData%uy,1) + OutData%uy(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! uz not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i4_l = IntKiBuf( Int_Xferred ) + i4_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%uz)) DEALLOCATE(OutData%uz) + ALLOCATE(OutData%uz(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%uz.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i4 = LBOUND(OutData%uz,4), UBOUND(OutData%uz,4) + DO i3 = LBOUND(OutData%uz,3), UBOUND(OutData%uz,3) + DO i2 = LBOUND(OutData%uz,2), UBOUND(OutData%uz,2) + DO i1 = LBOUND(OutData%uz,1), UBOUND(OutData%uz,1) + OutData%uz(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ax not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i4_l = IntKiBuf( Int_Xferred ) + i4_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%ax)) DEALLOCATE(OutData%ax) + ALLOCATE(OutData%ax(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ax.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i4 = LBOUND(OutData%ax,4), UBOUND(OutData%ax,4) + DO i3 = LBOUND(OutData%ax,3), UBOUND(OutData%ax,3) + DO i2 = LBOUND(OutData%ax,2), UBOUND(OutData%ax,2) + DO i1 = LBOUND(OutData%ax,1), UBOUND(OutData%ax,1) + OutData%ax(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ay not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i4_l = IntKiBuf( Int_Xferred ) + i4_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%ay)) DEALLOCATE(OutData%ay) + ALLOCATE(OutData%ay(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ay.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i4 = LBOUND(OutData%ay,4), UBOUND(OutData%ay,4) + DO i3 = LBOUND(OutData%ay,3), UBOUND(OutData%ay,3) + DO i2 = LBOUND(OutData%ay,2), UBOUND(OutData%ay,2) + DO i1 = LBOUND(OutData%ay,1), UBOUND(OutData%ay,1) + OutData%ay(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! az not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i4_l = IntKiBuf( Int_Xferred ) + i4_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%az)) DEALLOCATE(OutData%az) + ALLOCATE(OutData%az(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%az.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i4 = LBOUND(OutData%az,4), UBOUND(OutData%az,4) + DO i3 = LBOUND(OutData%az,3), UBOUND(OutData%az,3) + DO i2 = LBOUND(OutData%az,2), UBOUND(OutData%az,2) + DO i1 = LBOUND(OutData%az,1), UBOUND(OutData%az,1) + OutData%az(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! PDyn not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i4_l = IntKiBuf( Int_Xferred ) + i4_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%PDyn)) DEALLOCATE(OutData%PDyn) + ALLOCATE(OutData%PDyn(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%PDyn.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i4 = LBOUND(OutData%PDyn,4), UBOUND(OutData%PDyn,4) + DO i3 = LBOUND(OutData%PDyn,3), UBOUND(OutData%PDyn,3) + DO i2 = LBOUND(OutData%PDyn,2), UBOUND(OutData%PDyn,2) + DO i1 = LBOUND(OutData%PDyn,1), UBOUND(OutData%PDyn,1) + OutData%PDyn(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! zeta not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%zeta)) DEALLOCATE(OutData%zeta) + ALLOCATE(OutData%zeta(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%zeta.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i3 = LBOUND(OutData%zeta,3), UBOUND(OutData%zeta,3) + DO i2 = LBOUND(OutData%zeta,2), UBOUND(OutData%zeta,2) + DO i1 = LBOUND(OutData%zeta,1), UBOUND(OutData%zeta,1) + OutData%zeta(i1,i2,i3) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! px not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%px)) DEALLOCATE(OutData%px) + ALLOCATE(OutData%px(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%px.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%px,1), UBOUND(OutData%px,1) + OutData%px(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! py not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%py)) DEALLOCATE(OutData%py) + ALLOCATE(OutData%py(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%py.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%py,1), UBOUND(OutData%py,1) + OutData%py(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! pz not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%pz)) DEALLOCATE(OutData%pz) + ALLOCATE(OutData%pz(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%pz.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%pz,1), UBOUND(OutData%pz,1) + OutData%pz(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + OutData%dtWave = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 END SUBROUTINE MD_UnPackParam SUBROUTINE MD_CopyInput( SrcInputData, DstInputData, CtrlCode, ErrStat, ErrMsg ) @@ -4619,7 +10289,7 @@ SUBROUTINE MD_CopyInput( SrcInputData, DstInputData, CtrlCode, ErrStat, ErrMsg ) ! ErrStat = ErrID_None ErrMsg = "" - CALL MeshCopy( SrcInputData%PtFairleadDisplacement, DstInputData%PtFairleadDisplacement, CtrlCode, ErrStat2, ErrMsg2 ) + CALL MeshCopy( SrcInputData%CoupledKinematics, DstInputData%CoupledKinematics, CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat>=AbortErrLev) RETURN IF (ALLOCATED(SrcInputData%DeltaL)) THEN @@ -4657,7 +10327,7 @@ SUBROUTINE MD_DestroyInput( InputData, ErrStat, ErrMsg ) ! ErrStat = ErrID_None ErrMsg = "" - CALL MeshDestroy( InputData%PtFairleadDisplacement, ErrStat, ErrMsg ) + CALL MeshDestroy( InputData%CoupledKinematics, ErrStat, ErrMsg ) IF (ALLOCATED(InputData%DeltaL)) THEN DEALLOCATE(InputData%DeltaL) ENDIF @@ -4702,20 +10372,20 @@ SUBROUTINE MD_PackInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Db_BufSz = 0 Int_BufSz = 0 ! Allocate buffers for subtypes, if any (we'll get sizes from these) - Int_BufSz = Int_BufSz + 3 ! PtFairleadDisplacement: size of buffers for each call to pack subtype - CALL MeshPack( InData%PtFairleadDisplacement, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, .TRUE. ) ! PtFairleadDisplacement + Int_BufSz = Int_BufSz + 3 ! CoupledKinematics: size of buffers for each call to pack subtype + CALL MeshPack( InData%CoupledKinematics, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, .TRUE. ) ! CoupledKinematics CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN - IF(ALLOCATED(Re_Buf)) THEN ! PtFairleadDisplacement + IF(ALLOCATED(Re_Buf)) THEN ! CoupledKinematics Re_BufSz = Re_BufSz + SIZE( Re_Buf ) DEALLOCATE(Re_Buf) END IF - IF(ALLOCATED(Db_Buf)) THEN ! PtFairleadDisplacement + IF(ALLOCATED(Db_Buf)) THEN ! CoupledKinematics Db_BufSz = Db_BufSz + SIZE( Db_Buf ) DEALLOCATE(Db_Buf) END IF - IF(ALLOCATED(Int_Buf)) THEN ! PtFairleadDisplacement + IF(ALLOCATED(Int_Buf)) THEN ! CoupledKinematics Int_BufSz = Int_BufSz + SIZE( Int_Buf ) DEALLOCATE(Int_Buf) END IF @@ -4756,7 +10426,7 @@ SUBROUTINE MD_PackInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Db_Xferred = 1 Int_Xferred = 1 - CALL MeshPack( InData%PtFairleadDisplacement, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, OnlySize ) ! PtFairleadDisplacement + CALL MeshPack( InData%CoupledKinematics, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, OnlySize ) ! CoupledKinematics CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -4876,7 +10546,7 @@ SUBROUTINE MD_UnPackInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) Int_Xferred = Int_Xferred + Buf_size END IF - CALL MeshUnpack( OutData%PtFairleadDisplacement, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2 ) ! PtFairleadDisplacement + CALL MeshUnpack( OutData%CoupledKinematics, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2 ) ! CoupledKinematics CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -4936,7 +10606,7 @@ SUBROUTINE MD_CopyOutput( SrcOutputData, DstOutputData, CtrlCode, ErrStat, ErrMs ! ErrStat = ErrID_None ErrMsg = "" - CALL MeshCopy( SrcOutputData%PtFairleadLoad, DstOutputData%PtFairleadLoad, CtrlCode, ErrStat2, ErrMsg2 ) + CALL MeshCopy( SrcOutputData%CoupledLoads, DstOutputData%CoupledLoads, CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat>=AbortErrLev) RETURN IF (ALLOCATED(SrcOutputData%WriteOutput)) THEN @@ -4962,7 +10632,7 @@ SUBROUTINE MD_DestroyOutput( OutputData, ErrStat, ErrMsg ) ! ErrStat = ErrID_None ErrMsg = "" - CALL MeshDestroy( OutputData%PtFairleadLoad, ErrStat, ErrMsg ) + CALL MeshDestroy( OutputData%CoupledLoads, ErrStat, ErrMsg ) IF (ALLOCATED(OutputData%WriteOutput)) THEN DEALLOCATE(OutputData%WriteOutput) ENDIF @@ -5004,20 +10674,20 @@ SUBROUTINE MD_PackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, S Db_BufSz = 0 Int_BufSz = 0 ! Allocate buffers for subtypes, if any (we'll get sizes from these) - Int_BufSz = Int_BufSz + 3 ! PtFairleadLoad: size of buffers for each call to pack subtype - CALL MeshPack( InData%PtFairleadLoad, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, .TRUE. ) ! PtFairleadLoad + Int_BufSz = Int_BufSz + 3 ! CoupledLoads: size of buffers for each call to pack subtype + CALL MeshPack( InData%CoupledLoads, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, .TRUE. ) ! CoupledLoads CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN - IF(ALLOCATED(Re_Buf)) THEN ! PtFairleadLoad + IF(ALLOCATED(Re_Buf)) THEN ! CoupledLoads Re_BufSz = Re_BufSz + SIZE( Re_Buf ) DEALLOCATE(Re_Buf) END IF - IF(ALLOCATED(Db_Buf)) THEN ! PtFairleadLoad + IF(ALLOCATED(Db_Buf)) THEN ! CoupledLoads Db_BufSz = Db_BufSz + SIZE( Db_Buf ) DEALLOCATE(Db_Buf) END IF - IF(ALLOCATED(Int_Buf)) THEN ! PtFairleadLoad + IF(ALLOCATED(Int_Buf)) THEN ! CoupledLoads Int_BufSz = Int_BufSz + SIZE( Int_Buf ) DEALLOCATE(Int_Buf) END IF @@ -5053,7 +10723,7 @@ SUBROUTINE MD_PackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, S Db_Xferred = 1 Int_Xferred = 1 - CALL MeshPack( InData%PtFairleadLoad, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, OnlySize ) ! PtFairleadLoad + CALL MeshPack( InData%CoupledLoads, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, OnlySize ) ! CoupledLoads CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -5158,7 +10828,7 @@ SUBROUTINE MD_UnPackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) Int_Xferred = Int_Xferred + Buf_size END IF - CALL MeshUnpack( OutData%PtFairleadLoad, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2 ) ! PtFairleadLoad + CALL MeshUnpack( OutData%CoupledLoads, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2 ) ! CoupledLoads CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -5280,7 +10950,7 @@ SUBROUTINE MD_Input_ExtrapInterp1(u1, u2, tin, u_out, tin_out, ErrStat, ErrMsg ) END IF ScaleFactor = t_out / t(2) - CALL MeshExtrapInterp1(u1%PtFairleadDisplacement, u2%PtFairleadDisplacement, tin, u_out%PtFairleadDisplacement, tin_out, ErrStat2, ErrMsg2 ) + CALL MeshExtrapInterp1(u1%CoupledKinematics, u2%CoupledKinematics, tin, u_out%CoupledKinematics, tin_out, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) IF (ALLOCATED(u_out%DeltaL) .AND. ALLOCATED(u1%DeltaL)) THEN DO i1 = LBOUND(u_out%DeltaL,1),UBOUND(u_out%DeltaL,1) @@ -5351,7 +11021,7 @@ SUBROUTINE MD_Input_ExtrapInterp2(u1, u2, u3, tin, u_out, tin_out, ErrStat, ErrM END IF ScaleFactor = t_out / (t(2) * t(3) * (t(2) - t(3))) - CALL MeshExtrapInterp2(u1%PtFairleadDisplacement, u2%PtFairleadDisplacement, u3%PtFairleadDisplacement, tin, u_out%PtFairleadDisplacement, tin_out, ErrStat2, ErrMsg2 ) + CALL MeshExtrapInterp2(u1%CoupledKinematics, u2%CoupledKinematics, u3%CoupledKinematics, tin, u_out%CoupledKinematics, tin_out, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) IF (ALLOCATED(u_out%DeltaL) .AND. ALLOCATED(u1%DeltaL)) THEN DO i1 = LBOUND(u_out%DeltaL,1),UBOUND(u_out%DeltaL,1) @@ -5464,7 +11134,7 @@ SUBROUTINE MD_Output_ExtrapInterp1(y1, y2, tin, y_out, tin_out, ErrStat, ErrMsg END IF ScaleFactor = t_out / t(2) - CALL MeshExtrapInterp1(y1%PtFairleadLoad, y2%PtFairleadLoad, tin, y_out%PtFairleadLoad, tin_out, ErrStat2, ErrMsg2 ) + CALL MeshExtrapInterp1(y1%CoupledLoads, y2%CoupledLoads, tin, y_out%CoupledLoads, tin_out, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) IF (ALLOCATED(y_out%WriteOutput) .AND. ALLOCATED(y1%WriteOutput)) THEN DO i1 = LBOUND(y_out%WriteOutput,1),UBOUND(y_out%WriteOutput,1) @@ -5529,7 +11199,7 @@ SUBROUTINE MD_Output_ExtrapInterp2(y1, y2, y3, tin, y_out, tin_out, ErrStat, Err END IF ScaleFactor = t_out / (t(2) * t(3) * (t(2) - t(3))) - CALL MeshExtrapInterp2(y1%PtFairleadLoad, y2%PtFairleadLoad, y3%PtFairleadLoad, tin, y_out%PtFairleadLoad, tin_out, ErrStat2, ErrMsg2 ) + CALL MeshExtrapInterp2(y1%CoupledLoads, y2%CoupledLoads, y3%CoupledLoads, tin, y_out%CoupledLoads, tin_out, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) IF (ALLOCATED(y_out%WriteOutput) .AND. ALLOCATED(y1%WriteOutput)) THEN DO i1 = LBOUND(y_out%WriteOutput,1),UBOUND(y_out%WriteOutput,1) From 8813a166774d8b1da5464779a43a2e899060d2ed Mon Sep 17 00:00:00 2001 From: Matt Hall Date: Fri, 8 Jan 2021 11:31:00 -0700 Subject: [PATCH 005/242] Properly hooking up CaEnd and CdEnd to Rods in MoorDyn. --- modules/moordyn/src/MoorDyn.f90 | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 4f644e665a..ba2af31c53 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -1645,6 +1645,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! time WRITE(UnOut,"(F10.4)", IOSTAT=ErrStat2, advance="no") (l-1)*p%dtWave + !WRITE(UnOut,"(F10.4)", IOSTAT=ErrStat2, advance="no") InitInp%WaveTime(l) ! wave elevation (all slices for now, to check) DO J = 1,5 !y @@ -4599,6 +4600,8 @@ SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) Rod%Cat = RodProp%Cat Rod%Cdn = RodProp%Cdn Rod%Cdt = RodProp%Cdt + Rod%CaEnd = RodProp%CaEnd + Rod%CdEnd = RodProp%CdEnd ! allocate node positions and velocities (NOTE: these arrays start at ZERO) @@ -5353,6 +5356,12 @@ SUBROUTINE Rod_DoRHS(Rod, LineList, p) ! axial drag Rod%Dq(:,I) = Rod%Dq(:,I) + VOF * 0.25* Pi*Rod%d*Rod%d * p%rhoW*Rod%CdEnd * MagVq * Vq +if ((Rod%time >= 1.0) .and. (Rod%time < 1.001)) then + print *, "at Dq end 0 of rod:" + print *, "CdEnd is on position vector:" + print *, Rod%CdEnd +end if + ! >>> what about rotational drag?? <<< eqn will be Pi* Rod%d**4/16.0 omega_rel?^2... *0.5 * Cd... ! Froud-Krylov force From 2b4a27a78b550292528970588638d28b80314669 Mon Sep 17 00:00:00 2001 From: Matt Hall Date: Fri, 8 Jan 2021 14:34:13 -0700 Subject: [PATCH 006/242] Tiny registry fix in MD --- modules/moordyn/src/MoorDyn_Registry.txt | 3 - modules/moordyn/src/MoorDyn_Types.f90 | 7508 +++++++++++++++++++++- 2 files changed, 7443 insertions(+), 68 deletions(-) diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 321f09e685..1d98a46563 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -199,11 +199,8 @@ typedef ^ ^ IntKi CtrlChan - typedef ^ ^ IntKi FairConnect - - - "IdNum of Connection at fairlead" typedef ^ ^ IntKi AnchConnect - - - "IdNum of Connection at anchor" typedef ^ ^ IntKi N - - - "The number of elements in the line" - -<<<<<<< HEAD typedef ^ ^ IntKi endTypeA - - - "type of connection at end A: 0=pinned to Connection, 1=cantilevered to Rod." - typedef ^ ^ IntKi endTypeB - - - "type of connection at end B: 0=pinned to Connection, 1=cantilevered to Rod." - -======= ->>>>>>> fbd7494e4438ca20d383f6eb6afe640438b6ca1c typedef ^ ^ DbKi UnstrLen - - - "unstretched length of the line" - typedef ^ ^ DbKi rho - - - "density" "[kg/m3]" typedef ^ ^ DbKi d - - - "volume-equivalent diameter" "[m]" diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index 823988cc7e..54d762d7e9 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -219,8 +219,179 @@ MODULE MoorDyn_Types INTEGER(IntKi) :: FairConnect !< IdNum of Connection at fairlead [-] INTEGER(IntKi) :: AnchConnect !< IdNum of Connection at anchor [-] INTEGER(IntKi) :: N !< The number of elements in the line [-] + INTEGER(IntKi) :: endTypeA !< type of connection at end A: 0=pinned to Connection, 1=cantilevered to Rod. [-] + INTEGER(IntKi) :: endTypeB !< type of connection at end B: 0=pinned to Connection, 1=cantilevered to Rod. [-] + REAL(DbKi) :: UnstrLen !< unstretched length of the line [-] + REAL(DbKi) :: rho !< density [[kg/m3]] + REAL(DbKi) :: d !< volume-equivalent diameter [[m]] + REAL(DbKi) :: EA !< stiffness [[N]] + REAL(DbKi) :: EI !< bending stiffness [[N-m]] + REAL(DbKi) :: BA !< internal damping coefficient times area for this line only [[N-s]] + REAL(DbKi) :: Can !< [[-]] + REAL(DbKi) :: Cat !< [[-]] + REAL(DbKi) :: Cdn !< [[-]] + REAL(DbKi) :: Cdt !< [[-]] + REAL(DbKi) :: time !< current time [[s]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: r !< node positions [-] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: rd !< node velocities [-] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: q !< node tangent vectors [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: l !< segment unstretched length [[m]] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: ld !< segment unstretched length rate of change (used in active tensioning) [[m]] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: lstr !< segment stretched length [[m]] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: lstrd !< segment change in stretched length [[m/s]] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: V !< segment volume [[m^3]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: U !< water velocity at node [[m/s]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Ud !< water acceleration at node [[m/s^2]] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: zeta !< water surface elevation above node [[m]] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: PDyn !< water dynamic pressure at node [[Pa]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: T !< segment tension vectors [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Td !< segment internal damping force vectors [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: W !< weight/buoyancy vectors [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Dp !< node drag (transverse) [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Dq !< node drag (axial) [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Ap !< node added mass forcing (transverse) [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Aq !< node added mass forcing (axial) [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: B !< node bottom contact force [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Fnet !< total force on node [[N]] + REAL(DbKi) , DIMENSION(:,:,:), ALLOCATABLE :: S !< node inverse mass matrix [[kg]] + REAL(DbKi) , DIMENSION(:,:,:), ALLOCATABLE :: M !< node mass matrix [[kg]] + REAL(DbKi) , DIMENSION(1:3) :: EndMomentA !< vector of end moments due to bending at line end A [[N-m]] + REAL(DbKi) , DIMENSION(1:3) :: EndMomentB !< vector of end moments due to bending at line end B [[N-m]] + INTEGER(IntKi) :: LineUnOut !< unit number of line output file [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: LineWrOutput !< one row of output data for this line [-] END TYPE MD_Line ! ======================= +! ========= MD_Fail ======= + TYPE, PUBLIC :: MD_Fail + INTEGER(IntKi) :: IdNum !< integer identifier of this failure [-] + END TYPE MD_Fail +! ======================= +! ========= MD_OutParmType ======= + TYPE, PUBLIC :: MD_OutParmType + CHARACTER(10) :: Name !< name of output channel [-] + CHARACTER(10) :: Units !< units string [-] + INTEGER(IntKi) :: QType !< type of quantity - 0=tension, 1=x, 2=y, 3=z... [-] + INTEGER(IntKi) :: OType !< type of object - 0=line, 1=connect [-] + INTEGER(IntKi) :: NodeID !< node number if OType=0. 0=anchor, -1=N=Fairlead [-] + INTEGER(IntKi) :: ObjID !< number of Connect or Line object [-] + END TYPE MD_OutParmType +! ======================= +! ========= MD_InitOutputType ======= + TYPE, PUBLIC :: MD_InitOutputType + CHARACTER(ChanLen) , DIMENSION(:), ALLOCATABLE :: writeOutputHdr !< first line output file contents: output variable names [-] + CHARACTER(ChanLen) , DIMENSION(:), ALLOCATABLE :: writeOutputUnt !< second line of output file contents: units [-] + TYPE(ProgDesc) :: Ver !< this module's name, version, and date [-] + END TYPE MD_InitOutputType +! ======================= +! ========= MD_ContinuousStateType ======= + TYPE, PUBLIC :: MD_ContinuousStateType + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: states !< state vector of mooring system, e.g. node coordinates and velocities [] + END TYPE MD_ContinuousStateType +! ======================= +! ========= MD_DiscreteStateType ======= + TYPE, PUBLIC :: MD_DiscreteStateType + REAL(SiKi) :: dummy !< Remove this variable if you have discrete states [-] + END TYPE MD_DiscreteStateType +! ======================= +! ========= MD_ConstraintStateType ======= + TYPE, PUBLIC :: MD_ConstraintStateType + REAL(SiKi) :: dummy !< Remove this variable if you have constraint states [-] + END TYPE MD_ConstraintStateType +! ======================= +! ========= MD_OtherStateType ======= + TYPE, PUBLIC :: MD_OtherStateType + REAL(SiKi) :: dummy !< Remove this variable if you have other states [-] + END TYPE MD_OtherStateType +! ======================= +! ========= MD_MiscVarType ======= + TYPE, PUBLIC :: MD_MiscVarType + TYPE(MD_LineProp) , DIMENSION(:), ALLOCATABLE :: LineTypeList !< array of properties for each line type [-] + TYPE(MD_RodProp) , DIMENSION(:), ALLOCATABLE :: RodTypeList !< array of properties for each rod type [-] + TYPE(MD_Body) :: GroundBody !< the single ground body which is the parent of all stationary connections [-] + TYPE(MD_Body) , DIMENSION(:), ALLOCATABLE :: BodyList !< array of body objects [-] + TYPE(MD_Rod) , DIMENSION(:), ALLOCATABLE :: RodList !< array of rod objects [-] + TYPE(MD_Connect) , DIMENSION(:), ALLOCATABLE :: ConnectList !< array of connection objects [-] + TYPE(MD_Line) , DIMENSION(:), ALLOCATABLE :: LineList !< array of line objects [-] + TYPE(MD_Fail) , DIMENSION(:), ALLOCATABLE :: FailList !< array of line objects [-] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: FreeConIs !< array of free connection indices in ConnectList vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: CpldConIs !< array of coupled/fairlead connection indices in ConnectList vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: FreeRodIs !< array of free rod indices in RodList vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: CpldRodIs !< array of coupled/fairlead rod indices in RodList vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: FreeBodyIs !< array of free body indices in BodyList vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: CpldBodyIs !< array of coupled body indices in BodyList vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: LineStateIs1 !< starting index of each line's states in state vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: LineStateIsN !< ending index of each line's states in state vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: ConStateIs1 !< starting index of each line's states in state vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: ConStateIsN !< ending index of each line's states in state vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: RodStateIs1 !< starting index of each rod's states in state vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: RodStateIsN !< ending index of each rod's states in state vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: BodyStateIs1 !< starting index of each body's states in state vector [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: BodyStateIsN !< ending index of each body's states in state vector [] + INTEGER(IntKi) :: Nx !< number of states and size of state vector [] + TYPE(MD_ContinuousStateType) :: xTemp !< contains temporary state vector used in integration (put here so it's only allocated once) [-] + TYPE(MD_ContinuousStateType) :: xdTemp !< contains temporary state derivative vector used in integration (put here so it's only allocated once) [-] + REAL(DbKi) , DIMENSION(1:6) :: zeros6 !< array of zeros for convenience [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: MDWrOutput !< Data from time step to be written to a MoorDyn output file [-] + END TYPE MD_MiscVarType +! ======================= +! ========= MD_ParameterType ======= + TYPE, PUBLIC :: MD_ParameterType + INTEGER(IntKi) :: nLineTypes = 0 !< number of line types [] + INTEGER(IntKi) :: nRodTypes = 0 !< number of rod types [] + INTEGER(IntKi) :: nConnects = 0 !< number of Connection objects [] + INTEGER(IntKi) :: nConnectsExtra = 0 !< number of Connection objects including space for extra ones that could arise from line failures [] + INTEGER(IntKi) :: nBodies = 0 !< number of Body objects [] + INTEGER(IntKi) :: nRods = 0 !< number of Rod objects [] + INTEGER(IntKi) :: nLines = 0 !< number of Line objects [] + INTEGER(IntKi) :: nFails = 0 !< number of failure conditions [] + INTEGER(IntKi) :: nFreeBodies = 0 !< [] + INTEGER(IntKi) :: nFreeRods = 0 !< [] + INTEGER(IntKi) :: nFreeCons = 0 !< [] + INTEGER(IntKi) :: nCpldBodies = 0 !< [] + INTEGER(IntKi) :: nCpldRods = 0 !< [] + INTEGER(IntKi) :: nCpldCons = 0 !< number of Fairlead Connections [] + INTEGER(IntKi) :: NConns = 0 !< number of Connect type Connections - not to be confused with NConnects [] + INTEGER(IntKi) :: NAnchs = 0 !< number of Anchor type Connections [] + REAL(DbKi) :: g = 9.81 !< gravitational constant (positive) [[m/s^2]] + REAL(DbKi) :: rhoW = 1025 !< density of seawater [[kg/m^3]] + REAL(DbKi) :: WtrDpth !< water depth [[m]] + REAL(DbKi) :: kBot !< bottom stiffness [[Pa/m]] + REAL(DbKi) :: cBot !< bottom damping [[Pa-s/m]] + REAL(DbKi) :: dtM0 !< desired mooring model time step [[s]] + REAL(DbKi) :: dtCoupling !< coupling time step that MoorDyn should expect [[s]] + INTEGER(IntKi) :: NumOuts !< Number of parameters in the output list (number of outputs requested) [-] + CHARACTER(1024) :: RootName !< RootName for writing output files [-] + TYPE(MD_OutParmType) , DIMENSION(:), ALLOCATABLE :: OutParam !< Names and units (and other characteristics) of all requested output parameters [-] + CHARACTER(1) :: Delim !< Column delimiter for output text files [-] + INTEGER(IntKi) :: MDUnOut !< Unit number of main output file [-] + INTEGER(IntKi) :: WaterKin !< Flag for whether or how to consider water kinematics [-] + REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: ux !< water velocities time series at each grid point [-] + REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: uy !< water velocities time series at each grid point [-] + REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: uz !< water velocities time series at each grid point [-] + REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: ax !< water accelerations time series at each grid point [-] + REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: ay !< water accelerations time series at each grid point [-] + REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: az !< water accelerations time series at each grid point [-] + REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: PDyn !< water dynamic pressure time series at each grid point [-] + REAL(DbKi) , DIMENSION(:,:,:), ALLOCATABLE :: zeta !< water surface elevations time series at each surface grid point [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: px !< [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: py !< [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: pz !< [-] + REAL(DbKi) :: dtWave !< [-] + END TYPE MD_ParameterType +! ======================= +! ========= MD_InputType ======= + TYPE, PUBLIC :: MD_InputType + TYPE(MeshType) :: CoupledKinematics !< mesh for position AND VELOCITY of each fairlead in X,Y,Z [[m, m/s]] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: DeltaL !< change in line length command for each channel [[m]] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: DeltaLdot !< rate of change of line length command for each channel [[m]] + END TYPE MD_InputType +! ======================= +! ========= MD_OutputType ======= + TYPE, PUBLIC :: MD_OutputType + TYPE(MeshType) :: CoupledLoads !< point mesh for fairlead forces in X,Y,Z [[N]] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: WriteOutput !< output vector returned to glue code [] + END TYPE MD_OutputType +! ======================= CONTAINS SUBROUTINE MD_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, ErrStat, ErrMsg ) TYPE(MD_InitInputType), INTENT(IN) :: SrcInitInputData @@ -233,6 +404,7 @@ SUBROUTINE MD_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, ErrSt INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: i4, i4_l, i4_u ! bounds (upper/lower) for an array dimension 4 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyInitInput' @@ -599,6 +771,7 @@ SUBROUTINE MD_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Err INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: i4, i4_l, i4_u ! bounds (upper/lower) for an array dimension 4 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackInitInput' @@ -3672,6 +3845,8 @@ SUBROUTINE MD_CopyLine( SrcLineData, DstLineData, CtrlCode, ErrStat, ErrMsg ) ! Local INTEGER(IntKi) :: i,j,k INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyLine' @@ -3685,6 +3860,346 @@ SUBROUTINE MD_CopyLine( SrcLineData, DstLineData, CtrlCode, ErrStat, ErrMsg ) DstLineData%FairConnect = SrcLineData%FairConnect DstLineData%AnchConnect = SrcLineData%AnchConnect DstLineData%N = SrcLineData%N + DstLineData%endTypeA = SrcLineData%endTypeA + DstLineData%endTypeB = SrcLineData%endTypeB + DstLineData%UnstrLen = SrcLineData%UnstrLen + DstLineData%rho = SrcLineData%rho + DstLineData%d = SrcLineData%d + DstLineData%EA = SrcLineData%EA + DstLineData%EI = SrcLineData%EI + DstLineData%BA = SrcLineData%BA + DstLineData%Can = SrcLineData%Can + DstLineData%Cat = SrcLineData%Cat + DstLineData%Cdn = SrcLineData%Cdn + DstLineData%Cdt = SrcLineData%Cdt + DstLineData%time = SrcLineData%time +IF (ALLOCATED(SrcLineData%r)) THEN + i1_l = LBOUND(SrcLineData%r,1) + i1_u = UBOUND(SrcLineData%r,1) + i2_l = LBOUND(SrcLineData%r,2) + i2_u = UBOUND(SrcLineData%r,2) + IF (.NOT. ALLOCATED(DstLineData%r)) THEN + ALLOCATE(DstLineData%r(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%r.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%r = SrcLineData%r +ENDIF +IF (ALLOCATED(SrcLineData%rd)) THEN + i1_l = LBOUND(SrcLineData%rd,1) + i1_u = UBOUND(SrcLineData%rd,1) + i2_l = LBOUND(SrcLineData%rd,2) + i2_u = UBOUND(SrcLineData%rd,2) + IF (.NOT. ALLOCATED(DstLineData%rd)) THEN + ALLOCATE(DstLineData%rd(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%rd.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%rd = SrcLineData%rd +ENDIF +IF (ALLOCATED(SrcLineData%q)) THEN + i1_l = LBOUND(SrcLineData%q,1) + i1_u = UBOUND(SrcLineData%q,1) + i2_l = LBOUND(SrcLineData%q,2) + i2_u = UBOUND(SrcLineData%q,2) + IF (.NOT. ALLOCATED(DstLineData%q)) THEN + ALLOCATE(DstLineData%q(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%q.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%q = SrcLineData%q +ENDIF +IF (ALLOCATED(SrcLineData%l)) THEN + i1_l = LBOUND(SrcLineData%l,1) + i1_u = UBOUND(SrcLineData%l,1) + IF (.NOT. ALLOCATED(DstLineData%l)) THEN + ALLOCATE(DstLineData%l(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%l.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%l = SrcLineData%l +ENDIF +IF (ALLOCATED(SrcLineData%ld)) THEN + i1_l = LBOUND(SrcLineData%ld,1) + i1_u = UBOUND(SrcLineData%ld,1) + IF (.NOT. ALLOCATED(DstLineData%ld)) THEN + ALLOCATE(DstLineData%ld(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%ld.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%ld = SrcLineData%ld +ENDIF +IF (ALLOCATED(SrcLineData%lstr)) THEN + i1_l = LBOUND(SrcLineData%lstr,1) + i1_u = UBOUND(SrcLineData%lstr,1) + IF (.NOT. ALLOCATED(DstLineData%lstr)) THEN + ALLOCATE(DstLineData%lstr(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%lstr.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%lstr = SrcLineData%lstr +ENDIF +IF (ALLOCATED(SrcLineData%lstrd)) THEN + i1_l = LBOUND(SrcLineData%lstrd,1) + i1_u = UBOUND(SrcLineData%lstrd,1) + IF (.NOT. ALLOCATED(DstLineData%lstrd)) THEN + ALLOCATE(DstLineData%lstrd(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%lstrd.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%lstrd = SrcLineData%lstrd +ENDIF +IF (ALLOCATED(SrcLineData%V)) THEN + i1_l = LBOUND(SrcLineData%V,1) + i1_u = UBOUND(SrcLineData%V,1) + IF (.NOT. ALLOCATED(DstLineData%V)) THEN + ALLOCATE(DstLineData%V(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%V.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%V = SrcLineData%V +ENDIF +IF (ALLOCATED(SrcLineData%U)) THEN + i1_l = LBOUND(SrcLineData%U,1) + i1_u = UBOUND(SrcLineData%U,1) + i2_l = LBOUND(SrcLineData%U,2) + i2_u = UBOUND(SrcLineData%U,2) + IF (.NOT. ALLOCATED(DstLineData%U)) THEN + ALLOCATE(DstLineData%U(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%U.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%U = SrcLineData%U +ENDIF +IF (ALLOCATED(SrcLineData%Ud)) THEN + i1_l = LBOUND(SrcLineData%Ud,1) + i1_u = UBOUND(SrcLineData%Ud,1) + i2_l = LBOUND(SrcLineData%Ud,2) + i2_u = UBOUND(SrcLineData%Ud,2) + IF (.NOT. ALLOCATED(DstLineData%Ud)) THEN + ALLOCATE(DstLineData%Ud(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%Ud.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%Ud = SrcLineData%Ud +ENDIF +IF (ALLOCATED(SrcLineData%zeta)) THEN + i1_l = LBOUND(SrcLineData%zeta,1) + i1_u = UBOUND(SrcLineData%zeta,1) + IF (.NOT. ALLOCATED(DstLineData%zeta)) THEN + ALLOCATE(DstLineData%zeta(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%zeta.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%zeta = SrcLineData%zeta +ENDIF +IF (ALLOCATED(SrcLineData%PDyn)) THEN + i1_l = LBOUND(SrcLineData%PDyn,1) + i1_u = UBOUND(SrcLineData%PDyn,1) + IF (.NOT. ALLOCATED(DstLineData%PDyn)) THEN + ALLOCATE(DstLineData%PDyn(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%PDyn.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%PDyn = SrcLineData%PDyn +ENDIF +IF (ALLOCATED(SrcLineData%T)) THEN + i1_l = LBOUND(SrcLineData%T,1) + i1_u = UBOUND(SrcLineData%T,1) + i2_l = LBOUND(SrcLineData%T,2) + i2_u = UBOUND(SrcLineData%T,2) + IF (.NOT. ALLOCATED(DstLineData%T)) THEN + ALLOCATE(DstLineData%T(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%T.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%T = SrcLineData%T +ENDIF +IF (ALLOCATED(SrcLineData%Td)) THEN + i1_l = LBOUND(SrcLineData%Td,1) + i1_u = UBOUND(SrcLineData%Td,1) + i2_l = LBOUND(SrcLineData%Td,2) + i2_u = UBOUND(SrcLineData%Td,2) + IF (.NOT. ALLOCATED(DstLineData%Td)) THEN + ALLOCATE(DstLineData%Td(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%Td.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%Td = SrcLineData%Td +ENDIF +IF (ALLOCATED(SrcLineData%W)) THEN + i1_l = LBOUND(SrcLineData%W,1) + i1_u = UBOUND(SrcLineData%W,1) + i2_l = LBOUND(SrcLineData%W,2) + i2_u = UBOUND(SrcLineData%W,2) + IF (.NOT. ALLOCATED(DstLineData%W)) THEN + ALLOCATE(DstLineData%W(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%W.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%W = SrcLineData%W +ENDIF +IF (ALLOCATED(SrcLineData%Dp)) THEN + i1_l = LBOUND(SrcLineData%Dp,1) + i1_u = UBOUND(SrcLineData%Dp,1) + i2_l = LBOUND(SrcLineData%Dp,2) + i2_u = UBOUND(SrcLineData%Dp,2) + IF (.NOT. ALLOCATED(DstLineData%Dp)) THEN + ALLOCATE(DstLineData%Dp(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%Dp.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%Dp = SrcLineData%Dp +ENDIF +IF (ALLOCATED(SrcLineData%Dq)) THEN + i1_l = LBOUND(SrcLineData%Dq,1) + i1_u = UBOUND(SrcLineData%Dq,1) + i2_l = LBOUND(SrcLineData%Dq,2) + i2_u = UBOUND(SrcLineData%Dq,2) + IF (.NOT. ALLOCATED(DstLineData%Dq)) THEN + ALLOCATE(DstLineData%Dq(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%Dq.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%Dq = SrcLineData%Dq +ENDIF +IF (ALLOCATED(SrcLineData%Ap)) THEN + i1_l = LBOUND(SrcLineData%Ap,1) + i1_u = UBOUND(SrcLineData%Ap,1) + i2_l = LBOUND(SrcLineData%Ap,2) + i2_u = UBOUND(SrcLineData%Ap,2) + IF (.NOT. ALLOCATED(DstLineData%Ap)) THEN + ALLOCATE(DstLineData%Ap(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%Ap.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%Ap = SrcLineData%Ap +ENDIF +IF (ALLOCATED(SrcLineData%Aq)) THEN + i1_l = LBOUND(SrcLineData%Aq,1) + i1_u = UBOUND(SrcLineData%Aq,1) + i2_l = LBOUND(SrcLineData%Aq,2) + i2_u = UBOUND(SrcLineData%Aq,2) + IF (.NOT. ALLOCATED(DstLineData%Aq)) THEN + ALLOCATE(DstLineData%Aq(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%Aq.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%Aq = SrcLineData%Aq +ENDIF +IF (ALLOCATED(SrcLineData%B)) THEN + i1_l = LBOUND(SrcLineData%B,1) + i1_u = UBOUND(SrcLineData%B,1) + i2_l = LBOUND(SrcLineData%B,2) + i2_u = UBOUND(SrcLineData%B,2) + IF (.NOT. ALLOCATED(DstLineData%B)) THEN + ALLOCATE(DstLineData%B(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%B.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%B = SrcLineData%B +ENDIF +IF (ALLOCATED(SrcLineData%Fnet)) THEN + i1_l = LBOUND(SrcLineData%Fnet,1) + i1_u = UBOUND(SrcLineData%Fnet,1) + i2_l = LBOUND(SrcLineData%Fnet,2) + i2_u = UBOUND(SrcLineData%Fnet,2) + IF (.NOT. ALLOCATED(DstLineData%Fnet)) THEN + ALLOCATE(DstLineData%Fnet(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%Fnet.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%Fnet = SrcLineData%Fnet +ENDIF +IF (ALLOCATED(SrcLineData%S)) THEN + i1_l = LBOUND(SrcLineData%S,1) + i1_u = UBOUND(SrcLineData%S,1) + i2_l = LBOUND(SrcLineData%S,2) + i2_u = UBOUND(SrcLineData%S,2) + i3_l = LBOUND(SrcLineData%S,3) + i3_u = UBOUND(SrcLineData%S,3) + IF (.NOT. ALLOCATED(DstLineData%S)) THEN + ALLOCATE(DstLineData%S(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%S.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%S = SrcLineData%S +ENDIF +IF (ALLOCATED(SrcLineData%M)) THEN + i1_l = LBOUND(SrcLineData%M,1) + i1_u = UBOUND(SrcLineData%M,1) + i2_l = LBOUND(SrcLineData%M,2) + i2_u = UBOUND(SrcLineData%M,2) + i3_l = LBOUND(SrcLineData%M,3) + i3_u = UBOUND(SrcLineData%M,3) + IF (.NOT. ALLOCATED(DstLineData%M)) THEN + ALLOCATE(DstLineData%M(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%M.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%M = SrcLineData%M +ENDIF + DstLineData%EndMomentA = SrcLineData%EndMomentA + DstLineData%EndMomentB = SrcLineData%EndMomentB + DstLineData%LineUnOut = SrcLineData%LineUnOut +IF (ALLOCATED(SrcLineData%LineWrOutput)) THEN + i1_l = LBOUND(SrcLineData%LineWrOutput,1) + i1_u = UBOUND(SrcLineData%LineWrOutput,1) + IF (.NOT. ALLOCATED(DstLineData%LineWrOutput)) THEN + ALLOCATE(DstLineData%LineWrOutput(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%LineWrOutput.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%LineWrOutput = SrcLineData%LineWrOutput +ENDIF END SUBROUTINE MD_CopyLine SUBROUTINE MD_DestroyLine( LineData, ErrStat, ErrMsg ) @@ -3696,6 +4211,78 @@ SUBROUTINE MD_DestroyLine( LineData, ErrStat, ErrMsg ) ! ErrStat = ErrID_None ErrMsg = "" +IF (ALLOCATED(LineData%r)) THEN + DEALLOCATE(LineData%r) +ENDIF +IF (ALLOCATED(LineData%rd)) THEN + DEALLOCATE(LineData%rd) +ENDIF +IF (ALLOCATED(LineData%q)) THEN + DEALLOCATE(LineData%q) +ENDIF +IF (ALLOCATED(LineData%l)) THEN + DEALLOCATE(LineData%l) +ENDIF +IF (ALLOCATED(LineData%ld)) THEN + DEALLOCATE(LineData%ld) +ENDIF +IF (ALLOCATED(LineData%lstr)) THEN + DEALLOCATE(LineData%lstr) +ENDIF +IF (ALLOCATED(LineData%lstrd)) THEN + DEALLOCATE(LineData%lstrd) +ENDIF +IF (ALLOCATED(LineData%V)) THEN + DEALLOCATE(LineData%V) +ENDIF +IF (ALLOCATED(LineData%U)) THEN + DEALLOCATE(LineData%U) +ENDIF +IF (ALLOCATED(LineData%Ud)) THEN + DEALLOCATE(LineData%Ud) +ENDIF +IF (ALLOCATED(LineData%zeta)) THEN + DEALLOCATE(LineData%zeta) +ENDIF +IF (ALLOCATED(LineData%PDyn)) THEN + DEALLOCATE(LineData%PDyn) +ENDIF +IF (ALLOCATED(LineData%T)) THEN + DEALLOCATE(LineData%T) +ENDIF +IF (ALLOCATED(LineData%Td)) THEN + DEALLOCATE(LineData%Td) +ENDIF +IF (ALLOCATED(LineData%W)) THEN + DEALLOCATE(LineData%W) +ENDIF +IF (ALLOCATED(LineData%Dp)) THEN + DEALLOCATE(LineData%Dp) +ENDIF +IF (ALLOCATED(LineData%Dq)) THEN + DEALLOCATE(LineData%Dq) +ENDIF +IF (ALLOCATED(LineData%Ap)) THEN + DEALLOCATE(LineData%Ap) +ENDIF +IF (ALLOCATED(LineData%Aq)) THEN + DEALLOCATE(LineData%Aq) +ENDIF +IF (ALLOCATED(LineData%B)) THEN + DEALLOCATE(LineData%B) +ENDIF +IF (ALLOCATED(LineData%Fnet)) THEN + DEALLOCATE(LineData%Fnet) +ENDIF +IF (ALLOCATED(LineData%S)) THEN + DEALLOCATE(LineData%S) +ENDIF +IF (ALLOCATED(LineData%M)) THEN + DEALLOCATE(LineData%M) +ENDIF +IF (ALLOCATED(LineData%LineWrOutput)) THEN + DEALLOCATE(LineData%LineWrOutput) +ENDIF END SUBROUTINE MD_DestroyLine SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) @@ -3740,60 +4327,6174 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_BufSz = Int_BufSz + 1 ! FairConnect Int_BufSz = Int_BufSz + 1 ! AnchConnect Int_BufSz = Int_BufSz + 1 ! N - IF ( Re_BufSz .GT. 0 ) THEN - ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF + Int_BufSz = Int_BufSz + 1 ! endTypeA + Int_BufSz = Int_BufSz + 1 ! endTypeB + Db_BufSz = Db_BufSz + 1 ! UnstrLen + Db_BufSz = Db_BufSz + 1 ! rho + Db_BufSz = Db_BufSz + 1 ! d + Db_BufSz = Db_BufSz + 1 ! EA + Db_BufSz = Db_BufSz + 1 ! EI + Db_BufSz = Db_BufSz + 1 ! BA + Db_BufSz = Db_BufSz + 1 ! Can + Db_BufSz = Db_BufSz + 1 ! Cat + Db_BufSz = Db_BufSz + 1 ! Cdn + Db_BufSz = Db_BufSz + 1 ! Cdt + Db_BufSz = Db_BufSz + 1 ! time + Int_BufSz = Int_BufSz + 1 ! r allocated yes/no + IF ( ALLOCATED(InData%r) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! r upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%r) ! r END IF - IF ( Db_BufSz .GT. 0 ) THEN - ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF + Int_BufSz = Int_BufSz + 1 ! rd allocated yes/no + IF ( ALLOCATED(InData%rd) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! rd upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%rd) ! rd END IF - IF ( Int_BufSz .GT. 0 ) THEN - ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF + Int_BufSz = Int_BufSz + 1 ! q allocated yes/no + IF ( ALLOCATED(InData%q) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! q upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%q) ! q END IF - IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) - - Re_Xferred = 1 - Db_Xferred = 1 - Int_Xferred = 1 - - IntKiBuf(Int_Xferred) = InData%IdNum - Int_Xferred = Int_Xferred + 1 - IntKiBuf(Int_Xferred) = InData%PropsIdNum - Int_Xferred = Int_Xferred + 1 - DO i1 = LBOUND(InData%OutFlagList,1), UBOUND(InData%OutFlagList,1) - IntKiBuf(Int_Xferred) = InData%OutFlagList(i1) - Int_Xferred = Int_Xferred + 1 - END DO - IntKiBuf(Int_Xferred) = InData%CtrlChan - Int_Xferred = Int_Xferred + 1 - IntKiBuf(Int_Xferred) = InData%FairConnect - Int_Xferred = Int_Xferred + 1 - IntKiBuf(Int_Xferred) = InData%AnchConnect - Int_Xferred = Int_Xferred + 1 - IntKiBuf(Int_Xferred) = InData%N - Int_Xferred = Int_Xferred + 1 - END SUBROUTINE MD_PackLine - - SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) - REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) - REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) - INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) - TYPE(MD_Line), INTENT(INOUT) :: OutData - INTEGER(IntKi), INTENT( OUT) :: ErrStat - CHARACTER(*), INTENT( OUT) :: ErrMsg - ! Local variables - INTEGER(IntKi) :: Buf_size + Int_BufSz = Int_BufSz + 1 ! l allocated yes/no + IF ( ALLOCATED(InData%l) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! l upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%l) ! l + END IF + Int_BufSz = Int_BufSz + 1 ! ld allocated yes/no + IF ( ALLOCATED(InData%ld) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! ld upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%ld) ! ld + END IF + Int_BufSz = Int_BufSz + 1 ! lstr allocated yes/no + IF ( ALLOCATED(InData%lstr) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! lstr upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%lstr) ! lstr + END IF + Int_BufSz = Int_BufSz + 1 ! lstrd allocated yes/no + IF ( ALLOCATED(InData%lstrd) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! lstrd upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%lstrd) ! lstrd + END IF + Int_BufSz = Int_BufSz + 1 ! V allocated yes/no + IF ( ALLOCATED(InData%V) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! V upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%V) ! V + END IF + Int_BufSz = Int_BufSz + 1 ! U allocated yes/no + IF ( ALLOCATED(InData%U) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! U upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%U) ! U + END IF + Int_BufSz = Int_BufSz + 1 ! Ud allocated yes/no + IF ( ALLOCATED(InData%Ud) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Ud upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Ud) ! Ud + END IF + Int_BufSz = Int_BufSz + 1 ! zeta allocated yes/no + IF ( ALLOCATED(InData%zeta) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! zeta upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%zeta) ! zeta + END IF + Int_BufSz = Int_BufSz + 1 ! PDyn allocated yes/no + IF ( ALLOCATED(InData%PDyn) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! PDyn upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%PDyn) ! PDyn + END IF + Int_BufSz = Int_BufSz + 1 ! T allocated yes/no + IF ( ALLOCATED(InData%T) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! T upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%T) ! T + END IF + Int_BufSz = Int_BufSz + 1 ! Td allocated yes/no + IF ( ALLOCATED(InData%Td) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Td upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Td) ! Td + END IF + Int_BufSz = Int_BufSz + 1 ! W allocated yes/no + IF ( ALLOCATED(InData%W) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! W upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%W) ! W + END IF + Int_BufSz = Int_BufSz + 1 ! Dp allocated yes/no + IF ( ALLOCATED(InData%Dp) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Dp upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Dp) ! Dp + END IF + Int_BufSz = Int_BufSz + 1 ! Dq allocated yes/no + IF ( ALLOCATED(InData%Dq) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Dq upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Dq) ! Dq + END IF + Int_BufSz = Int_BufSz + 1 ! Ap allocated yes/no + IF ( ALLOCATED(InData%Ap) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Ap upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Ap) ! Ap + END IF + Int_BufSz = Int_BufSz + 1 ! Aq allocated yes/no + IF ( ALLOCATED(InData%Aq) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Aq upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Aq) ! Aq + END IF + Int_BufSz = Int_BufSz + 1 ! B allocated yes/no + IF ( ALLOCATED(InData%B) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! B upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%B) ! B + END IF + Int_BufSz = Int_BufSz + 1 ! Fnet allocated yes/no + IF ( ALLOCATED(InData%Fnet) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Fnet upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Fnet) ! Fnet + END IF + Int_BufSz = Int_BufSz + 1 ! S allocated yes/no + IF ( ALLOCATED(InData%S) ) THEN + Int_BufSz = Int_BufSz + 2*3 ! S upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%S) ! S + END IF + Int_BufSz = Int_BufSz + 1 ! M allocated yes/no + IF ( ALLOCATED(InData%M) ) THEN + Int_BufSz = Int_BufSz + 2*3 ! M upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%M) ! M + END IF + Db_BufSz = Db_BufSz + SIZE(InData%EndMomentA) ! EndMomentA + Db_BufSz = Db_BufSz + SIZE(InData%EndMomentB) ! EndMomentB + Int_BufSz = Int_BufSz + 1 ! LineUnOut + Int_BufSz = Int_BufSz + 1 ! LineWrOutput allocated yes/no + IF ( ALLOCATED(InData%LineWrOutput) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! LineWrOutput upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%LineWrOutput) ! LineWrOutput + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IntKiBuf(Int_Xferred) = InData%IdNum + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%PropsIdNum + Int_Xferred = Int_Xferred + 1 + DO i1 = LBOUND(InData%OutFlagList,1), UBOUND(InData%OutFlagList,1) + IntKiBuf(Int_Xferred) = InData%OutFlagList(i1) + Int_Xferred = Int_Xferred + 1 + END DO + IntKiBuf(Int_Xferred) = InData%CtrlChan + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%FairConnect + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%AnchConnect + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%N + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%endTypeA + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%endTypeB + Int_Xferred = Int_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%UnstrLen + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%rho + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%d + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%EA + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%EI + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%BA + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%Can + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%Cat + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%Cdn + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%Cdt + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%time + Db_Xferred = Db_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%r) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%r,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%r,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%r,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%r,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%r,2), UBOUND(InData%r,2) + DO i1 = LBOUND(InData%r,1), UBOUND(InData%r,1) + DbKiBuf(Db_Xferred) = InData%r(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%rd) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%rd,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%rd,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%rd,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%rd,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%rd,2), UBOUND(InData%rd,2) + DO i1 = LBOUND(InData%rd,1), UBOUND(InData%rd,1) + DbKiBuf(Db_Xferred) = InData%rd(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%q) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%q,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%q,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%q,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%q,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%q,2), UBOUND(InData%q,2) + DO i1 = LBOUND(InData%q,1), UBOUND(InData%q,1) + DbKiBuf(Db_Xferred) = InData%q(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%l) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%l,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%l,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%l,1), UBOUND(InData%l,1) + DbKiBuf(Db_Xferred) = InData%l(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%ld) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ld,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ld,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%ld,1), UBOUND(InData%ld,1) + DbKiBuf(Db_Xferred) = InData%ld(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%lstr) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%lstr,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%lstr,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%lstr,1), UBOUND(InData%lstr,1) + DbKiBuf(Db_Xferred) = InData%lstr(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%lstrd) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%lstrd,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%lstrd,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%lstrd,1), UBOUND(InData%lstrd,1) + DbKiBuf(Db_Xferred) = InData%lstrd(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%V) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%V,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%V,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%V,1), UBOUND(InData%V,1) + DbKiBuf(Db_Xferred) = InData%V(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%U) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%U,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%U,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%U,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%U,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%U,2), UBOUND(InData%U,2) + DO i1 = LBOUND(InData%U,1), UBOUND(InData%U,1) + DbKiBuf(Db_Xferred) = InData%U(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%Ud) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Ud,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Ud,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Ud,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Ud,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Ud,2), UBOUND(InData%Ud,2) + DO i1 = LBOUND(InData%Ud,1), UBOUND(InData%Ud,1) + DbKiBuf(Db_Xferred) = InData%Ud(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%zeta) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%zeta,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%zeta,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%zeta,1), UBOUND(InData%zeta,1) + DbKiBuf(Db_Xferred) = InData%zeta(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%PDyn) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%PDyn,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PDyn,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%PDyn,1), UBOUND(InData%PDyn,1) + DbKiBuf(Db_Xferred) = InData%PDyn(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%T) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%T,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%T,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%T,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%T,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%T,2), UBOUND(InData%T,2) + DO i1 = LBOUND(InData%T,1), UBOUND(InData%T,1) + DbKiBuf(Db_Xferred) = InData%T(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%Td) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Td,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Td,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Td,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Td,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Td,2), UBOUND(InData%Td,2) + DO i1 = LBOUND(InData%Td,1), UBOUND(InData%Td,1) + DbKiBuf(Db_Xferred) = InData%Td(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%W) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%W,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%W,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%W,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%W,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%W,2), UBOUND(InData%W,2) + DO i1 = LBOUND(InData%W,1), UBOUND(InData%W,1) + DbKiBuf(Db_Xferred) = InData%W(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%Dp) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Dp,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Dp,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Dp,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Dp,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Dp,2), UBOUND(InData%Dp,2) + DO i1 = LBOUND(InData%Dp,1), UBOUND(InData%Dp,1) + DbKiBuf(Db_Xferred) = InData%Dp(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%Dq) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Dq,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Dq,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Dq,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Dq,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Dq,2), UBOUND(InData%Dq,2) + DO i1 = LBOUND(InData%Dq,1), UBOUND(InData%Dq,1) + DbKiBuf(Db_Xferred) = InData%Dq(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%Ap) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Ap,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Ap,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Ap,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Ap,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Ap,2), UBOUND(InData%Ap,2) + DO i1 = LBOUND(InData%Ap,1), UBOUND(InData%Ap,1) + DbKiBuf(Db_Xferred) = InData%Ap(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%Aq) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Aq,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Aq,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Aq,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Aq,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Aq,2), UBOUND(InData%Aq,2) + DO i1 = LBOUND(InData%Aq,1), UBOUND(InData%Aq,1) + DbKiBuf(Db_Xferred) = InData%Aq(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%B) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%B,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%B,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%B,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%B,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%B,2), UBOUND(InData%B,2) + DO i1 = LBOUND(InData%B,1), UBOUND(InData%B,1) + DbKiBuf(Db_Xferred) = InData%B(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%Fnet) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Fnet,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Fnet,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Fnet,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Fnet,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Fnet,2), UBOUND(InData%Fnet,2) + DO i1 = LBOUND(InData%Fnet,1), UBOUND(InData%Fnet,1) + DbKiBuf(Db_Xferred) = InData%Fnet(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%S) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%S,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%S,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%S,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%S,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%S,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%S,3) + Int_Xferred = Int_Xferred + 2 + + DO i3 = LBOUND(InData%S,3), UBOUND(InData%S,3) + DO i2 = LBOUND(InData%S,2), UBOUND(InData%S,2) + DO i1 = LBOUND(InData%S,1), UBOUND(InData%S,1) + DbKiBuf(Db_Xferred) = InData%S(i1,i2,i3) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%M) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%M,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%M,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%M,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%M,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%M,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%M,3) + Int_Xferred = Int_Xferred + 2 + + DO i3 = LBOUND(InData%M,3), UBOUND(InData%M,3) + DO i2 = LBOUND(InData%M,2), UBOUND(InData%M,2) + DO i1 = LBOUND(InData%M,1), UBOUND(InData%M,1) + DbKiBuf(Db_Xferred) = InData%M(i1,i2,i3) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END IF + DO i1 = LBOUND(InData%EndMomentA,1), UBOUND(InData%EndMomentA,1) + DbKiBuf(Db_Xferred) = InData%EndMomentA(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%EndMomentB,1), UBOUND(InData%EndMomentB,1) + DbKiBuf(Db_Xferred) = InData%EndMomentB(i1) + Db_Xferred = Db_Xferred + 1 + END DO + IntKiBuf(Int_Xferred) = InData%LineUnOut + Int_Xferred = Int_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%LineWrOutput) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%LineWrOutput,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%LineWrOutput,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%LineWrOutput,1), UBOUND(InData%LineWrOutput,1) + DbKiBuf(Db_Xferred) = InData%LineWrOutput(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + END SUBROUTINE MD_PackLine + + SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_Line), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackLine' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%IdNum = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%PropsIdNum = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + i1_l = LBOUND(OutData%OutFlagList,1) + i1_u = UBOUND(OutData%OutFlagList,1) + DO i1 = LBOUND(OutData%OutFlagList,1), UBOUND(OutData%OutFlagList,1) + OutData%OutFlagList(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + OutData%CtrlChan = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%FairConnect = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%AnchConnect = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%N = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%endTypeA = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%endTypeB = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%UnstrLen = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%rho = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%d = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%EA = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%EI = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%BA = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%Can = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%Cat = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%Cdn = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%Cdt = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%time = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! r not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%r)) DEALLOCATE(OutData%r) + ALLOCATE(OutData%r(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%r.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%r,2), UBOUND(OutData%r,2) + DO i1 = LBOUND(OutData%r,1), UBOUND(OutData%r,1) + OutData%r(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! rd not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%rd)) DEALLOCATE(OutData%rd) + ALLOCATE(OutData%rd(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%rd.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%rd,2), UBOUND(OutData%rd,2) + DO i1 = LBOUND(OutData%rd,1), UBOUND(OutData%rd,1) + OutData%rd(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! q not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%q)) DEALLOCATE(OutData%q) + ALLOCATE(OutData%q(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%q.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%q,2), UBOUND(OutData%q,2) + DO i1 = LBOUND(OutData%q,1), UBOUND(OutData%q,1) + OutData%q(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! l not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%l)) DEALLOCATE(OutData%l) + ALLOCATE(OutData%l(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%l.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%l,1), UBOUND(OutData%l,1) + OutData%l(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ld not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%ld)) DEALLOCATE(OutData%ld) + ALLOCATE(OutData%ld(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ld.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%ld,1), UBOUND(OutData%ld,1) + OutData%ld(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! lstr not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%lstr)) DEALLOCATE(OutData%lstr) + ALLOCATE(OutData%lstr(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%lstr.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%lstr,1), UBOUND(OutData%lstr,1) + OutData%lstr(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! lstrd not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%lstrd)) DEALLOCATE(OutData%lstrd) + ALLOCATE(OutData%lstrd(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%lstrd.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%lstrd,1), UBOUND(OutData%lstrd,1) + OutData%lstrd(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! V not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%V)) DEALLOCATE(OutData%V) + ALLOCATE(OutData%V(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%V.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%V,1), UBOUND(OutData%V,1) + OutData%V(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! U not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%U)) DEALLOCATE(OutData%U) + ALLOCATE(OutData%U(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%U.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%U,2), UBOUND(OutData%U,2) + DO i1 = LBOUND(OutData%U,1), UBOUND(OutData%U,1) + OutData%U(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Ud not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Ud)) DEALLOCATE(OutData%Ud) + ALLOCATE(OutData%Ud(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Ud.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Ud,2), UBOUND(OutData%Ud,2) + DO i1 = LBOUND(OutData%Ud,1), UBOUND(OutData%Ud,1) + OutData%Ud(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! zeta not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%zeta)) DEALLOCATE(OutData%zeta) + ALLOCATE(OutData%zeta(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%zeta.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%zeta,1), UBOUND(OutData%zeta,1) + OutData%zeta(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! PDyn not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%PDyn)) DEALLOCATE(OutData%PDyn) + ALLOCATE(OutData%PDyn(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%PDyn.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%PDyn,1), UBOUND(OutData%PDyn,1) + OutData%PDyn(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! T not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%T)) DEALLOCATE(OutData%T) + ALLOCATE(OutData%T(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%T.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%T,2), UBOUND(OutData%T,2) + DO i1 = LBOUND(OutData%T,1), UBOUND(OutData%T,1) + OutData%T(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Td not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Td)) DEALLOCATE(OutData%Td) + ALLOCATE(OutData%Td(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Td.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Td,2), UBOUND(OutData%Td,2) + DO i1 = LBOUND(OutData%Td,1), UBOUND(OutData%Td,1) + OutData%Td(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! W not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%W)) DEALLOCATE(OutData%W) + ALLOCATE(OutData%W(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%W.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%W,2), UBOUND(OutData%W,2) + DO i1 = LBOUND(OutData%W,1), UBOUND(OutData%W,1) + OutData%W(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Dp not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Dp)) DEALLOCATE(OutData%Dp) + ALLOCATE(OutData%Dp(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Dp.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Dp,2), UBOUND(OutData%Dp,2) + DO i1 = LBOUND(OutData%Dp,1), UBOUND(OutData%Dp,1) + OutData%Dp(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Dq not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Dq)) DEALLOCATE(OutData%Dq) + ALLOCATE(OutData%Dq(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Dq.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Dq,2), UBOUND(OutData%Dq,2) + DO i1 = LBOUND(OutData%Dq,1), UBOUND(OutData%Dq,1) + OutData%Dq(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Ap not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Ap)) DEALLOCATE(OutData%Ap) + ALLOCATE(OutData%Ap(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Ap.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Ap,2), UBOUND(OutData%Ap,2) + DO i1 = LBOUND(OutData%Ap,1), UBOUND(OutData%Ap,1) + OutData%Ap(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Aq not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Aq)) DEALLOCATE(OutData%Aq) + ALLOCATE(OutData%Aq(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Aq.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Aq,2), UBOUND(OutData%Aq,2) + DO i1 = LBOUND(OutData%Aq,1), UBOUND(OutData%Aq,1) + OutData%Aq(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! B not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%B)) DEALLOCATE(OutData%B) + ALLOCATE(OutData%B(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%B.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%B,2), UBOUND(OutData%B,2) + DO i1 = LBOUND(OutData%B,1), UBOUND(OutData%B,1) + OutData%B(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Fnet not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Fnet)) DEALLOCATE(OutData%Fnet) + ALLOCATE(OutData%Fnet(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Fnet.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Fnet,2), UBOUND(OutData%Fnet,2) + DO i1 = LBOUND(OutData%Fnet,1), UBOUND(OutData%Fnet,1) + OutData%Fnet(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! S not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%S)) DEALLOCATE(OutData%S) + ALLOCATE(OutData%S(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%S.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i3 = LBOUND(OutData%S,3), UBOUND(OutData%S,3) + DO i2 = LBOUND(OutData%S,2), UBOUND(OutData%S,2) + DO i1 = LBOUND(OutData%S,1), UBOUND(OutData%S,1) + OutData%S(i1,i2,i3) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! M not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%M)) DEALLOCATE(OutData%M) + ALLOCATE(OutData%M(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%M.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i3 = LBOUND(OutData%M,3), UBOUND(OutData%M,3) + DO i2 = LBOUND(OutData%M,2), UBOUND(OutData%M,2) + DO i1 = LBOUND(OutData%M,1), UBOUND(OutData%M,1) + OutData%M(i1,i2,i3) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END IF + i1_l = LBOUND(OutData%EndMomentA,1) + i1_u = UBOUND(OutData%EndMomentA,1) + DO i1 = LBOUND(OutData%EndMomentA,1), UBOUND(OutData%EndMomentA,1) + OutData%EndMomentA(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%EndMomentB,1) + i1_u = UBOUND(OutData%EndMomentB,1) + DO i1 = LBOUND(OutData%EndMomentB,1), UBOUND(OutData%EndMomentB,1) + OutData%EndMomentB(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + OutData%LineUnOut = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! LineWrOutput not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%LineWrOutput)) DEALLOCATE(OutData%LineWrOutput) + ALLOCATE(OutData%LineWrOutput(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%LineWrOutput.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%LineWrOutput,1), UBOUND(OutData%LineWrOutput,1) + OutData%LineWrOutput(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + END SUBROUTINE MD_UnPackLine + + SUBROUTINE MD_CopyFail( SrcFailData, DstFailData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_Fail), INTENT(IN) :: SrcFailData + TYPE(MD_Fail), INTENT(INOUT) :: DstFailData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyFail' +! + ErrStat = ErrID_None + ErrMsg = "" + DstFailData%IdNum = SrcFailData%IdNum + END SUBROUTINE MD_CopyFail + + SUBROUTINE MD_DestroyFail( FailData, ErrStat, ErrMsg ) + TYPE(MD_Fail), INTENT(INOUT) :: FailData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyFail' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE MD_DestroyFail + + SUBROUTINE MD_PackFail( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(MD_Fail), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackFail' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! IdNum + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IntKiBuf(Int_Xferred) = InData%IdNum + Int_Xferred = Int_Xferred + 1 + END SUBROUTINE MD_PackFail + + SUBROUTINE MD_UnPackFail( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_Fail), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackFail' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%IdNum = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END SUBROUTINE MD_UnPackFail + + SUBROUTINE MD_CopyOutParmType( SrcOutParmTypeData, DstOutParmTypeData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_OutParmType), INTENT(IN) :: SrcOutParmTypeData + TYPE(MD_OutParmType), INTENT(INOUT) :: DstOutParmTypeData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyOutParmType' +! + ErrStat = ErrID_None + ErrMsg = "" + DstOutParmTypeData%Name = SrcOutParmTypeData%Name + DstOutParmTypeData%Units = SrcOutParmTypeData%Units + DstOutParmTypeData%QType = SrcOutParmTypeData%QType + DstOutParmTypeData%OType = SrcOutParmTypeData%OType + DstOutParmTypeData%NodeID = SrcOutParmTypeData%NodeID + DstOutParmTypeData%ObjID = SrcOutParmTypeData%ObjID + END SUBROUTINE MD_CopyOutParmType + + SUBROUTINE MD_DestroyOutParmType( OutParmTypeData, ErrStat, ErrMsg ) + TYPE(MD_OutParmType), INTENT(INOUT) :: OutParmTypeData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyOutParmType' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE MD_DestroyOutParmType + + SUBROUTINE MD_PackOutParmType( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(MD_OutParmType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackOutParmType' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1*LEN(InData%Name) ! Name + Int_BufSz = Int_BufSz + 1*LEN(InData%Units) ! Units + Int_BufSz = Int_BufSz + 1 ! QType + Int_BufSz = Int_BufSz + 1 ! OType + Int_BufSz = Int_BufSz + 1 ! NodeID + Int_BufSz = Int_BufSz + 1 ! ObjID + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + DO I = 1, LEN(InData%Name) + IntKiBuf(Int_Xferred) = ICHAR(InData%Name(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + DO I = 1, LEN(InData%Units) + IntKiBuf(Int_Xferred) = ICHAR(InData%Units(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + IntKiBuf(Int_Xferred) = InData%QType + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%OType + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%NodeID + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%ObjID + Int_Xferred = Int_Xferred + 1 + END SUBROUTINE MD_PackOutParmType + + SUBROUTINE MD_UnPackOutParmType( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_OutParmType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackOutParmType' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + DO I = 1, LEN(OutData%Name) + OutData%Name(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + DO I = 1, LEN(OutData%Units) + OutData%Units(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + OutData%QType = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%OType = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%NodeID = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%ObjID = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END SUBROUTINE MD_UnPackOutParmType + + SUBROUTINE MD_CopyInitOutput( SrcInitOutputData, DstInitOutputData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_InitOutputType), INTENT(IN) :: SrcInitOutputData + TYPE(MD_InitOutputType), INTENT(INOUT) :: DstInitOutputData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyInitOutput' +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(SrcInitOutputData%writeOutputHdr)) THEN + i1_l = LBOUND(SrcInitOutputData%writeOutputHdr,1) + i1_u = UBOUND(SrcInitOutputData%writeOutputHdr,1) + IF (.NOT. ALLOCATED(DstInitOutputData%writeOutputHdr)) THEN + ALLOCATE(DstInitOutputData%writeOutputHdr(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitOutputData%writeOutputHdr.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitOutputData%writeOutputHdr = SrcInitOutputData%writeOutputHdr +ENDIF +IF (ALLOCATED(SrcInitOutputData%writeOutputUnt)) THEN + i1_l = LBOUND(SrcInitOutputData%writeOutputUnt,1) + i1_u = UBOUND(SrcInitOutputData%writeOutputUnt,1) + IF (.NOT. ALLOCATED(DstInitOutputData%writeOutputUnt)) THEN + ALLOCATE(DstInitOutputData%writeOutputUnt(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitOutputData%writeOutputUnt.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitOutputData%writeOutputUnt = SrcInitOutputData%writeOutputUnt +ENDIF + CALL NWTC_Library_Copyprogdesc( SrcInitOutputData%Ver, DstInitOutputData%Ver, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + END SUBROUTINE MD_CopyInitOutput + + SUBROUTINE MD_DestroyInitOutput( InitOutputData, ErrStat, ErrMsg ) + TYPE(MD_InitOutputType), INTENT(INOUT) :: InitOutputData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyInitOutput' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(InitOutputData%writeOutputHdr)) THEN + DEALLOCATE(InitOutputData%writeOutputHdr) +ENDIF +IF (ALLOCATED(InitOutputData%writeOutputUnt)) THEN + DEALLOCATE(InitOutputData%writeOutputUnt) +ENDIF + CALL NWTC_Library_Destroyprogdesc( InitOutputData%Ver, ErrStat, ErrMsg ) + END SUBROUTINE MD_DestroyInitOutput + + SUBROUTINE MD_PackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(MD_InitOutputType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackInitOutput' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! writeOutputHdr allocated yes/no + IF ( ALLOCATED(InData%writeOutputHdr) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! writeOutputHdr upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%writeOutputHdr)*LEN(InData%writeOutputHdr) ! writeOutputHdr + END IF + Int_BufSz = Int_BufSz + 1 ! writeOutputUnt allocated yes/no + IF ( ALLOCATED(InData%writeOutputUnt) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! writeOutputUnt upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%writeOutputUnt)*LEN(InData%writeOutputUnt) ! writeOutputUnt + END IF + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + Int_BufSz = Int_BufSz + 3 ! Ver: size of buffers for each call to pack subtype + CALL NWTC_Library_Packprogdesc( Re_Buf, Db_Buf, Int_Buf, InData%Ver, ErrStat2, ErrMsg2, .TRUE. ) ! Ver + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! Ver + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! Ver + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! Ver + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IF ( .NOT. ALLOCATED(InData%writeOutputHdr) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%writeOutputHdr,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%writeOutputHdr,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%writeOutputHdr,1), UBOUND(InData%writeOutputHdr,1) + DO I = 1, LEN(InData%writeOutputHdr) + IntKiBuf(Int_Xferred) = ICHAR(InData%writeOutputHdr(i1)(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + END DO + END IF + IF ( .NOT. ALLOCATED(InData%writeOutputUnt) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%writeOutputUnt,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%writeOutputUnt,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%writeOutputUnt,1), UBOUND(InData%writeOutputUnt,1) + DO I = 1, LEN(InData%writeOutputUnt) + IntKiBuf(Int_Xferred) = ICHAR(InData%writeOutputUnt(i1)(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + END DO + END IF + CALL NWTC_Library_Packprogdesc( Re_Buf, Db_Buf, Int_Buf, InData%Ver, ErrStat2, ErrMsg2, OnlySize ) ! Ver + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END SUBROUTINE MD_PackInitOutput + + SUBROUTINE MD_UnPackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_InitOutputType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackInitOutput' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! writeOutputHdr not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%writeOutputHdr)) DEALLOCATE(OutData%writeOutputHdr) + ALLOCATE(OutData%writeOutputHdr(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%writeOutputHdr.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%writeOutputHdr,1), UBOUND(OutData%writeOutputHdr,1) + DO I = 1, LEN(OutData%writeOutputHdr) + OutData%writeOutputHdr(i1)(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! writeOutputUnt not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%writeOutputUnt)) DEALLOCATE(OutData%writeOutputUnt) + ALLOCATE(OutData%writeOutputUnt(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%writeOutputUnt.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%writeOutputUnt,1), UBOUND(OutData%writeOutputUnt,1) + DO I = 1, LEN(OutData%writeOutputUnt) + OutData%writeOutputUnt(i1)(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + END DO + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL NWTC_Library_Unpackprogdesc( Re_Buf, Db_Buf, Int_Buf, OutData%Ver, ErrStat2, ErrMsg2 ) ! Ver + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END SUBROUTINE MD_UnPackInitOutput + + SUBROUTINE MD_CopyContState( SrcContStateData, DstContStateData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_ContinuousStateType), INTENT(IN) :: SrcContStateData + TYPE(MD_ContinuousStateType), INTENT(INOUT) :: DstContStateData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyContState' +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(SrcContStateData%states)) THEN + i1_l = LBOUND(SrcContStateData%states,1) + i1_u = UBOUND(SrcContStateData%states,1) + IF (.NOT. ALLOCATED(DstContStateData%states)) THEN + ALLOCATE(DstContStateData%states(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstContStateData%states.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstContStateData%states = SrcContStateData%states +ENDIF + END SUBROUTINE MD_CopyContState + + SUBROUTINE MD_DestroyContState( ContStateData, ErrStat, ErrMsg ) + TYPE(MD_ContinuousStateType), INTENT(INOUT) :: ContStateData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyContState' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(ContStateData%states)) THEN + DEALLOCATE(ContStateData%states) +ENDIF + END SUBROUTINE MD_DestroyContState + + SUBROUTINE MD_PackContState( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(MD_ContinuousStateType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackContState' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! states allocated yes/no + IF ( ALLOCATED(InData%states) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! states upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%states) ! states + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IF ( .NOT. ALLOCATED(InData%states) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%states,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%states,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%states,1), UBOUND(InData%states,1) + DbKiBuf(Db_Xferred) = InData%states(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + END SUBROUTINE MD_PackContState + + SUBROUTINE MD_UnPackContState( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_ContinuousStateType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackContState' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! states not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%states)) DEALLOCATE(OutData%states) + ALLOCATE(OutData%states(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%states.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%states,1), UBOUND(OutData%states,1) + OutData%states(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + END SUBROUTINE MD_UnPackContState + + SUBROUTINE MD_CopyDiscState( SrcDiscStateData, DstDiscStateData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_DiscreteStateType), INTENT(IN) :: SrcDiscStateData + TYPE(MD_DiscreteStateType), INTENT(INOUT) :: DstDiscStateData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyDiscState' +! + ErrStat = ErrID_None + ErrMsg = "" + DstDiscStateData%dummy = SrcDiscStateData%dummy + END SUBROUTINE MD_CopyDiscState + + SUBROUTINE MD_DestroyDiscState( DiscStateData, ErrStat, ErrMsg ) + TYPE(MD_DiscreteStateType), INTENT(INOUT) :: DiscStateData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyDiscState' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE MD_DestroyDiscState + + SUBROUTINE MD_PackDiscState( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(MD_DiscreteStateType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackDiscState' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Re_BufSz = Re_BufSz + 1 ! dummy + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + ReKiBuf(Re_Xferred) = InData%dummy + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE MD_PackDiscState + + SUBROUTINE MD_UnPackDiscState( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_DiscreteStateType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackDiscState' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%dummy = REAL(ReKiBuf(Re_Xferred), SiKi) + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE MD_UnPackDiscState + + SUBROUTINE MD_CopyConstrState( SrcConstrStateData, DstConstrStateData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_ConstraintStateType), INTENT(IN) :: SrcConstrStateData + TYPE(MD_ConstraintStateType), INTENT(INOUT) :: DstConstrStateData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyConstrState' +! + ErrStat = ErrID_None + ErrMsg = "" + DstConstrStateData%dummy = SrcConstrStateData%dummy + END SUBROUTINE MD_CopyConstrState + + SUBROUTINE MD_DestroyConstrState( ConstrStateData, ErrStat, ErrMsg ) + TYPE(MD_ConstraintStateType), INTENT(INOUT) :: ConstrStateData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyConstrState' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE MD_DestroyConstrState + + SUBROUTINE MD_PackConstrState( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(MD_ConstraintStateType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackConstrState' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Re_BufSz = Re_BufSz + 1 ! dummy + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + ReKiBuf(Re_Xferred) = InData%dummy + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE MD_PackConstrState + + SUBROUTINE MD_UnPackConstrState( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_ConstraintStateType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackConstrState' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%dummy = REAL(ReKiBuf(Re_Xferred), SiKi) + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE MD_UnPackConstrState + + SUBROUTINE MD_CopyOtherState( SrcOtherStateData, DstOtherStateData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_OtherStateType), INTENT(IN) :: SrcOtherStateData + TYPE(MD_OtherStateType), INTENT(INOUT) :: DstOtherStateData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyOtherState' +! + ErrStat = ErrID_None + ErrMsg = "" + DstOtherStateData%dummy = SrcOtherStateData%dummy + END SUBROUTINE MD_CopyOtherState + + SUBROUTINE MD_DestroyOtherState( OtherStateData, ErrStat, ErrMsg ) + TYPE(MD_OtherStateType), INTENT(INOUT) :: OtherStateData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyOtherState' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE MD_DestroyOtherState + + SUBROUTINE MD_PackOtherState( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(MD_OtherStateType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackOtherState' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Re_BufSz = Re_BufSz + 1 ! dummy + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + ReKiBuf(Re_Xferred) = InData%dummy + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE MD_PackOtherState + + SUBROUTINE MD_UnPackOtherState( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_OtherStateType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackOtherState' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%dummy = REAL(ReKiBuf(Re_Xferred), SiKi) + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE MD_UnPackOtherState + + SUBROUTINE MD_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_MiscVarType), INTENT(IN) :: SrcMiscData + TYPE(MD_MiscVarType), INTENT(INOUT) :: DstMiscData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyMisc' +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(SrcMiscData%LineTypeList)) THEN + i1_l = LBOUND(SrcMiscData%LineTypeList,1) + i1_u = UBOUND(SrcMiscData%LineTypeList,1) + IF (.NOT. ALLOCATED(DstMiscData%LineTypeList)) THEN + ALLOCATE(DstMiscData%LineTypeList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%LineTypeList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcMiscData%LineTypeList,1), UBOUND(SrcMiscData%LineTypeList,1) + CALL MD_Copylineprop( SrcMiscData%LineTypeList(i1), DstMiscData%LineTypeList(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF +IF (ALLOCATED(SrcMiscData%RodTypeList)) THEN + i1_l = LBOUND(SrcMiscData%RodTypeList,1) + i1_u = UBOUND(SrcMiscData%RodTypeList,1) + IF (.NOT. ALLOCATED(DstMiscData%RodTypeList)) THEN + ALLOCATE(DstMiscData%RodTypeList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%RodTypeList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcMiscData%RodTypeList,1), UBOUND(SrcMiscData%RodTypeList,1) + CALL MD_Copyrodprop( SrcMiscData%RodTypeList(i1), DstMiscData%RodTypeList(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF + CALL MD_Copybody( SrcMiscData%GroundBody, DstMiscData%GroundBody, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN +IF (ALLOCATED(SrcMiscData%BodyList)) THEN + i1_l = LBOUND(SrcMiscData%BodyList,1) + i1_u = UBOUND(SrcMiscData%BodyList,1) + IF (.NOT. ALLOCATED(DstMiscData%BodyList)) THEN + ALLOCATE(DstMiscData%BodyList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%BodyList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcMiscData%BodyList,1), UBOUND(SrcMiscData%BodyList,1) + CALL MD_Copybody( SrcMiscData%BodyList(i1), DstMiscData%BodyList(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF +IF (ALLOCATED(SrcMiscData%RodList)) THEN + i1_l = LBOUND(SrcMiscData%RodList,1) + i1_u = UBOUND(SrcMiscData%RodList,1) + IF (.NOT. ALLOCATED(DstMiscData%RodList)) THEN + ALLOCATE(DstMiscData%RodList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%RodList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcMiscData%RodList,1), UBOUND(SrcMiscData%RodList,1) + CALL MD_Copyrod( SrcMiscData%RodList(i1), DstMiscData%RodList(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF +IF (ALLOCATED(SrcMiscData%ConnectList)) THEN + i1_l = LBOUND(SrcMiscData%ConnectList,1) + i1_u = UBOUND(SrcMiscData%ConnectList,1) + IF (.NOT. ALLOCATED(DstMiscData%ConnectList)) THEN + ALLOCATE(DstMiscData%ConnectList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%ConnectList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcMiscData%ConnectList,1), UBOUND(SrcMiscData%ConnectList,1) + CALL MD_Copyconnect( SrcMiscData%ConnectList(i1), DstMiscData%ConnectList(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF +IF (ALLOCATED(SrcMiscData%LineList)) THEN + i1_l = LBOUND(SrcMiscData%LineList,1) + i1_u = UBOUND(SrcMiscData%LineList,1) + IF (.NOT. ALLOCATED(DstMiscData%LineList)) THEN + ALLOCATE(DstMiscData%LineList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%LineList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcMiscData%LineList,1), UBOUND(SrcMiscData%LineList,1) + CALL MD_Copyline( SrcMiscData%LineList(i1), DstMiscData%LineList(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF +IF (ALLOCATED(SrcMiscData%FailList)) THEN + i1_l = LBOUND(SrcMiscData%FailList,1) + i1_u = UBOUND(SrcMiscData%FailList,1) + IF (.NOT. ALLOCATED(DstMiscData%FailList)) THEN + ALLOCATE(DstMiscData%FailList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%FailList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcMiscData%FailList,1), UBOUND(SrcMiscData%FailList,1) + CALL MD_Copyfail( SrcMiscData%FailList(i1), DstMiscData%FailList(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF +IF (ALLOCATED(SrcMiscData%FreeConIs)) THEN + i1_l = LBOUND(SrcMiscData%FreeConIs,1) + i1_u = UBOUND(SrcMiscData%FreeConIs,1) + IF (.NOT. ALLOCATED(DstMiscData%FreeConIs)) THEN + ALLOCATE(DstMiscData%FreeConIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%FreeConIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%FreeConIs = SrcMiscData%FreeConIs +ENDIF +IF (ALLOCATED(SrcMiscData%CpldConIs)) THEN + i1_l = LBOUND(SrcMiscData%CpldConIs,1) + i1_u = UBOUND(SrcMiscData%CpldConIs,1) + IF (.NOT. ALLOCATED(DstMiscData%CpldConIs)) THEN + ALLOCATE(DstMiscData%CpldConIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%CpldConIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%CpldConIs = SrcMiscData%CpldConIs +ENDIF +IF (ALLOCATED(SrcMiscData%FreeRodIs)) THEN + i1_l = LBOUND(SrcMiscData%FreeRodIs,1) + i1_u = UBOUND(SrcMiscData%FreeRodIs,1) + IF (.NOT. ALLOCATED(DstMiscData%FreeRodIs)) THEN + ALLOCATE(DstMiscData%FreeRodIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%FreeRodIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%FreeRodIs = SrcMiscData%FreeRodIs +ENDIF +IF (ALLOCATED(SrcMiscData%CpldRodIs)) THEN + i1_l = LBOUND(SrcMiscData%CpldRodIs,1) + i1_u = UBOUND(SrcMiscData%CpldRodIs,1) + IF (.NOT. ALLOCATED(DstMiscData%CpldRodIs)) THEN + ALLOCATE(DstMiscData%CpldRodIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%CpldRodIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%CpldRodIs = SrcMiscData%CpldRodIs +ENDIF +IF (ALLOCATED(SrcMiscData%FreeBodyIs)) THEN + i1_l = LBOUND(SrcMiscData%FreeBodyIs,1) + i1_u = UBOUND(SrcMiscData%FreeBodyIs,1) + IF (.NOT. ALLOCATED(DstMiscData%FreeBodyIs)) THEN + ALLOCATE(DstMiscData%FreeBodyIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%FreeBodyIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%FreeBodyIs = SrcMiscData%FreeBodyIs +ENDIF +IF (ALLOCATED(SrcMiscData%CpldBodyIs)) THEN + i1_l = LBOUND(SrcMiscData%CpldBodyIs,1) + i1_u = UBOUND(SrcMiscData%CpldBodyIs,1) + IF (.NOT. ALLOCATED(DstMiscData%CpldBodyIs)) THEN + ALLOCATE(DstMiscData%CpldBodyIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%CpldBodyIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%CpldBodyIs = SrcMiscData%CpldBodyIs +ENDIF +IF (ALLOCATED(SrcMiscData%LineStateIs1)) THEN + i1_l = LBOUND(SrcMiscData%LineStateIs1,1) + i1_u = UBOUND(SrcMiscData%LineStateIs1,1) + IF (.NOT. ALLOCATED(DstMiscData%LineStateIs1)) THEN + ALLOCATE(DstMiscData%LineStateIs1(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%LineStateIs1.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%LineStateIs1 = SrcMiscData%LineStateIs1 +ENDIF +IF (ALLOCATED(SrcMiscData%LineStateIsN)) THEN + i1_l = LBOUND(SrcMiscData%LineStateIsN,1) + i1_u = UBOUND(SrcMiscData%LineStateIsN,1) + IF (.NOT. ALLOCATED(DstMiscData%LineStateIsN)) THEN + ALLOCATE(DstMiscData%LineStateIsN(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%LineStateIsN.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%LineStateIsN = SrcMiscData%LineStateIsN +ENDIF +IF (ALLOCATED(SrcMiscData%ConStateIs1)) THEN + i1_l = LBOUND(SrcMiscData%ConStateIs1,1) + i1_u = UBOUND(SrcMiscData%ConStateIs1,1) + IF (.NOT. ALLOCATED(DstMiscData%ConStateIs1)) THEN + ALLOCATE(DstMiscData%ConStateIs1(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%ConStateIs1.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%ConStateIs1 = SrcMiscData%ConStateIs1 +ENDIF +IF (ALLOCATED(SrcMiscData%ConStateIsN)) THEN + i1_l = LBOUND(SrcMiscData%ConStateIsN,1) + i1_u = UBOUND(SrcMiscData%ConStateIsN,1) + IF (.NOT. ALLOCATED(DstMiscData%ConStateIsN)) THEN + ALLOCATE(DstMiscData%ConStateIsN(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%ConStateIsN.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%ConStateIsN = SrcMiscData%ConStateIsN +ENDIF +IF (ALLOCATED(SrcMiscData%RodStateIs1)) THEN + i1_l = LBOUND(SrcMiscData%RodStateIs1,1) + i1_u = UBOUND(SrcMiscData%RodStateIs1,1) + IF (.NOT. ALLOCATED(DstMiscData%RodStateIs1)) THEN + ALLOCATE(DstMiscData%RodStateIs1(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%RodStateIs1.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%RodStateIs1 = SrcMiscData%RodStateIs1 +ENDIF +IF (ALLOCATED(SrcMiscData%RodStateIsN)) THEN + i1_l = LBOUND(SrcMiscData%RodStateIsN,1) + i1_u = UBOUND(SrcMiscData%RodStateIsN,1) + IF (.NOT. ALLOCATED(DstMiscData%RodStateIsN)) THEN + ALLOCATE(DstMiscData%RodStateIsN(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%RodStateIsN.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%RodStateIsN = SrcMiscData%RodStateIsN +ENDIF +IF (ALLOCATED(SrcMiscData%BodyStateIs1)) THEN + i1_l = LBOUND(SrcMiscData%BodyStateIs1,1) + i1_u = UBOUND(SrcMiscData%BodyStateIs1,1) + IF (.NOT. ALLOCATED(DstMiscData%BodyStateIs1)) THEN + ALLOCATE(DstMiscData%BodyStateIs1(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%BodyStateIs1.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%BodyStateIs1 = SrcMiscData%BodyStateIs1 +ENDIF +IF (ALLOCATED(SrcMiscData%BodyStateIsN)) THEN + i1_l = LBOUND(SrcMiscData%BodyStateIsN,1) + i1_u = UBOUND(SrcMiscData%BodyStateIsN,1) + IF (.NOT. ALLOCATED(DstMiscData%BodyStateIsN)) THEN + ALLOCATE(DstMiscData%BodyStateIsN(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%BodyStateIsN.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%BodyStateIsN = SrcMiscData%BodyStateIsN +ENDIF + DstMiscData%Nx = SrcMiscData%Nx + CALL MD_CopyContState( SrcMiscData%xTemp, DstMiscData%xTemp, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + CALL MD_CopyContState( SrcMiscData%xdTemp, DstMiscData%xdTemp, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + DstMiscData%zeros6 = SrcMiscData%zeros6 +IF (ALLOCATED(SrcMiscData%MDWrOutput)) THEN + i1_l = LBOUND(SrcMiscData%MDWrOutput,1) + i1_u = UBOUND(SrcMiscData%MDWrOutput,1) + IF (.NOT. ALLOCATED(DstMiscData%MDWrOutput)) THEN + ALLOCATE(DstMiscData%MDWrOutput(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%MDWrOutput.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%MDWrOutput = SrcMiscData%MDWrOutput +ENDIF + END SUBROUTINE MD_CopyMisc + + SUBROUTINE MD_DestroyMisc( MiscData, ErrStat, ErrMsg ) + TYPE(MD_MiscVarType), INTENT(INOUT) :: MiscData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyMisc' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(MiscData%LineTypeList)) THEN +DO i1 = LBOUND(MiscData%LineTypeList,1), UBOUND(MiscData%LineTypeList,1) + CALL MD_Destroylineprop( MiscData%LineTypeList(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(MiscData%LineTypeList) +ENDIF +IF (ALLOCATED(MiscData%RodTypeList)) THEN +DO i1 = LBOUND(MiscData%RodTypeList,1), UBOUND(MiscData%RodTypeList,1) + CALL MD_Destroyrodprop( MiscData%RodTypeList(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(MiscData%RodTypeList) +ENDIF + CALL MD_Destroybody( MiscData%GroundBody, ErrStat, ErrMsg ) +IF (ALLOCATED(MiscData%BodyList)) THEN +DO i1 = LBOUND(MiscData%BodyList,1), UBOUND(MiscData%BodyList,1) + CALL MD_Destroybody( MiscData%BodyList(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(MiscData%BodyList) +ENDIF +IF (ALLOCATED(MiscData%RodList)) THEN +DO i1 = LBOUND(MiscData%RodList,1), UBOUND(MiscData%RodList,1) + CALL MD_Destroyrod( MiscData%RodList(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(MiscData%RodList) +ENDIF +IF (ALLOCATED(MiscData%ConnectList)) THEN +DO i1 = LBOUND(MiscData%ConnectList,1), UBOUND(MiscData%ConnectList,1) + CALL MD_Destroyconnect( MiscData%ConnectList(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(MiscData%ConnectList) +ENDIF +IF (ALLOCATED(MiscData%LineList)) THEN +DO i1 = LBOUND(MiscData%LineList,1), UBOUND(MiscData%LineList,1) + CALL MD_Destroyline( MiscData%LineList(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(MiscData%LineList) +ENDIF +IF (ALLOCATED(MiscData%FailList)) THEN +DO i1 = LBOUND(MiscData%FailList,1), UBOUND(MiscData%FailList,1) + CALL MD_Destroyfail( MiscData%FailList(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(MiscData%FailList) +ENDIF +IF (ALLOCATED(MiscData%FreeConIs)) THEN + DEALLOCATE(MiscData%FreeConIs) +ENDIF +IF (ALLOCATED(MiscData%CpldConIs)) THEN + DEALLOCATE(MiscData%CpldConIs) +ENDIF +IF (ALLOCATED(MiscData%FreeRodIs)) THEN + DEALLOCATE(MiscData%FreeRodIs) +ENDIF +IF (ALLOCATED(MiscData%CpldRodIs)) THEN + DEALLOCATE(MiscData%CpldRodIs) +ENDIF +IF (ALLOCATED(MiscData%FreeBodyIs)) THEN + DEALLOCATE(MiscData%FreeBodyIs) +ENDIF +IF (ALLOCATED(MiscData%CpldBodyIs)) THEN + DEALLOCATE(MiscData%CpldBodyIs) +ENDIF +IF (ALLOCATED(MiscData%LineStateIs1)) THEN + DEALLOCATE(MiscData%LineStateIs1) +ENDIF +IF (ALLOCATED(MiscData%LineStateIsN)) THEN + DEALLOCATE(MiscData%LineStateIsN) +ENDIF +IF (ALLOCATED(MiscData%ConStateIs1)) THEN + DEALLOCATE(MiscData%ConStateIs1) +ENDIF +IF (ALLOCATED(MiscData%ConStateIsN)) THEN + DEALLOCATE(MiscData%ConStateIsN) +ENDIF +IF (ALLOCATED(MiscData%RodStateIs1)) THEN + DEALLOCATE(MiscData%RodStateIs1) +ENDIF +IF (ALLOCATED(MiscData%RodStateIsN)) THEN + DEALLOCATE(MiscData%RodStateIsN) +ENDIF +IF (ALLOCATED(MiscData%BodyStateIs1)) THEN + DEALLOCATE(MiscData%BodyStateIs1) +ENDIF +IF (ALLOCATED(MiscData%BodyStateIsN)) THEN + DEALLOCATE(MiscData%BodyStateIsN) +ENDIF + CALL MD_DestroyContState( MiscData%xTemp, ErrStat, ErrMsg ) + CALL MD_DestroyContState( MiscData%xdTemp, ErrStat, ErrMsg ) +IF (ALLOCATED(MiscData%MDWrOutput)) THEN + DEALLOCATE(MiscData%MDWrOutput) +ENDIF + END SUBROUTINE MD_DestroyMisc + + SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(MD_MiscVarType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackMisc' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! LineTypeList allocated yes/no + IF ( ALLOCATED(InData%LineTypeList) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! LineTypeList upper/lower bounds for each dimension + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + DO i1 = LBOUND(InData%LineTypeList,1), UBOUND(InData%LineTypeList,1) + Int_BufSz = Int_BufSz + 3 ! LineTypeList: size of buffers for each call to pack subtype + CALL MD_Packlineprop( Re_Buf, Db_Buf, Int_Buf, InData%LineTypeList(i1), ErrStat2, ErrMsg2, .TRUE. ) ! LineTypeList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! LineTypeList + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! LineTypeList + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! LineTypeList + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 1 ! RodTypeList allocated yes/no + IF ( ALLOCATED(InData%RodTypeList) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! RodTypeList upper/lower bounds for each dimension + DO i1 = LBOUND(InData%RodTypeList,1), UBOUND(InData%RodTypeList,1) + Int_BufSz = Int_BufSz + 3 ! RodTypeList: size of buffers for each call to pack subtype + CALL MD_Packrodprop( Re_Buf, Db_Buf, Int_Buf, InData%RodTypeList(i1), ErrStat2, ErrMsg2, .TRUE. ) ! RodTypeList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! RodTypeList + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! RodTypeList + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! RodTypeList + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 3 ! GroundBody: size of buffers for each call to pack subtype + CALL MD_Packbody( Re_Buf, Db_Buf, Int_Buf, InData%GroundBody, ErrStat2, ErrMsg2, .TRUE. ) ! GroundBody + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! GroundBody + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! GroundBody + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! GroundBody + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + Int_BufSz = Int_BufSz + 1 ! BodyList allocated yes/no + IF ( ALLOCATED(InData%BodyList) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! BodyList upper/lower bounds for each dimension + DO i1 = LBOUND(InData%BodyList,1), UBOUND(InData%BodyList,1) + Int_BufSz = Int_BufSz + 3 ! BodyList: size of buffers for each call to pack subtype + CALL MD_Packbody( Re_Buf, Db_Buf, Int_Buf, InData%BodyList(i1), ErrStat2, ErrMsg2, .TRUE. ) ! BodyList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! BodyList + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! BodyList + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! BodyList + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 1 ! RodList allocated yes/no + IF ( ALLOCATED(InData%RodList) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! RodList upper/lower bounds for each dimension + DO i1 = LBOUND(InData%RodList,1), UBOUND(InData%RodList,1) + Int_BufSz = Int_BufSz + 3 ! RodList: size of buffers for each call to pack subtype + CALL MD_Packrod( Re_Buf, Db_Buf, Int_Buf, InData%RodList(i1), ErrStat2, ErrMsg2, .TRUE. ) ! RodList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! RodList + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! RodList + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! RodList + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 1 ! ConnectList allocated yes/no + IF ( ALLOCATED(InData%ConnectList) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! ConnectList upper/lower bounds for each dimension + DO i1 = LBOUND(InData%ConnectList,1), UBOUND(InData%ConnectList,1) + Int_BufSz = Int_BufSz + 3 ! ConnectList: size of buffers for each call to pack subtype + CALL MD_Packconnect( Re_Buf, Db_Buf, Int_Buf, InData%ConnectList(i1), ErrStat2, ErrMsg2, .TRUE. ) ! ConnectList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! ConnectList + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! ConnectList + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! ConnectList + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 1 ! LineList allocated yes/no + IF ( ALLOCATED(InData%LineList) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! LineList upper/lower bounds for each dimension + DO i1 = LBOUND(InData%LineList,1), UBOUND(InData%LineList,1) + Int_BufSz = Int_BufSz + 3 ! LineList: size of buffers for each call to pack subtype + CALL MD_Packline( Re_Buf, Db_Buf, Int_Buf, InData%LineList(i1), ErrStat2, ErrMsg2, .TRUE. ) ! LineList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! LineList + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! LineList + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! LineList + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 1 ! FailList allocated yes/no + IF ( ALLOCATED(InData%FailList) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! FailList upper/lower bounds for each dimension + DO i1 = LBOUND(InData%FailList,1), UBOUND(InData%FailList,1) + Int_BufSz = Int_BufSz + 3 ! FailList: size of buffers for each call to pack subtype + CALL MD_Packfail( Re_Buf, Db_Buf, Int_Buf, InData%FailList(i1), ErrStat2, ErrMsg2, .TRUE. ) ! FailList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! FailList + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! FailList + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! FailList + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 1 ! FreeConIs allocated yes/no + IF ( ALLOCATED(InData%FreeConIs) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! FreeConIs upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%FreeConIs) ! FreeConIs + END IF + Int_BufSz = Int_BufSz + 1 ! CpldConIs allocated yes/no + IF ( ALLOCATED(InData%CpldConIs) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! CpldConIs upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%CpldConIs) ! CpldConIs + END IF + Int_BufSz = Int_BufSz + 1 ! FreeRodIs allocated yes/no + IF ( ALLOCATED(InData%FreeRodIs) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! FreeRodIs upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%FreeRodIs) ! FreeRodIs + END IF + Int_BufSz = Int_BufSz + 1 ! CpldRodIs allocated yes/no + IF ( ALLOCATED(InData%CpldRodIs) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! CpldRodIs upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%CpldRodIs) ! CpldRodIs + END IF + Int_BufSz = Int_BufSz + 1 ! FreeBodyIs allocated yes/no + IF ( ALLOCATED(InData%FreeBodyIs) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! FreeBodyIs upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%FreeBodyIs) ! FreeBodyIs + END IF + Int_BufSz = Int_BufSz + 1 ! CpldBodyIs allocated yes/no + IF ( ALLOCATED(InData%CpldBodyIs) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! CpldBodyIs upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%CpldBodyIs) ! CpldBodyIs + END IF + Int_BufSz = Int_BufSz + 1 ! LineStateIs1 allocated yes/no + IF ( ALLOCATED(InData%LineStateIs1) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! LineStateIs1 upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%LineStateIs1) ! LineStateIs1 + END IF + Int_BufSz = Int_BufSz + 1 ! LineStateIsN allocated yes/no + IF ( ALLOCATED(InData%LineStateIsN) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! LineStateIsN upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%LineStateIsN) ! LineStateIsN + END IF + Int_BufSz = Int_BufSz + 1 ! ConStateIs1 allocated yes/no + IF ( ALLOCATED(InData%ConStateIs1) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! ConStateIs1 upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%ConStateIs1) ! ConStateIs1 + END IF + Int_BufSz = Int_BufSz + 1 ! ConStateIsN allocated yes/no + IF ( ALLOCATED(InData%ConStateIsN) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! ConStateIsN upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%ConStateIsN) ! ConStateIsN + END IF + Int_BufSz = Int_BufSz + 1 ! RodStateIs1 allocated yes/no + IF ( ALLOCATED(InData%RodStateIs1) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! RodStateIs1 upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%RodStateIs1) ! RodStateIs1 + END IF + Int_BufSz = Int_BufSz + 1 ! RodStateIsN allocated yes/no + IF ( ALLOCATED(InData%RodStateIsN) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! RodStateIsN upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%RodStateIsN) ! RodStateIsN + END IF + Int_BufSz = Int_BufSz + 1 ! BodyStateIs1 allocated yes/no + IF ( ALLOCATED(InData%BodyStateIs1) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! BodyStateIs1 upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%BodyStateIs1) ! BodyStateIs1 + END IF + Int_BufSz = Int_BufSz + 1 ! BodyStateIsN allocated yes/no + IF ( ALLOCATED(InData%BodyStateIsN) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! BodyStateIsN upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%BodyStateIsN) ! BodyStateIsN + END IF + Int_BufSz = Int_BufSz + 1 ! Nx + Int_BufSz = Int_BufSz + 3 ! xTemp: size of buffers for each call to pack subtype + CALL MD_PackContState( Re_Buf, Db_Buf, Int_Buf, InData%xTemp, ErrStat2, ErrMsg2, .TRUE. ) ! xTemp + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! xTemp + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! xTemp + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! xTemp + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + Int_BufSz = Int_BufSz + 3 ! xdTemp: size of buffers for each call to pack subtype + CALL MD_PackContState( Re_Buf, Db_Buf, Int_Buf, InData%xdTemp, ErrStat2, ErrMsg2, .TRUE. ) ! xdTemp + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! xdTemp + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! xdTemp + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! xdTemp + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + Db_BufSz = Db_BufSz + SIZE(InData%zeros6) ! zeros6 + Int_BufSz = Int_BufSz + 1 ! MDWrOutput allocated yes/no + IF ( ALLOCATED(InData%MDWrOutput) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! MDWrOutput upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%MDWrOutput) ! MDWrOutput + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IF ( .NOT. ALLOCATED(InData%LineTypeList) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%LineTypeList,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%LineTypeList,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%LineTypeList,1), UBOUND(InData%LineTypeList,1) + CALL MD_Packlineprop( Re_Buf, Db_Buf, Int_Buf, InData%LineTypeList(i1), ErrStat2, ErrMsg2, OnlySize ) ! LineTypeList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF + IF ( .NOT. ALLOCATED(InData%RodTypeList) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%RodTypeList,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%RodTypeList,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%RodTypeList,1), UBOUND(InData%RodTypeList,1) + CALL MD_Packrodprop( Re_Buf, Db_Buf, Int_Buf, InData%RodTypeList(i1), ErrStat2, ErrMsg2, OnlySize ) ! RodTypeList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF + CALL MD_Packbody( Re_Buf, Db_Buf, Int_Buf, InData%GroundBody, ErrStat2, ErrMsg2, OnlySize ) ! GroundBody + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF ( .NOT. ALLOCATED(InData%BodyList) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%BodyList,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%BodyList,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%BodyList,1), UBOUND(InData%BodyList,1) + CALL MD_Packbody( Re_Buf, Db_Buf, Int_Buf, InData%BodyList(i1), ErrStat2, ErrMsg2, OnlySize ) ! BodyList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF + IF ( .NOT. ALLOCATED(InData%RodList) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%RodList,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%RodList,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%RodList,1), UBOUND(InData%RodList,1) + CALL MD_Packrod( Re_Buf, Db_Buf, Int_Buf, InData%RodList(i1), ErrStat2, ErrMsg2, OnlySize ) ! RodList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF + IF ( .NOT. ALLOCATED(InData%ConnectList) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ConnectList,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ConnectList,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%ConnectList,1), UBOUND(InData%ConnectList,1) + CALL MD_Packconnect( Re_Buf, Db_Buf, Int_Buf, InData%ConnectList(i1), ErrStat2, ErrMsg2, OnlySize ) ! ConnectList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF + IF ( .NOT. ALLOCATED(InData%LineList) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%LineList,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%LineList,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%LineList,1), UBOUND(InData%LineList,1) + CALL MD_Packline( Re_Buf, Db_Buf, Int_Buf, InData%LineList(i1), ErrStat2, ErrMsg2, OnlySize ) ! LineList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF + IF ( .NOT. ALLOCATED(InData%FailList) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%FailList,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%FailList,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%FailList,1), UBOUND(InData%FailList,1) + CALL MD_Packfail( Re_Buf, Db_Buf, Int_Buf, InData%FailList(i1), ErrStat2, ErrMsg2, OnlySize ) ! FailList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF + IF ( .NOT. ALLOCATED(InData%FreeConIs) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%FreeConIs,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%FreeConIs,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%FreeConIs,1), UBOUND(InData%FreeConIs,1) + IntKiBuf(Int_Xferred) = InData%FreeConIs(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%CpldConIs) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%CpldConIs,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%CpldConIs,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%CpldConIs,1), UBOUND(InData%CpldConIs,1) + IntKiBuf(Int_Xferred) = InData%CpldConIs(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%FreeRodIs) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%FreeRodIs,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%FreeRodIs,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%FreeRodIs,1), UBOUND(InData%FreeRodIs,1) + IntKiBuf(Int_Xferred) = InData%FreeRodIs(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%CpldRodIs) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%CpldRodIs,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%CpldRodIs,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%CpldRodIs,1), UBOUND(InData%CpldRodIs,1) + IntKiBuf(Int_Xferred) = InData%CpldRodIs(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%FreeBodyIs) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%FreeBodyIs,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%FreeBodyIs,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%FreeBodyIs,1), UBOUND(InData%FreeBodyIs,1) + IntKiBuf(Int_Xferred) = InData%FreeBodyIs(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%CpldBodyIs) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%CpldBodyIs,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%CpldBodyIs,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%CpldBodyIs,1), UBOUND(InData%CpldBodyIs,1) + IntKiBuf(Int_Xferred) = InData%CpldBodyIs(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%LineStateIs1) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%LineStateIs1,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%LineStateIs1,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%LineStateIs1,1), UBOUND(InData%LineStateIs1,1) + IntKiBuf(Int_Xferred) = InData%LineStateIs1(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%LineStateIsN) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%LineStateIsN,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%LineStateIsN,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%LineStateIsN,1), UBOUND(InData%LineStateIsN,1) + IntKiBuf(Int_Xferred) = InData%LineStateIsN(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%ConStateIs1) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ConStateIs1,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ConStateIs1,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%ConStateIs1,1), UBOUND(InData%ConStateIs1,1) + IntKiBuf(Int_Xferred) = InData%ConStateIs1(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%ConStateIsN) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ConStateIsN,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ConStateIsN,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%ConStateIsN,1), UBOUND(InData%ConStateIsN,1) + IntKiBuf(Int_Xferred) = InData%ConStateIsN(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%RodStateIs1) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%RodStateIs1,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%RodStateIs1,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%RodStateIs1,1), UBOUND(InData%RodStateIs1,1) + IntKiBuf(Int_Xferred) = InData%RodStateIs1(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%RodStateIsN) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%RodStateIsN,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%RodStateIsN,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%RodStateIsN,1), UBOUND(InData%RodStateIsN,1) + IntKiBuf(Int_Xferred) = InData%RodStateIsN(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%BodyStateIs1) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%BodyStateIs1,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%BodyStateIs1,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%BodyStateIs1,1), UBOUND(InData%BodyStateIs1,1) + IntKiBuf(Int_Xferred) = InData%BodyStateIs1(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%BodyStateIsN) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%BodyStateIsN,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%BodyStateIsN,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%BodyStateIsN,1), UBOUND(InData%BodyStateIsN,1) + IntKiBuf(Int_Xferred) = InData%BodyStateIsN(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IntKiBuf(Int_Xferred) = InData%Nx + Int_Xferred = Int_Xferred + 1 + CALL MD_PackContState( Re_Buf, Db_Buf, Int_Buf, InData%xTemp, ErrStat2, ErrMsg2, OnlySize ) ! xTemp + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + CALL MD_PackContState( Re_Buf, Db_Buf, Int_Buf, InData%xdTemp, ErrStat2, ErrMsg2, OnlySize ) ! xdTemp + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + DO i1 = LBOUND(InData%zeros6,1), UBOUND(InData%zeros6,1) + DbKiBuf(Db_Xferred) = InData%zeros6(i1) + Db_Xferred = Db_Xferred + 1 + END DO + IF ( .NOT. ALLOCATED(InData%MDWrOutput) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%MDWrOutput,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%MDWrOutput,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%MDWrOutput,1), UBOUND(InData%MDWrOutput,1) + DbKiBuf(Db_Xferred) = InData%MDWrOutput(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + END SUBROUTINE MD_PackMisc + + SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_MiscVarType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackMisc' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! LineTypeList not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%LineTypeList)) DEALLOCATE(OutData%LineTypeList) + ALLOCATE(OutData%LineTypeList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%LineTypeList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%LineTypeList,1), UBOUND(OutData%LineTypeList,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_Unpacklineprop( Re_Buf, Db_Buf, Int_Buf, OutData%LineTypeList(i1), ErrStat2, ErrMsg2 ) ! LineTypeList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! RodTypeList not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%RodTypeList)) DEALLOCATE(OutData%RodTypeList) + ALLOCATE(OutData%RodTypeList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%RodTypeList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%RodTypeList,1), UBOUND(OutData%RodTypeList,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_Unpackrodprop( Re_Buf, Db_Buf, Int_Buf, OutData%RodTypeList(i1), ErrStat2, ErrMsg2 ) ! RodTypeList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_Unpackbody( Re_Buf, Db_Buf, Int_Buf, OutData%GroundBody, ErrStat2, ErrMsg2 ) ! GroundBody + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! BodyList not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%BodyList)) DEALLOCATE(OutData%BodyList) + ALLOCATE(OutData%BodyList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%BodyList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%BodyList,1), UBOUND(OutData%BodyList,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_Unpackbody( Re_Buf, Db_Buf, Int_Buf, OutData%BodyList(i1), ErrStat2, ErrMsg2 ) ! BodyList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! RodList not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%RodList)) DEALLOCATE(OutData%RodList) + ALLOCATE(OutData%RodList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%RodList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%RodList,1), UBOUND(OutData%RodList,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_Unpackrod( Re_Buf, Db_Buf, Int_Buf, OutData%RodList(i1), ErrStat2, ErrMsg2 ) ! RodList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ConnectList not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%ConnectList)) DEALLOCATE(OutData%ConnectList) + ALLOCATE(OutData%ConnectList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ConnectList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%ConnectList,1), UBOUND(OutData%ConnectList,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_Unpackconnect( Re_Buf, Db_Buf, Int_Buf, OutData%ConnectList(i1), ErrStat2, ErrMsg2 ) ! ConnectList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! LineList not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%LineList)) DEALLOCATE(OutData%LineList) + ALLOCATE(OutData%LineList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%LineList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%LineList,1), UBOUND(OutData%LineList,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_Unpackline( Re_Buf, Db_Buf, Int_Buf, OutData%LineList(i1), ErrStat2, ErrMsg2 ) ! LineList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! FailList not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%FailList)) DEALLOCATE(OutData%FailList) + ALLOCATE(OutData%FailList(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%FailList.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%FailList,1), UBOUND(OutData%FailList,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_Unpackfail( Re_Buf, Db_Buf, Int_Buf, OutData%FailList(i1), ErrStat2, ErrMsg2 ) ! FailList + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! FreeConIs not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%FreeConIs)) DEALLOCATE(OutData%FreeConIs) + ALLOCATE(OutData%FreeConIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%FreeConIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%FreeConIs,1), UBOUND(OutData%FreeConIs,1) + OutData%FreeConIs(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! CpldConIs not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%CpldConIs)) DEALLOCATE(OutData%CpldConIs) + ALLOCATE(OutData%CpldConIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%CpldConIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%CpldConIs,1), UBOUND(OutData%CpldConIs,1) + OutData%CpldConIs(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! FreeRodIs not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%FreeRodIs)) DEALLOCATE(OutData%FreeRodIs) + ALLOCATE(OutData%FreeRodIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%FreeRodIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%FreeRodIs,1), UBOUND(OutData%FreeRodIs,1) + OutData%FreeRodIs(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! CpldRodIs not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%CpldRodIs)) DEALLOCATE(OutData%CpldRodIs) + ALLOCATE(OutData%CpldRodIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%CpldRodIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%CpldRodIs,1), UBOUND(OutData%CpldRodIs,1) + OutData%CpldRodIs(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! FreeBodyIs not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%FreeBodyIs)) DEALLOCATE(OutData%FreeBodyIs) + ALLOCATE(OutData%FreeBodyIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%FreeBodyIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%FreeBodyIs,1), UBOUND(OutData%FreeBodyIs,1) + OutData%FreeBodyIs(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! CpldBodyIs not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%CpldBodyIs)) DEALLOCATE(OutData%CpldBodyIs) + ALLOCATE(OutData%CpldBodyIs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%CpldBodyIs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%CpldBodyIs,1), UBOUND(OutData%CpldBodyIs,1) + OutData%CpldBodyIs(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! LineStateIs1 not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%LineStateIs1)) DEALLOCATE(OutData%LineStateIs1) + ALLOCATE(OutData%LineStateIs1(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%LineStateIs1.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%LineStateIs1,1), UBOUND(OutData%LineStateIs1,1) + OutData%LineStateIs1(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! LineStateIsN not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%LineStateIsN)) DEALLOCATE(OutData%LineStateIsN) + ALLOCATE(OutData%LineStateIsN(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%LineStateIsN.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%LineStateIsN,1), UBOUND(OutData%LineStateIsN,1) + OutData%LineStateIsN(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ConStateIs1 not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%ConStateIs1)) DEALLOCATE(OutData%ConStateIs1) + ALLOCATE(OutData%ConStateIs1(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ConStateIs1.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%ConStateIs1,1), UBOUND(OutData%ConStateIs1,1) + OutData%ConStateIs1(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ConStateIsN not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%ConStateIsN)) DEALLOCATE(OutData%ConStateIsN) + ALLOCATE(OutData%ConStateIsN(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ConStateIsN.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%ConStateIsN,1), UBOUND(OutData%ConStateIsN,1) + OutData%ConStateIsN(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! RodStateIs1 not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%RodStateIs1)) DEALLOCATE(OutData%RodStateIs1) + ALLOCATE(OutData%RodStateIs1(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%RodStateIs1.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%RodStateIs1,1), UBOUND(OutData%RodStateIs1,1) + OutData%RodStateIs1(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! RodStateIsN not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%RodStateIsN)) DEALLOCATE(OutData%RodStateIsN) + ALLOCATE(OutData%RodStateIsN(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%RodStateIsN.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%RodStateIsN,1), UBOUND(OutData%RodStateIsN,1) + OutData%RodStateIsN(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! BodyStateIs1 not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%BodyStateIs1)) DEALLOCATE(OutData%BodyStateIs1) + ALLOCATE(OutData%BodyStateIs1(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%BodyStateIs1.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%BodyStateIs1,1), UBOUND(OutData%BodyStateIs1,1) + OutData%BodyStateIs1(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! BodyStateIsN not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%BodyStateIsN)) DEALLOCATE(OutData%BodyStateIsN) + ALLOCATE(OutData%BodyStateIsN(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%BodyStateIsN.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%BodyStateIsN,1), UBOUND(OutData%BodyStateIsN,1) + OutData%BodyStateIsN(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + OutData%Nx = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_UnpackContState( Re_Buf, Db_Buf, Int_Buf, OutData%xTemp, ErrStat2, ErrMsg2 ) ! xTemp + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_UnpackContState( Re_Buf, Db_Buf, Int_Buf, OutData%xdTemp, ErrStat2, ErrMsg2 ) ! xdTemp + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + i1_l = LBOUND(OutData%zeros6,1) + i1_u = UBOUND(OutData%zeros6,1) + DO i1 = LBOUND(OutData%zeros6,1), UBOUND(OutData%zeros6,1) + OutData%zeros6(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! MDWrOutput not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%MDWrOutput)) DEALLOCATE(OutData%MDWrOutput) + ALLOCATE(OutData%MDWrOutput(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%MDWrOutput.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%MDWrOutput,1), UBOUND(OutData%MDWrOutput,1) + OutData%MDWrOutput(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + END SUBROUTINE MD_UnPackMisc + + SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_ParameterType), INTENT(IN) :: SrcParamData + TYPE(MD_ParameterType), INTENT(INOUT) :: DstParamData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: i4, i4_l, i4_u ! bounds (upper/lower) for an array dimension 4 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyParam' +! + ErrStat = ErrID_None + ErrMsg = "" + DstParamData%nLineTypes = SrcParamData%nLineTypes + DstParamData%nRodTypes = SrcParamData%nRodTypes + DstParamData%nConnects = SrcParamData%nConnects + DstParamData%nConnectsExtra = SrcParamData%nConnectsExtra + DstParamData%nBodies = SrcParamData%nBodies + DstParamData%nRods = SrcParamData%nRods + DstParamData%nLines = SrcParamData%nLines + DstParamData%nFails = SrcParamData%nFails + DstParamData%nFreeBodies = SrcParamData%nFreeBodies + DstParamData%nFreeRods = SrcParamData%nFreeRods + DstParamData%nFreeCons = SrcParamData%nFreeCons + DstParamData%nCpldBodies = SrcParamData%nCpldBodies + DstParamData%nCpldRods = SrcParamData%nCpldRods + DstParamData%nCpldCons = SrcParamData%nCpldCons + DstParamData%NConns = SrcParamData%NConns + DstParamData%NAnchs = SrcParamData%NAnchs + DstParamData%g = SrcParamData%g + DstParamData%rhoW = SrcParamData%rhoW + DstParamData%WtrDpth = SrcParamData%WtrDpth + DstParamData%kBot = SrcParamData%kBot + DstParamData%cBot = SrcParamData%cBot + DstParamData%dtM0 = SrcParamData%dtM0 + DstParamData%dtCoupling = SrcParamData%dtCoupling + DstParamData%NumOuts = SrcParamData%NumOuts + DstParamData%RootName = SrcParamData%RootName +IF (ALLOCATED(SrcParamData%OutParam)) THEN + i1_l = LBOUND(SrcParamData%OutParam,1) + i1_u = UBOUND(SrcParamData%OutParam,1) + IF (.NOT. ALLOCATED(DstParamData%OutParam)) THEN + ALLOCATE(DstParamData%OutParam(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%OutParam.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcParamData%OutParam,1), UBOUND(SrcParamData%OutParam,1) + CALL MD_Copyoutparmtype( SrcParamData%OutParam(i1), DstParamData%OutParam(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF + DstParamData%Delim = SrcParamData%Delim + DstParamData%MDUnOut = SrcParamData%MDUnOut + DstParamData%WaterKin = SrcParamData%WaterKin +IF (ALLOCATED(SrcParamData%ux)) THEN + i1_l = LBOUND(SrcParamData%ux,1) + i1_u = UBOUND(SrcParamData%ux,1) + i2_l = LBOUND(SrcParamData%ux,2) + i2_u = UBOUND(SrcParamData%ux,2) + i3_l = LBOUND(SrcParamData%ux,3) + i3_u = UBOUND(SrcParamData%ux,3) + i4_l = LBOUND(SrcParamData%ux,4) + i4_u = UBOUND(SrcParamData%ux,4) + IF (.NOT. ALLOCATED(DstParamData%ux)) THEN + ALLOCATE(DstParamData%ux(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%ux.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%ux = SrcParamData%ux +ENDIF +IF (ALLOCATED(SrcParamData%uy)) THEN + i1_l = LBOUND(SrcParamData%uy,1) + i1_u = UBOUND(SrcParamData%uy,1) + i2_l = LBOUND(SrcParamData%uy,2) + i2_u = UBOUND(SrcParamData%uy,2) + i3_l = LBOUND(SrcParamData%uy,3) + i3_u = UBOUND(SrcParamData%uy,3) + i4_l = LBOUND(SrcParamData%uy,4) + i4_u = UBOUND(SrcParamData%uy,4) + IF (.NOT. ALLOCATED(DstParamData%uy)) THEN + ALLOCATE(DstParamData%uy(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%uy.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%uy = SrcParamData%uy +ENDIF +IF (ALLOCATED(SrcParamData%uz)) THEN + i1_l = LBOUND(SrcParamData%uz,1) + i1_u = UBOUND(SrcParamData%uz,1) + i2_l = LBOUND(SrcParamData%uz,2) + i2_u = UBOUND(SrcParamData%uz,2) + i3_l = LBOUND(SrcParamData%uz,3) + i3_u = UBOUND(SrcParamData%uz,3) + i4_l = LBOUND(SrcParamData%uz,4) + i4_u = UBOUND(SrcParamData%uz,4) + IF (.NOT. ALLOCATED(DstParamData%uz)) THEN + ALLOCATE(DstParamData%uz(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%uz.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%uz = SrcParamData%uz +ENDIF +IF (ALLOCATED(SrcParamData%ax)) THEN + i1_l = LBOUND(SrcParamData%ax,1) + i1_u = UBOUND(SrcParamData%ax,1) + i2_l = LBOUND(SrcParamData%ax,2) + i2_u = UBOUND(SrcParamData%ax,2) + i3_l = LBOUND(SrcParamData%ax,3) + i3_u = UBOUND(SrcParamData%ax,3) + i4_l = LBOUND(SrcParamData%ax,4) + i4_u = UBOUND(SrcParamData%ax,4) + IF (.NOT. ALLOCATED(DstParamData%ax)) THEN + ALLOCATE(DstParamData%ax(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%ax.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%ax = SrcParamData%ax +ENDIF +IF (ALLOCATED(SrcParamData%ay)) THEN + i1_l = LBOUND(SrcParamData%ay,1) + i1_u = UBOUND(SrcParamData%ay,1) + i2_l = LBOUND(SrcParamData%ay,2) + i2_u = UBOUND(SrcParamData%ay,2) + i3_l = LBOUND(SrcParamData%ay,3) + i3_u = UBOUND(SrcParamData%ay,3) + i4_l = LBOUND(SrcParamData%ay,4) + i4_u = UBOUND(SrcParamData%ay,4) + IF (.NOT. ALLOCATED(DstParamData%ay)) THEN + ALLOCATE(DstParamData%ay(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%ay.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%ay = SrcParamData%ay +ENDIF +IF (ALLOCATED(SrcParamData%az)) THEN + i1_l = LBOUND(SrcParamData%az,1) + i1_u = UBOUND(SrcParamData%az,1) + i2_l = LBOUND(SrcParamData%az,2) + i2_u = UBOUND(SrcParamData%az,2) + i3_l = LBOUND(SrcParamData%az,3) + i3_u = UBOUND(SrcParamData%az,3) + i4_l = LBOUND(SrcParamData%az,4) + i4_u = UBOUND(SrcParamData%az,4) + IF (.NOT. ALLOCATED(DstParamData%az)) THEN + ALLOCATE(DstParamData%az(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%az.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%az = SrcParamData%az +ENDIF +IF (ALLOCATED(SrcParamData%PDyn)) THEN + i1_l = LBOUND(SrcParamData%PDyn,1) + i1_u = UBOUND(SrcParamData%PDyn,1) + i2_l = LBOUND(SrcParamData%PDyn,2) + i2_u = UBOUND(SrcParamData%PDyn,2) + i3_l = LBOUND(SrcParamData%PDyn,3) + i3_u = UBOUND(SrcParamData%PDyn,3) + i4_l = LBOUND(SrcParamData%PDyn,4) + i4_u = UBOUND(SrcParamData%PDyn,4) + IF (.NOT. ALLOCATED(DstParamData%PDyn)) THEN + ALLOCATE(DstParamData%PDyn(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%PDyn.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%PDyn = SrcParamData%PDyn +ENDIF +IF (ALLOCATED(SrcParamData%zeta)) THEN + i1_l = LBOUND(SrcParamData%zeta,1) + i1_u = UBOUND(SrcParamData%zeta,1) + i2_l = LBOUND(SrcParamData%zeta,2) + i2_u = UBOUND(SrcParamData%zeta,2) + i3_l = LBOUND(SrcParamData%zeta,3) + i3_u = UBOUND(SrcParamData%zeta,3) + IF (.NOT. ALLOCATED(DstParamData%zeta)) THEN + ALLOCATE(DstParamData%zeta(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%zeta.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%zeta = SrcParamData%zeta +ENDIF +IF (ALLOCATED(SrcParamData%px)) THEN + i1_l = LBOUND(SrcParamData%px,1) + i1_u = UBOUND(SrcParamData%px,1) + IF (.NOT. ALLOCATED(DstParamData%px)) THEN + ALLOCATE(DstParamData%px(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%px.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%px = SrcParamData%px +ENDIF +IF (ALLOCATED(SrcParamData%py)) THEN + i1_l = LBOUND(SrcParamData%py,1) + i1_u = UBOUND(SrcParamData%py,1) + IF (.NOT. ALLOCATED(DstParamData%py)) THEN + ALLOCATE(DstParamData%py(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%py.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%py = SrcParamData%py +ENDIF +IF (ALLOCATED(SrcParamData%pz)) THEN + i1_l = LBOUND(SrcParamData%pz,1) + i1_u = UBOUND(SrcParamData%pz,1) + IF (.NOT. ALLOCATED(DstParamData%pz)) THEN + ALLOCATE(DstParamData%pz(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%pz.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%pz = SrcParamData%pz +ENDIF + DstParamData%dtWave = SrcParamData%dtWave + END SUBROUTINE MD_CopyParam + + SUBROUTINE MD_DestroyParam( ParamData, ErrStat, ErrMsg ) + TYPE(MD_ParameterType), INTENT(INOUT) :: ParamData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyParam' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(ParamData%OutParam)) THEN +DO i1 = LBOUND(ParamData%OutParam,1), UBOUND(ParamData%OutParam,1) + CALL MD_Destroyoutparmtype( ParamData%OutParam(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(ParamData%OutParam) +ENDIF +IF (ALLOCATED(ParamData%ux)) THEN + DEALLOCATE(ParamData%ux) +ENDIF +IF (ALLOCATED(ParamData%uy)) THEN + DEALLOCATE(ParamData%uy) +ENDIF +IF (ALLOCATED(ParamData%uz)) THEN + DEALLOCATE(ParamData%uz) +ENDIF +IF (ALLOCATED(ParamData%ax)) THEN + DEALLOCATE(ParamData%ax) +ENDIF +IF (ALLOCATED(ParamData%ay)) THEN + DEALLOCATE(ParamData%ay) +ENDIF +IF (ALLOCATED(ParamData%az)) THEN + DEALLOCATE(ParamData%az) +ENDIF +IF (ALLOCATED(ParamData%PDyn)) THEN + DEALLOCATE(ParamData%PDyn) +ENDIF +IF (ALLOCATED(ParamData%zeta)) THEN + DEALLOCATE(ParamData%zeta) +ENDIF +IF (ALLOCATED(ParamData%px)) THEN + DEALLOCATE(ParamData%px) +ENDIF +IF (ALLOCATED(ParamData%py)) THEN + DEALLOCATE(ParamData%py) +ENDIF +IF (ALLOCATED(ParamData%pz)) THEN + DEALLOCATE(ParamData%pz) +ENDIF + END SUBROUTINE MD_DestroyParam + + SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(MD_ParameterType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackParam' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! nLineTypes + Int_BufSz = Int_BufSz + 1 ! nRodTypes + Int_BufSz = Int_BufSz + 1 ! nConnects + Int_BufSz = Int_BufSz + 1 ! nConnectsExtra + Int_BufSz = Int_BufSz + 1 ! nBodies + Int_BufSz = Int_BufSz + 1 ! nRods + Int_BufSz = Int_BufSz + 1 ! nLines + Int_BufSz = Int_BufSz + 1 ! nFails + Int_BufSz = Int_BufSz + 1 ! nFreeBodies + Int_BufSz = Int_BufSz + 1 ! nFreeRods + Int_BufSz = Int_BufSz + 1 ! nFreeCons + Int_BufSz = Int_BufSz + 1 ! nCpldBodies + Int_BufSz = Int_BufSz + 1 ! nCpldRods + Int_BufSz = Int_BufSz + 1 ! nCpldCons + Int_BufSz = Int_BufSz + 1 ! NConns + Int_BufSz = Int_BufSz + 1 ! NAnchs + Db_BufSz = Db_BufSz + 1 ! g + Db_BufSz = Db_BufSz + 1 ! rhoW + Db_BufSz = Db_BufSz + 1 ! WtrDpth + Db_BufSz = Db_BufSz + 1 ! kBot + Db_BufSz = Db_BufSz + 1 ! cBot + Db_BufSz = Db_BufSz + 1 ! dtM0 + Db_BufSz = Db_BufSz + 1 ! dtCoupling + Int_BufSz = Int_BufSz + 1 ! NumOuts + Int_BufSz = Int_BufSz + 1*LEN(InData%RootName) ! RootName + Int_BufSz = Int_BufSz + 1 ! OutParam allocated yes/no + IF ( ALLOCATED(InData%OutParam) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! OutParam upper/lower bounds for each dimension + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + DO i1 = LBOUND(InData%OutParam,1), UBOUND(InData%OutParam,1) + Int_BufSz = Int_BufSz + 3 ! OutParam: size of buffers for each call to pack subtype + CALL MD_Packoutparmtype( Re_Buf, Db_Buf, Int_Buf, InData%OutParam(i1), ErrStat2, ErrMsg2, .TRUE. ) ! OutParam + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! OutParam + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! OutParam + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! OutParam + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 1*LEN(InData%Delim) ! Delim + Int_BufSz = Int_BufSz + 1 ! MDUnOut + Int_BufSz = Int_BufSz + 1 ! WaterKin + Int_BufSz = Int_BufSz + 1 ! ux allocated yes/no + IF ( ALLOCATED(InData%ux) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! ux upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%ux) ! ux + END IF + Int_BufSz = Int_BufSz + 1 ! uy allocated yes/no + IF ( ALLOCATED(InData%uy) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! uy upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%uy) ! uy + END IF + Int_BufSz = Int_BufSz + 1 ! uz allocated yes/no + IF ( ALLOCATED(InData%uz) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! uz upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%uz) ! uz + END IF + Int_BufSz = Int_BufSz + 1 ! ax allocated yes/no + IF ( ALLOCATED(InData%ax) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! ax upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%ax) ! ax + END IF + Int_BufSz = Int_BufSz + 1 ! ay allocated yes/no + IF ( ALLOCATED(InData%ay) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! ay upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%ay) ! ay + END IF + Int_BufSz = Int_BufSz + 1 ! az allocated yes/no + IF ( ALLOCATED(InData%az) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! az upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%az) ! az + END IF + Int_BufSz = Int_BufSz + 1 ! PDyn allocated yes/no + IF ( ALLOCATED(InData%PDyn) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! PDyn upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%PDyn) ! PDyn + END IF + Int_BufSz = Int_BufSz + 1 ! zeta allocated yes/no + IF ( ALLOCATED(InData%zeta) ) THEN + Int_BufSz = Int_BufSz + 2*3 ! zeta upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%zeta) ! zeta + END IF + Int_BufSz = Int_BufSz + 1 ! px allocated yes/no + IF ( ALLOCATED(InData%px) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! px upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%px) ! px + END IF + Int_BufSz = Int_BufSz + 1 ! py allocated yes/no + IF ( ALLOCATED(InData%py) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! py upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%py) ! py + END IF + Int_BufSz = Int_BufSz + 1 ! pz allocated yes/no + IF ( ALLOCATED(InData%pz) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! pz upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%pz) ! pz + END IF + Db_BufSz = Db_BufSz + 1 ! dtWave + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IntKiBuf(Int_Xferred) = InData%nLineTypes + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nRodTypes + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nConnects + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nConnectsExtra + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nBodies + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nRods + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nLines + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nFails + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nFreeBodies + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nFreeRods + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nFreeCons + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nCpldBodies + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nCpldRods + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nCpldCons + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%NConns + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%NAnchs + Int_Xferred = Int_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%g + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%rhoW + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%WtrDpth + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%kBot + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%cBot + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%dtM0 + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%dtCoupling + Db_Xferred = Db_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%NumOuts + Int_Xferred = Int_Xferred + 1 + DO I = 1, LEN(InData%RootName) + IntKiBuf(Int_Xferred) = ICHAR(InData%RootName(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + IF ( .NOT. ALLOCATED(InData%OutParam) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%OutParam,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%OutParam,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%OutParam,1), UBOUND(InData%OutParam,1) + CALL MD_Packoutparmtype( Re_Buf, Db_Buf, Int_Buf, InData%OutParam(i1), ErrStat2, ErrMsg2, OnlySize ) ! OutParam + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF + DO I = 1, LEN(InData%Delim) + IntKiBuf(Int_Xferred) = ICHAR(InData%Delim(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + IntKiBuf(Int_Xferred) = InData%MDUnOut + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%WaterKin + Int_Xferred = Int_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%ux) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ux,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ux,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ux,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ux,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ux,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ux,3) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ux,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ux,4) + Int_Xferred = Int_Xferred + 2 + + DO i4 = LBOUND(InData%ux,4), UBOUND(InData%ux,4) + DO i3 = LBOUND(InData%ux,3), UBOUND(InData%ux,3) + DO i2 = LBOUND(InData%ux,2), UBOUND(InData%ux,2) + DO i1 = LBOUND(InData%ux,1), UBOUND(InData%ux,1) + DbKiBuf(Db_Xferred) = InData%ux(i1,i2,i3,i4) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%uy) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uy,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uy,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uy,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uy,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uy,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uy,3) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uy,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uy,4) + Int_Xferred = Int_Xferred + 2 + + DO i4 = LBOUND(InData%uy,4), UBOUND(InData%uy,4) + DO i3 = LBOUND(InData%uy,3), UBOUND(InData%uy,3) + DO i2 = LBOUND(InData%uy,2), UBOUND(InData%uy,2) + DO i1 = LBOUND(InData%uy,1), UBOUND(InData%uy,1) + DbKiBuf(Db_Xferred) = InData%uy(i1,i2,i3,i4) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%uz) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uz,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uz,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uz,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uz,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uz,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uz,3) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uz,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uz,4) + Int_Xferred = Int_Xferred + 2 + + DO i4 = LBOUND(InData%uz,4), UBOUND(InData%uz,4) + DO i3 = LBOUND(InData%uz,3), UBOUND(InData%uz,3) + DO i2 = LBOUND(InData%uz,2), UBOUND(InData%uz,2) + DO i1 = LBOUND(InData%uz,1), UBOUND(InData%uz,1) + DbKiBuf(Db_Xferred) = InData%uz(i1,i2,i3,i4) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%ax) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ax,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ax,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ax,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ax,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ax,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ax,3) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ax,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ax,4) + Int_Xferred = Int_Xferred + 2 + + DO i4 = LBOUND(InData%ax,4), UBOUND(InData%ax,4) + DO i3 = LBOUND(InData%ax,3), UBOUND(InData%ax,3) + DO i2 = LBOUND(InData%ax,2), UBOUND(InData%ax,2) + DO i1 = LBOUND(InData%ax,1), UBOUND(InData%ax,1) + DbKiBuf(Db_Xferred) = InData%ax(i1,i2,i3,i4) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%ay) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ay,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ay,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ay,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ay,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ay,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ay,3) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%ay,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ay,4) + Int_Xferred = Int_Xferred + 2 + + DO i4 = LBOUND(InData%ay,4), UBOUND(InData%ay,4) + DO i3 = LBOUND(InData%ay,3), UBOUND(InData%ay,3) + DO i2 = LBOUND(InData%ay,2), UBOUND(InData%ay,2) + DO i1 = LBOUND(InData%ay,1), UBOUND(InData%ay,1) + DbKiBuf(Db_Xferred) = InData%ay(i1,i2,i3,i4) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%az) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%az,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%az,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%az,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%az,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%az,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%az,3) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%az,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%az,4) + Int_Xferred = Int_Xferred + 2 + + DO i4 = LBOUND(InData%az,4), UBOUND(InData%az,4) + DO i3 = LBOUND(InData%az,3), UBOUND(InData%az,3) + DO i2 = LBOUND(InData%az,2), UBOUND(InData%az,2) + DO i1 = LBOUND(InData%az,1), UBOUND(InData%az,1) + DbKiBuf(Db_Xferred) = InData%az(i1,i2,i3,i4) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%PDyn) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%PDyn,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PDyn,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%PDyn,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PDyn,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%PDyn,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PDyn,3) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%PDyn,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PDyn,4) + Int_Xferred = Int_Xferred + 2 + + DO i4 = LBOUND(InData%PDyn,4), UBOUND(InData%PDyn,4) + DO i3 = LBOUND(InData%PDyn,3), UBOUND(InData%PDyn,3) + DO i2 = LBOUND(InData%PDyn,2), UBOUND(InData%PDyn,2) + DO i1 = LBOUND(InData%PDyn,1), UBOUND(InData%PDyn,1) + DbKiBuf(Db_Xferred) = InData%PDyn(i1,i2,i3,i4) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%zeta) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%zeta,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%zeta,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%zeta,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%zeta,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%zeta,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%zeta,3) + Int_Xferred = Int_Xferred + 2 + + DO i3 = LBOUND(InData%zeta,3), UBOUND(InData%zeta,3) + DO i2 = LBOUND(InData%zeta,2), UBOUND(InData%zeta,2) + DO i1 = LBOUND(InData%zeta,1), UBOUND(InData%zeta,1) + DbKiBuf(Db_Xferred) = InData%zeta(i1,i2,i3) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%px) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%px,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%px,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%px,1), UBOUND(InData%px,1) + DbKiBuf(Db_Xferred) = InData%px(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%py) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%py,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%py,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%py,1), UBOUND(InData%py,1) + DbKiBuf(Db_Xferred) = InData%py(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%pz) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%pz,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%pz,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%pz,1), UBOUND(InData%pz,1) + DbKiBuf(Db_Xferred) = InData%pz(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + DbKiBuf(Db_Xferred) = InData%dtWave + Db_Xferred = Db_Xferred + 1 + END SUBROUTINE MD_PackParam + + SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_ParameterType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: i4, i4_l, i4_u ! bounds (upper/lower) for an array dimension 4 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackParam' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%nLineTypes = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nRodTypes = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nConnects = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nConnectsExtra = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nBodies = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nRods = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nLines = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nFails = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nFreeBodies = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nFreeRods = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nFreeCons = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nCpldBodies = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nCpldRods = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nCpldCons = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%NConns = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%NAnchs = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%g = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%rhoW = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%WtrDpth = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%kBot = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%cBot = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%dtM0 = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%dtCoupling = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%NumOuts = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + DO I = 1, LEN(OutData%RootName) + OutData%RootName(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! OutParam not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%OutParam)) DEALLOCATE(OutData%OutParam) + ALLOCATE(OutData%OutParam(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%OutParam.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%OutParam,1), UBOUND(OutData%OutParam,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_Unpackoutparmtype( Re_Buf, Db_Buf, Int_Buf, OutData%OutParam(i1), ErrStat2, ErrMsg2 ) ! OutParam + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + DO I = 1, LEN(OutData%Delim) + OutData%Delim(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + OutData%MDUnOut = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%WaterKin = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ux not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i4_l = IntKiBuf( Int_Xferred ) + i4_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%ux)) DEALLOCATE(OutData%ux) + ALLOCATE(OutData%ux(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ux.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i4 = LBOUND(OutData%ux,4), UBOUND(OutData%ux,4) + DO i3 = LBOUND(OutData%ux,3), UBOUND(OutData%ux,3) + DO i2 = LBOUND(OutData%ux,2), UBOUND(OutData%ux,2) + DO i1 = LBOUND(OutData%ux,1), UBOUND(OutData%ux,1) + OutData%ux(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! uy not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i4_l = IntKiBuf( Int_Xferred ) + i4_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%uy)) DEALLOCATE(OutData%uy) + ALLOCATE(OutData%uy(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%uy.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i4 = LBOUND(OutData%uy,4), UBOUND(OutData%uy,4) + DO i3 = LBOUND(OutData%uy,3), UBOUND(OutData%uy,3) + DO i2 = LBOUND(OutData%uy,2), UBOUND(OutData%uy,2) + DO i1 = LBOUND(OutData%uy,1), UBOUND(OutData%uy,1) + OutData%uy(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! uz not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i4_l = IntKiBuf( Int_Xferred ) + i4_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%uz)) DEALLOCATE(OutData%uz) + ALLOCATE(OutData%uz(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%uz.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i4 = LBOUND(OutData%uz,4), UBOUND(OutData%uz,4) + DO i3 = LBOUND(OutData%uz,3), UBOUND(OutData%uz,3) + DO i2 = LBOUND(OutData%uz,2), UBOUND(OutData%uz,2) + DO i1 = LBOUND(OutData%uz,1), UBOUND(OutData%uz,1) + OutData%uz(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ax not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i4_l = IntKiBuf( Int_Xferred ) + i4_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%ax)) DEALLOCATE(OutData%ax) + ALLOCATE(OutData%ax(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ax.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i4 = LBOUND(OutData%ax,4), UBOUND(OutData%ax,4) + DO i3 = LBOUND(OutData%ax,3), UBOUND(OutData%ax,3) + DO i2 = LBOUND(OutData%ax,2), UBOUND(OutData%ax,2) + DO i1 = LBOUND(OutData%ax,1), UBOUND(OutData%ax,1) + OutData%ax(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ay not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i4_l = IntKiBuf( Int_Xferred ) + i4_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%ay)) DEALLOCATE(OutData%ay) + ALLOCATE(OutData%ay(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ay.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i4 = LBOUND(OutData%ay,4), UBOUND(OutData%ay,4) + DO i3 = LBOUND(OutData%ay,3), UBOUND(OutData%ay,3) + DO i2 = LBOUND(OutData%ay,2), UBOUND(OutData%ay,2) + DO i1 = LBOUND(OutData%ay,1), UBOUND(OutData%ay,1) + OutData%ay(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! az not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i4_l = IntKiBuf( Int_Xferred ) + i4_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%az)) DEALLOCATE(OutData%az) + ALLOCATE(OutData%az(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%az.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i4 = LBOUND(OutData%az,4), UBOUND(OutData%az,4) + DO i3 = LBOUND(OutData%az,3), UBOUND(OutData%az,3) + DO i2 = LBOUND(OutData%az,2), UBOUND(OutData%az,2) + DO i1 = LBOUND(OutData%az,1), UBOUND(OutData%az,1) + OutData%az(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! PDyn not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i4_l = IntKiBuf( Int_Xferred ) + i4_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%PDyn)) DEALLOCATE(OutData%PDyn) + ALLOCATE(OutData%PDyn(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%PDyn.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i4 = LBOUND(OutData%PDyn,4), UBOUND(OutData%PDyn,4) + DO i3 = LBOUND(OutData%PDyn,3), UBOUND(OutData%PDyn,3) + DO i2 = LBOUND(OutData%PDyn,2), UBOUND(OutData%PDyn,2) + DO i1 = LBOUND(OutData%PDyn,1), UBOUND(OutData%PDyn,1) + OutData%PDyn(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! zeta not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%zeta)) DEALLOCATE(OutData%zeta) + ALLOCATE(OutData%zeta(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%zeta.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i3 = LBOUND(OutData%zeta,3), UBOUND(OutData%zeta,3) + DO i2 = LBOUND(OutData%zeta,2), UBOUND(OutData%zeta,2) + DO i1 = LBOUND(OutData%zeta,1), UBOUND(OutData%zeta,1) + OutData%zeta(i1,i2,i3) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! px not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%px)) DEALLOCATE(OutData%px) + ALLOCATE(OutData%px(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%px.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%px,1), UBOUND(OutData%px,1) + OutData%px(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! py not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%py)) DEALLOCATE(OutData%py) + ALLOCATE(OutData%py(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%py.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%py,1), UBOUND(OutData%py,1) + OutData%py(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! pz not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%pz)) DEALLOCATE(OutData%pz) + ALLOCATE(OutData%pz(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%pz.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%pz,1), UBOUND(OutData%pz,1) + OutData%pz(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + OutData%dtWave = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END SUBROUTINE MD_UnPackParam + + SUBROUTINE MD_CopyInput( SrcInputData, DstInputData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_InputType), INTENT(INOUT) :: SrcInputData + TYPE(MD_InputType), INTENT(INOUT) :: DstInputData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyInput' +! + ErrStat = ErrID_None + ErrMsg = "" + CALL MeshCopy( SrcInputData%CoupledKinematics, DstInputData%CoupledKinematics, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat>=AbortErrLev) RETURN +IF (ALLOCATED(SrcInputData%DeltaL)) THEN + i1_l = LBOUND(SrcInputData%DeltaL,1) + i1_u = UBOUND(SrcInputData%DeltaL,1) + IF (.NOT. ALLOCATED(DstInputData%DeltaL)) THEN + ALLOCATE(DstInputData%DeltaL(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInputData%DeltaL.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInputData%DeltaL = SrcInputData%DeltaL +ENDIF +IF (ALLOCATED(SrcInputData%DeltaLdot)) THEN + i1_l = LBOUND(SrcInputData%DeltaLdot,1) + i1_u = UBOUND(SrcInputData%DeltaLdot,1) + IF (.NOT. ALLOCATED(DstInputData%DeltaLdot)) THEN + ALLOCATE(DstInputData%DeltaLdot(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInputData%DeltaLdot.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInputData%DeltaLdot = SrcInputData%DeltaLdot +ENDIF + END SUBROUTINE MD_CopyInput + + SUBROUTINE MD_DestroyInput( InputData, ErrStat, ErrMsg ) + TYPE(MD_InputType), INTENT(INOUT) :: InputData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyInput' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + CALL MeshDestroy( InputData%CoupledKinematics, ErrStat, ErrMsg ) +IF (ALLOCATED(InputData%DeltaL)) THEN + DEALLOCATE(InputData%DeltaL) +ENDIF +IF (ALLOCATED(InputData%DeltaLdot)) THEN + DEALLOCATE(InputData%DeltaLdot) +ENDIF + END SUBROUTINE MD_DestroyInput + + SUBROUTINE MD_PackInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(MD_InputType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackInput' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + Int_BufSz = Int_BufSz + 3 ! CoupledKinematics: size of buffers for each call to pack subtype + CALL MeshPack( InData%CoupledKinematics, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, .TRUE. ) ! CoupledKinematics + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! CoupledKinematics + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! CoupledKinematics + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! CoupledKinematics + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + Int_BufSz = Int_BufSz + 1 ! DeltaL allocated yes/no + IF ( ALLOCATED(InData%DeltaL) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! DeltaL upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%DeltaL) ! DeltaL + END IF + Int_BufSz = Int_BufSz + 1 ! DeltaLdot allocated yes/no + IF ( ALLOCATED(InData%DeltaLdot) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! DeltaLdot upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%DeltaLdot) ! DeltaLdot + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + CALL MeshPack( InData%CoupledKinematics, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, OnlySize ) ! CoupledKinematics + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF ( .NOT. ALLOCATED(InData%DeltaL) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%DeltaL,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%DeltaL,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%DeltaL,1), UBOUND(InData%DeltaL,1) + ReKiBuf(Re_Xferred) = InData%DeltaL(i1) + Re_Xferred = Re_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%DeltaLdot) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%DeltaLdot,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%DeltaLdot,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%DeltaLdot,1), UBOUND(InData%DeltaLdot,1) + ReKiBuf(Re_Xferred) = InData%DeltaLdot(i1) + Re_Xferred = Re_Xferred + 1 + END DO + END IF + END SUBROUTINE MD_PackInput + + SUBROUTINE MD_UnPackInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_InputType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size INTEGER(IntKi) :: Re_Xferred INTEGER(IntKi) :: Db_Xferred INTEGER(IntKi) :: Int_Xferred @@ -3801,7 +10502,7 @@ SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 - CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackLine' + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackInput' ! buffers to store meshes, if any REAL(ReKi), ALLOCATABLE :: Re_Buf(:) REAL(DbKi), ALLOCATABLE :: Db_Buf(:) @@ -3812,25 +10513,702 @@ SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) Re_Xferred = 1 Db_Xferred = 1 Int_Xferred = 1 - OutData%IdNum = IntKiBuf(Int_Xferred) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MeshUnpack( OutData%CoupledKinematics, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2 ) ! CoupledKinematics + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! DeltaL not allocated Int_Xferred = Int_Xferred + 1 - OutData%PropsIdNum = IntKiBuf(Int_Xferred) + ELSE Int_Xferred = Int_Xferred + 1 - i1_l = LBOUND(OutData%OutFlagList,1) - i1_u = UBOUND(OutData%OutFlagList,1) - DO i1 = LBOUND(OutData%OutFlagList,1), UBOUND(OutData%OutFlagList,1) - OutData%OutFlagList(i1) = IntKiBuf(Int_Xferred) - Int_Xferred = Int_Xferred + 1 - END DO - OutData%CtrlChan = IntKiBuf(Int_Xferred) + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%DeltaL)) DEALLOCATE(OutData%DeltaL) + ALLOCATE(OutData%DeltaL(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%DeltaL.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%DeltaL,1), UBOUND(OutData%DeltaL,1) + OutData%DeltaL(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! DeltaLdot not allocated Int_Xferred = Int_Xferred + 1 - OutData%FairConnect = IntKiBuf(Int_Xferred) + ELSE Int_Xferred = Int_Xferred + 1 - OutData%AnchConnect = IntKiBuf(Int_Xferred) + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%DeltaLdot)) DEALLOCATE(OutData%DeltaLdot) + ALLOCATE(OutData%DeltaLdot(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%DeltaLdot.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%DeltaLdot,1), UBOUND(OutData%DeltaLdot,1) + OutData%DeltaLdot(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END IF + END SUBROUTINE MD_UnPackInput + + SUBROUTINE MD_CopyOutput( SrcOutputData, DstOutputData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_OutputType), INTENT(INOUT) :: SrcOutputData + TYPE(MD_OutputType), INTENT(INOUT) :: DstOutputData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyOutput' +! + ErrStat = ErrID_None + ErrMsg = "" + CALL MeshCopy( SrcOutputData%CoupledLoads, DstOutputData%CoupledLoads, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat>=AbortErrLev) RETURN +IF (ALLOCATED(SrcOutputData%WriteOutput)) THEN + i1_l = LBOUND(SrcOutputData%WriteOutput,1) + i1_u = UBOUND(SrcOutputData%WriteOutput,1) + IF (.NOT. ALLOCATED(DstOutputData%WriteOutput)) THEN + ALLOCATE(DstOutputData%WriteOutput(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstOutputData%WriteOutput.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstOutputData%WriteOutput = SrcOutputData%WriteOutput +ENDIF + END SUBROUTINE MD_CopyOutput + + SUBROUTINE MD_DestroyOutput( OutputData, ErrStat, ErrMsg ) + TYPE(MD_OutputType), INTENT(INOUT) :: OutputData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyOutput' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + CALL MeshDestroy( OutputData%CoupledLoads, ErrStat, ErrMsg ) +IF (ALLOCATED(OutputData%WriteOutput)) THEN + DEALLOCATE(OutputData%WriteOutput) +ENDIF + END SUBROUTINE MD_DestroyOutput + + SUBROUTINE MD_PackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(MD_OutputType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackOutput' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + Int_BufSz = Int_BufSz + 3 ! CoupledLoads: size of buffers for each call to pack subtype + CALL MeshPack( InData%CoupledLoads, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, .TRUE. ) ! CoupledLoads + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! CoupledLoads + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! CoupledLoads + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! CoupledLoads + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + Int_BufSz = Int_BufSz + 1 ! WriteOutput allocated yes/no + IF ( ALLOCATED(InData%WriteOutput) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! WriteOutput upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%WriteOutput) ! WriteOutput + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + CALL MeshPack( InData%CoupledLoads, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, OnlySize ) ! CoupledLoads + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF ( .NOT. ALLOCATED(InData%WriteOutput) ) THEN + IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 - OutData%N = IntKiBuf(Int_Xferred) + ELSE + IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - END SUBROUTINE MD_UnPackLine + IntKiBuf( Int_Xferred ) = LBOUND(InData%WriteOutput,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WriteOutput,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%WriteOutput,1), UBOUND(InData%WriteOutput,1) + ReKiBuf(Re_Xferred) = InData%WriteOutput(i1) + Re_Xferred = Re_Xferred + 1 + END DO + END IF + END SUBROUTINE MD_PackOutput + + SUBROUTINE MD_UnPackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_OutputType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackOutput' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MeshUnpack( OutData%CoupledLoads, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2 ) ! CoupledLoads + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WriteOutput not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%WriteOutput)) DEALLOCATE(OutData%WriteOutput) + ALLOCATE(OutData%WriteOutput(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%WriteOutput.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%WriteOutput,1), UBOUND(OutData%WriteOutput,1) + OutData%WriteOutput(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END IF + END SUBROUTINE MD_UnPackOutput + + + SUBROUTINE MD_Input_ExtrapInterp(u, t, u_out, t_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Input u_out at time t_out, from previous/future time +! values of u (which has values associated with times in t). Order of the interpolation is given by the size of u +! +! expressions below based on either +! +! f(t) = a +! f(t) = a + b * t, or +! f(t) = a + b * t + c * t**2 +! +! where a, b and c are determined as the solution to +! f(t1) = u1, f(t2) = u2, f(t3) = u3 (as appropriate) +! +!.................................................................................................................................. + + TYPE(MD_InputType), INTENT(INOUT) :: u(:) ! Input at t1 > t2 > t3 + REAL(DbKi), INTENT(IN ) :: t(:) ! Times associated with the Inputs + TYPE(MD_InputType), INTENT(INOUT) :: u_out ! Input at tin_out + REAL(DbKi), INTENT(IN ) :: t_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + INTEGER(IntKi) :: order ! order of polynomial fit (max 2) + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + CHARACTER(*), PARAMETER :: RoutineName = 'MD_Input_ExtrapInterp' + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + if ( size(t) .ne. size(u)) then + CALL SetErrStat(ErrID_Fatal,'size(t) must equal size(u)',ErrStat,ErrMsg,RoutineName) + RETURN + endif + order = SIZE(u) - 1 + IF ( order .eq. 0 ) THEN + CALL MD_CopyInput(u(1), u_out, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE IF ( order .eq. 1 ) THEN + CALL MD_Input_ExtrapInterp1(u(1), u(2), t, u_out, t_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE IF ( order .eq. 2 ) THEN + CALL MD_Input_ExtrapInterp2(u(1), u(2), u(3), t, u_out, t_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE + CALL SetErrStat(ErrID_Fatal,'size(u) must be less than 4 (order must be less than 3).',ErrStat,ErrMsg,RoutineName) + RETURN + ENDIF + END SUBROUTINE MD_Input_ExtrapInterp + + + SUBROUTINE MD_Input_ExtrapInterp1(u1, u2, tin, u_out, tin_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Input u_out at time t_out, from previous/future time +! values of u (which has values associated with times in t). Order of the interpolation is 1. +! +! f(t) = a + b * t, or +! +! where a and b are determined as the solution to +! f(t1) = u1, f(t2) = u2 +! +!.................................................................................................................................. + + TYPE(MD_InputType), INTENT(INOUT) :: u1 ! Input at t1 > t2 + TYPE(MD_InputType), INTENT(INOUT) :: u2 ! Input at t2 + REAL(DbKi), INTENT(IN ) :: tin(2) ! Times associated with the Inputs + TYPE(MD_InputType), INTENT(INOUT) :: u_out ! Input at tin_out + REAL(DbKi), INTENT(IN ) :: tin_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + REAL(DbKi) :: t(2) ! Times associated with the Inputs + REAL(DbKi) :: t_out ! Time to which to be extrap/interpd + CHARACTER(*), PARAMETER :: RoutineName = 'MD_Input_ExtrapInterp1' + REAL(DbKi) :: b ! temporary for extrapolation/interpolation + REAL(DbKi) :: ScaleFactor ! temporary for extrapolation/interpolation + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + INTEGER :: i01 ! dim1 level 0 counter variable for arrays of ddts + INTEGER :: i1 ! dim1 counter variable for arrays + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + ! we'll subtract a constant from the times to resolve some + ! numerical issues when t gets large (and to simplify the equations) + t = tin - tin(1) + t_out = tin_out - tin(1) + + IF ( EqualRealNos( t(1), t(2) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(2) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + + ScaleFactor = t_out / t(2) + CALL MeshExtrapInterp1(u1%CoupledKinematics, u2%CoupledKinematics, tin, u_out%CoupledKinematics, tin_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) +IF (ALLOCATED(u_out%DeltaL) .AND. ALLOCATED(u1%DeltaL)) THEN + DO i1 = LBOUND(u_out%DeltaL,1),UBOUND(u_out%DeltaL,1) + b = -(u1%DeltaL(i1) - u2%DeltaL(i1)) + u_out%DeltaL(i1) = u1%DeltaL(i1) + b * ScaleFactor + END DO +END IF ! check if allocated +IF (ALLOCATED(u_out%DeltaLdot) .AND. ALLOCATED(u1%DeltaLdot)) THEN + DO i1 = LBOUND(u_out%DeltaLdot,1),UBOUND(u_out%DeltaLdot,1) + b = -(u1%DeltaLdot(i1) - u2%DeltaLdot(i1)) + u_out%DeltaLdot(i1) = u1%DeltaLdot(i1) + b * ScaleFactor + END DO +END IF ! check if allocated + END SUBROUTINE MD_Input_ExtrapInterp1 + + + SUBROUTINE MD_Input_ExtrapInterp2(u1, u2, u3, tin, u_out, tin_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Input u_out at time t_out, from previous/future time +! values of u (which has values associated with times in t). Order of the interpolation is 2. +! +! expressions below based on either +! +! f(t) = a + b * t + c * t**2 +! +! where a, b and c are determined as the solution to +! f(t1) = u1, f(t2) = u2, f(t3) = u3 +! +!.................................................................................................................................. + + TYPE(MD_InputType), INTENT(INOUT) :: u1 ! Input at t1 > t2 > t3 + TYPE(MD_InputType), INTENT(INOUT) :: u2 ! Input at t2 > t3 + TYPE(MD_InputType), INTENT(INOUT) :: u3 ! Input at t3 + REAL(DbKi), INTENT(IN ) :: tin(3) ! Times associated with the Inputs + TYPE(MD_InputType), INTENT(INOUT) :: u_out ! Input at tin_out + REAL(DbKi), INTENT(IN ) :: tin_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + REAL(DbKi) :: t(3) ! Times associated with the Inputs + REAL(DbKi) :: t_out ! Time to which to be extrap/interpd + INTEGER(IntKi) :: order ! order of polynomial fit (max 2) + REAL(DbKi) :: b ! temporary for extrapolation/interpolation + REAL(DbKi) :: c ! temporary for extrapolation/interpolation + REAL(DbKi) :: ScaleFactor ! temporary for extrapolation/interpolation + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + CHARACTER(*), PARAMETER :: RoutineName = 'MD_Input_ExtrapInterp2' + INTEGER :: i01 ! dim1 level 0 counter variable for arrays of ddts + INTEGER :: i1 ! dim1 counter variable for arrays + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + ! we'll subtract a constant from the times to resolve some + ! numerical issues when t gets large (and to simplify the equations) + t = tin - tin(1) + t_out = tin_out - tin(1) + + IF ( EqualRealNos( t(1), t(2) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(2) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + ELSE IF ( EqualRealNos( t(2), t(3) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(2) must not equal t(3) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + ELSE IF ( EqualRealNos( t(1), t(3) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(3) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + + ScaleFactor = t_out / (t(2) * t(3) * (t(2) - t(3))) + CALL MeshExtrapInterp2(u1%CoupledKinematics, u2%CoupledKinematics, u3%CoupledKinematics, tin, u_out%CoupledKinematics, tin_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) +IF (ALLOCATED(u_out%DeltaL) .AND. ALLOCATED(u1%DeltaL)) THEN + DO i1 = LBOUND(u_out%DeltaL,1),UBOUND(u_out%DeltaL,1) + b = (t(3)**2*(u1%DeltaL(i1) - u2%DeltaL(i1)) + t(2)**2*(-u1%DeltaL(i1) + u3%DeltaL(i1)))* scaleFactor + c = ( (t(2)-t(3))*u1%DeltaL(i1) + t(3)*u2%DeltaL(i1) - t(2)*u3%DeltaL(i1) ) * scaleFactor + u_out%DeltaL(i1) = u1%DeltaL(i1) + b + c * t_out + END DO +END IF ! check if allocated +IF (ALLOCATED(u_out%DeltaLdot) .AND. ALLOCATED(u1%DeltaLdot)) THEN + DO i1 = LBOUND(u_out%DeltaLdot,1),UBOUND(u_out%DeltaLdot,1) + b = (t(3)**2*(u1%DeltaLdot(i1) - u2%DeltaLdot(i1)) + t(2)**2*(-u1%DeltaLdot(i1) + u3%DeltaLdot(i1)))* scaleFactor + c = ( (t(2)-t(3))*u1%DeltaLdot(i1) + t(3)*u2%DeltaLdot(i1) - t(2)*u3%DeltaLdot(i1) ) * scaleFactor + u_out%DeltaLdot(i1) = u1%DeltaLdot(i1) + b + c * t_out + END DO +END IF ! check if allocated + END SUBROUTINE MD_Input_ExtrapInterp2 + + + SUBROUTINE MD_Output_ExtrapInterp(y, t, y_out, t_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Output y_out at time t_out, from previous/future time +! values of y (which has values associated with times in t). Order of the interpolation is given by the size of y +! +! expressions below based on either +! +! f(t) = a +! f(t) = a + b * t, or +! f(t) = a + b * t + c * t**2 +! +! where a, b and c are determined as the solution to +! f(t1) = y1, f(t2) = y2, f(t3) = y3 (as appropriate) +! +!.................................................................................................................................. + + TYPE(MD_OutputType), INTENT(INOUT) :: y(:) ! Output at t1 > t2 > t3 + REAL(DbKi), INTENT(IN ) :: t(:) ! Times associated with the Outputs + TYPE(MD_OutputType), INTENT(INOUT) :: y_out ! Output at tin_out + REAL(DbKi), INTENT(IN ) :: t_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + INTEGER(IntKi) :: order ! order of polynomial fit (max 2) + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + CHARACTER(*), PARAMETER :: RoutineName = 'MD_Output_ExtrapInterp' + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + if ( size(t) .ne. size(y)) then + CALL SetErrStat(ErrID_Fatal,'size(t) must equal size(y)',ErrStat,ErrMsg,RoutineName) + RETURN + endif + order = SIZE(y) - 1 + IF ( order .eq. 0 ) THEN + CALL MD_CopyOutput(y(1), y_out, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE IF ( order .eq. 1 ) THEN + CALL MD_Output_ExtrapInterp1(y(1), y(2), t, y_out, t_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE IF ( order .eq. 2 ) THEN + CALL MD_Output_ExtrapInterp2(y(1), y(2), y(3), t, y_out, t_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE + CALL SetErrStat(ErrID_Fatal,'size(y) must be less than 4 (order must be less than 3).',ErrStat,ErrMsg,RoutineName) + RETURN + ENDIF + END SUBROUTINE MD_Output_ExtrapInterp + + + SUBROUTINE MD_Output_ExtrapInterp1(y1, y2, tin, y_out, tin_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Output y_out at time t_out, from previous/future time +! values of y (which has values associated with times in t). Order of the interpolation is 1. +! +! f(t) = a + b * t, or +! +! where a and b are determined as the solution to +! f(t1) = y1, f(t2) = y2 +! +!.................................................................................................................................. + + TYPE(MD_OutputType), INTENT(INOUT) :: y1 ! Output at t1 > t2 + TYPE(MD_OutputType), INTENT(INOUT) :: y2 ! Output at t2 + REAL(DbKi), INTENT(IN ) :: tin(2) ! Times associated with the Outputs + TYPE(MD_OutputType), INTENT(INOUT) :: y_out ! Output at tin_out + REAL(DbKi), INTENT(IN ) :: tin_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + REAL(DbKi) :: t(2) ! Times associated with the Outputs + REAL(DbKi) :: t_out ! Time to which to be extrap/interpd + CHARACTER(*), PARAMETER :: RoutineName = 'MD_Output_ExtrapInterp1' + REAL(DbKi) :: b ! temporary for extrapolation/interpolation + REAL(DbKi) :: ScaleFactor ! temporary for extrapolation/interpolation + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + INTEGER :: i01 ! dim1 level 0 counter variable for arrays of ddts + INTEGER :: i1 ! dim1 counter variable for arrays + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + ! we'll subtract a constant from the times to resolve some + ! numerical issues when t gets large (and to simplify the equations) + t = tin - tin(1) + t_out = tin_out - tin(1) + + IF ( EqualRealNos( t(1), t(2) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(2) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + + ScaleFactor = t_out / t(2) + CALL MeshExtrapInterp1(y1%CoupledLoads, y2%CoupledLoads, tin, y_out%CoupledLoads, tin_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) +IF (ALLOCATED(y_out%WriteOutput) .AND. ALLOCATED(y1%WriteOutput)) THEN + DO i1 = LBOUND(y_out%WriteOutput,1),UBOUND(y_out%WriteOutput,1) + b = -(y1%WriteOutput(i1) - y2%WriteOutput(i1)) + y_out%WriteOutput(i1) = y1%WriteOutput(i1) + b * ScaleFactor + END DO +END IF ! check if allocated + END SUBROUTINE MD_Output_ExtrapInterp1 + + + SUBROUTINE MD_Output_ExtrapInterp2(y1, y2, y3, tin, y_out, tin_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Output y_out at time t_out, from previous/future time +! values of y (which has values associated with times in t). Order of the interpolation is 2. +! +! expressions below based on either +! +! f(t) = a + b * t + c * t**2 +! +! where a, b and c are determined as the solution to +! f(t1) = y1, f(t2) = y2, f(t3) = y3 +! +!.................................................................................................................................. + + TYPE(MD_OutputType), INTENT(INOUT) :: y1 ! Output at t1 > t2 > t3 + TYPE(MD_OutputType), INTENT(INOUT) :: y2 ! Output at t2 > t3 + TYPE(MD_OutputType), INTENT(INOUT) :: y3 ! Output at t3 + REAL(DbKi), INTENT(IN ) :: tin(3) ! Times associated with the Outputs + TYPE(MD_OutputType), INTENT(INOUT) :: y_out ! Output at tin_out + REAL(DbKi), INTENT(IN ) :: tin_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + REAL(DbKi) :: t(3) ! Times associated with the Outputs + REAL(DbKi) :: t_out ! Time to which to be extrap/interpd + INTEGER(IntKi) :: order ! order of polynomial fit (max 2) + REAL(DbKi) :: b ! temporary for extrapolation/interpolation + REAL(DbKi) :: c ! temporary for extrapolation/interpolation + REAL(DbKi) :: ScaleFactor ! temporary for extrapolation/interpolation + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + CHARACTER(*), PARAMETER :: RoutineName = 'MD_Output_ExtrapInterp2' + INTEGER :: i01 ! dim1 level 0 counter variable for arrays of ddts + INTEGER :: i1 ! dim1 counter variable for arrays + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + ! we'll subtract a constant from the times to resolve some + ! numerical issues when t gets large (and to simplify the equations) + t = tin - tin(1) + t_out = tin_out - tin(1) + + IF ( EqualRealNos( t(1), t(2) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(2) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + ELSE IF ( EqualRealNos( t(2), t(3) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(2) must not equal t(3) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + ELSE IF ( EqualRealNos( t(1), t(3) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(3) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + + ScaleFactor = t_out / (t(2) * t(3) * (t(2) - t(3))) + CALL MeshExtrapInterp2(y1%CoupledLoads, y2%CoupledLoads, y3%CoupledLoads, tin, y_out%CoupledLoads, tin_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) +IF (ALLOCATED(y_out%WriteOutput) .AND. ALLOCATED(y1%WriteOutput)) THEN + DO i1 = LBOUND(y_out%WriteOutput,1),UBOUND(y_out%WriteOutput,1) + b = (t(3)**2*(y1%WriteOutput(i1) - y2%WriteOutput(i1)) + t(2)**2*(-y1%WriteOutput(i1) + y3%WriteOutput(i1)))* scaleFactor + c = ( (t(2)-t(3))*y1%WriteOutput(i1) + t(3)*y2%WriteOutput(i1) - t(2)*y3%WriteOutput(i1) ) * scaleFactor + y_out%WriteOutput(i1) = y1%WriteOutput(i1) + b + c * t_out + END DO +END IF ! check if allocated + END SUBROUTINE MD_Output_ExtrapInterp2 END MODULE MoorDyn_Types !ENDOFREGISTRYGENERATEDFILE From f16b590cdf3ba6aae354e85a4fc644b958cf94cf Mon Sep 17 00:00:00 2001 From: Matt Hall Date: Wed, 13 Jan 2021 18:25:32 -0700 Subject: [PATCH 007/242] Adjusting hard-coded wave kinematics grid functionality to use public parameters of the Waves module. This makes it quicker to adjust the grid settings and recompile. Also fixed the allocation oversight that prevented WaveMod=0 cases from running (given this wave grid functionality). --- modules/hydrodyn/src/HydroDyn.f90 | 9 +- modules/hydrodyn/src/HydroDyn_Input.f90 | 24 ++--- modules/hydrodyn/src/Waves.f90 | 29 +++++- modules/moordyn/src/MoorDyn.f90 | 112 +++++++++++---------- modules/openfast-library/src/FAST_Subs.f90 | 10 +- 5 files changed, 106 insertions(+), 78 deletions(-) diff --git a/modules/hydrodyn/src/HydroDyn.f90 b/modules/hydrodyn/src/HydroDyn.f90 index 18b0ddf9e1..d144653c50 100644 --- a/modules/hydrodyn/src/HydroDyn.f90 +++ b/modules/hydrodyn/src/HydroDyn.f90 @@ -33,6 +33,7 @@ MODULE HydroDyn USE HydroDyn_Input USE HydroDyn_Output USE Current + USE Waves USE Waves2 #ifdef USE_FIT USE FIT_MODULES @@ -1351,10 +1352,10 @@ SUBROUTINE HydroDyn_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, I !::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: !@mhall: Making sure wave info from the grid points are saved for output here... - ALLOCATE ( InitOut%WaveVel (InitLocal%Morison%NStepWave, 200, 3) ) - ALLOCATE ( InitOut%WaveAcc (InitLocal%Morison%NStepWave, 200, 3) ) - ALLOCATE ( InitOut%WaveDynP (InitLocal%Morison%NStepWave, 200) ) - ALLOCATE ( InitOut%WaveElev (InitLocal%Morison%NStepWave, 25) ) ! unlike the above, this array is just 5x5, for the surface. + ALLOCATE ( InitOut%WaveVel (InitLocal%Morison%NStepWave, WaveGrid_n, 3) ) + ALLOCATE ( InitOut%WaveAcc (InitLocal%Morison%NStepWave, WaveGrid_n, 3) ) + ALLOCATE ( InitOut%WaveDynP (InitLocal%Morison%NStepWave, WaveGrid_n) ) + ALLOCATE ( InitOut%WaveElev (InitLocal%Morison%NStepWave, WaveGrid_nx*WaveGrid_ny) ) ! unlike the above, this array is just 5x5, for the surface. ALLOCATE ( InitOut%WaveTime (InitLocal%Morison%NStepWave) ) diff --git a/modules/hydrodyn/src/HydroDyn_Input.f90 b/modules/hydrodyn/src/HydroDyn_Input.f90 index 65f2e82898..efa96f6230 100644 --- a/modules/hydrodyn/src/HydroDyn_Input.f90 +++ b/modules/hydrodyn/src/HydroDyn_Input.f90 @@ -2556,7 +2556,7 @@ SUBROUTINE HydroDynInput_ProcessInitData( InitInp, ErrStat, ErrMsg ) ! WaveTMax - Analysis time for incident wave calculations. - IF ( InitInp%Waves%WaveMod == 0 ) THEN ! .TRUE if we have incident waves. + IF ( InitInp%Waves%WaveMod == 0 ) THEN ! .TRUE if we DO NOT HAVE have incident waves. ! TODO: Issue warning if WaveTMax was not already 0.0 in this case. IF ( .NOT. EqualRealNos(InitInp%Waves%WaveTMax, 0.0_DbKi) ) THEN @@ -4264,8 +4264,8 @@ SUBROUTINE HydroDynInput_ProcessInitData( InitInp, ErrStat, ErrMsg ) IF ( ErrStat >= AbortErrLev ) RETURN ! Set the number and global Z locations for the X and Y components of the current velocities - ! @mhall: hard-coding an extra 200 points to make a water kinematics grid - InitInp%Current%NMorisonNodes = InitInp%Morison%NNodes + 200 + ! @mhall: hard-coding an extra WaveGrid_n points to make a water kinematics grid + InitInp%Current%NMorisonNodes = InitInp%Morison%NNodes + WaveGrid_n ALLOCATE ( InitInp%Current%MorisonNodezi(InitInp%Current%NMorisonNodes), STAT = ErrStat2 ) IF ( ErrStat2 /= ErrID_None ) THEN @@ -4276,8 +4276,8 @@ SUBROUTINE HydroDynInput_ProcessInitData( InitInp, ErrStat, ErrMsg ) ! Establish the number and locations where the wave kinematics will be computed - ! @mhall: hard-coding an extra 200 points to make a water kinematics grid - InitInp%Waves%NWaveKin = InitInp%Morison%NNodes + 200 ! Number of points where the incident wave kinematics will be computed (-) + ! @mhall: hard-coding an extra WaveGrid_n points to make a water kinematics grid + InitInp%Waves%NWaveKin = InitInp%Morison%NNodes + WaveGrid_n ! Number of points where the incident wave kinematics will be computed (-) ALLOCATE ( InitInp%Waves%WaveKinxi(InitInp%Waves%NWaveKin), STAT = ErrStat2 ) IF ( ErrStat2 /= ErrID_None ) THEN CALL SetErrStat( ErrID_Fatal,'Error allocating space for WaveKinxi array.',ErrStat,ErrMsg,RoutineName) @@ -4301,13 +4301,13 @@ SUBROUTINE HydroDynInput_ProcessInitData( InitInp, ErrStat, ErrMsg ) InitInp%Current%MorisonNodezi(I) = InitInp%Waves%WaveKinzi(I) END DO !@mhall: hard-coding the coordinates of those additional nodes for the grid (remember, must be in increasing order) <<< move these to module global parameters<<<< - DO I=1,8 !z - DO J = 1,5 !y - DO K = 1,5 !x - Itemp = InitInp%Morison%NNodes + (I-1)*25.0 + (J-1)*5.0 + K ! index of actual node - InitInp%Waves%WaveKinzi(Itemp) = 1.0 - 2.0**(8-I) ! -127, -63, -31, -15, -7, -3, -1, 0 - InitInp%Waves%WaveKinyi(Itemp) = -60.0 + 20.0*J - InitInp%Waves%WaveKinxi(Itemp) = -60.0 + 20.0*K + DO I=1,WaveGrid_nz !z + DO J = 1,WaveGrid_ny !y + DO K = 1,WaveGrid_nx !x + Itemp = InitInp%Morison%NNodes + (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node + InitInp%Waves%WaveKinzi(Itemp) = 1.0 - 2.0**(WaveGrid_nz-I) ! -127, -63, -31, -15, -7, -3, -1, 0 + InitInp%Waves%WaveKinyi(Itemp) = WaveGrid_y0 + WaveGrid_dy*J + InitInp%Waves%WaveKinxi(Itemp) = WaveGrid_x0 + WaveGrid_dx*K InitInp%Current%MorisonNodezi(Itemp)= InitInp%Waves%WaveKinzi(I) END DO END DO diff --git a/modules/hydrodyn/src/Waves.f90 b/modules/hydrodyn/src/Waves.f90 index db8571ea44..ced0b8f2f7 100644 --- a/modules/hydrodyn/src/Waves.f90 +++ b/modules/hydrodyn/src/Waves.f90 @@ -34,6 +34,21 @@ MODULE Waves TYPE(ProgDesc), PARAMETER :: Waves_ProgDesc = ProgDesc( 'Waves', '', '' ) + + ! ..... @mhall: Public variables for hard-coded wave kinematics grid (temporary solution) ........................... + + INTEGER, PUBLIC :: WaveGrid_n = 150 ! Number of wave kinematics grid points = nx*ny*nz + + REAL(SiKi), PUBLIC :: WaveGrid_x0 = -45.0 ! first grid point in x direction + REAL(SiKi), PUBLIC :: WaveGrid_dx = 15.0 ! step size in x direction + INTEGER, PUBLIC :: WaveGrid_nx = 10 ! Number of wave kinematics grid points in x + + REAL(SiKi), PUBLIC :: WaveGrid_y0 = -35.0 ! same for y + REAL(SiKi), PUBLIC :: WaveGrid_dy = 35.0 + INTEGER, PUBLIC :: WaveGrid_ny = 3 + + INTEGER, PUBLIC :: WaveGrid_nz = 5 ! Number of wave kinematics grid points in z (locations decided by 1.0 - 2.0**(WaveGrid_nz-I)) + ! ..... Public Subroutines ................................................................................................... PUBLIC :: WavePkShpDefault ! Return the default value of the peak shape parameter of the incident wave spectrum @@ -1784,15 +1799,15 @@ SUBROUTINE VariousWaves_Init ( InitInp, InitOut, ErrStat, ErrMsg ) ! :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: !@mhall: hard-coding some additional wave elevation time series output for now - ALLOCATE ( InitOut%WaveElevMD (0:InitOut%NStepWave, 25), STAT=ErrStatTmp ) + ALLOCATE ( InitOut%WaveElevMD (0:InitOut%NStepWave, WaveGrid_nx*WaveGrid_ny), STAT=ErrStatTmp ) IF (ErrStatTmp /= 0) CALL SetErrStat(ErrID_Fatal,'Cannot allocate array InitOut%WaveElevMD.', ErrStat,ErrMsg,'VariousWaves_Init') - DO J = 1,5 !y = -60.0 + 20.0*J - DO K = 1,5 !x = -60.0 + 20.0*K + DO J = 1,WaveGrid_ny !y = -60.0 + 20.0*J + DO K = 1,WaveGrid_nx !x = -60.0 + 20.0*K - I = (J-1)*5.0 + K ! index of actual node + I = (J-1)*WaveGrid_nx + K ! index of actual node - CALL WaveElevTimeSeriesAtXY( -60.0 + 20.0*K, -60.0 + 20.0*J, InitOut%WaveElevMD(:,I), ErrStatTmp, ErrMsgTmp ) + CALL WaveElevTimeSeriesAtXY( WaveGrid_x0 + WaveGrid_dx*K, WaveGrid_y0 + WaveGrid_dy*J, InitOut%WaveElevMD(:,I), ErrStatTmp, ErrMsgTmp ) CALL SetErrStat(ErrStatTmp,'Error occured while applying the FFT to InitOut%WaveElevMD.',ErrStat,ErrMsg,'VariousWaves_Init') IF ( ErrStat >= AbortErrLev ) THEN CALL CleanUp() @@ -2202,6 +2217,10 @@ SUBROUTINE Waves_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, Init CASE ( 0 ) ! None=still water. + !@mhall: :::: ensure all arrays needed for the wave grid to MoorDyn are allocated in the WaveMod=0 case too :::: + ALLOCATE ( InitOut%WaveElevMD (0:InitOut%NStepWave, WaveGrid_nx*WaveGrid_ny), STAT=ErrStatTmp ) + ! ::::: end ::::: + CALL StillWaterWaves_Init( InitInp, InitOut, ErrStatTmp, ErrMsgTmp ) CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,'Waves_Init') IF ( ErrStat >= AbortErrLev ) RETURN diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index ba2af31c53..41c2a37f9d 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -22,6 +22,8 @@ MODULE MoorDyn USE MoorDyn_Types USE MoorDyn_IO USE NWTC_Library + + USE WAVES ! seeing if I can get waves data here directly... IMPLICIT NONE @@ -1426,6 +1428,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CtrlCode = MESH_SIBLING, IOS = COMPONENT_OUTPUT, & Force=.TRUE., Moment=.TRUE., ErrStat = ErrStat2, ErrMess=ErrMsg2 ) + CALL MeshCommit ( y%CoupledLoads, ErrStat, ErrMsg ) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN + CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN @@ -1507,35 +1513,35 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! :::::::::::::::: the above will be used eventually. For now, let's store wave info grids within this module ::::::::::::::::: ! allocate arrays I = SIZE(InitInp%WaveTime) - ALLOCATE ( p%ux (I,8,5,5), STAT = ErrStat2 ) - ALLOCATE ( p%uy (I,8,5,5), STAT = ErrStat2 ) - ALLOCATE ( p%uz (I,8,5,5), STAT = ErrStat2 ) - ALLOCATE ( p%ax (I,8,5,5), STAT = ErrStat2 ) - ALLOCATE ( p%ay (I,8,5,5), STAT = ErrStat2 ) - ALLOCATE ( p%az (I,8,5,5), STAT = ErrStat2 ) - ALLOCATE ( p%PDyn(I,8,5,5), STAT = ErrStat2 ) - ALLOCATE ( p%zeta(I,5,5), STAT = ErrStat2 ) ! 2D grid over x and y only - ALLOCATE ( p%px(5), STAT = ErrStat2 ) - ALLOCATE ( p%py(5), STAT = ErrStat2 ) - ALLOCATE ( p%pz(8), STAT = ErrStat2 ) + ALLOCATE ( p%ux (I,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ALLOCATE ( p%uy (I,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ALLOCATE ( p%uz (I,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ALLOCATE ( p%ax (I,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ALLOCATE ( p%ay (I,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ALLOCATE ( p%az (I,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ALLOCATE ( p%PDyn(I,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ALLOCATE ( p%zeta(I,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) ! 2D grid over x and y only + ALLOCATE ( p%px(WaveGrid_nx), STAT = ErrStat2 ) + ALLOCATE ( p%py(WaveGrid_ny), STAT = ErrStat2 ) + ALLOCATE ( p%pz(WaveGrid_nz), STAT = ErrStat2 ) ! get grid and time info (currenltly this is hard-coded to match what's in HydroDyn_Input - DO I=1,8 - p%pz(I) = 1.0 - 2.0**(8-I) ! -127, -63, -31, -15, -7, -3, -1, 0 + DO I=1,WaveGrid_nz + p%pz(I) = 1.0 - 2.0**(WaveGrid_nz-I) ! -127, -63, -31, -15, -7, -3, -1, 0 END DO - DO J = 1,5 - p%py(J) = -60.0 + 20.0*J + DO J = 1,WaveGrid_ny + p%py(J) = WaveGrid_y0 + WaveGrid_dy*J END DO - DO K = 1,5 - p%px(K) = -60.0 + 20.0*K + DO K = 1,WaveGrid_nx + p%px(K) = WaveGrid_x0 + WaveGrid_dx*K END DO p%dtWave = InitInp%WaveTime(2) - InitInp%WaveTime(1) ! fill in the grid data (the for loops match those in HydroDyn_Input) - DO I=1,8 - DO J = 1,5 - DO K = 1,5 - Itemp = (I-1)*25.0 + (J-1)*5.0 + K ! index of actual node on 3D grid + DO I=1,WaveGrid_nz + DO J = 1,WaveGrid_ny + DO K = 1,WaveGrid_nx + Itemp = (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node on 3D grid p%ux (:,I,J,K) = InitInp%WaveVel( :,Itemp,1) ! note: indices are t, z, y, x p%uy (:,I,J,K) = InitInp%WaveVel( :,Itemp,2) @@ -1549,9 +1555,9 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er END DO ! fill in the grid data (the for loops match those in HydroDyn_Input) - DO J = 1,5 - DO K = 1,5 - Itemp = (J-1)*5.0 + K ! index of actual node on surface 2D grid + DO J = 1,WaveGrid_ny + DO K = 1,WaveGrid_nx + Itemp = (J-1)*WaveGrid_nx + K ! index of actual node on surface 2D grid p%zeta(:,J,K) = InitInp%WaveElev(:,Itemp) END DO END DO @@ -1574,15 +1580,15 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - X input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) Frmt = '('//TRIM(Int2LStr(5))//'(A1,e10.4))' - WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%px(I))), I=1,5 ) + WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%px(I))), I=1,WaveGrid_nx ) WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - Y input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) Frmt = '('//TRIM(Int2LStr(5))//'(A1,e10.4))' - WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%py(I))), I=1,5 ) + WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%py(I))), I=1,WaveGrid_ny ) WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - Z input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) Frmt = '('//TRIM(Int2LStr(8))//'(A1,e10.4))' - WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%pz(I))), I=1,8 ) + WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%pz(I))), I=1,WaveGrid_nz ) CLOSE(UnOut, IOSTAT = ErrStat ) IF ( ErrStat /= 0 ) THEN @@ -1605,32 +1611,32 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! time WRITE(UnOut,"(A10)", IOSTAT=ErrStat2, advance="no") "Time" - DO J = 1,5 !y - DO K = 1,5 !x + DO J = 1,WaveGrid_ny !y + DO K = 1,WaveGrid_nx !x WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ze0", Num2Lstr(J+10*K) END DO END DO - DO I=1,8 !z - DO J = 1,5 !y - DO K = 1,5 !x + DO I=1,WaveGrid_nz !z + DO J = 1,WaveGrid_ny !y + DO K = 1,WaveGrid_nx !x WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ux", Num2Lstr(I+10*J+100*K) WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " uy", Num2Lstr(I+10*J+100*K) WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " uz", Num2Lstr(I+10*J+100*K) END DO END DO END DO - DO I=1,8 !z - DO J = 1,5 !y - DO K = 1,5 !x + DO I=1,WaveGrid_nz !z + DO J = 1,WaveGrid_ny !y + DO K = 1,WaveGrid_nx !x WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ax", Num2Lstr(I+10*J+100*K) WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ay", Num2Lstr(I+10*J+100*K) WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " az", Num2Lstr(I+10*J+100*K) END DO END DO END DO - DO I=1,8 !z - DO J = 1,5 !y - DO K = 1,5 !x + DO I=1,WaveGrid_nz !z + DO J = 1,WaveGrid_ny !y + DO K = 1,WaveGrid_nx !x WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " pd", Num2Lstr(I+10*J+100*K) END DO END DO @@ -1648,19 +1654,19 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !WRITE(UnOut,"(F10.4)", IOSTAT=ErrStat2, advance="no") InitInp%WaveTime(l) ! wave elevation (all slices for now, to check) - DO J = 1,5 !y - DO K = 1,5 !x - Itemp = (J-1)*5.0 + K ! index of actual node + DO J = 1,WaveGrid_ny !y + DO K = 1,WaveGrid_nx !x + Itemp = (J-1)*WaveGrid_nx + K ! index of actual node WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%zeta(l,J,K) END DO END DO ! wave velocities - DO I=1,8 !z - DO J = 1,5 !y - DO K = 1,5 !x - Itemp = (I-1)*25.0 + (J-1)*5.0 + K ! index of actual node + DO I=1,WaveGrid_nz !z + DO J = 1,WaveGrid_ny !y + DO K = 1,WaveGrid_nx !x + Itemp = (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%ux(l,I,J,K) WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%uy(l,I,J,K) @@ -1670,10 +1676,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er END DO ! wave accelerations - DO I=1,8 !z - DO J = 1,5 !y - DO K = 1,5 !x - Itemp = (I-1)*25.0 + (J-1)*5.0 + K ! index of actual node + DO I=1,WaveGrid_nz !z + DO J = 1,WaveGrid_ny !y + DO K = 1,WaveGrid_nx !x + Itemp = (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%ax(l,I,J,K) WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%ay(l,I,J,K) @@ -1683,10 +1689,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er END DO ! dynamic pressure - DO I=1,8 !z - DO J = 1,5 !y - DO K = 1,5 !x - Itemp = (I-1)*25.0 + (J-1)*5.0 + K ! index of actual node + DO I=1,WaveGrid_nz !z + DO J = 1,WaveGrid_ny !y + DO K = 1,WaveGrid_nx !x + Itemp = (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%PDyn(l,I,J,K) END DO diff --git a/modules/openfast-library/src/FAST_Subs.f90 b/modules/openfast-library/src/FAST_Subs.f90 index 7141e874f0..bce64bedfb 100644 --- a/modules/openfast-library/src/FAST_Subs.f90 +++ b/modules/openfast-library/src/FAST_Subs.f90 @@ -23,6 +23,8 @@ MODULE FAST_Subs USE FAST_Solver USE FAST_Linear + + USE Waves, ONLY : WaveGrid_n IMPLICIT NONE @@ -1010,10 +1012,10 @@ SUBROUTINE FAST_InitializeAll( t_initial, p_FAST, y_FAST, m_FAST, ED, BD, SrvD, !::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: !@mhall: for now, passing some hardcoded wave kinematics grid info from HD to MD - ALLOCATE ( Init%InData_MD%WaveVel (HD%p%NStepWave, 200, 3) ) - ALLOCATE ( Init%InData_MD%WaveAcc (HD%p%NStepWave, 200, 3) ) - ALLOCATE ( Init%InData_MD%WavePDyn (HD%p%NStepWave, 200) ) - ALLOCATE ( Init%InData_MD%WaveElev (HD%p%NStepWave, 200) ) + ALLOCATE ( Init%InData_MD%WaveVel (HD%p%NStepWave, WaveGrid_n, 3) ) + ALLOCATE ( Init%InData_MD%WaveAcc (HD%p%NStepWave, WaveGrid_n, 3) ) + ALLOCATE ( Init%InData_MD%WavePDyn (HD%p%NStepWave, WaveGrid_n) ) + ALLOCATE ( Init%InData_MD%WaveElev (HD%p%NStepWave, WaveGrid_n) ) ALLOCATE ( Init%InData_MD%WaveTime (HD%p%NStepWave) ) Init%InData_MD%WaveVel = Init%OutData_HD%WaveVel From 734e2468ffacbbcbed704661deb38a98d0814b3f Mon Sep 17 00:00:00 2001 From: Matt Hall Date: Fri, 15 Jan 2021 11:45:32 -0700 Subject: [PATCH 008/242] Small adjustment/bugfix to wave grid, MoorDyn coupled Rod bugfix, MoorDyn tidying. --- modules/hydrodyn/src/HydroDyn_Input.f90 | 4 +- modules/hydrodyn/src/Waves.f90 | 6 +- modules/moordyn/src/MoorDyn.f90 | 260 ++++++++++-------------- 3 files changed, 117 insertions(+), 153 deletions(-) diff --git a/modules/hydrodyn/src/HydroDyn_Input.f90 b/modules/hydrodyn/src/HydroDyn_Input.f90 index efa96f6230..c4329ce67d 100644 --- a/modules/hydrodyn/src/HydroDyn_Input.f90 +++ b/modules/hydrodyn/src/HydroDyn_Input.f90 @@ -4306,8 +4306,8 @@ SUBROUTINE HydroDynInput_ProcessInitData( InitInp, ErrStat, ErrMsg ) DO K = 1,WaveGrid_nx !x Itemp = InitInp%Morison%NNodes + (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node InitInp%Waves%WaveKinzi(Itemp) = 1.0 - 2.0**(WaveGrid_nz-I) ! -127, -63, -31, -15, -7, -3, -1, 0 - InitInp%Waves%WaveKinyi(Itemp) = WaveGrid_y0 + WaveGrid_dy*J - InitInp%Waves%WaveKinxi(Itemp) = WaveGrid_x0 + WaveGrid_dx*K + InitInp%Waves%WaveKinyi(Itemp) = WaveGrid_y0 + WaveGrid_dy*(J-1) + InitInp%Waves%WaveKinxi(Itemp) = WaveGrid_x0 + WaveGrid_dx*(K-1) InitInp%Current%MorisonNodezi(Itemp)= InitInp%Waves%WaveKinzi(I) END DO END DO diff --git a/modules/hydrodyn/src/Waves.f90 b/modules/hydrodyn/src/Waves.f90 index ced0b8f2f7..60d17ea75c 100644 --- a/modules/hydrodyn/src/Waves.f90 +++ b/modules/hydrodyn/src/Waves.f90 @@ -39,8 +39,8 @@ MODULE Waves INTEGER, PUBLIC :: WaveGrid_n = 150 ! Number of wave kinematics grid points = nx*ny*nz - REAL(SiKi), PUBLIC :: WaveGrid_x0 = -45.0 ! first grid point in x direction - REAL(SiKi), PUBLIC :: WaveGrid_dx = 15.0 ! step size in x direction + REAL(SiKi), PUBLIC :: WaveGrid_x0 = -35.0 ! first grid point in x direction + REAL(SiKi), PUBLIC :: WaveGrid_dx = 10.0 ! step size in x direction INTEGER, PUBLIC :: WaveGrid_nx = 10 ! Number of wave kinematics grid points in x REAL(SiKi), PUBLIC :: WaveGrid_y0 = -35.0 ! same for y @@ -1807,7 +1807,7 @@ SUBROUTINE VariousWaves_Init ( InitInp, InitOut, ErrStat, ErrMsg ) I = (J-1)*WaveGrid_nx + K ! index of actual node - CALL WaveElevTimeSeriesAtXY( WaveGrid_x0 + WaveGrid_dx*K, WaveGrid_y0 + WaveGrid_dy*J, InitOut%WaveElevMD(:,I), ErrStatTmp, ErrMsgTmp ) + CALL WaveElevTimeSeriesAtXY( WaveGrid_x0 + WaveGrid_dx*(K-1), WaveGrid_y0 + WaveGrid_dy*(J-1), InitOut%WaveElevMD(:,I), ErrStatTmp, ErrMsgTmp ) CALL SetErrStat(ErrStatTmp,'Error occured while applying the FFT to InitOut%WaveElevMD.',ErrStat,ErrMsg,'VariousWaves_Init') IF ( ErrStat >= AbortErrLev ) THEN CALL CleanUp() diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 41c2a37f9d..53739f1714 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -29,7 +29,7 @@ MODULE MoorDyn PRIVATE - TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a1', '5 Jan. 2020' ) + TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a2', '13 Jan. 2020' ) PUBLIC :: MD_Init @@ -182,7 +182,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! ----------------- go through file contents a first time, counting each entry ----------------------- i = 0 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i !read a line + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 !read a line do while ( ErrStat2 == 0 ) @@ -190,81 +190,81 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er if (INDEX(Line, "---") > 0) then ! look for a header line if ( ( INDEX(Line, "LINE DICTIONARY") > 0) .or. ( INDEX(Line, "LINE TYPES") > 0) ) then ! if line dictionary header -print *, "line dictionary" + ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! find how many elements of this type there are - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nLineTypes = p%nLineTypes + 1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," linetype on prev line" + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 END DO else if ( (INDEX(Line, "ROD DICTIONARY") > 0) .or. ( INDEX(Line, "ROD TYPES") > 0) ) then ! if rod dictionary header -print *, "rod dictionary" + ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! find how many elements of this type there are - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nRodTypes = p%nRodTypes + 1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," rod type on prev line" + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 END DO else if ((INDEX(Line, "BODY LIST") > 0 ) .or. (INDEX(Line, "BODY PROPERTIES") > 0 )) then -print *, "body list" + ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! find how many elements of this type there are - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nBodies = p%nBodies + 1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," body on prev line" + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 END DO else if ((INDEX(Line, "ROD LIST") > 0) .or. (INDEX(Line, "ROD PROPERTIES") > 0)) then ! if rod properties header -print *, "rod list" + ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! find how many elements of this type there are - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nRods = p%nRods + 1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," rod on prev line" + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 END DO else if ( (INDEX(Line, "CONNECTION PROPERTIES") > 0) .or. (INDEX(Line, "NODE PROPERTIES") > 0) ) then ! if node properties header -print *, "connections" + ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! find how many elements of this type there are - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nConnects = p%nConnects + 1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," con on prev line" + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 END DO else if (INDEX(Line, "LINE PROPERTIES") > 0) then ! if line properties header -print *, "lines" + ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! find how many elements of this type there are - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nLines = p%nLines + 1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," line on prev line" + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 END DO else if (INDEX(Line, "FAILURE") > 0) then ! if failure conditions header @@ -272,42 +272,42 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er print *, " Reading failure conditions: "; ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! find how many elements of this type there are - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nFails = p%nFails + 1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 END DO else if (INDEX(Line, "OPTIONS") > 0) then ! if options header -print *, "options" + ! don't skip any lines (no column headers for the options section) ! find how many options have been specified - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line nOpts = nOpts + 1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," option on prev line" + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 END DO else if (INDEX(Line, "OUTPUT") > 0) then ! if output header -print *, "output" + ! we don't need to count this section... - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 else ! otherwise ignore this line that isn't a recognized header line and read the next line - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," --- unrecognized header" + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 end if else ! otherwise ignore this line, which doesn't have the "---" or header line and read the next line - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i, " .." + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 end if end do @@ -380,7 +380,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er Nx = 0 ! set state counter to zero i = 0 ! set line number counter to zero - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i !read a line + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 !read a line do while ( ErrStat2 == 0 ) @@ -392,14 +392,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er print *, "Reading line types" ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! process each line DO l = 1,p%nLineTypes !read into a line - READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! parse out entries: Name Diam MassDenInAir EA cIntDamp >>EI(new)<< Can Cat Cdn Cdt READ(Line,*,IOSTAT=ErrStat2) m%LineTypeList(l)%name, m%LineTypeList(l)%d, & @@ -445,14 +445,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er print *, "Reading rod types" ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! process each line DO l = 1,p%nRodTypes !read into a line - READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! parse out entries: Name Diam MassDenInAir Can Cat Cdn Cdt IF (ErrStat2 == 0) THEN @@ -478,14 +478,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if ((INDEX(Line, "BODY LIST") > 0 ) .or. (INDEX(Line, "BODY PROPERTIES") > 0 )) then ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! process each body DO l = 1,p%nBodies !read into a line - READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! parse out entries: Node Type X Y Z M V FX FY FZ CdA Ca IF (ErrStat2 == 0) THEN @@ -572,14 +572,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er print *, "Reading rods" ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! process each rod DO l = 1,p%nRods !read into a line - READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! parse out entries: RodID Type/BodyID RodType Xa Ya Za Xb Yb Zb NumSegs Flags/Outputs IF (ErrStat2 == 0) THEN @@ -645,11 +645,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er return end if - else if ((let1 == "VESSEL") .or. (let1 == "VES")) then ! if a rigid fairlead, add to list and add - m%RodList(l)%typeNum = -2 - m%CpldRodIs(p%nCpldRods) = l; p%nCpldRods=p%nCpldRods+1 ! add this rod to coupled list + else if ((let1 == "VESSEL") .or. (let1 == "VES")) then ! if a rigidly coupled rod, add to list and add + m%RodList(l)%typeNum = -2 + + p%nCpldRods=p%nCpldRods+1 ! add this rod to coupled list + + m%CpldRodIs(p%nCpldRods) = l - else if ((let1 == "VESSELPINNED") .or. (let1 == "VESPIN")) then ! if a pinned fairlead, add to list and add + else if ((let1 == "VESSELPINNED") .or. (let1 == "VESPIN")) then ! if a pinned coupled rod, add to list and add m%RodList(l)%typeNum = -1 p%nCpldRods=p%nCpldRods+1 ! add @@ -749,14 +752,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er print *, "Reading connections" ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! process each point DO l = 1,p%nConnects !read into a line - READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! parse out entries: Node Type X Y Z M V FX FY FZ CdA Ca IF (ErrStat2 == 0) THEN @@ -879,14 +882,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er print *, "Reading lines" ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! process each line DO l = 1,p%nLines !read into a line - READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! parse out entries: LineID LineType UnstrLen NumSegs NodeA NodeB Flags/Outputs IF (ErrStat2 == 0) THEN @@ -1051,14 +1054,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! TODO: add stuff <<<<<<<< ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! process each line DO l = 1,p%nFails !read into a line - READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 END DO @@ -1073,7 +1076,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er DO l = 1,nOpts !read into a line - READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 ! parse out entries: value, option keyword READ(Line,*,IOSTAT=ErrStat2) OptValue, OptString ! look at first two entries, ignore remaining words in line, which should be comments @@ -1140,7 +1143,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! Read in all of the lines containing output parameters and store them in OutList(:) DO ! read a line - READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i + READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 CALL Conv2UC(Line) ! convert to uppercase for easy string matching @@ -1172,13 +1175,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !------------------------------------------------------------------------------------------- else ! otherwise ignore this line that isn't a recognized header line and read the next line - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i," --- unrecognized header" + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 end if !------------------------------------------------------------------------------------------- else ! otherwise ignore this line, which doesn't have the "---" or header line and read the next line - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1; print*,i, " .." + read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 end if end do @@ -1530,10 +1533,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er p%pz(I) = 1.0 - 2.0**(WaveGrid_nz-I) ! -127, -63, -31, -15, -7, -3, -1, 0 END DO DO J = 1,WaveGrid_ny - p%py(J) = WaveGrid_y0 + WaveGrid_dy*J + p%py(J) = WaveGrid_y0 + WaveGrid_dy*(J-1) END DO DO K = 1,WaveGrid_nx - p%px(K) = WaveGrid_x0 + WaveGrid_dx*K + p%px(K) = WaveGrid_x0 + WaveGrid_dx*(K-1) END DO p%dtWave = InitInp%WaveTime(2) - InitInp%WaveTime(1) @@ -2388,6 +2391,8 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er INTEGER(IntKi) :: Istart ! start index of line/connect in state vector INTEGER(IntKi) :: Iend ! end index of line/connect in state vector + REAL(DbKi) :: temp(3) ! temporary for passing kinematics + REAL(DbKi) :: r6_in(6) ! temporary for passing kinematics REAL(DbKi) :: v6_in(6) ! temporary for passing kinematics REAL(DbKi) :: a6_in(6) ! temporary for passing kinematics @@ -2439,18 +2444,6 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er a6_in(1:3) = u%CoupledKinematics%TranslationAcc(:,J) a6_in(4:6) = u%CoupledKinematics%RotationAcc(:,J) - - if ((t >= 1.0) .and. (t < 1.001)) then - print *, "orientation matrix from mesh:" - print *, u%CoupledKinematics%Orientation(:,:,J) - print *, "Euler extract:" - print *, EulerExtract( u%CoupledKinematics%Orientation(:,:,J) ) - print *, "Euler extract of transpose" - print *, EulerExtract( transpose(u%CoupledKinematics%Orientation(:,:,J) )) - - print *, "done" - end if - CALL Body_SetKinematics(m%BodyList(m%CpldBodyIs(l)), r6_in, v6_in, a6_in, t, m) END DO @@ -2459,22 +2452,11 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er J = J + 1 r6_in(1:3) = u%CoupledKinematics%Position(:,J) + u%CoupledKinematics%TranslationDisp(:,J) - r6_in(4:6) = EulerExtract( TRANSPOSE( u%CoupledKinematics%Orientation(:,:,J) ) ) ! <<<< should make sure this works <<< + r6_in(4:6) = MATMUL( u%CoupledKinematics%Orientation(:,:,J) , (/0.0, 0.0, 1.0/) ) ! <<<< CHECK ! adjustment because rod's rotational entries are a unit vector, q v6_in(1:3) = u%CoupledKinematics%TranslationVel(:,J) v6_in(4:6) = u%CoupledKinematics%RotationVel(:,J) a6_in(1:3) = u%CoupledKinematics%TranslationAcc(:,J) a6_in(4:6) = u%CoupledKinematics%RotationAcc(:,J) - - if ((t >= 1.0) .and. (t < 1.001)) then - print *, "orientation matrix from mesh:" - print *, u%CoupledKinematics%Orientation(:,:,J) - print *, "Euler extract:" - print *, EulerExtract( u%CoupledKinematics%Orientation(:,:,J) ) - print *, "Euler extract of transpose" - print *, EulerExtract( transpose(u%CoupledKinematics%Orientation(:,:,J) )) - - print *, "done" - end if CALL Rod_SetKinematics(m%RodList(m%CpldRodIs(l)), r6_in, v6_in, a6_in, t, m%LineList) @@ -4738,10 +4720,6 @@ SUBROUTINE Rod_Initialize(Rod, states, LineList) ! note: this may also be called by a coupled rod (type = -1) in which case states will be empty - print *, " states: ", states - print *, " r0: ", Rod%r(:,0) - print *, " q : ", Rod%q - END SUBROUTINE Rod_Initialize !-------------------------------------------------------------- @@ -5215,7 +5193,7 @@ SUBROUTINE Rod_DoRHS(Rod, LineList, p) if ((Rod%r(3,0) < zeta) .and. (Rod%r(3,N) > zeta)) then ! check if it's crossing the water plane (should also add some limits to avoid near-horizontals at some point) h0 = (zeta - Rod%r(3,0))/Rod%q(3) ! distance along rod centerline from end A to the waterplane else if (Rod%r(3,0) < zeta) then - h0 = 2.0*Rod%UnstrLen ! fully submerged case + h0 = 2.0*Rod%UnstrLen ! fully submerged case <<<<<< remove the 2.0 and double check there are no if statements that get changed <<<< else h0 = 0.0_DbKi ! fully unsubmerged case (ever applicable?) end if @@ -5361,13 +5339,8 @@ SUBROUTINE Rod_DoRHS(Rod, LineList, p) ! axial drag Rod%Dq(:,I) = Rod%Dq(:,I) + VOF * 0.25* Pi*Rod%d*Rod%d * p%rhoW*Rod%CdEnd * MagVq * Vq - -if ((Rod%time >= 1.0) .and. (Rod%time < 1.001)) then - print *, "at Dq end 0 of rod:" - print *, "CdEnd is on position vector:" - print *, Rod%CdEnd -end if - + + ! >>> what about rotational drag?? <<< eqn will be Pi* Rod%d**4/16.0 omega_rel?^2... *0.5 * Cd... ! Froud-Krylov force @@ -5857,13 +5830,6 @@ SUBROUTINE Body_SetDependentKin(Body, t, m) !CALL SmllRotTrans('', Body%r6(4), Body%r6(5), Body%r6(6), Body%TransMat, '', ErrStat2, ErrMsg2) Body%OrMat = EulerConstruct( Body%r6(4:6) ) ! full Euler angle approach <<<< need to check order -if ((t >= 1.0) .and. (t < 1.001)) then - print *, "orientation matrix OrMat of Body:" - print *, Body%OrMat - print *, "based on position vector:" - print *, Body%r6 -end if - ! set kinematics of any dependent connections do l = 1,Body%nAttachedC @@ -6146,14 +6112,12 @@ SUBROUTINE getWaveKin(p, x, y, z, t, U, Ud, zeta, PDyn) SUBROUTINE getInterpNums(xlist, xin, i, fout) - Real(DbKi), INTENT (IN ) :: xlist(:) - Real(DbKi), INTENT (IN ) :: xin - Integer(IntKi),INTENT ( OUT) :: i - Real(DbKi), INTENT ( OUT) :: fout + Real(DbKi), INTENT (IN ) :: xlist(:) ! list of x values + Real(DbKi), INTENT (IN ) :: xin ! x value to be interpolated + Integer(IntKi),INTENT ( OUT) :: i ! lower index to interpolate from + Real(DbKi), INTENT ( OUT) :: fout ! fraction to return such that y* = y[i] + fout*(y[i+1]-y[i]) Integer(IntKi) :: nx - ! Parameters: list of x values, number of x values, x value to be interpolated, fraction to return - ! Returns the lower index to interpolate from. such that y* = y[i] + fout*(y[i+1]-y[i]) nx = SIZE(xlist) @@ -6180,7 +6144,7 @@ SUBROUTINE getInterpNums(xlist, xin, i, fout) SUBROUTINE calculate4Dinterpolation(f, ix0, iy0, iz0, it0, fx, fy, fz, ft, c) Real(DbKi), INTENT (IN ) :: f(:,:,:,:) ! data array - INTEGER(IntKi), INTENT (IN ) :: ix0, iy0, iz0, it0 ! indeces for interpolation + INTEGER(IntKi), INTENT (IN ) :: ix0, iy0, iz0, it0 ! indices for interpolation Real(DbKi), INTENT (IN ) :: fx, fy, fz, ft ! interpolation fractions Real(DbKi), INTENT ( OUT) :: c ! the output value @@ -6213,24 +6177,24 @@ SUBROUTINE calculate4Dinterpolation(f, ix0, iy0, iz0, it0, fx, fy, fz, ft, c) it1 = it0+1 end if - c000 = f(it0,iz0,iy0,ix0)*(1-ft) + f(it1,iz0,iy0,ix0)*ft - c001 = f(it0,iz1,iy0,ix0)*(1-ft) + f(it1,iz1,iy0,ix0)*ft - c010 = f(it0,iz0,iy1,ix0)*(1-ft) + f(it1,iz0,iy1,ix0)*ft - c011 = f(it0,iz1,iy1,ix0)*(1-ft) + f(it1,iz1,iy1,ix0)*ft - c100 = f(it0,iz0,iy0,ix1)*(1-ft) + f(it1,iz0,iy0,ix1)*ft - c101 = f(it0,iz1,iy0,ix1)*(1-ft) + f(it1,iz1,iy0,ix1)*ft - c110 = f(it0,iz0,iy1,ix1)*(1-ft) + f(it1,iz0,iy1,ix1)*ft - c111 = f(it0,iz1,iy1,ix1)*(1-ft) + f(it1,iz1,iy1,ix1)*ft + c000 = f(it0,iz0,iy0,ix0)*(1.0-ft) + f(it1,iz0,iy0,ix0)*ft + c001 = f(it0,iz1,iy0,ix0)*(1.0-ft) + f(it1,iz1,iy0,ix0)*ft + c010 = f(it0,iz0,iy1,ix0)*(1.0-ft) + f(it1,iz0,iy1,ix0)*ft + c011 = f(it0,iz1,iy1,ix0)*(1.0-ft) + f(it1,iz1,iy1,ix0)*ft + c100 = f(it0,iz0,iy0,ix1)*(1.0-ft) + f(it1,iz0,iy0,ix1)*ft + c101 = f(it0,iz1,iy0,ix1)*(1.0-ft) + f(it1,iz1,iy0,ix1)*ft + c110 = f(it0,iz0,iy1,ix1)*(1.0-ft) + f(it1,iz0,iy1,ix1)*ft + c111 = f(it0,iz1,iy1,ix1)*(1.0-ft) + f(it1,iz1,iy1,ix1)*ft - c00 = c000*(1-fx) + c100*fx - c01 = c001*(1-fx) + c101*fx - c10 = c010*(1-fx) + c110*fx - c11 = c011*(1-fx) + c111*fx + c00 = c000*(1.0-fx) + c100*fx + c01 = c001*(1.0-fx) + c101*fx + c10 = c010*(1.0-fx) + c110*fx + c11 = c011*(1.0-fx) + c111*fx - c0 = c00 *(1-fy) + c10 *fy - c1 = c01 *(1-fy) + c11 *fy + c0 = c00 *(1.0-fy) + c10 *fy + c1 = c01 *(1.0-fy) + c11 *fy - c = c0 *(1-fz) + c1 *fz + c = c0 *(1.0-fz) + c1 *fz END SUBROUTINE @@ -6238,7 +6202,7 @@ SUBROUTINE calculate4Dinterpolation(f, ix0, iy0, iz0, it0, fx, fy, fz, ft, c) SUBROUTINE calculate3Dinterpolation(f, ix0, iy0, iz0, fx, fy, fz, c) Real(DbKi), INTENT (IN ) :: f(:,:,:) ! data array - INTEGER(IntKi), INTENT (IN ) :: ix0, iy0, iz0 ! indeces for interpolation + INTEGER(IntKi), INTENT (IN ) :: ix0, iy0, iz0 ! indices for interpolation Real(DbKi), INTENT (IN ) :: fx, fy, fz ! interpolation fractions Real(DbKi), INTENT ( OUT) :: c ! the output value @@ -6276,15 +6240,15 @@ SUBROUTINE calculate3Dinterpolation(f, ix0, iy0, iz0, fx, fy, fz, c) c110 = f(iz0,iy1,ix1) c111 = f(iz1,iy1,ix1) - c00 = c000*(1-fx) + c100*fx - c01 = c001*(1-fx) + c101*fx - c10 = c010*(1-fx) + c110*fx - c11 = c011*(1-fx) + c111*fx + c00 = c000*(1.0-fx) + c100*fx + c01 = c001*(1.0-fx) + c101*fx + c10 = c010*(1.0-fx) + c110*fx + c11 = c011*(1.0-fx) + c111*fx - c0 = c00 *(1-fy) + c10 *fy - c1 = c01 *(1-fy) + c11 *fy + c0 = c00 *(1.0-fy) + c10 *fy + c1 = c01 *(1.0-fy) + c11 *fy - c = c0 *(1-fz) + c1 *fz + c = c0 *(1.0-fz) + c1 *fz END SUBROUTINE From 130547e455f13ce2158b57fe2a3fd14955e5b570 Mon Sep 17 00:00:00 2001 From: Matt Hall Date: Fri, 15 Jan 2021 12:53:01 -0700 Subject: [PATCH 009/242] Correction of my earlier edits in FAST_Subs - no if statements for allocating all mooring models' inputs. --- modules/openfast-library/src/FAST_Subs.f90 | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) diff --git a/modules/openfast-library/src/FAST_Subs.f90 b/modules/openfast-library/src/FAST_Subs.f90 index bce64bedfb..f17e03314f 100644 --- a/modules/openfast-library/src/FAST_Subs.f90 +++ b/modules/openfast-library/src/FAST_Subs.f90 @@ -929,28 +929,18 @@ SUBROUTINE FAST_InitializeAll( t_initial, p_FAST, y_FAST, m_FAST, ED, BD, SrvD, CALL Cleanup() RETURN END IF - !@mhall: clean this up! <<<<<<<<<<<<<<<< ::::::::::::::::::::::::::::: - if (allocated(FEAM%Input)) then - print *, "FEAM%Input is already allocated!!" - end if - if (allocated(FEAM%InputTimes)) then - print *, "FEAM%InputTimes is already allocated!!" - end if - IF (p_FAST%CompMooring == Module_FEAM) THEN - ALLOCATE( FEAM%Input( p_FAST%InterpOrder+1 ), FEAM%InputTimes( p_FAST%InterpOrder+1 ), STAT = ErrStat2 ) + ALLOCATE( FEAM%Input( p_FAST%InterpOrder+1 ), FEAM%InputTimes( p_FAST%InterpOrder+1 ), STAT = ErrStat2 ) IF (ErrStat2 /= 0) THEN CALL SetErrStat(ErrID_Fatal,"Error allocating FEAM%Input and FEAM%InputTimes.",ErrStat,ErrMsg,RoutineName) CALL Cleanup() RETURN END IF - ELSE IF (p_FAST%CompMooring == Module_Orca) THEN - ALLOCATE( Orca%Input( p_FAST%InterpOrder+1 ), Orca%InputTimes( p_FAST%InterpOrder+1 ), STAT = ErrStat2 ) + ALLOCATE( Orca%Input( p_FAST%InterpOrder+1 ), Orca%InputTimes( p_FAST%InterpOrder+1 ), STAT = ErrStat2 ) IF (ErrStat2 /= 0) THEN CALL SetErrStat(ErrID_Fatal,"Error allocating Orca%Input and Orca%InputTimes.",ErrStat,ErrMsg,RoutineName) CALL Cleanup() RETURN END IF - END IF ! ........................ ! initialize MAP From 82a1d22403842ad976245a58fa5dc000bff46645 Mon Sep 17 00:00:00 2001 From: Matt Hall Date: Thu, 21 Jan 2021 13:44:31 -0700 Subject: [PATCH 010/242] Bug fixes in wave grid implementation and MD Rod Froude-Krylov force. - Solved memory bug in WameMod=0 case by reordering htings. - Removing unnecessary USE WAVES and specifying "only..." in MD. - In MD, now using WaveTimes array rather than (J-1)*dtWave. - Fixed error in MD Rod Froude Krylov force calculation (was 50% too small) --- modules/hydrodyn/src/HydroDyn.f90 | 1 - modules/hydrodyn/src/Waves.f90 | 8 ++-- modules/moordyn/src/MoorDyn.f90 | 56 +++++++++++----------- modules/moordyn/src/MoorDyn_Registry.txt | 2 +- modules/moordyn/src/MoorDyn_Types.f90 | 61 +++++++++++++++++++++--- 5 files changed, 87 insertions(+), 41 deletions(-) diff --git a/modules/hydrodyn/src/HydroDyn.f90 b/modules/hydrodyn/src/HydroDyn.f90 index d144653c50..fb730da27b 100644 --- a/modules/hydrodyn/src/HydroDyn.f90 +++ b/modules/hydrodyn/src/HydroDyn.f90 @@ -33,7 +33,6 @@ MODULE HydroDyn USE HydroDyn_Input USE HydroDyn_Output USE Current - USE Waves USE Waves2 #ifdef USE_FIT USE FIT_MODULES diff --git a/modules/hydrodyn/src/Waves.f90 b/modules/hydrodyn/src/Waves.f90 index 60d17ea75c..580dd0d2dc 100644 --- a/modules/hydrodyn/src/Waves.f90 +++ b/modules/hydrodyn/src/Waves.f90 @@ -2217,14 +2217,14 @@ SUBROUTINE Waves_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, Init CASE ( 0 ) ! None=still water. + CALL StillWaterWaves_Init( InitInp, InitOut, ErrStatTmp, ErrMsgTmp ) + CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,'Waves_Init') + !@mhall: :::: ensure all arrays needed for the wave grid to MoorDyn are allocated in the WaveMod=0 case too :::: ALLOCATE ( InitOut%WaveElevMD (0:InitOut%NStepWave, WaveGrid_nx*WaveGrid_ny), STAT=ErrStatTmp ) ! ::::: end ::::: - - CALL StillWaterWaves_Init( InitInp, InitOut, ErrStatTmp, ErrMsgTmp ) - CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,'Waves_Init') + IF ( ErrStat >= AbortErrLev ) RETURN - CASE ( 1, 2, 3, 4, 10 ) ! 1, 10: Plane progressive (regular) wave, 2: JONSWAP/Pierson-Moskowitz spectrum (irregular) wave, 3: white-noise, or 4: user-defined spectrum (irregular) wave. diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 53739f1714..1fb33b0d5c 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -23,13 +23,13 @@ MODULE MoorDyn USE MoorDyn_IO USE NWTC_Library - USE WAVES ! seeing if I can get waves data here directly... + USE WAVES, only: WaveGrid_n, WaveGrid_x0, WaveGrid_dx, WaveGrid_nx, WaveGrid_y0, WaveGrid_dy, WaveGrid_ny, WaveGrid_nz ! seeing if I can get waves data here directly... IMPLICIT NONE PRIVATE - TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a2', '13 Jan. 2020' ) + TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a3', '20 Jan. 2020' ) PUBLIC :: MD_Init @@ -77,6 +77,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er REAL(DbKi) :: dtM ! actual mooring dynamics time step INTEGER(IntKi) :: NdtM ! number of time steps to integrate through with RK2 + INTEGER(IntKi) :: ntWave ! number of time steps of wave data TYPE(MD_InputType) :: u_array(1) ! a size-one array for u to make call to TimeStep happy REAL(DbKi) :: t_array(1) ! a size-one array saying time is 0 to make call to TimeStep happy @@ -1431,9 +1432,9 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CtrlCode = MESH_SIBLING, IOS = COMPONENT_OUTPUT, & Force=.TRUE., Moment=.TRUE., ErrStat = ErrStat2, ErrMess=ErrMsg2 ) - CALL MeshCommit ( y%CoupledLoads, ErrStat, ErrMsg ) - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN + !CALL MeshCommit ( y%CoupledLoads, ErrStat, ErrMsg ) + ! CALL CheckError( ErrStat2, ErrMsg2 ) + ! IF (ErrStat >= AbortErrLev) RETURN CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN @@ -1513,20 +1514,21 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! END DO ! END DO - ! :::::::::::::::: the above will be used eventually. For now, let's store wave info grids within this module ::::::::::::::::: + ! :::::::::::::::: the above might be used eventually. For now, let's store wave info grids within this module ::::::::::::::::: ! allocate arrays - I = SIZE(InitInp%WaveTime) - ALLOCATE ( p%ux (I,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ALLOCATE ( p%uy (I,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ALLOCATE ( p%uz (I,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ALLOCATE ( p%ax (I,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ALLOCATE ( p%ay (I,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ALLOCATE ( p%az (I,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ALLOCATE ( p%PDyn(I,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ALLOCATE ( p%zeta(I,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) ! 2D grid over x and y only + ntWave = SIZE(InitInp%WaveTime) + ALLOCATE ( p%ux (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ALLOCATE ( p%uy (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ALLOCATE ( p%uz (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ALLOCATE ( p%ax (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ALLOCATE ( p%ay (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ALLOCATE ( p%az (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ALLOCATE ( p%PDyn(ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ALLOCATE ( p%zeta(ntWave,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) ! 2D grid over x and y only ALLOCATE ( p%px(WaveGrid_nx), STAT = ErrStat2 ) ALLOCATE ( p%py(WaveGrid_ny), STAT = ErrStat2 ) ALLOCATE ( p%pz(WaveGrid_nz), STAT = ErrStat2 ) + ALLOCATE ( p%tWave(ntWave), STAT = ErrStat2 ) ! get grid and time info (currenltly this is hard-coded to match what's in HydroDyn_Input DO I=1,WaveGrid_nz @@ -1538,7 +1540,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er DO K = 1,WaveGrid_nx p%px(K) = WaveGrid_x0 + WaveGrid_dx*(K-1) END DO - p%dtWave = InitInp%WaveTime(2) - InitInp%WaveTime(1) + + p%tWave = InitInp%WaveTime ! fill in the grid data (the for loops match those in HydroDyn_Input) DO I=1,WaveGrid_nz @@ -1653,7 +1656,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er DO l=1, SIZE(InitInp%WaveTime) ! loop through all time steps ! time - WRITE(UnOut,"(F10.4)", IOSTAT=ErrStat2, advance="no") (l-1)*p%dtWave + WRITE(UnOut,"(F10.4)", IOSTAT=ErrStat2, advance="no") p%tWave(l) !WRITE(UnOut,"(F10.4)", IOSTAT=ErrStat2, advance="no") InitInp%WaveTime(l) ! wave elevation (all slices for now, to check) @@ -3341,7 +3344,7 @@ SUBROUTINE Catenary ( XF_In, ZF_In, L_In , EA_In, & ELSEIF ( ZF < 0.0_DbKi ) THEN ! .TRUE. if the fairlead has passed below its anchor ErrStat = ErrID_Warn - ErrMsg = ' A fairlead has passed below its anchor.' + ErrMsg = " A line's fairlead is defined as below its anchor. You may need to swap a line's fairlead and anchor end nodes." return ELSEIF ( L <= 0.0_DbKi ) THEN ! .TRUE. when the unstretched line length is specified incorrectly @@ -5287,12 +5290,12 @@ SUBROUTINE Rod_DoRHS(Rod, LineList, p) aq = DOT_PRODUCT(Rod%Ud(:,I), Rod%q) * Rod%q ! tangential component of fluid acceleration ap = Rod%Ud(:,I) - aq ! normal component of fluid acceleration ! transverse Froude-Krylov force - Rod%Ap(:,I) = VOF * p%rhoW*(1.0+Rod%Can)*0.5* v_i * ap ! <<< are these equations right?? + Rod%Ap(:,I) = VOF * p%rhoW*(1.0+Rod%Can)* v_i * ap ! ! axial Froude-Krylov force - Rod%Aq(:,I) = 0.0_DbKi ! p%rhoW*(1.0+Rod%Cat)*0.5* v_i * aq ! <<< are these equations right?? + Rod%Aq(:,I) = 0.0_DbKi ! p%rhoW*(1.0+Rod%Cat)* v_i * aq ! <<< just put a taper-based term here eventually? ! dynamic pressure - Rod%Pd(:,I) = 0.0_DbKi ! assuming zero for sides + Rod%Pd(:,I) = 0.0_DbKi ! assuming zero for sides for now, until taper comes into play ! bottom contact (stiffness and damping, vertical-only for now) - updated Nov 24 for general case where anchor and fairlead ends may deal with bottom contact forces IF (Rod%r(3,I) < -p%WtrDpth) THEN @@ -6088,13 +6091,10 @@ SUBROUTINE getWaveKin(p, x, y, z, t, U, Ud, zeta, PDyn) Real(DbKi) :: qt ! used in time step interpolation - CALL getInterpNums(p%px, x, ix, fx) - CALL getInterpNums(p%py, y, iy, fy) - CALL getInterpNums(p%pz, z, iz, fz) - - qt = t / real(p%dtWave, DbKi) - it = floor(qt) + 1 ! adjust by 1 for fortran's indexing starting at 1 - ft = qt - it + 1.0; !(t-(it*dtWave))/dtWave ! //remainder(t,dtWave)/dtWave; + CALL getInterpNums(p%px , x, ix, fx) + CALL getInterpNums(p%py , y, iy, fy) + CALL getInterpNums(p%pz , z, iz, fz) + CALL getInterpNums(p%tWave, t, it, ft) CALL calculate3Dinterpolation(p%zeta, ix, iy, it, fx, fy, ft, zeta) diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 1d98a46563..d2d859fadd 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -347,7 +347,7 @@ typedef ^ ^ DbKi zeta {:}{:}{:} typedef ^ ^ DbKi px {:} - - "" - typedef ^ ^ DbKi py {:} - - "" - typedef ^ ^ DbKi pz {:} - - "" - -typedef ^ ^ DbKi dtWave - - - "" - +typedef ^ ^ DbKi tWave {:} - - "wave data time step array" - # ============================== Inputs ============================================================================================================================================ diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index 54d762d7e9..23ba05024e 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -376,7 +376,7 @@ MODULE MoorDyn_Types REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: px !< [-] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: py !< [-] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: pz !< [-] - REAL(DbKi) :: dtWave !< [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: tWave !< wave data time step array [-] END TYPE MD_ParameterType ! ======================= ! ========= MD_InputType ======= @@ -9203,7 +9203,18 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) END IF DstParamData%pz = SrcParamData%pz ENDIF - DstParamData%dtWave = SrcParamData%dtWave +IF (ALLOCATED(SrcParamData%tWave)) THEN + i1_l = LBOUND(SrcParamData%tWave,1) + i1_u = UBOUND(SrcParamData%tWave,1) + IF (.NOT. ALLOCATED(DstParamData%tWave)) THEN + ALLOCATE(DstParamData%tWave(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%tWave.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%tWave = SrcParamData%tWave +ENDIF END SUBROUTINE MD_CopyParam SUBROUTINE MD_DestroyParam( ParamData, ErrStat, ErrMsg ) @@ -9253,6 +9264,9 @@ SUBROUTINE MD_DestroyParam( ParamData, ErrStat, ErrMsg ) ENDIF IF (ALLOCATED(ParamData%pz)) THEN DEALLOCATE(ParamData%pz) +ENDIF +IF (ALLOCATED(ParamData%tWave)) THEN + DEALLOCATE(ParamData%tWave) ENDIF END SUBROUTINE MD_DestroyParam @@ -9398,7 +9412,11 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Int_BufSz = Int_BufSz + 2*1 ! pz upper/lower bounds for each dimension Db_BufSz = Db_BufSz + SIZE(InData%pz) ! pz END IF - Db_BufSz = Db_BufSz + 1 ! dtWave + Int_BufSz = Int_BufSz + 1 ! tWave allocated yes/no + IF ( ALLOCATED(InData%tWave) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! tWave upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%tWave) ! tWave + END IF IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) IF (ErrStat2 /= 0) THEN @@ -9807,8 +9825,21 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Db_Xferred = Db_Xferred + 1 END DO END IF - DbKiBuf(Db_Xferred) = InData%dtWave - Db_Xferred = Db_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%tWave) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%tWave,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%tWave,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%tWave,1), UBOUND(InData%tWave,1) + DbKiBuf(Db_Xferred) = InData%tWave(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF END SUBROUTINE MD_PackParam SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) @@ -10270,8 +10301,24 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg Db_Xferred = Db_Xferred + 1 END DO END IF - OutData%dtWave = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! tWave not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%tWave)) DEALLOCATE(OutData%tWave) + ALLOCATE(OutData%tWave(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%tWave.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%tWave,1), UBOUND(OutData%tWave,1) + OutData%tWave(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF END SUBROUTINE MD_UnPackParam SUBROUTINE MD_CopyInput( SrcInputData, DstInputData, CtrlCode, ErrStat, ErrMsg ) From 983d4a7c3b31dc8c4b7101dc96cff48925edfe40 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Wed, 3 Feb 2021 20:41:24 -0700 Subject: [PATCH 011/242] Ensuring no side forces in node weights --- modules/moordyn/src/MoorDyn.f90 | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 1fb33b0d5c..ad0b424dfc 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -5961,7 +5961,8 @@ SUBROUTINE Body_DoRHS(Body, m, p) Body%M = RotateM6(Body%M0, Body%OrMat) !gravity on core body - + Fgrav(1) = 0.0_DbKi + Fgrav(2) = 0.0_DbKi Fgrav(3) = Body%bodyV * p%rhow * p%g - Body%bodyM * p%g ! weight+buoyancy vector body_rCGrotated = MATMUL(Body%OrMat, Body%rCG) ! rotateVector3(body_rCG, OrMat, body_rCGrotated); ! relative vector to body CG in inertial orientation From 5d17d6d73a722da4ae01f6f0cb022cba8c6d646c Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Thu, 18 Mar 2021 18:44:29 -0600 Subject: [PATCH 012/242] The first version shared for USFLOWT. --- modules/hydrodyn/src/Waves.f90 | 1 + modules/moordyn/src/MoorDyn.f90 | 687 ++++++++++++++--------- modules/moordyn/src/MoorDyn_IO.f90 | 89 ++- modules/moordyn/src/MoorDyn_Registry.txt | 17 +- modules/moordyn/src/MoorDyn_Types.f90 | 14 + 5 files changed, 501 insertions(+), 307 deletions(-) diff --git a/modules/hydrodyn/src/Waves.f90 b/modules/hydrodyn/src/Waves.f90 index 580dd0d2dc..9cc8b600da 100644 --- a/modules/hydrodyn/src/Waves.f90 +++ b/modules/hydrodyn/src/Waves.f90 @@ -2222,6 +2222,7 @@ SUBROUTINE Waves_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, Init !@mhall: :::: ensure all arrays needed for the wave grid to MoorDyn are allocated in the WaveMod=0 case too :::: ALLOCATE ( InitOut%WaveElevMD (0:InitOut%NStepWave, WaveGrid_nx*WaveGrid_ny), STAT=ErrStatTmp ) + InitOut%WaveElevMD = 0.0_DbKi ! zero it ! ::::: end ::::: IF ( ErrStat >= AbortErrLev ) RETURN diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index ad0b424dfc..5fc8c670f2 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -29,8 +29,9 @@ MODULE MoorDyn PRIVATE - TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a3', '20 Jan. 2020' ) + TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a8', '15 Feb. 2021' ) + INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output PUBLIC :: MD_Init PUBLIC :: MD_UpdateStates @@ -113,7 +114,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CHARACTER(25) :: let3 REAL(DbKi) :: tempArray(6) - REAL(ReKi) :: rRef(6) + REAL(ReKi) :: rRef(6) ! used to pass positions to mesh (real type precision) REAL(DbKi) :: rRefDub(3) ! for reading output channels @@ -136,6 +137,9 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL DispNVD( MD_ProgDesc ) InitOut%Ver = MD_ProgDesc + CALL WrScr(' This is an alpha version of MoorDyn-F v2, with significant input file changes from v1.') + CALL WrScr(' Copyright: (C) 2021 National Renewable Energy Laboratory, (C) 2019 Matt Hall') + !--------------------------------------------------------------------------------------------- ! Get all the inputs taken care of @@ -151,6 +155,25 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er p%RootName = TRIM(InitInp%RootName)//'.MD' ! all files written from this module will have this root name + ! Check for farm-level inputs (indicating that this MoorDyn isntance is being run from FAST.Farm) + !intead of below, check first dimension of PtfmInit + !p%nTurbines = SIZE(InitInp%FarmCoupledKinematics) ! the number of turbines in the array (0 indicates a regular OpenFAST simulation with 1 turbine) + ! + !IF (p%nTurbines > 0) then + ! WrScr(' >>> MoorDyn is running in array mode <<< ') + ! + ! ! copy over the meshes to be this instance's array of mesh inputs (a duplicate) + ! ! >>> initialize array u%FarmCoupledKinematics + ! ! >>> copy each mesh from InitInp%FarmCoupledKinematics into it + ! ! CALL MeshCopy ( SrcMesh = InitInp%FarmCoupledKinematics, DestMesh = u%FarmCoupledKinematics, CtrlCode = MESH_SIBLING, IOS=COMPONENT_OUTPUT, Force=.TRUE., Moment=.TRUE., ErrStat = ErrStat2, ErrMess=ErrMsg2 ) + ! + ! ! OR instead of copying the meshes, could just make empty meshes, then populate with exactly the elements needed at the farm level + ! !InitInp%FarmNCpldBodies + ! !InitInp%FarmNCpldRods + ! !InitInp%FarmNCpldCons + ! + !END IF + !--------------------------------------------------------------------------------------------- ! read input file and create cross-referenced mooring system objects !--------------------------------------------------------------------------------------------- @@ -176,10 +199,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er END IF - !CALL WrScr( ' MD_Init: Opening MoorDyn input file: '//FileName ) + CALL WrScr( ' Reading MoorDyn input file: '//FileName ) + - ! ----------------- go through file contents a first time, counting each entry ----------------------- i = 0 @@ -216,7 +239,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 END DO - else if ((INDEX(Line, "BODY LIST") > 0 ) .or. (INDEX(Line, "BODY PROPERTIES") > 0 )) then + else if ((INDEX(Line, "BODIES") > 0 ) .or. (INDEX(Line, "BODY LIST") > 0 ) .or. (INDEX(Line, "BODY PROPERTIES") > 0 )) then ! skip following two lines (label line and unit line) read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 @@ -229,7 +252,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 END DO - else if ((INDEX(Line, "ROD LIST") > 0) .or. (INDEX(Line, "ROD PROPERTIES") > 0)) then ! if rod properties header + else if ((INDEX(Line, "RODS") > 0 ) .or. (INDEX(Line, "ROD LIST") > 0) .or. (INDEX(Line, "ROD PROPERTIES") > 0)) then ! if rod properties header ! skip following two lines (label line and unit line) read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 @@ -242,7 +265,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 END DO - else if ( (INDEX(Line, "CONNECTION PROPERTIES") > 0) .or. (INDEX(Line, "NODE PROPERTIES") > 0) ) then ! if node properties header + else if ((INDEX(Line, "POINTS") > 0 ) .or. (INDEX(Line, "CONNECTION PROPERTIES") > 0) .or. (INDEX(Line, "NODE PROPERTIES") > 0) .or. (INDEX(Line, "POINT PROPERTIES") > 0) .or. (INDEX(Line, "POINT LIST") > 0) ) then ! if node properties header ! skip following two lines (label line and unit line) read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 @@ -255,7 +278,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 END DO - else if (INDEX(Line, "LINE PROPERTIES") > 0) then ! if line properties header + else if ((INDEX(Line, "LINES") > 0 ) .or. (INDEX(Line, "LINE PROPERTIES") > 0) .or. (INDEX(Line, "LINE LIST") > 0) ) then ! if line properties header ! skip following two lines (label line and unit line) read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 @@ -270,7 +293,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if (INDEX(Line, "FAILURE") > 0) then ! if failure conditions header - print *, " Reading failure conditions: "; + IF (wordy > 1) print *, " Reading failure conditions: "; ! skip following two lines (label line and unit line) read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 @@ -315,12 +338,12 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er p%nConnectsExtra = p%nConnects + 2*p%nLines ! set maximum number of connections, accounting for possible detachment of each line end and a connection for that - print *, "Identified ", p%nLineTypes , "LineTypes in input file." - print *, "Identified ", p%nRodTypes , "RodTypes in input file." - print *, "Identified ", p%nBodies , "Bodies in input file." - print *, "Identified ", p%nRods , "Rods in input file." - print *, "Identified ", p%nConnects , "Connections in input file." - print *, "Identified ", p%nLines , "Lines in input file." + IF (wordy > 0) print *, " Identified ", p%nLineTypes , "LineTypes in input file." + IF (wordy > 0) print *, " Identified ", p%nRodTypes , "RodTypes in input file." + IF (wordy > 0) print *, " Identified ", p%nBodies , "Bodies in input file." + IF (wordy > 0) print *, " Identified ", p%nRods , "Rods in input file." + IF (wordy > 0) print *, " Identified ", p%nConnects , "Connections in input file." + IF (wordy > 0) print *, " Identified ", p%nLines , "Lines in input file." @@ -390,7 +413,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !------------------------------------------------------------------------------------------- if ( ( INDEX(Line, "LINE DICTIONARY") > 0) .or. ( INDEX(Line, "LINE TYPES") > 0) ) then ! if line dictionary header - print *, "Reading line types" + IF (wordy > 0) print *, "Reading line types" ! skip following two lines (label line and unit line) read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 @@ -405,7 +428,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! parse out entries: Name Diam MassDenInAir EA cIntDamp >>EI(new)<< Can Cat Cdn Cdt READ(Line,*,IOSTAT=ErrStat2) m%LineTypeList(l)%name, m%LineTypeList(l)%d, & m%LineTypeList(l)%w, tempString1, tempString2, tempString3, & - m%LineTypeList(l)%Can, m%LineTypeList(l)%Cat, m%LineTypeList(l)%Cdn, m%LineTypeList(l)%Cdt + m%LineTypeList(l)%Cdn, m%LineTypeList(l)%Can, m%LineTypeList(l)%Cdt, m%LineTypeList(l)%Cat IF ( ErrStat2 /= ErrID_None ) THEN CALL SetErrStat( ErrID_Fatal, 'Failed to process line type inputs of entry '//trim(Num2LStr(l))//'. Check formatting and correct number of columns.', ErrStat, ErrMsg, RoutineName ) @@ -443,7 +466,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !------------------------------------------------------------------------------------------- else if ( (INDEX(Line, "ROD DICTIONARY") > 0) .or. ( INDEX(Line, "ROD TYPES") > 0) ) then ! if rod dictionary header - print *, "Reading rod types" + IF (wordy > 0) print *, "Reading rod types" ! skip following two lines (label line and unit line) read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 @@ -458,8 +481,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! parse out entries: Name Diam MassDenInAir Can Cat Cdn Cdt IF (ErrStat2 == 0) THEN READ(Line,*,IOSTAT=ErrStat2) m%RodTypeList(l)%name, m%RodTypeList(l)%d, m%RodTypeList(l)%w, & - m%RodTypeList(l)%Can, m%RodTypeList(l)%Cat, m%RodTypeList(l)%Cdn, m%RodTypeList(l)%Cdt, & - m%RodTypeList(l)%CaEnd, m%RodTypeList(l)%CdEnd + m%RodTypeList(l)%Cdn, m%RodTypeList(l)%Can, m%RodTypeList(l)%CdEnd, m%RodTypeList(l)%CaEnd + + m%RodTypeList(l)%Cdt = 0.0_DbKi ! not used + m%RodTypeList(l)%Cat = 0.0_DbKi ! not used END IF ! specify IdNum of rod type for error checking @@ -476,7 +501,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !------------------------------------------------------------------------------------------- - else if ((INDEX(Line, "BODY LIST") > 0 ) .or. (INDEX(Line, "BODY PROPERTIES") > 0 )) then + else if ((INDEX(Line, "BODIES") > 0 ) .or. (INDEX(Line, "BODY LIST") > 0 ) .or. (INDEX(Line, "BODY PROPERTIES") > 0 )) then ! skip following two lines (label line and unit line) read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 @@ -488,13 +513,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !read into a line read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - ! parse out entries: Node Type X Y Z M V FX FY FZ CdA Ca + ! parse out entries: Name/ID X0 Y0 Z0 r0 p0 y0 Xcg Ycg Zcg M V IX IY IZ CdA-x,y,z Ca-x,y,z IF (ErrStat2 == 0) THEN READ(Line,*,IOSTAT=ErrStat2) tempString1, & tempArray(1), tempArray(2), tempArray(3), tempArray(4), tempArray(5), tempArray(6), & m%BodyList(l)%rCG(1), m%BodyList(l)%rCG(2), m%BodyList(l)%rCG(3), & - m%BodyList(l)%bodyM, m%BodyList(l)%bodyV, m%BodyList(l)%bodyI(1), m%BodyList(l)%bodyI(2), m%BodyList(l)%bodyI(3), & - m%BodyList(l)%bodyCdA(1), m%BodyList(l)%bodyCa(1) + m%BodyList(l)%bodyM, m%BodyList(l)%bodyV, & + m%BodyList(l)%bodyI(1), m%BodyList(l)%bodyCdA(1), m%BodyList(l)%bodyCa(1) END IF IF ( ErrStat2 /= 0 ) THEN @@ -507,13 +532,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er - !----------- process body type (not considering input fixed bodies for now, only the GroundBody) ----------------- + !----------- process body type ----------------- call DecomposeString(tempString1, let1, num1, let2, num2, let3) READ(num1, *) m%BodyList(l)%IdNum ! convert to int, representing parent body index - if ((let2 == "COUPLED") .or. (let2 == "VESSEL")) then ! if a coupled body + if ((let2 == "COUPLED") .or. (let2 == "VESSEL") .or. (let1 == "CPLD") .or. (let1 == "VES")) then ! if a coupled body m%BodyList(l)%typeNum = -1 p%nCpldBodies=p%nCpldBodies+1 ! add this rod to coupled list @@ -547,14 +572,11 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er RETURN END IF - ! set initial velocity to zero - m%BodyList(l)%v6 = 0.0_DbKi - !also set number of attached rods and points to zero initially - m%BodyList(l)%nAttachedC = 0 - m%BodyList(l)%nAttachedR = 0 - - ! if there was a body setup function, it would get called here, but I don't think it's needed. + ! set up body + CALL Body_Setup( m%BodyList(l), tempArray, p%rhoW, ErrStat2, ErrMsg2) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN IF ( ErrStat2 /= 0 ) THEN CALL SetErrStat( ErrID_Fatal, 'Failed to read data for body '//trim(Num2LStr(l)), ErrStat, ErrMsg, RoutineName ) @@ -562,15 +584,15 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er RETURN END IF - print *, "Set up body ", l, " of type ", m%BodyList(l)%typeNum + IF (wordy > 1) print *, "Set up body ", l, " of type ", m%BodyList(l)%typeNum END DO !------------------------------------------------------------------------------------------- - else if ((INDEX(Line, "ROD LIST") > 0) .or. (INDEX(Line, "ROD PROPERTIES") > 0)) then ! if rod properties header + else if ((INDEX(Line, "RODS") > 0 ) .or. (INDEX(Line, "ROD LIST") > 0) .or. (INDEX(Line, "ROD PROPERTIES") > 0)) then ! if rod properties header - print *, "Reading rods" + IF (wordy > 0) print *, "Reading Rods" ! skip following two lines (label line and unit line) read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 @@ -646,14 +668,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er return end if - else if ((let1 == "VESSEL") .or. (let1 == "VES")) then ! if a rigidly coupled rod, add to list and add + else if ((let1 == "VESSEL") .or. (let1 == "VES") .or. (let1 == "COUPLED") .or. (let1 == "CPLD")) then ! if a rigidly coupled rod, add to list and add m%RodList(l)%typeNum = -2 p%nCpldRods=p%nCpldRods+1 ! add this rod to coupled list m%CpldRodIs(p%nCpldRods) = l - else if ((let1 == "VESSELPINNED") .or. (let1 == "VESPIN")) then ! if a pinned coupled rod, add to list and add + else if ((let1 == "VESSELPINNED") .or. (let1 == "VESPIN") .or. (let1 == "COUPLEDPINNED") .or. (let1 == "CPLDPIN")) then ! if a pinned coupled rod, add to list and add m%RodList(l)%typeNum = -1 p%nCpldRods=p%nCpldRods+1 ! add @@ -736,7 +758,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF (ErrStat >= AbortErrLev) RETURN ! note: Rod was already added to its respective parent body if type > 0 - + IF ( ErrStat2 /= 0 ) THEN CALL SetErrStat( ErrID_Fatal, 'Failed to read rod data for Rod '//trim(Num2LStr(l)), ErrStat, ErrMsg, RoutineName ) CALL CleanUp() @@ -748,9 +770,9 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !------------------------------------------------------------------------------------------- - else if ( (INDEX(Line, "CONNECTION PROPERTIES") > 0) .or. (INDEX(Line, "NODE PROPERTIES") > 0) ) then ! if node properties header + else if ((INDEX(Line, "POINTS") > 0 ) .or. (INDEX(Line, "CONNECTION PROPERTIES") > 0) .or. (INDEX(Line, "NODE PROPERTIES") > 0) .or. (INDEX(Line, "POINT PROPERTIES") > 0) .or. (INDEX(Line, "POINT LIST") > 0) ) then ! if node properties header - print *, "Reading connections" + IF (wordy > 0) print *, "Reading Points" ! skip following two lines (label line and unit line) read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 @@ -766,8 +788,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF (ErrStat2 == 0) THEN READ(Line,*,IOSTAT=ErrStat2) m%ConnectList(l)%IdNum, tempString1, tempArray(1), & tempArray(2), tempArray(3), m%ConnectList(l)%conM, & - m%ConnectList(l)%conV, m%ConnectList(l)%conFX, m%ConnectList(l)%conFY, & - m%ConnectList(l)%conFZ, m%ConnectList(l)%conCdA, m%ConnectList(l)%conCa + m%ConnectList(l)%conV, m%ConnectList(l)%conCdA, m%ConnectList(l)%conCa + + ! not used + m%ConnectList(l)%conFX = 0.0_DbKi + m%ConnectList(l)%conFY = 0.0_DbKi + m%ConnectList(l)%conFZ = 0.0_DbKi + END IF @@ -811,12 +838,12 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er return end if - else if ((let1 == "VESSEL") .or. (let1 == "VES")) then ! if a fairlead, add to list and add + else if ((let1 == "VESSEL") .or. (let1 == "VES") .or. (let1 == "COUPLED") .or. (let1 == "CPLD")) then ! if a fairlead, add to list and add m%ConnectList(l)%typeNum = -1 p%nCpldCons=p%nCpldCons+1 ! add this rod to coupled list m%CpldConIs(p%nCpldCons) = l - ! this is temporary for backwards compatibility >>>>> will need to update for more versatile coupling >>>> + ! this is temporary for backwards compatibility >>>>> will need to update for more versatile coupling >>>> <<<<<<< this looks pretty good. Make sure it's done only once - either here or near end of init. Same for Rods and bodies. CALL SmllRotTrans('PtfmInit', InitInp%PtfmInit(4),InitInp%PtfmInit(5),InitInp%PtfmInit(6), OrMat, '', ErrStat2, ErrMsg2) ! set initial node position, including adjustments due to initial platform rotations and translations <<< could convert to array math @@ -837,7 +864,12 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er m%ConnectList(l)%r = tempArray(1:3) ! set initial node position - + else if ((let1 = 'TURBINE') .or. (let1 == "T")) then + ! iTurbine = num1 + ! >>> nvm: this is where we could identify the element index in the corresponding mesh in u%FarmCoupledKinematics(iTurbine) for this coupled point + ! nvm: using info from InitInp%FarmNCpldBodies, InitInp%FarmNCpldRods, !InitInp%FarmNCpldCons + ! >>> nvm: Then store iTurbine and the element index IN THIS POINT OBJECT, for easy use with input and output meshes! <<< + ! >>> Do all the initialization stuff as is done with normal coupled points. else CALL SetErrStat( ErrID_Severe, "Unidentified Type/BodyID for Connection "//trim(Num2LStr(l))//": "//trim(tempString2), ErrStat, ErrMsg, RoutineName ) @@ -873,14 +905,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er RETURN END IF - print *, "Set up connection ", l, " of type ", m%ConnectList(l)%typeNum + IF (wordy > 0) print *, "Set up Point ", l, " of type ", m%ConnectList(l)%typeNum END DO ! l = 1,p%nRods !------------------------------------------------------------------------------------------- - else if (INDEX(Line, "LINE PROPERTIES") > 0) then ! if line properties header + else if ((INDEX(Line, "LINES") > 0 ) .or. (INDEX(Line, "LINE PROPERTIES") > 0) .or. (INDEX(Line, "LINE LIST") > 0) ) then ! if line properties header - print *, "Reading lines" + IF (wordy > 0) print *, "Reading Lines" ! skip following two lines (label line and unit line) read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 @@ -1050,7 +1082,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !------------------------------------------------------------------------------------------- else if (INDEX(Line, "FAILURE") > 0) then ! if failure conditions header - print *, " Reading failure conditions: (not implemented yet) "; + IF (wordy > 0) print *, " Reading failure conditions: (not implemented yet) "; ! TODO: add stuff <<<<<<<< @@ -1069,7 +1101,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !------------------------------------------------------------------------------------------- else if (INDEX(Line, "OPTIONS") > 0) then ! if options header - print *, "Reading options" + IF (wordy > 0) print *, "Reading Options" ! (don't skip any lines) @@ -1124,7 +1156,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !------------------------------------------------------------------------------------------- else if (INDEX(Line, "OUTPUT") > 0) then ! if output header - print *, "Reading outputs" + IF (wordy > 0) print *, "Reading Outputs" ! (don't skip any lines) @@ -1205,12 +1237,11 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! Connect mooring system together and make necessary allocations !------------------------------------------------------------------------------------------------- - CALL WrNR( ' Created mooring system. ' ) + CALL WrNr(' Created mooring system: ' ) ! p%NAnchs = 0 ! this is the number of "fixed" type Connections. <<<<<<<<<<<<<< - -! CALL WrScr(trim(Num2LStr(p%nCpldCons))//' fairleads, '//trim(Num2LStr(p%NAnchs))//' anchors, '//trim(Num2LStr(p%nFreeCons))//' connects.') + CALL WrScr(trim(Num2LStr(p%nLines))//' lines, '//trim(Num2LStr(p%NConnects))//' points, '//trim(Num2LStr(p%nRods))//' rods, '//trim(Num2LStr(p%nBodies))//' bodies.') @@ -1228,25 +1259,25 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! END IF ! END DO - print *, "nLineTypes = ",p%nLineTypes - print *, "nRodTypes = ",p%nRodTypes - print *, "nConnects = ",p%nConnects - print *, "nConnectsExtra = ",p%nConnectsExtra - print *, "nBodies = ",p%nBodies - print *, "nRods = ",p%nRods - print *, "nLines = ",p%nLines - print *, "nFails = ",p%nFails - print *, "nFreeBodies = ",p%nFreeBodies - print *, "nFreeRods = ",p%nFreeRods - print *, "nFreeCons = ",p%nFreeCons - print *, "nCpldBodies = ",p%nCpldBodies - print *, "nCpldRods = ",p%nCpldRods - print *, "nCpldCons = ",p%nCpldCons - print *, "NConns = ",p%NConns - print *, "NAnchs = ",p%NAnchs - - print *, "FreeConIs are ", m%FreeConIs - print *, "CpldConIs are ", m%CpldConIs + IF (wordy > 1) print *, "nLineTypes = ",p%nLineTypes + IF (wordy > 1) print *, "nRodTypes = ",p%nRodTypes + IF (wordy > 1) print *, "nConnects = ",p%nConnects + IF (wordy > 1) print *, "nConnectsExtra = ",p%nConnectsExtra + IF (wordy > 1) print *, "nBodies = ",p%nBodies + IF (wordy > 1) print *, "nRods = ",p%nRods + IF (wordy > 1) print *, "nLines = ",p%nLines + IF (wordy > 1) print *, "nFails = ",p%nFails + IF (wordy > 1) print *, "nFreeBodies = ",p%nFreeBodies + IF (wordy > 1) print *, "nFreeRods = ",p%nFreeRods + IF (wordy > 1) print *, "nFreeCons = ",p%nFreeCons + IF (wordy > 1) print *, "nCpldBodies = ",p%nCpldBodies + IF (wordy > 1) print *, "nCpldRods = ",p%nCpldRods + IF (wordy > 1) print *, "nCpldCons = ",p%nCpldCons + IF (wordy > 1) print *, "NConns = ",p%NConns + IF (wordy > 1) print *, "NAnchs = ",p%NAnchs + + IF (wordy > 2) print *, "FreeConIs are ", m%FreeConIs + IF (wordy > 2) print *, "CpldConIs are ", m%CpldConIs !------------------------------------------------------------------------------------ @@ -1309,7 +1340,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! the number of states is Nx m%Nx = Nx - print *, "allocating state vectors to size ", Nx + IF (wordy > 0) print *, "allocating state vectors to size ", Nx ! allocate state vector and temporary state vectors based on size just calculated ALLOCATE ( x%states(m%Nx), m%xTemp%states(m%Nx), m%xdTemp%states(m%Nx), STAT = ErrStat ) @@ -1361,7 +1392,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF (ErrStat >= AbortErrLev) RETURN ! note: in MoorDyn-F v2, the points in the mesh correspond in order to all the coupled bodies, then rods, then connections - ! NOTE: InitInp%PtfmInit should be replaced by specific values for each coupled body/rod/connect at some point <<<<< + ! >>> make sure all coupled objects have been offset correctly by the PtfmInit values, including if it's a farm situation -- below or where the objects are first created <<<< J = 0 ! this is the counter through the mesh points @@ -1397,7 +1428,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL MeshPositionNode(u%CoupledKinematics, J, rRef, ErrStat2, ErrMsg2) ! defaults to identity orientation matrix u%CoupledKinematics%TranslationDisp(:,J) = 0.0_ReKi ! no displacement from reference position CALL MeshConstructElement(u%CoupledKinematics, ELEMENT_POINT, ErrStat2, ErrMsg2, J) - CALL Rod_SetKinematics(m%RodList(m%CpldRodIs(l)), tempArray, m%zeros6, m%zeros6, 0.0_DbKi, m%LineList) + + CALL Rod_SetKinematics(m%RodList(m%CpldRodIs(l)), DBLE(rRef), m%zeros6, m%zeros6, 0.0_DbKi, m) END DO DO l = 1,p%nCpldCons ! keeping this one simple for now, positioning at whatever is specified by glue code <<< @@ -1411,7 +1443,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! lastly, do this to set the attached line endpoint positions: rRefDub = rRef(1:3) - CALL Connect_SetKinematics(m%ConnectList(m%CpldConIs(l)), rRefDub, m%zeros6(1:3), m%zeros6(1:3), 0.0_DbKi, m%LineList) + CALL Connect_SetKinematics(m%ConnectList(m%CpldConIs(l)), rRefDub, m%zeros6(1:3), m%zeros6(1:3), 0.0_DbKi, m) END DO CALL CheckError( ErrStat2, ErrMsg2 ) @@ -1432,9 +1464,9 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CtrlCode = MESH_SIBLING, IOS = COMPONENT_OUTPUT, & Force=.TRUE., Moment=.TRUE., ErrStat = ErrStat2, ErrMess=ErrMsg2 ) - !CALL MeshCommit ( y%CoupledLoads, ErrStat, ErrMsg ) - ! CALL CheckError( ErrStat2, ErrMsg2 ) - ! IF (ErrStat >= AbortErrLev) RETURN + + ! >>>>>> ensure the output mesh includes all elements from u%(Farm)CoupledKinematics, OR make a seperate array of output meshes for each turbine <<<<<<<<< + CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN @@ -1459,6 +1491,9 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! ----------------------------- Arrays for wave kinematics ----------------------------- + + m%WaveTi = 1 ! set initial wave grid time interpolation index to 1 to start with + ! :::::::::::::: BELOW WILL BE USED EVENTUALLY WHEN WAVE INFO IS AN INPUT :::::::::::::::::: ! ! The rAll array contains all nodes or reference points in the system ! ! (x,y,z global coordinates for each) in the order of bodies, rods, points, internal line nodes. @@ -1755,12 +1790,12 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! Go through independent (including pinned) Rods and write the coordinates to the state vector DO l = 1,p%nFreeRods - CALL Rod_Initialize(m%RodList(m%FreeRodIs(l)), x%states(m%RodStateIs1(l):m%RodStateIsN(l)), m%LineList) + CALL Rod_Initialize(m%RodList(m%FreeRodIs(l)), x%states(m%RodStateIs1(l):m%RodStateIsN(l)), m) END DO ! Go through independent connections (Connects) and write the coordinates to the state vector and set positions of attached line ends DO l = 1, p%nFreeCons - CALL Connect_Initialize(m%ConnectList(m%FreeConIs(l)), x%states(m%ConStateIs1(l) : m%conStateIsN(l)), m%LineList) + CALL Connect_Initialize(m%ConnectList(m%FreeConIs(l)), x%states(m%ConStateIs1(l) : m%conStateIsN(l)), m) END DO @@ -1779,10 +1814,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL Line_Initialize( m%LineList(l), m%LineTypeList(m%LineList(l)%PropsIdNum), p%rhoW , ErrStat2, ErrMsg2) CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN - IF (ErrStat >= ErrId_Warn) CALL WrScr(" Catenary solver failed for one or more lines. Using linear node spacing.") ! make this statement more accurate + !IF (ErrStat >= ErrId_Warn) CALL WrScr(" Note: Catenary pre-solver was unsuccessful for one or more lines so started with linear node spacing instead.") ! make this statement more accurate -! print *, "Line ", l, " with NumSegs =", N -! print *, "its states range from index ", m%LineStateIs1(l), " to ", m%LineStateIsN(l) + IF (wordy > 2) print *, "Line ", l, " with NumSegs =", N + IF (wordy > 2) print *, "its states range from index ", m%LineStateIs1(l), " to ", m%LineStateIsN(l) ! assign the resulting internal node positions to the integrator initial state vector! (velocities leave at 0) DO I = 1, N-1 @@ -1807,12 +1842,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! -------------------------------------------------------------------- - -! print *,"Done setup of the system (before any dynamic relaxation. State vector is as follows:" - -! DO I = 1, m%Nx -! print *, x%states(I) - + IF (wordy > 2) THEN + print *,"Done setup of the system (before any dynamic relaxation. State vector is as follows:" + + DO I = 1, m%Nx + print *, x%states(I) + END DO + END IF ! ! try writing output for troubleshooting purposes (TEMPORARY) ! CALL MDIO_WriteOutputs(-1.0_DbKi, p, m, y, ErrStat, ErrMsg) @@ -1829,7 +1865,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! only do this if TMaxIC > 0 if (InitInp%TMaxIC > 0.0_DbKi) then - CALL WrScr(" Finalizing ICs using dynamic relaxation."//NewLine) ! newline because next line writes over itself + CALL WrScr(" Finalizing initial conditions using dynamic relaxation."//NewLine) ! newline because next line writes over itself ! boost drag coefficient of each line type <<<<<<<< does this actually do anything or do lines hold these coefficients??? DO I = 1, p%nLineTypes @@ -1881,8 +1917,11 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er END DO IF (ErrStat == ErrID_Fatal) THEN - print *, "NaN detected at time ", t, " during IC gen. Here is the state vector: " - print *, x%states + print *, "NaN detected at time ", t, " during MoorDyn's dynamic relaxation process." + IF (wordy > 1) THEN + print *, "Here is the state vector: " + print *, x%states + END IF EXIT END IF @@ -1962,6 +2001,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er xd%dummy = 0 z%dummy = 0 + CALL WrScr(' MoorDyn initialization completed.') ! TODO: add feature for automatic water depth increase based on max anchor depth! @@ -2121,8 +2161,11 @@ SUBROUTINE MD_UpdateStates( t, n, u, t_array, p, x, xd, z, other, m, ErrStat, Er END DO IF (ErrStat == ErrID_Fatal) THEN - print *, "NaN detected at time ", t2, ". Here is the state vector: " - print *, x%states + print *, "NaN detected at time ", t2, " in MoorDyn." + IF (wordy > 1) THEN + print *, ". Here is the state vector: " + print *, x%states + END IF EXIT END IF @@ -2149,8 +2192,11 @@ SUBROUTINE MD_UpdateStates( t, n, u, t_array, p, x, xd, z, other, m, ErrStat, Er END DO IF (ErrStat == ErrID_Fatal) THEN - print *, "NaN detected at time ", t2, ". Here is the state vector: " - print *, x%states + print *, "NaN detected at time ", t2, " in MoorDyn." + IF (wordy > 1) THEN + print *, ". Here is the state vector: " + print *, x%states + END IF END IF CONTAINS @@ -2284,14 +2330,14 @@ SUBROUTINE MD_CalcOutput( t, u, p, x, xd, z, other, y, m, ErrStat, ErrMsg ) DO l = 1,p%nCpldRods J = J + 1 - CALL Rod_GetCoupledForce(m%RodList(m%CpldRodIs(l)), F6net, m%LineList, p) + CALL Rod_GetCoupledForce(m%RodList(m%CpldRodIs(l)), F6net, m, p) y%CoupledLoads%Force( :,J) = F6net(1:3) y%CoupledLoads%Moment(:,J) = F6net(4:6) END DO DO l = 1,p%nCpldCons J = J + 1 - CALL Connect_GetCoupledForce(m%ConnectList(m%CpldConIs(l)), F6net(1:3), m%LineList, p) + CALL Connect_GetCoupledForce(m%ConnectList(m%CpldConIs(l)), F6net(1:3), m, p) y%CoupledLoads%Force(:,J) = F6net(1:3) END DO @@ -2441,7 +2487,8 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er DO l = 1,p%nCpldBodies J = J + 1 r6_in(1:3) = u%CoupledKinematics%Position(:,J) + u%CoupledKinematics%TranslationDisp(:,J) - r6_in(4:6) = EulerExtract( TRANSPOSE( u%CoupledKinematics%Orientation(:,:,J) ) ) + !r6_in(4:6) = EulerExtract( TRANSPOSE( u%CoupledKinematics%Orientation(:,:,J) ) ) + r6_in(4:6) = EulerExtract( u%CoupledKinematics%Orientation(:,:,J) ) ! <<< changing back v6_in(1:3) = u%CoupledKinematics%TranslationVel(:,J) v6_in(4:6) = u%CoupledKinematics%RotationVel(:,J) a6_in(1:3) = u%CoupledKinematics%TranslationAcc(:,J) @@ -2461,7 +2508,7 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er a6_in(1:3) = u%CoupledKinematics%TranslationAcc(:,J) a6_in(4:6) = u%CoupledKinematics%RotationAcc(:,J) - CALL Rod_SetKinematics(m%RodList(m%CpldRodIs(l)), r6_in, v6_in, a6_in, t, m%LineList) + CALL Rod_SetKinematics(m%RodList(m%CpldRodIs(l)), r6_in, v6_in, a6_in, t, m) END DO @@ -2472,7 +2519,7 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er r_in = u%CoupledKinematics%Position(:,J) + u%CoupledKinematics%TranslationDisp(:,J) rd_in = u%CoupledKinematics%TranslationVel(:,J) a_in(1:3) = u%CoupledKinematics%TranslationAcc(:,J) - CALL Connect_SetKinematics(m%ConnectList(m%CpldConIs(l)), r_in, rd_in, a_in, t, m%LineList) + CALL Connect_SetKinematics(m%ConnectList(m%CpldConIs(l)), r_in, rd_in, a_in, t, m) !print *, u%PtFairleadDisplacement%Position(:,l) + u%PtFairleadDisplacement%TranslationDisp(:,l) !print *, u%PtFairleadDisplacement%TranslationVel(:,l) @@ -2480,6 +2527,15 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er END DO + ! >>>>> in theory I would repeat the above but for each turbine in the case of array use here <<<<< + ! DO I = 1,p%nTurbines + ! J = 0? + ! other logic? + ! nvm: need to get kinematics from entries in u%FarmCoupledKinematics(I)%Position etc. + ! nvm: using knowledge of p%meshIndex or something + ! in theory might also support individual line tensioning control commands from turbines this way too, or maybe it's supercontroller level (not a short term problem though) + + ! apply line length changes from active tensioning if applicable DO L = 1, p%NLines IF (m%LineList(L)%CtrlChan > 0) then @@ -2489,16 +2545,16 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er ErrStat = ErrID_Fatal ErrMsg = ' Active tension command will make a segment longer than the limit of twice its original length.' print *, u%DeltaL(m%LineList(L)%CtrlChan), " is an increase of more than ", (m%LineList(L)%UnstrLen / m%LineList(L)%N) - print *, u%DeltaL - print*, m%LineList(L)%CtrlChan + IF (wordy > 0) print *, u%DeltaL + IF (wordy > 0) print*, m%LineList(L)%CtrlChan RETURN END IF IF ( u%DeltaL(m%LineList(L)%CtrlChan) < -0.5 * m%LineList(L)%UnstrLen / m%LineList(L)%N ) then ErrStat = ErrID_Fatal ErrMsg = ' Active tension command will make a segment shorter than the limit of half its original length.' print *, u%DeltaL(m%LineList(L)%CtrlChan), " is a reduction of more than half of ", (m%LineList(L)%UnstrLen / m%LineList(L)%N) - print *, u%DeltaL - print*, m%LineList(L)%CtrlChan + IF (wordy > 0) print *, u%DeltaL + IF (wordy > 0) print*, m%LineList(L)%CtrlChan RETURN END IF @@ -2559,14 +2615,14 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er ! give independent or pinned rods' latest state variables (kinematics will also be assigned to attached line ends) DO l = 1,p%nFreeRods - CALL Rod_SetState(m%RodList(m%FreeRodIs(l)), x%states(m%RodStateIs1(l):m%RodStateIsN(l)), t, m%LineList) + CALL Rod_SetState(m%RodList(m%FreeRodIs(l)), x%states(m%RodStateIs1(l):m%RodStateIsN(l)), t, m) END DO ! give Connects (independent connections) latest state variable values (kinematics will also be assigned to attached line ends) DO l = 1,p%nFreeCons ! Print *, "calling SetState for free connection, con#", m%FreeConIs(l), " with state range: ", m%ConStateIs1(l), "-", m%ConStateIsN(l) !K=K+1 - CALL Connect_SetState(m%ConnectList(m%FreeConIs(l)), x%states(m%ConStateIs1(l):m%ConStateIsN(l)), t, m%LineList) + CALL Connect_SetState(m%ConnectList(m%FreeConIs(l)), x%states(m%ConStateIs1(l):m%ConStateIsN(l)), t, m) END DO ! give Lines latest state variable values for internal nodes @@ -2578,18 +2634,18 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er ! calculate line dynamics (and calculate line forces and masses attributed to connections) DO l = 1,p%nLines - CALL Line_GetStateDeriv(m%LineList(l), dxdt%states(m%LineStateIs1(l):m%LineStateIsN(l)), p) !dt might also be passed for fancy friction models + CALL Line_GetStateDeriv(m%LineList(l), dxdt%states(m%LineStateIs1(l):m%LineStateIsN(l)), m, p) !dt might also be passed for fancy friction models END DO ! calculate connect dynamics (including contributions from attached lines ! as well as hydrodynamic forces etc. on connect object itself if applicable) DO l = 1,p%nFreeCons - CALL Connect_GetStateDeriv(m%ConnectList(m%FreeConIs(l)), dxdt%states(m%ConStateIs1(l):m%ConStateIsN(l)), m%LineList, p) + CALL Connect_GetStateDeriv(m%ConnectList(m%FreeConIs(l)), dxdt%states(m%ConStateIs1(l):m%ConStateIsN(l)), m, p) END DO ! calculate dynamics of independent Rods DO l = 1,p%nFreeRods - CALL Rod_GetStateDeriv(m%RodList(m%FreeRodIs(l)), dxdt%states(m%RodStateIs1(l):m%RodStateIsN(l)), m%LineList, p) + CALL Rod_GetStateDeriv(m%RodList(m%FreeRodIs(l)), dxdt%states(m%RodStateIs1(l):m%RodStateIsN(l)), m, p) END DO ! calculate dynamics of Bodies @@ -2608,11 +2664,11 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er ! >>>>>>>> here we should pass along accelerations and include inertial loads in the calculation!!! << 1) print *, 'Based on zeta, BA set to ', Line%BA - ! print *, 'Negative BA input detected, treating as -zeta. For zeta = ', -LineProp%BA, ', setting BA to ', Line%BA + IF (wordy > 1) print *, 'Negative BA input detected, treating as -zeta. For zeta = ', -LineProp%BA, ', setting BA to ', Line%BA ELSE Line%BA = LineProp%BA - ! temp = Line%N * Line%BA / Line%UnstrLen * SQRT(1.0/(LineProp%EA * LineProp%w)) - ! print *, 'BA set as input to ', Line%BA, '. Corresponding zeta is ', temp + IF (wordy > 1) temp = Line%N * Line%BA / Line%UnstrLen * SQRT(1.0/(LineProp%EA * LineProp%w)) + IF (wordy > 1) print *, 'BA set as input to ', Line%BA, '. Corresponding zeta is ', temp END IF !temp = 2*Line%N / Line%UnstrLen * sqrt( LineProp%EA / LineProp%w) / TwoPi @@ -3772,12 +3829,12 @@ END SUBROUTINE Line_SetState !-------------------------------------------------------------- !-------------------------------------------------------------- - SUBROUTINE Line_GetStateDeriv(Line, Xd, p) !, FairFtot, FairMtot, AnchFtot, AnchMtot) + SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, AnchMtot) - TYPE(MD_Line), INTENT(INOUT) :: Line ! the current Line object - Real(DbKi), INTENT(INOUT) :: Xd(:) ! state derivative vector section for this line - TYPE(MD_ParameterType), INTENT(IN ) :: p ! Parameters - + TYPE(MD_Line), INTENT(INOUT) :: Line ! the current Line object + Real(DbKi), INTENT(INOUT) :: Xd(:) ! state derivative vector section for this line + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + TYPE(MD_ParameterType), INTENT(IN ) :: p ! Parameters ! Real(DbKi), INTENT( IN ) :: X(:) ! state vector, provided ! Real(DbKi), INTENT( INOUT ) :: Xd(:) ! derivative of state vector, returned ! cahnged to INOUT @@ -3854,7 +3911,7 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, p) !, FairFtot, FairMtot, AnchFtot, Anc IF (p%WaterKin > 0) THEN ! wave kinematics interpolated from global grid in Waves object DO i=0,N - CALL getWaveKin(p, Line%r(1,i), Line%r(2,i), Line%r(3,i), Line%time, Line%U(:,i), Line%Ud(:,i), Line%zeta(i), Line%PDyn(i)) + CALL getWaveKin(p, Line%r(1,i), Line%r(2,i), Line%r(3,i), Line%time, m%WaveTi, Line%U(:,i), Line%Ud(:,i), Line%zeta(i), Line%PDyn(i)) END DO END IF @@ -4011,35 +4068,36 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, p) !, FairFtot, FairMtot, AnchFtot, Anc ! check for NaNs DO J = 1, 6*(N-1) IF (Is_NaN(REAL(Xd(J),DbKi))) THEN - print *, "NaN detected at time ", Line%time, " in Line ", Line%IdNum, " state derivatives:" - print *, Xd - - - - print *, "m_i p%rhoW v_i Line%Can Line%Cat" - print *, m_i - print *, p%rhoW - print *, v_i - print *, Line%Can - print *, Line%Cat - - print *, "Line%q" - print *, Line%q - - print *, "Line%r" - print *, Line%r - - - print *, "Here is the mass matrix set" - print *, Line%M - - print *, "Here is the inverted mass matrix set" - print *, Line%S - - print *, "Here is the net force set" - print *, Line%Fnet - - + print *, "NaN detected at time ", Line%time, " in Line ", Line%IdNum, " in MoorDyn." + IF (wordy > 1) THEN + print *, "state derivatives:" + print *, Xd + + + + print *, "m_i p%rhoW v_i Line%Can Line%Cat" + print *, m_i + print *, p%rhoW + print *, v_i + print *, Line%Can + print *, Line%Cat + + print *, "Line%q" + print *, Line%q + + print *, "Line%r" + print *, Line%r + + + print *, "Here is the mass matrix set" + print *, Line%M + + print *, "Here is the inverted mass matrix set" + print *, Line%S + + print *, "Here is the net force set" + print *, Line%Fnet + END IF EXIT END IF @@ -4181,6 +4239,7 @@ END SUBROUTINE Line_SetEndOrientation !-------------------------------------------------------------- + !-------------------------------------------------------------- ! Connection-Specific Subroutines !-------------------------------------------------------------- @@ -4190,11 +4249,11 @@ END SUBROUTINE Line_SetEndOrientation !-------------------------------------------------------------- - SUBROUTINE Connect_Initialize(Connect, states, LineList) + SUBROUTINE Connect_Initialize(Connect, states, m) Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object Real(DbKi), INTENT(INOUT) :: states(6) ! state vector section for this Connection - Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects INTEGER(IntKi) :: l @@ -4203,8 +4262,8 @@ SUBROUTINE Connect_Initialize(Connect, states, LineList) ! pass kinematics to any attached lines so they have initial positions at this initialization stage DO l=1,Connect%nAttached - print *, "Connect ", Connect%IdNum, " setting end kinematics of line ", Connect%attached(l), " to ", Connect%r - CALL Line_SetEndKinematics(LineList(Connect%attached(l)), Connect%r, Connect%rd, 0.0_DbKi, Connect%Top(l)) + IF (wordy > 1) print *, "Connect ", Connect%IdNum, " setting end kinematics of line ", Connect%attached(l), " to ", Connect%r + CALL Line_SetEndKinematics(m%LineList(Connect%attached(l)), Connect%r, Connect%rd, 0.0_DbKi, Connect%Top(l)) END DO @@ -4213,10 +4272,10 @@ SUBROUTINE Connect_Initialize(Connect, states, LineList) states(1:3) = Connect%rd - print *, "Initialized Connection ", Connect%IdNum + IF (wordy > 0) print *, "Initialized Connection ", Connect%IdNum else - print *," Error: wrong connection type given to Connect_Initialize for number ", Connect%idNum + print *," Error: wrong Point type given to Connect_Initialize for number ", Connect%idNum end if END SUBROUTINE Connect_Initialize @@ -4224,14 +4283,15 @@ END SUBROUTINE Connect_Initialize !-------------------------------------------------------------- - SUBROUTINE Connect_SetKinematics(Connect, r_in, rd_in, a_in, t, LineList) + SUBROUTINE Connect_SetKinematics(Connect, r_in, rd_in, a_in, t, m) Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object Real(DbKi), INTENT(IN ) :: r_in( 3) ! position Real(DbKi), INTENT(IN ) :: rd_in(3) ! velocity Real(DbKi), INTENT(IN ) :: a_in(3) ! acceleration (only used for coupled connects) Real(DbKi), INTENT(IN ) :: t ! instantaneous time - Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + INTEGER(IntKi) :: l @@ -4248,7 +4308,7 @@ SUBROUTINE Connect_SetKinematics(Connect, r_in, rd_in, a_in, t, LineList) ! pass latest kinematics to any attached lines DO l=1,Connect%nAttached - CALL Line_SetEndKinematics(LineList(Connect%attached(l)), Connect%r, Connect%rd, t, Connect%Top(l)) + CALL Line_SetEndKinematics(m%LineList(Connect%attached(l)), Connect%r, Connect%rd, t, Connect%Top(l)) END DO ! else @@ -4262,12 +4322,12 @@ END SUBROUTINE Connect_SetKinematics !-------------------------------------------------------------- !-------------------------------------------------------------- - SUBROUTINE Connect_SetState(Connect, X, t, LineList) + SUBROUTINE Connect_SetState(Connect, X, t, m) Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object Real(DbKi), INTENT(IN ) :: X(:) ! state vector section for this line Real(DbKi), INTENT(IN ) :: t ! instantaneous time - Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects INTEGER(IntKi) :: l ! index of segments or nodes along line INTEGER(IntKi) :: J ! index @@ -4284,18 +4344,19 @@ SUBROUTINE Connect_SetState(Connect, X, t, LineList) ! pass latest kinematics to any attached lines DO l=1,Connect%nAttached - CALL Line_SetEndKinematics(LineList(Connect%attached(l)), Connect%r, Connect%rd, t, Connect%Top(l)) + CALL Line_SetEndKinematics(m%LineList(Connect%attached(l)), Connect%r, Connect%rd, t, Connect%Top(l)) END DO END SUBROUTINE Connect_SetState !-------------------------------------------------------------- !-------------------------------------------------------------- - SUBROUTINE Connect_GetStateDeriv(Connect, Xd, LineList, p) + SUBROUTINE Connect_GetStateDeriv(Connect, Xd, m, p) Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object Real(DbKi), INTENT(INOUT) :: Xd(:) ! state derivative vector section for this line - Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables @@ -4308,7 +4369,7 @@ SUBROUTINE Connect_GetStateDeriv(Connect, Xd, LineList, p) Real(DbKi) :: S(3,3) ! inverse mass matrix - CALL Connect_DoRHS(Connect, LineList, p) + CALL Connect_DoRHS(Connect, m, p) ! // solve for accelerations in [M]{a}={f} using LU decomposition ! double M_tot[9]; // serialize total mass matrix for easy processing @@ -4336,8 +4397,9 @@ SUBROUTINE Connect_GetStateDeriv(Connect, Xd, LineList, p) ! check for NaNs DO J = 1, 6 IF (Is_NaN(REAL(Xd(J),DbKi))) THEN - print *, "NaN detected at time ", Connect%time, " in Connection ",Connect%IdNum, " state derivatives:" - print *, Xd + print *, "NaN detected at time ", Connect%time, " in Point ",Connect%IdNum, " in MoorDyn." + IF (wordy > 1) print *, "state derivatives:" + IF (wordy > 1) print *, Xd EXIT END IF END DO @@ -4346,10 +4408,10 @@ END SUBROUTINE Connect_GetStateDeriv !-------------------------------------------------------------- !-------------------------------------------------------------- - SUBROUTINE Connect_DoRHS(Connect, LineList, p) + SUBROUTINE Connect_DoRHS(Connect, m, p) - Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object - Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables @@ -4390,7 +4452,7 @@ SUBROUTINE Connect_DoRHS(Connect, LineList, p) ! ! print *, " attached line ID", m%LineList(Connect%attached(l))%IdNum - CALL Line_GetEndStuff(LineList(Connect%attached(l)), Fnet_i, Moment_dummy, M_i, Connect%Top(l)) + CALL Line_GetEndStuff(m%LineList(Connect%attached(l)), Fnet_i, Moment_dummy, M_i, Connect%Top(l)) ! sum quantitites Connect%Fnet = Connect%Fnet + Fnet_i @@ -4432,25 +4494,25 @@ END SUBROUTINE Connect_DoRHS ! calculate the force including inertial loads on connect that is coupled !-------------------------------------------------------------- - SUBROUTINE Connect_GetCoupledForce(Connect, Fnet_out, LineList, p) + SUBROUTINE Connect_GetCoupledForce(Connect, Fnet_out, m, p) Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connect object Real(DbKi), INTENT( OUT) :: Fnet_out(3) ! force and moment vector about rRef - Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters Real(DbKi) :: F_iner(3) ! inertial force IF (Connect%typeNum == -1) then ! calculate forces and masses of connect - CALL Connect_DoRHS(Connect, LineList, p) + CALL Connect_DoRHS(Connect, m, p) ! add inertial loads as appropriate F_iner = -MATMUL(Connect%M, Connect%a) ! inertial loads Fnet_out = Connect%Fnet + F_iner ! add inertial loads ELSE - print *, "Connect_GetCoupledForce called for wrong (uncoupled) connect type!" + print *, "Connect_GetCoupledForce called for wrong (uncoupled) Point type in MoorDyn!" END IF END SUBROUTINE Connect_GetCoupledForce @@ -4458,19 +4520,19 @@ END SUBROUTINE Connect_GetCoupledForce ! calculate the force and mass contributions of the connect on the parent body (only for type 3 connects?) !-------------------------------------------------------------- - SUBROUTINE Connect_GetNetForceAndMass(Connect, rRef, Fnet_out, M_out, LineList, p) + SUBROUTINE Connect_GetNetForceAndMass(Connect, rRef, Fnet_out, M_out, m, p) Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connect object Real(DbKi), INTENT(IN ) :: rRef(3) ! global coordinates of reference point (i.e. the parent body) Real(DbKi), INTENT( OUT) :: Fnet_out(6) ! force and moment vector about rRef Real(DbKi), INTENT( OUT) :: M_out(6,6) ! mass and inertia matrix about rRef - Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters Real(DbKi) :: rRel( 3) ! position of connection relative to the body reference point (global orientation frame) - CALL Connect_DoRHS(Connect, LineList, p) + CALL Connect_DoRHS(Connect, m, p) rRel = Connect%r - rRef ! vector from body reference point to node @@ -4493,14 +4555,14 @@ SUBROUTINE Connect_AddLine(Connect, lineID, TopOfLine) Integer(IntKi), INTENT( IN ) :: lineID Integer(IntKi), INTENT( IN ) :: TopOfLine - Print*, "L", lineID, "->C", Connect%IdNum + IF (wordy > 0) Print*, "L", lineID, "->C", Connect%IdNum IF (Connect%nAttached <10) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. Connect%nAttached = Connect%nAttached + 1 ! add the line to the number connected Connect%Attached(Connect%nAttached) = lineID Connect%Top(Connect%nAttached) = TopOfLine ! attached to line ... 1 = top/fairlead(end B), 0 = bottom/anchor(end A) ELSE - Print*, "too many lines connected!" + Print*, "Too many lines connected to Point ", Connect%IdNum, " in MoorDyn!" END IF END SUBROUTINE Connect_AddLine @@ -4510,7 +4572,7 @@ END SUBROUTINE Connect_AddLine !-------------------------------------------------------------- SUBROUTINE Connect_RemoveLine(Connect, lineID, TopOfLine, rEnd, rdEnd) - Type(MD_Connect), INTENT (INOUT) :: Connect ! the Connection object + Type(MD_Connect), INTENT (INOUT) :: Connect ! the Connection object Integer(IntKi), INTENT( IN ) :: lineID Integer(IntKi), INTENT( OUT) :: TopOfLine REAL(DbKi), INTENT(INOUT) :: rEnd(3) @@ -4567,7 +4629,6 @@ END SUBROUTINE Connect_RemoveLine !----------------------------------------------------------------------- SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) - ! calculate initial profile of the line using quasi-static model TYPE(MD_Rod), INTENT(INOUT) :: Rod ! the single rod object of interest TYPE(MD_RodProp), INTENT(INOUT) :: RodProp ! the single rod property set for the line of interest @@ -4597,15 +4658,15 @@ SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) ! allocate node positions and velocities (NOTE: these arrays start at ZERO) ALLOCATE ( Rod%r(3, 0:N), Rod%rd(3, 0:N), STAT = ErrStat ) ! <<<<<< add error checks here - IF ( ErrStat /= ErrID_None ) print *, "Alloc error 1!!!!!!!!!!!!!" + IF ( ErrStat /= ErrID_None ) print *, "Alloc error 1 in MoorDyn" ! allocate segment scalar quantities ALLOCATE ( Rod%l(N), Rod%V(N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) print *, "Alloc error 2!!!!!!!!!!!!!" + IF ( ErrStat /= ErrID_None ) print *, "Alloc error 2 in MoorDyn" ! allocate water related vectors ALLOCATE ( Rod%U(3, 0:N), Rod%Ud(3, 0:N), Rod%zeta(0:N), Rod%PDyn(0:N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) print *, "Alloc error 3!!!!!!!!!!!!!" + IF ( ErrStat /= ErrID_None ) print *, "Alloc error 3 in MoorDyn" ! set to zero initially (important of wave kinematics are not being used) Rod%U = 0.0_DbKi Rod%Ud = 0.0_DbKi @@ -4615,11 +4676,11 @@ SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) ! allocate node force vectors ALLOCATE ( Rod%W(3, 0:N), Rod%Bo(3, 0:N), Rod%Dp(3, 0:N), Rod%Dq(3, 0:N), Rod%Ap(3, 0:N), & Rod%Aq(3, 0:N), Rod%Pd(3, 0:N), Rod%B(3, 0:N), Rod%Fnet(3, 0:N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) print *, "Alloc error 4!!!!!!!!!!!!!" + IF ( ErrStat /= ErrID_None ) print *, "Alloc error 4 in MoorDyn" ! allocate mass and inverse mass matrices for each node (including ends) ALLOCATE ( Rod%M(3, 3, 0:N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) print *, "Alloc error 5!!!!!!!!!!!!!" + IF ( ErrStat /= ErrID_None ) print *, "Alloc error 5 in MoorDyn" @@ -4669,7 +4730,7 @@ SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) ! >>> why are the above assignments making l V W and B appear as "undefined pointer/array"s??? <<< - print *, "Set up Rod ",Rod%IdNum, ", type ", Rod%typeNum + IF (wordy > 0) print *, "Set up Rod ",Rod%IdNum, ", type ", Rod%typeNum ! need to add cleanup sub <<< @@ -4684,17 +4745,18 @@ END SUBROUTINE Rod_Setup ! Notes: r6 and v6 must already be set. ! ground- or body-pinned rods have already had setKinematics called to set first 3 elements of r6, v6. !-------------------------------------------------------------- - SUBROUTINE Rod_Initialize(Rod, states, LineList) + SUBROUTINE Rod_Initialize(Rod, states, m) TYPE(MD_Rod), INTENT(INOUT) :: Rod ! the rod object Real(DbKi), INTENT(INOUT) :: states(:) ! state vector section for this line - TYPE(MD_Line), INTENT(INOUT) :: LineList(:) ! passing along all mooring objects + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + INTEGER(IntKi) :: l ! index of segments or nodes along line REAL(DbKi) :: rRef(3) ! reference position of mesh node REAL(DbKi) :: OrMat(3,3) ! DCM for body orientation based on r6_in - print *, "initializing Rod ", Rod%idNum + IF (wordy > 0) print *, "initializing Rod ", Rod%idNum ! the r6 and v6 vectors should have already been set ! r and rd of ends have already been set by setup function or by parent object <<<<< right? <<<<< @@ -4703,7 +4765,7 @@ SUBROUTINE Rod_Initialize(Rod, states, LineList) ! Pass kinematics to any attached lines (this is just like what a Connection does, except for both ends) ! so that they have the correct initial positions at this initialization stage. - if (Rod%typeNum >- 2) CALL Rod_SetDependentKin(Rod, 0.0_DbKi, LineList) ! don't call this for type -2 coupled Rods as it's already been called + if (Rod%typeNum >- 2) CALL Rod_SetDependentKin(Rod, 0.0_DbKi, m) ! don't call this for type -2 coupled Rods as it's already been called ! assign the resulting kinematics to its part of the state vector (only matters if it's an independent Rod) @@ -4732,14 +4794,14 @@ END SUBROUTINE Rod_Initialize ! set kinematics for Rods ONLY if they are attached to a body (including a coupled body) or coupled (otherwise shouldn't be called) !-------------------------------------------------------------- - SUBROUTINE Rod_SetKinematics(Rod, r6_in, v6_in, a6_in, t, LineList) + SUBROUTINE Rod_SetKinematics(Rod, r6_in, v6_in, a6_in, t, m) Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object Real(DbKi), INTENT(IN ) :: r6_in(6) ! 6-DOF position Real(DbKi), INTENT(IN ) :: v6_in(6) ! 6-DOF velocity Real(DbKi), INTENT(IN ) :: a6_in(6) ! 6-DOF acceleration (only used for coupled rods) Real(DbKi), INTENT(IN ) :: t ! instantaneous time - Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects INTEGER(IntKi) :: l @@ -4754,7 +4816,7 @@ SUBROUTINE Rod_SetKinematics(Rod, r6_in, v6_in, a6_in, t, LineList) call ScaleVector(Rod%r6(4:6), 1.0_DbKi, Rod%r6(4:6)); ! enforce direction vector to be a unit vector ! since this rod has no states and all DOFs have been set, pass its kinematics to dependent Lines - CALL Rod_SetDependentKin(Rod, t, LineList) + CALL Rod_SetDependentKin(Rod, t, m) else if (abs(Rod%typeNum) == 1) then ! rod end A pinned to a body, or ground, or coupling point @@ -4768,27 +4830,28 @@ SUBROUTINE Rod_SetKinematics(Rod, r6_in, v6_in, a6_in, t, LineList) ! handled, along with passing kinematics to dependent lines, by separate call to setState else - print *, "Error: Rod_SetKinematics called for a free Rod." ! <<< + print *, "Error: Rod_SetKinematics called for a free Rod in MoorDyn." ! <<< end if ! update Rod direction unit vector (simply equal to last three entries of r6, presumably these were set elsewhere for pinned Rods) Rod%q = Rod%r6(4:6) + END SUBROUTINE Rod_SetKinematics !-------------------------------------------------------------- ! pass the latest states to the rod if it has any DOFs/states (then update rod end kinematics including attached lines) !-------------------------------------------------------------- - SUBROUTINE Rod_SetState(Rod, X, t, LineList) + SUBROUTINE Rod_SetState(Rod, X, t, m) Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object - Real(DbKi), INTENT(IN ) :: X(:) ! state vector section for this line - Real(DbKi), INTENT(IN ) :: t ! instantaneous time - Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + Real(DbKi), INTENT(IN ) :: X(:) ! state vector section for this line + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - INTEGER(IntKi) :: J ! index + INTEGER(IntKi) :: J ! index ! for a free Rod, there are 12 states: @@ -4818,7 +4881,7 @@ SUBROUTINE Rod_SetState(Rod, X, t, LineList) Rod%v6(4:6) = X(4:6) ! (rotational velocities about unrotated axes) - CALL Rod_SetDependentKin(Rod, t, LineList) + CALL Rod_SetDependentKin(Rod, t, m) else if (abs(Rod%typeNum) == 1) then ! pinned rod type (coupled or attached to something)t previously via setPinKin) @@ -4829,10 +4892,10 @@ SUBROUTINE Rod_SetState(Rod, X, t, LineList) Rod%v6(4:6) = X(1:3) ! (rotational velocities about unrotated axes) - CALL Rod_SetDependentKin(Rod, t, LineList) + CALL Rod_SetDependentKin(Rod, t, m) else - print *, "Error: Rod::setState called for a non-free rod type" ! <<< + print *, "Error: Rod::setState called for a non-free rod type in MoorDyn" ! <<< end if ! update Rod direction unit vector (simply equal to last three entries of r6) @@ -4845,11 +4908,11 @@ END SUBROUTINE Rod_SetState ! Set the Rod end kinematics then set the kinematics of dependent objects (any attached lines). ! This also determines the orientation of zero-length rods. !-------------------------------------------------------------- - SUBROUTINE Rod_SetDependentKin(Rod, t, LineList) + SUBROUTINE Rod_SetDependentKin(Rod, t, m) Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object Real(DbKi), INTENT(IN ) :: t ! instantaneous time - Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects (for simplicity, since Bodies deal with Rods and Connections) INTEGER(IntKi) :: l ! index of segments or nodes along line INTEGER(IntKi) :: J ! index @@ -4874,15 +4937,15 @@ SUBROUTINE Rod_SetDependentKin(Rod, t, LineList) !print *, Rod%r(:,0) if (Rod%N > 0) then ! set end B nodes only if the rod isn't zero length - call transformKinematicsAtoB(Rod%r6(1:3), Rod%r6(4:6), Rod%UnstrLen, Rod%v6, Rod%r(:,N), Rod%rd(:,N)) ! end B + CALL transformKinematicsAtoB(Rod%r6(1:3), Rod%r6(4:6), Rod%UnstrLen, Rod%v6, Rod%r(:,N), Rod%rd(:,N)) ! end B end if ! pass end node kinematics to any attached lines (this is just like what a Connection does, except for both ends) DO l=1,Rod%nAttachedA - CALL Line_SetEndKinematics(LineList(Rod%attachedA(l)), Rod%r(:,0), Rod%rd(:,0), t, Rod%TopA(l)) + CALL Line_SetEndKinematics(m%LineList(Rod%attachedA(l)), Rod%r(:,0), Rod%rd(:,0), t, Rod%TopA(l)) END DO DO l=1,Rod%nAttachedB - CALL Line_SetEndKinematics(LineList(Rod%attachedB(l)), Rod%r(:,N), Rod%rd(:,N), t, Rod%TopB(l)) + CALL Line_SetEndKinematics(m%LineList(Rod%attachedB(l)), Rod%r(:,N), Rod%rd(:,N), t, Rod%TopB(l)) END DO @@ -4891,7 +4954,7 @@ SUBROUTINE Rod_SetDependentKin(Rod, t, LineList) DO l=1,Rod%nAttachedA - CALL Line_GetEndSegmentInfo(LineList(Rod%attachedA(l)), qEnd, EIend, dlEnd, Rod%TopA(l)) + CALL Line_GetEndSegmentInfo(m%LineList(Rod%attachedA(l)), qEnd, EIend, dlEnd, Rod%TopA(l)) qMomentSum = qMomentSum + qEnd*EIend/dlEnd ! add each component to the summation vector @@ -4899,7 +4962,7 @@ SUBROUTINE Rod_SetDependentKin(Rod, t, LineList) DO l=1,Rod%nAttachedB - CALL Line_GetEndSegmentInfo(LineList(Rod%attachedB(l)), qEnd, EIend, dlEnd, Rod%TopB(l)) + CALL Line_GetEndSegmentInfo(m%LineList(Rod%attachedB(l)), qEnd, EIend, dlEnd, Rod%TopB(l)) qMomentSum = qMomentSum + qEnd*EIend/dlEnd ! add each component to the summation vector @@ -4911,21 +4974,21 @@ SUBROUTINE Rod_SetDependentKin(Rod, t, LineList) ! pass Rod orientation to any attached lines (this is just like what a Connection does, except for both ends) DO l=1,Rod%nAttachedA - CALL Line_SetEndOrientation(LineList(Rod%attachedA(l)), Rod%q, Rod%TopA(l), 0) + CALL Line_SetEndOrientation(m%LineList(Rod%attachedA(l)), Rod%q, Rod%TopA(l), 0) END DO DO l=1,Rod%nAttachedB - CALL Line_SetEndOrientation(LineList(Rod%attachedB(l)), Rod%q, Rod%TopB(l), 1) + CALL Line_SetEndOrientation(m%LineList(Rod%attachedB(l)), Rod%q, Rod%TopB(l), 1) END DO END SUBROUTINE Rod_SetDependentKin !-------------------------------------------------------------- !-------------------------------------------------------------- - SUBROUTINE Rod_GetStateDeriv(Rod, Xd, LineList, p) + SUBROUTINE Rod_GetStateDeriv(Rod, Xd, m, p) Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object Real(DbKi), INTENT(INOUT) :: Xd(:) ! state derivative vector section for this line - Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects (for simplicity, since Bodies deal with Rods and Connections) TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables @@ -4943,7 +5006,7 @@ SUBROUTINE Rod_GetStateDeriv(Rod, Xd, LineList, p) Real(DbKi) :: LU_temp(6,6) ! temporary matrix for LU decomposition - CALL Rod_GetNetForceAndMass(Rod, Rod%r(:,0), Fnet, M_out, LineList, p) + CALL Rod_GetNetForceAndMass(Rod, Rod%r(:,0), Fnet, M_out, m, p) @@ -4996,17 +5059,20 @@ SUBROUTINE Rod_GetStateDeriv(Rod, Xd, LineList, p) ! check for NaNs (should check all state derivatives, not just first 6) DO J = 1, 6 IF (Is_NaN(REAL(Xd(J),DbKi))) THEN - print *, "NaN detected at time ", Rod%time, " in Rod ",Rod%IdNum, " state derivatives:" - print *, Xd - - print *, "r0" - print *, Rod%r(:,0) - print *, "F" - print *, Fnet - print *, "M" - print *, M_out - print *, "acc" - print *, acc + print *, "NaN detected at time ", Rod%time, " in Rod ",Rod%IdNum + IF (wordy > 1) THEN + print *, " state derivatives:" + print *, Xd + + print *, "r0" + print *, Rod%r(:,0) + print *, "F" + print *, Fnet + print *, "M" + print *, M_out + print *, "acc" + print *, acc + END IF EXIT END IF @@ -5018,17 +5084,17 @@ END SUBROUTINE Rod_GetStateDeriv ! calculate the aggregate 3/6DOF rigid-body loads of a coupled rod including inertial loads !-------------------------------------------------------------- - SUBROUTINE Rod_GetCoupledForce(Rod, Fnet_out, LineList, p) + SUBROUTINE Rod_GetCoupledForce(Rod, Fnet_out, m, p) Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object Real(DbKi), INTENT( OUT) :: Fnet_out(6) ! force and moment vector - Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters Real(DbKi) :: F6_iner(6) ! inertial reaction force ! do calculations of forces and masses on each rod node - CALL Rod_DoRHS(Rod, LineList, p) + CALL Rod_DoRHS(Rod, m, p) ! add inertial loads as appropriate (written out in a redundant way just for clarity, and to support load separation in future) ! fixed coupled rod @@ -5054,19 +5120,19 @@ END SUBROUTINE Rod_GetCoupledForce ! calculate the aggregate 6DOF rigid-body force and mass data of the rod !-------------------------------------------------------------- - SUBROUTINE Rod_GetNetForceAndMass(Rod, rRef, Fnet_out, M_out, LineList, p) + SUBROUTINE Rod_GetNetForceAndMass(Rod, rRef, Fnet_out, M_out, m, p) Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object Real(DbKi), INTENT(IN ) :: rRef(3) ! global coordinates of reference point (end A for free Rods) Real(DbKi), INTENT( OUT) :: Fnet_out(6) ! force and moment vector about rRef Real(DbKi), INTENT( OUT) :: M_out(6,6) ! mass and inertia matrix about rRef - Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters Real(DbKi) :: rRel( 3) ! relative position of each node i from rRef ! do calculations of forces and masses on each rod node - CALL Rod_DoRHS(Rod, LineList, p) + CALL Rod_DoRHS(Rod, m, p) ! note: Some difference from MoorDyn C here. If this function is called by the Rod itself, the reference point must be end A @@ -5091,10 +5157,10 @@ END SUBROUTINE Rod_GetNetForceAndMass ! calculate the forces on the rod, including from attached lines !-------------------------------------------------------------- - SUBROUTINE Rod_DoRHS(Rod, LineList, p) + SUBROUTINE Rod_DoRHS(Rod, m, p) Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rodion object - Type(MD_Line), INTENT(INOUT) :: LineList(:) ! array of all the Line objects + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables @@ -5112,7 +5178,7 @@ SUBROUTINE Rod_DoRHS(Rod, LineList, p) Real(DbKi) :: m_i, v_i ! Real(DbKi) :: zeta ! wave elevation above/below a given node - Real(DbKi) :: h0 ! distance along rod centerline from end A to the waterplane + !Real(DbKi) :: h0 ! distance along rod centerline from end A to the waterplane Real(DbKi) :: deltaL ! submerged length of a given segment Real(DbKi) :: Lsum ! cumulative length along rod axis from bottom Real(DbKi) :: dL ! length attributed to node @@ -5173,7 +5239,7 @@ SUBROUTINE Rod_DoRHS(Rod, LineList, p) IF (p%WaterKin == 1) THEN ! wave kinematics interpolated from global grid in Waves object DO i=0,N - CALL getWaveKin(p, Rod%r(1,i), Rod%r(2,i), Rod%r(3,i), Rod%time, Rod%U(:,i), Rod%Ud(:,i), Rod%zeta(i), Rod%PDyn(i)) + CALL getWaveKin(p, Rod%r(1,i), Rod%r(2,i), Rod%r(3,i), Rod%time, m%WaveTi, Rod%U(:,i), Rod%Ud(:,i), Rod%zeta(i), Rod%PDyn(i)) !F(i) = 1.0 ! set VOF value to one for now (everything submerged - eventually this should be element-based!!!) <<<< ! <<<< currently F is not being used and instead a VOF variable is used within the node loop END DO @@ -5194,11 +5260,11 @@ SUBROUTINE Rod_DoRHS(Rod, LineList, p) zeta = Rod%zeta(N)! just use the wave elevation computed at the location of the top node for now if ((Rod%r(3,0) < zeta) .and. (Rod%r(3,N) > zeta)) then ! check if it's crossing the water plane (should also add some limits to avoid near-horizontals at some point) - h0 = (zeta - Rod%r(3,0))/Rod%q(3) ! distance along rod centerline from end A to the waterplane + Rod%h0 = (zeta - Rod%r(3,0))/Rod%q(3) ! distance along rod centerline from end A to the waterplane else if (Rod%r(3,0) < zeta) then - h0 = 2.0*Rod%UnstrLen ! fully submerged case <<<<<< remove the 2.0 and double check there are no if statements that get changed <<<< + Rod%h0 = Rod%UnstrLen ! fully submerged case <<<<<< remove the 2.0 and double check there are no if statements that get changed <<<< else - h0 = 0.0_DbKi ! fully unsubmerged case (ever applicable?) + Rod%h0 = 0.0_DbKi ! fully unsubmerged case (ever applicable?) end if @@ -5224,10 +5290,10 @@ SUBROUTINE Rod_DoRHS(Rod, LineList, p) END IF ! get scalar for submerged portion - IF (Lsum + dL < h0) THEN ! if fully submerged + IF (Lsum + dL <= Rod%h0) THEN ! if fully submerged VOF = 1.0_DbKi - ELSE IF (Lsum < h0) THEN ! if partially below waterline - VOF = (h0 - Lsum)/dL + ELSE IF (Lsum < Rod%h0) THEN ! if partially below waterline + VOF = (Rod%h0 - Lsum)/dL ELSE ! must be out of water VOF = 0.0_DbKi END IF @@ -5328,7 +5394,7 @@ SUBROUTINE Rod_DoRHS(Rod, LineList, p) ! ------ now add forces, moments, and added mass from Rod end effects (these can exist even if N==0) ------- ! end A - IF ((I==0) .and. (h0 > 0.0_ReKi)) THEN ! if this is end A and it is submerged + IF ((I==0) .and. (Rod%h0 > 0.0_ReKi)) THEN ! if this is end A and it is submerged ! >>> eventually should consider a VOF approach for the ends hTilt = 0.5*Rod%d/cosPhi <<< @@ -5365,7 +5431,7 @@ SUBROUTINE Rod_DoRHS(Rod, LineList, p) END IF - IF ((I==N) .and. (h0 > Rod%UnstrLen)) THEN ! if this end B and it is submerged (note, if N=0, both this and previous if statement are true) + IF ((I==N) .and. (Rod%h0 >= Rod%UnstrLen)) THEN ! if this end B and it is submerged (note, if N=0, both this and previous if statement are true) ! buoyancy force Ftemp = VOF * 0.25*Pi*Rod%d*Rod%d * p%rhoW*p%g*Rod%r(3,I) @@ -5419,7 +5485,7 @@ SUBROUTINE Rod_DoRHS(Rod, LineList, p) ! loop through lines attached to end A DO l=1,Rod%nAttachedA - CALL Line_GetEndStuff(LineList(Rod%attachedA(l)), Fnet_i, Mnet_i, Mass_i, Rod%TopA(l)) + CALL Line_GetEndStuff(m%LineList(Rod%attachedA(l)), Fnet_i, Mnet_i, Mass_i, Rod%TopA(l)) ! sum quantitites Rod%Fnet(:,0)= Rod%Fnet(:,0) + Fnet_i ! total force @@ -5431,7 +5497,7 @@ SUBROUTINE Rod_DoRHS(Rod, LineList, p) ! loop through lines attached to end B DO l=1,Rod%nAttachedB - CALL Line_GetEndStuff(LineList(Rod%attachedB(l)), Fnet_i, Mnet_i, Mass_i, Rod%TopB(l)) + CALL Line_GetEndStuff(m%LineList(Rod%attachedB(l)), Fnet_i, Mnet_i, Mass_i, Rod%TopB(l)) ! sum quantitites Rod%Fnet(:,N)= Rod%Fnet(:,N) + Fnet_i ! total force @@ -5523,26 +5589,26 @@ SUBROUTINE Rod_AddLine(Rod, lineID, TopOfLine, endB) if (endB==1) then ! attaching to end B - Print*, "L", lineID, "->R", Rod%IdNum , "b" + IF (wordy > 0) Print*, "L", lineID, "->R", Rod%IdNum , "b" IF (Rod%nAttachedB <10) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. Rod%nAttachedB = Rod%nAttachedB + 1 ! add the line to the number connected Rod%AttachedB(Rod%nAttachedB) = lineID Rod%TopB(Rod%nAttachedB) = TopOfLine ! attached to line ... 1 = top/fairlead(end B), 0 = bottom/anchor(end A) ELSE - Print*, "too many lines connected!" + Print*, "too many lines connected to Rod ", Rod%IdNum, " in MoorDyn!" END IF else ! attaching to end A - Print*, "L", lineID, "->R", Rod%IdNum , "a" + IF (wordy > 0) Print*, "L", lineID, "->R", Rod%IdNum , "a" IF (Rod%nAttachedA <10) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. Rod%nAttachedA = Rod%nAttachedA + 1 ! add the line to the number connected Rod%AttachedA(Rod%nAttachedA) = lineID Rod%TopA(Rod%nAttachedA) = TopOfLine ! attached to line ... 1 = top/fairlead(end B), 0 = bottom/anchor(end A) ELSE - Print*, "too many lines connected!" + Print*, "too many lines connected to Rod ", Rod%IdNum, " in MoorDyn!" END IF end if @@ -5643,6 +5709,60 @@ END SUBROUTINE Rod_RemoveLine !-------------------------------------------------------------- + SUBROUTINE Body_Setup( Body, tempArray, rhoW, ErrStat, ErrMsg) + + TYPE(MD_Body), INTENT(INOUT) :: Body ! the single body object of interest + REAL(DbKi), INTENT(IN) :: tempArray(6) ! initial pose of body + REAL(DbKi), INTENT(IN) :: rhoW + INTEGER, INTENT(INOUT ) :: ErrStat ! returns a non-zero value when an error occurs + CHARACTER(*), INTENT(INOUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None + + INTEGER(4) :: J ! Generic index + INTEGER(4) :: K ! Generic index + INTEGER(IntKi) :: N + + REAL(DbKi) :: Mtemp(6,6) + + ! set initial velocity to zero + Body%v6 = 0.0_DbKi + + !also set number of attached rods and points to zero initially + Body%nAttachedC = 0 + Body%nAttachedR = 0 + + ! for now take one entry and apply to all three DOFs just using a single entry for all axes <<<<< + DO J=2,3 + Body%BodyI(J) = Body%BodyI(1) + Body%BodyCdA(J) = Body%BodyCdA(1) + Body%BodyCa(J) = Body%BodyCa(1) + END DO + + ! set up body initial mass matrix (excluding any rods or attachements) + DO J=1,3 + Mtemp(J,J) = Body%bodyM ! fill in mass + Mtemp(3+J,3+J) = Body%bodyI(J) ! fill in inertia + END DO + + CALL TranslateMass6to6DOF(Body%rCG, Mtemp, Body%M0) ! account for potential CG offset <<< is the direction right? <<< + + DO J=1,6 + Body%M0(J,J) = Body%M0(J,J) + Body%bodyV*Body%bodyCa(1) ! add added mass in each direction about ref point (so only diagonals) <<< eventually expand to multi D + END DO + + ! --------------- if this is an independent body (not coupled) ---------- + ! set initial position and orientation of body from input file + Body%r6 = tempArray + + ! calculate orientation matrix based on latest angles + !RotMat(r6[3], r6[4], r6[5], OrMat); + Body%OrMat = TRANSPOSE( EulerConstruct( Body%r6(4:6) ) ) ! full Euler angle approach <<<< need to check order + + IF (wordy > 0) print *, "Set up Body ",Body%IdNum, ", type ", Body%typeNum + + ! need to add cleanup sub <<< + + END SUBROUTINE Body_Setup + ! ! used to initialize bodies that aren't free i.e. don't have states ! !-------------------------------------------------------------- ! SUBROUTINE Body_InitializeUnfree(Body, r6_in, mesh, mesh_index, m) @@ -5690,7 +5810,7 @@ END SUBROUTINE Rod_RemoveLine SUBROUTINE Body_Initialize(Body, states, m) Type(MD_Body), INTENT(INOUT) :: Body ! the Body object - Real(DbKi), INTENT(INOUT) :: states(6) ! state vector section for this Body + Real(DbKi), INTENT(INOUT) :: states(:) ! state vector section for this Body TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects INTEGER(IntKi) :: l ! index of segments or nodes along line @@ -5707,7 +5827,7 @@ SUBROUTINE Body_Initialize(Body, states, m) ! If any Rod is fixed to the body (not pinned), initialize it now because otherwise it won't be initialized DO l=1, Body%nAttachedR - if (m%RodList(Body%attachedR(l))%typeNum == 2) CALL Rod_Initialize(m%RodList(Body%attachedR(l)), dummyStates, m%LineList) + if (m%RodList(Body%attachedR(l))%typeNum == 2) CALL Rod_Initialize(m%RodList(Body%attachedR(l)), dummyStates, m) END DO ! Note: Connections don't need any initialization @@ -5731,7 +5851,7 @@ SUBROUTINE Body_InitializeUnfree(Body, m) ! If any Rod is fixed to the body (not pinned), initialize it now because otherwise it won't be initialized DO l=1, Body%nAttachedR - if (m%RodList(Body%attachedR(l))%typeNum == 2) CALL Rod_Initialize(m%RodList(Body%attachedR(l)), dummyStates, m%LineList) + if (m%RodList(Body%attachedR(l))%typeNum == 2) CALL Rod_Initialize(m%RodList(Body%attachedR(l)), dummyStates, m) END DO ! Note: Connections don't need any initialization @@ -5831,7 +5951,7 @@ SUBROUTINE Body_SetDependentKin(Body, t, m) ! calculate orientation matrix based on latest angles !CALL SmllRotTrans('', Body%r6(4), Body%r6(5), Body%r6(6), Body%TransMat, '', ErrStat2, ErrMsg2) - Body%OrMat = EulerConstruct( Body%r6(4:6) ) ! full Euler angle approach <<<< need to check order + Body%OrMat = TRANSPOSE( EulerConstruct( Body%r6(4:6) ) ) ! full Euler angle approach <<<< need to check order ! set kinematics of any dependent connections do l = 1,Body%nAttachedC @@ -5841,7 +5961,7 @@ SUBROUTINE Body_SetDependentKin(Body, t, m) ! >>> need to add acceleration terms here too? <<< ! pass above to the connection and get it to calculate the forces - CALL Connect_SetKinematics( m%ConnectList(Body%attachedC(l)), rConnect, rdConnect, m%zeros6(1:3), t, m%LineList) + CALL Connect_SetKinematics( m%ConnectList(Body%attachedC(l)), rConnect, rdConnect, m%zeros6(1:3), t, m) end do ! set kinematics of any dependent Rods @@ -5858,7 +5978,7 @@ SUBROUTINE Body_SetDependentKin(Body, t, m) aRod(4:6) = Body%a6(4:6) ! pass above to the rod and get it to calculate the forces - CALL Rod_SetKinematics(m%RodList(Body%attachedR(l)), rRod, vRod, aRod, t, m%LineList) + CALL Rod_SetKinematics(m%RodList(Body%attachedR(l)), rRod, vRod, aRod, t, m) end do END SUBROUTINE Body_SetDependentKin @@ -5885,7 +6005,7 @@ SUBROUTINE Body_GetCoupledForce(Body, Fnet_out, m, p) Fnet_out = Body%F6net + F6_iner ! add inertial loads else - print *, "ERROR, Body_GetCoupledForce called for wrong (non-coupled) body type!" + print *, "ERROR, Body_GetCoupledForce called for wrong (non-coupled) body type in MoorDyn!" end if END SUBROUTINE Body_GetCoupledForce @@ -5924,8 +6044,9 @@ SUBROUTINE Body_GetStateDeriv(Body, Xd, m, p) ! check for NaNs (should check all state derivatives, not just first 6) DO J = 1, 6 IF (Is_NaN(REAL(Xd(J),DbKi))) THEN - print *, "NaN detected at time ", Body%time, " in Body ",Body%IdNum, " state derivatives:" - print *, Xd + print *, "NaN detected at time ", Body%time, " in Body ",Body%IdNum, "in MoorDyn," + IF (wordy > 0) print *, "state derivatives:" + IF (wordy > 0) print *, Xd EXIT END IF END DO @@ -5989,7 +6110,7 @@ SUBROUTINE Body_DoRHS(Body, m, p) do l = 1,Body%nAttachedC ! get net force and mass from Connection on body ref point (global orientation) - CALL Connect_GetNetForceAndMass( m%ConnectList(Body%attachedC(l)), Body%r6(1:3), F6_i, M6_i, m%LineList, p) + CALL Connect_GetNetForceAndMass( m%ConnectList(Body%attachedC(l)), Body%r6(1:3), F6_i, M6_i, m, p) ! sum quantitites Body%F6net = Body%F6net + F6_i @@ -6000,7 +6121,7 @@ SUBROUTINE Body_DoRHS(Body, m, p) do l=1,Body%nAttachedR ! get net force and mass from Rod on body ref point (global orientation) - CALL Rod_GetNetForceAndMass(m%RodList(Body%attachedR(l)), Body%r6(1:3), F6_i, M6_i, m%LineList, p) + CALL Rod_GetNetForceAndMass(m%RodList(Body%attachedR(l)), Body%r6(1:3), F6_i, M6_i, m, p) ! sum quantitites Body%F6net = Body%F6net + F6_i @@ -6023,14 +6144,14 @@ SUBROUTINE Body_AddConnect(Body, connectID, coords) REAL(DbKi), INTENT(IN ) :: coords(3) - Print*, "C", connectID, "->B", Body%IdNum + IF (wordy > 0) Print*, "C", connectID, "->B", Body%IdNum IF(Body%nAttachedC < 30) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. Body%nAttachedC = Body%nAttachedC + 1 ! increment the number connected Body%AttachedC(Body%nAttachedC) = connectID Body%rConnectRel(:,Body%nAttachedC) = coords ! store relative position of connect on body ELSE - Print*, "too many connections attached!" + Print*, "too many Points attached to Body ", Body%IdNum, " in MoorDyn!" END IF END SUBROUTINE Body_AddConnect @@ -6047,7 +6168,7 @@ SUBROUTINE Body_AddRod(Body, rodID, coords) REAL(DbKi) :: tempUnitVec(3) REAL(DbKi) :: dummyLength - Print*, "R", rodID, "->B", Body%IdNum + IF (wordy > 0) Print*, "R", rodID, "->B", Body%IdNum IF(Body%nAttachedR < 30) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. Body%nAttachedR = Body%nAttachedR + 1 ! increment the number connected @@ -6061,7 +6182,7 @@ SUBROUTINE Body_AddRod(Body, rodID, coords) Body%r6RodRel(4:6, Body%nAttachedR) = tempUnitVec ELSE - Print*, "too many rods attached!" + Print*, "too many rods attached to Body ", Body%IdNum, " in MoorDyn" END IF END SUBROUTINE Body_AddRod @@ -6071,7 +6192,7 @@ END SUBROUTINE Body_AddRod ! master function to get wave/water kinematics at a given point -- called by each object fro grid-based data - SUBROUTINE getWaveKin(p, x, y, z, t, U, Ud, zeta, PDyn) + SUBROUTINE getWaveKin(p, x, y, z, t, tindex, U, Ud, zeta, PDyn) ! note, this whole approach assuems that px, py, and pz are in increasing order @@ -6080,6 +6201,7 @@ SUBROUTINE getWaveKin(p, x, y, z, t, U, Ud, zeta, PDyn) Real(DbKi), INTENT (IN ) :: y Real(DbKi), INTENT (IN ) :: z Real(DbKi), INTENT (IN ) :: t + INTEGER(IntKi), INTENT (INOUT) :: tindex ! pass time index to try starting from, returns identified time index Real(DbKi), INTENT (INOUT) :: U(3) Real(DbKi), INTENT (INOUT) :: Ud(3) Real(DbKi), INTENT (INOUT) :: zeta @@ -6092,10 +6214,11 @@ SUBROUTINE getWaveKin(p, x, y, z, t, U, Ud, zeta, PDyn) Real(DbKi) :: qt ! used in time step interpolation - CALL getInterpNums(p%px , x, ix, fx) - CALL getInterpNums(p%py , y, iy, fy) - CALL getInterpNums(p%pz , z, iz, fz) - CALL getInterpNums(p%tWave, t, it, ft) + CALL getInterpNums(p%px , x, 1, ix, fx) + CALL getInterpNums(p%py , y, 1, iy, fy) + CALL getInterpNums(p%pz , z, 1, iz, fz) + CALL getInterpNums(p%tWave, t, tindex, it, ft) + tindex = it CALL calculate3Dinterpolation(p%zeta, ix, iy, it, fx, fy, ft, zeta) @@ -6108,16 +6231,19 @@ SUBROUTINE getWaveKin(p, x, y, z, t, U, Ud, zeta, PDyn) CALL calculate4Dinterpolation(p%ay, ix, iy, iz, it, fx, fy, fz, ft, Ud(2) ) CALL calculate4Dinterpolation(p%az, ix, iy, iz, it, fx, fy, fz, ft, Ud(3) ) + END SUBROUTINE - SUBROUTINE getInterpNums(xlist, xin, i, fout) + SUBROUTINE getInterpNums(xlist, xin, istart, i, fout) Real(DbKi), INTENT (IN ) :: xlist(:) ! list of x values Real(DbKi), INTENT (IN ) :: xin ! x value to be interpolated + Integer(IntKi),INTENT (IN ) :: istart ! first lower index to try Integer(IntKi),INTENT ( OUT) :: i ! lower index to interpolate from Real(DbKi), INTENT ( OUT) :: fout ! fraction to return such that y* = y[i] + fout*(y[i+1]-y[i]) + Integer(IntKi) :: i1 = 1 ! the index we'll start at Integer(IntKi) :: nx nx = SIZE(xlist) @@ -6126,12 +6252,15 @@ SUBROUTINE getInterpNums(xlist, xin, i, fout) i = 1_IntKi fout = 0.0_DbKi - else if (xin >= xlist(nx)) THEN ! above highest data point + else if (xlist(nx) <= xin) THEN ! above highest data point i = nx fout = 0.0_DbKi else ! within the data range - DO i = 1, nx-1 + + IF (xlist(istart) < xin) i1 = istart ! if istart is below the actual value, start with it instead of starting at 1 to save time + + DO i = i1, nx-1 IF (xlist(i+1) > xin) THEN fout = (xin - xlist(i) )/( xlist(i+1) - xlist(i) ) exit @@ -6322,7 +6451,7 @@ subroutine GetOrientationAngles(p1, p2, phi, sinPhi, cosPhi, tanPhi, beta, sinBe vecLen = SQRT(Dot_Product(vec,vec)) vecLen2D = SQRT(vec(1)**2+vec(2)**2) if ( vecLen < 0.000001 ) then - print *, "ERROR in GetOrientationAngles" !call SeterrStat(ErrID_Fatal, 'An element of the Morison structure has co-located endpoints! This should never occur. Please review your model.', errStat, errMsg, 'Morison_CalcOutput' ) + print *, "ERROR in GetOrientationAngles in MoorDyn" !call SeterrStat(ErrID_Fatal, 'An element of the Morison structure has co-located endpoints! This should never occur. Please review your model.', errStat, errMsg, 'Morison_CalcOutput' ) print *, p1 print *, p2 k_hat = 1.0/0.0 diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index f86a703f44..f113898f36 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -28,6 +28,7 @@ MODULE MoorDyn_IO PRIVATE + INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output INTEGER, PARAMETER :: nCoef = 30 ! maximum number of entries to allow in nonlinear coefficient lookup tables ! it would be nice if the above worked for everything, but I think it needs to also be matched in the Registry @@ -63,17 +64,22 @@ MODULE MoorDyn_IO INTEGER, PARAMETER :: FX = 11 INTEGER, PARAMETER :: FY = 12 INTEGER, PARAMETER :: FZ = 13 - INTEGER, PARAMETER :: Pitch = 14 - INTEGER, PARAMETER :: Roll = 15 - INTEGER, PARAMETER :: Yaw = 16 + INTEGER, PARAMETER :: MX = 14 + INTEGER, PARAMETER :: MY = 15 + INTEGER, PARAMETER :: MZ = 16 + INTEGER, PARAMETER :: Pitch = 17 + INTEGER, PARAMETER :: Roll = 18 + INTEGER, PARAMETER :: Yaw = 19 + INTEGER, PARAMETER :: Sub = 20 ! List of units corresponding to the quantities parameters for QTypes - CHARACTER(ChanLen), PARAMETER :: UnitList(0:16) = (/ & + CHARACTER(ChanLen), PARAMETER :: UnitList(0:20) = (/ & "(s) ","(m) ","(m) ","(m) ", & "(m/s) ","(m/s) ","(m/s) ", & "(m/s2) ","(m/s2) ","(m/s2) ", & "(N) ","(N) ","(N) ","(N) ", & - "(deg) ","(deg) ","(deg) "/) + "(Nm) ","(Nm) ","(Nm) ", & + "(deg) ","(deg) ","(deg) ","(frac) "/) CHARACTER(28), PARAMETER :: OutPFmt = "( I4, 3X,A 10,1 X, A10 )" ! Output format parameter output list. CHARACTER(28), PARAMETER :: OutSFmt = "ES10.3E2" @@ -357,23 +363,21 @@ SUBROUTINE MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat, ErrMsg ) ! fairlead tension case (updated) <<<<<<<<<<<<<<<<<<<<<<<<<<< these are not currently working - need new way to find ObjID IF (let1 == 'FAIRTEN') THEN - p%OutParam(I)%OType = 2 ! connection object type + p%OutParam(I)%OType = 1 ! connection object type p%OutParam(I)%QType = Ten ! tension quantity type p%OutParam(I)%Units = UnitList(Ten) ! set units according to QType - READ (num1,*) oID ! this is the line number - p%OutParam(I)%ObjID = m%LineList(oID)%FairConnect ! get the connection ID of the fairlead - p%OutParam(I)%NodeID = -1 ! not used. m%LineList(oID)%N ! specify node N (fairlead) - print *, "WARNING - FAIRTEN and ANCHTEN results aren't supported yet in MD v2" - + READ (num1,*) oID ! this is the line number + p%OutParam(I)%ObjID = oID ! record the ID of the line + p%OutParam(I)%NodeID = m%LineList(oID)%N ! specify node N (end B, fairlead) + ! achor tension case ELSE IF (let1 == 'ANCHTEN') THEN - p%OutParam(I)%OType = 2 ! connectoin object type + p%OutParam(I)%OType = 1 ! connectoin object type p%OutParam(I)%QType = Ten ! tension quantity type p%OutParam(I)%Units = UnitList(Ten) ! set units according to QType - READ (num1,*) oID ! this is the line number - p%OutParam(I)%ObjID = m%LineList(oID)%AnchConnect ! get the connection ID of the fairlead - p%OutParam(I)%NodeID = -1 ! not used. m%LineList(oID)%0 ! specify node 0 (anchor) - print *, "WARNING - FAIRTEN and ANCHTEN results aren't supported yet in MD v2" + READ (num1,*) oID ! this is the line number + p%OutParam(I)%ObjID = oID ! record the ID of the line + p%OutParam(I)%NodeID = 0 ! specify node 0 (end A, anchor) ! more general case ELSE @@ -462,7 +466,7 @@ SUBROUTINE MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat, ErrMsg ) p%OutParam(I)%Units = UnitList(FY) ELSE IF (qVal == 'FZ') THEN p%OutParam(I)%QType = FZ - p%OutParam(I)%Units = UnitList(FZ) + p%OutParam(I)%Units = UnitList(FZ) ! <<<< should add moments as well <<<< ELSE IF (qVal == 'ROLL') THEN p%OutParam(I)%QType = Roll p%OutParam(I)%Units = UnitList(Roll) @@ -472,6 +476,9 @@ SUBROUTINE MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat, ErrMsg ) ELSE IF (qVal == 'YAW') THEN p%OutParam(I)%QType = Yaw p%OutParam(I)%Units = UnitList(Yaw) + ELSE IF (qVal == 'SUB') THEN + p%OutParam(I)%QType = Sub + p%OutParam(I)%Units = UnitList(Sub) ELSE CALL DenoteInvalidOutput(p%OutParam(I)) ! flag as invalid CALL WrScr('Warning: invalid output specifier '//trim(OutListTmp)//'. Quantity type not recognized.') @@ -708,7 +715,7 @@ SUBROUTINE MDIO_OpenOutput( OutRootName, p, m, InitOut, ErrStat, ErrMsg ) + (m%LineList(I)%N + 1)*SUM(m%LineList(I)%OutFlagList(7:9)) & + m%LineList(I)%N*SUM(m%LineList(I)%OutFlagList(10:18)) - PRINT *, LineNumOuts, " output channels" + if (wordy > 2) PRINT *, LineNumOuts, " output channels" Frmt = '(A10,'//TRIM(Int2LStr(1 + LineNumOuts))//'(A1,A12))' ! should evenutally use user specified format? !Frmt = '(A10,'//TRIM(Int2LStr(3+3*m%LineList(I)%N))//'(A1,A12))' @@ -862,7 +869,7 @@ SUBROUTINE MDIO_OpenOutput( OutRootName, p, m, InitOut, ErrStat, ErrMsg ) + (m%RodList(I)%N + 1)*SUM(m%RodList(I)%OutFlagList(10:11)) & + m%RodList(I)%N*SUM(m%RodList(I)%OutFlagList(12:18)) - PRINT *, RodNumOuts, " output channels" + if (wordy > 2) PRINT *, RodNumOuts, " output channels" Frmt = '(A10,'//TRIM(Int2LStr(1 + RodNumOuts))//'(A1,A12))' ! should evenutally use user specified format? !Frmt = '(A10,'//TRIM(Int2LStr(3+3*m%RodList(I)%N))//'(A1,A12))' @@ -1108,8 +1115,8 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) CASE (VelZ) y%WriteOutput(I) = m%LineList(p%OutParam(I)%ObjID)%rd(3,p%OutParam(I)%NodeID) ! z velocity CASE (Ten) - y%WriteOutput(I) = TwoNorm(m%LineList(p%OutParam(I)%ObjID)%T(:,p%OutParam(I)%NodeID)) ! this is actually the segment tension ( 1 < NodeID < N ) Should deal with properly! - ! ^^^^^^^^^^^^^^^^^^^^^^^^ The above should be changed to give a node-specific output including weight, as is done in the C version <<<< + y%WriteOutput(I) = Line_GetNodeTen(m%LineList(p%OutParam(I)%ObjID), p%OutParam(I)%NodeID, p) ! this is actually the segment tension ( 1 < NodeID < N ) Should deal with properly! + CASE DEFAULT y%WriteOutput(I) = 0.0_ReKi ErrStat = ErrID_Warn @@ -1166,9 +1173,11 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) CASE (FZ) y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%F6net(3) ! total force in z CASE (Roll) - y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%roll ! rod roll + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%roll*180.0/pi ! rod roll CASE (Pitch) - y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%pitch ! rod pitch + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%pitch*180.0/pi ! rod pitch + CASE (Sub) + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%h0 / m%RodList(p%OutParam(I)%ObjID)%UnstrLen ! rod submergence CASE DEFAULT y%WriteOutput(I) = 0.0_ReKi ErrStat = ErrID_Warn @@ -1196,11 +1205,11 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) CASE (FZ) y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%F6net(3) ! total force in z CASE (Roll) - y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%r6(4) ! roll + y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%r6(4)*180.0/pi ! roll CASE (Pitch) - y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%r6(5) ! pitch + y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%r6(5)*180.0/pi ! pitch CASE (Yaw) - y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%r6(6) ! yaw + y%WriteOutput(I) = m%BodyList(p%OutParam(I)%ObjID)%r6(6)*180.0/pi ! yaw CASE DEFAULT y%WriteOutput(I) = 0.0_ReKi ErrStat = ErrID_Warn @@ -1518,4 +1527,32 @@ END SUBROUTINE MDIO_WriteOutputs !----------------------------------------------------------------------------------------============ + ! get tension at any node including fairlead or anchor (accounting for weight in these latter cases) + !-------------------------------------------------------------- + FUNCTION Line_GetNodeTen(Line, i, p) result(NodeTen) + + TYPE(MD_Line), INTENT(IN ) :: Line ! label for the current line, for convenience + INTEGER(IntKi), INTENT(IN ) :: i ! node index to get tension at + TYPE(MD_ParameterType), INTENT(IN ) :: p ! Parameters + REAL(DbKi) :: NodeTen ! returned calculation of tension at node + + INTEGER(IntKi) :: J + REAL(DbKi) :: Tmag_squared + + if (i==0) then + NodeTen = sqrt( Line%Fnet(1,i)**2 + Line%Fnet(2,i) + (Line%Fnet(3,i) + Line%M(1,1,i)*(-p%g))**2 ) + else if (i==Line%N) then + NodeTen = sqrt( Line%Fnet(1,i)**2 + Line%Fnet(2,i) + (Line%Fnet(3,i) + Line%M(1,1,i)*(-p%g))**2 ) + else + Tmag_squared = 0.0_DbKi + DO J=1,3 + Tmag_squared = Tmag_squared + 0.25*(Line%T(J,i) + Line%Td(J,i) + Line%T(J,i+1) + Line%Td(J,i+1))**2 ! take average of tension in adjacent segments + END DO + NodeTen = sqrt(Tmag_squared) + end if + + END FUNCTION Line_GetNodeTen + !-------------------------------------------------------------- + + END MODULE MoorDyn_IO diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index d2d859fadd..b60f4335fa 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -18,7 +18,7 @@ include Registry_NWTC_Library.txt typedef MoorDyn/MD InitInputType ReKi g - -999.9 - "gravity constant" "[m/s^2]" typedef ^ ^ ReKi rhoW - -999.9 - "sea density" "[kg/m^3]" typedef ^ ^ ReKi WtrDepth - -999.9 - "depth of water" "[m]" -typedef ^ ^ ReKi PtfmInit {6} - - "initial position of platform" - +typedef ^ ^ ReKi PtfmInit {:}{:} - - "initial position of platform(s) shape: 6, nTurbines" - typedef ^ ^ CHARACTER(1024) FileName - "" - "MoorDyn input file" typedef ^ ^ CHARACTER(1024) RootName - - - "RootName for writing output files" - typedef ^ ^ LOGICAL Echo - "" - "echo parameter - do we want to echo the header line describing the input file?" @@ -39,6 +39,12 @@ typedef ^ ^ ReKi WavePDyn typedef ^ ^ ReKi WaveElev {:}{:} - - "" - typedef ^ ^ DbKi WaveTime {:} - - "Should this be double precision?" - +# nvm # Farm-level simulation inputs - these are passed by FAST.Farm - the arrays are populated from the individual turbine-level MoorDyn instances +# nvm typedef ^ ^ MeshType FarmCoupledKinematics {:} - - "array of input kinematics meshes from each of the turbine-level MoorDyn instances" "[m, m/s]" +# nvm typedef ^ ^ IntKi FarmNCpldBodies {:} - - "" "" +# nvm typedef ^ ^ IntKi FarmNCpldRods {:} - - "" "" +# nvm typedef ^ ^ IntKi FarmNCpldCons {:} - - "number of Fairlead Connections" "" + # ====================================== Internal data types ======================================================================== @@ -160,6 +166,7 @@ typedef ^ ^ DbKi CaEnd - typedef ^ ^ DbKi time - - - "current time" "[s]" typedef ^ ^ DbKi roll - - - "roll relative to vertical" "deg" typedef ^ ^ DbKi pitch - - - "pitch relative to vertical" "deg" +typedef ^ ^ DbKi h0 - - - "submerged length of rod axis, distance along rod centerline from end A to the waterplane (0 <= h0 <= L)" "m" typedef ^ ^ DbKi r {:}{:} - - "node positions" - typedef ^ ^ DbKi rd {:}{:} - - "node velocities" - typedef ^ ^ DbKi q {3} - - "tangent vector for rod as a whole" - @@ -299,6 +306,7 @@ typedef ^ ^ IntKi RodStateIsN {:} typedef ^ ^ IntKi BodyStateIs1 {:} - - "starting index of each body's states in state vector" "" typedef ^ ^ IntKi BodyStateIsN {:} - - "ending index of each body's states in state vector" "" typedef ^ ^ IntKi Nx - - - "number of states and size of state vector" "" +typedef ^ ^ IntKi WaveTi - - - "current interpolation index for wave time series data" "" typedef ^ ^ MD_ContinuousStateType xTemp - - - "contains temporary state vector used in integration (put here so it's only allocated once)" typedef ^ ^ MD_ContinuousStateType xdTemp - - - "contains temporary state derivative vector used in integration (put here so it's only allocated once)" typedef ^ ^ DbKi zeros6 {6} - - "array of zeros for convenience" @@ -335,6 +343,7 @@ typedef ^ ^ MD_OutParmType OutParam {:} typedef ^ ^ CHARACTER(1) Delim - - - "Column delimiter for output text files" - typedef ^ ^ IntKi MDUnOut - - - "Unit number of main output file" typedef ^ ^ IntKi WaterKin - - - "Flag for whether or how to consider water kinematics" +typedef ^ ^ IntKi nTurbines - - - "Number of turbines if MoorDyn is performing an array-level simulation with FAST.Farm, otherwise 0" typedef ^ ^ DbKi ux {:}{:}{:}{:} - - "water velocities time series at each grid point" - typedef ^ ^ DbKi uy {:}{:}{:}{:} - - "water velocities time series at each grid point" - @@ -351,9 +360,12 @@ typedef ^ ^ DbKi tWave {:} # ============================== Inputs ============================================================================================================================================ -typedef ^ InputType MeshType CoupledKinematics - - - "mesh for position AND VELOCITY of each fairlead in X,Y,Z" "[m, m/s]" +#typedef ^ InputType MeshType CoupledKinematics - - - "mesh for position AND VELOCITY of each fairlead in X,Y,Z" "[m, m/s]" +typedef ^ InputType MeshType CoupledKinematics {:} - - "array of meshes for position AND VELOCITY of each fairlead in X,Y,Z" "[m, m/s]" typedef ^ ^ ReKi DeltaL {:} - - "change in line length command for each channel" "[m]" typedef ^ ^ ReKi DeltaLdot {:} - - "rate of change of line length command for each channel" "[m]" + + #typedef ^ ^ DbKi U {:}{:} - - "water velocities at each node" - #typedef ^ ^ DbKi Ud {:}{:} - - "water accelerations at each node" - #typedef ^ ^ DbKi zeta {:} - - "water surface elevations above each node" - @@ -362,5 +374,6 @@ typedef ^ ^ ReKi DeltaLdot {:} ## ============================== Outputs ============================================================================================================================================ typedef ^ OutputType MeshType CoupledLoads - - - "point mesh for fairlead forces in X,Y,Z" "[N]" +>>> should this be an array? #typedef ^ ^ DbKi rAll {:}{:} - - "Mesh of all point positions: bodies, rods, points, line internal nodes" - typedef ^ ^ ReKi WriteOutput {:} - - "output vector returned to glue code" "" diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index 23ba05024e..ab5ad35c38 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -180,6 +180,7 @@ MODULE MoorDyn_Types REAL(DbKi) :: time !< current time [[s]] REAL(DbKi) :: roll !< roll relative to vertical [deg] REAL(DbKi) :: pitch !< pitch relative to vertical [deg] + REAL(DbKi) :: h0 !< submerged length of rod axis, distance along rod centerline from end A to the waterplane (0 <= h0 <= L) [m] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: r !< node positions [-] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: rd !< node velocities [-] REAL(DbKi) , DIMENSION(1:3) :: q !< tangent vector for rod as a whole [-] @@ -328,6 +329,7 @@ MODULE MoorDyn_Types INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: BodyStateIs1 !< starting index of each body's states in state vector [] INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: BodyStateIsN !< ending index of each body's states in state vector [] INTEGER(IntKi) :: Nx !< number of states and size of state vector [] + INTEGER(IntKi) :: WaveTi !< current interpolation index for wave time series data [] TYPE(MD_ContinuousStateType) :: xTemp !< contains temporary state vector used in integration (put here so it's only allocated once) [-] TYPE(MD_ContinuousStateType) :: xdTemp !< contains temporary state derivative vector used in integration (put here so it's only allocated once) [-] REAL(DbKi) , DIMENSION(1:6) :: zeros6 !< array of zeros for convenience [-] @@ -2258,6 +2260,7 @@ SUBROUTINE MD_CopyRod( SrcRodData, DstRodData, CtrlCode, ErrStat, ErrMsg ) DstRodData%time = SrcRodData%time DstRodData%roll = SrcRodData%roll DstRodData%pitch = SrcRodData%pitch + DstRodData%h0 = SrcRodData%h0 IF (ALLOCATED(SrcRodData%r)) THEN i1_l = LBOUND(SrcRodData%r,1) i1_u = UBOUND(SrcRodData%r,1) @@ -2657,6 +2660,7 @@ SUBROUTINE MD_PackRod( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Size Db_BufSz = Db_BufSz + 1 ! time Db_BufSz = Db_BufSz + 1 ! roll Db_BufSz = Db_BufSz + 1 ! pitch + Db_BufSz = Db_BufSz + 1 ! h0 Int_BufSz = Int_BufSz + 1 ! r allocated yes/no IF ( ALLOCATED(InData%r) ) THEN Int_BufSz = Int_BufSz + 2*2 ! r upper/lower bounds for each dimension @@ -2854,6 +2858,8 @@ SUBROUTINE MD_PackRod( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Size Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%pitch Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%h0 + Db_Xferred = Db_Xferred + 1 IF ( .NOT. ALLOCATED(InData%r) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -3359,6 +3365,8 @@ SUBROUTINE MD_UnPackRod( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) Db_Xferred = Db_Xferred + 1 OutData%pitch = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 + OutData%h0 = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! r not allocated Int_Xferred = Int_Xferred + 1 ELSE @@ -7066,6 +7074,7 @@ SUBROUTINE MD_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) DstMiscData%BodyStateIsN = SrcMiscData%BodyStateIsN ENDIF DstMiscData%Nx = SrcMiscData%Nx + DstMiscData%WaveTi = SrcMiscData%WaveTi CALL MD_CopyContState( SrcMiscData%xTemp, DstMiscData%xTemp, CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) IF (ErrStat>=AbortErrLev) RETURN @@ -7473,6 +7482,7 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_BufSz = Int_BufSz + SIZE(InData%BodyStateIsN) ! BodyStateIsN END IF Int_BufSz = Int_BufSz + 1 ! Nx + Int_BufSz = Int_BufSz + 1 ! WaveTi Int_BufSz = Int_BufSz + 3 ! xTemp: size of buffers for each call to pack subtype CALL MD_PackContState( Re_Buf, Db_Buf, Int_Buf, InData%xTemp, ErrStat2, ErrMsg2, .TRUE. ) ! xTemp CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) @@ -8066,6 +8076,8 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz END DO END IF IntKiBuf(Int_Xferred) = InData%Nx + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%WaveTi Int_Xferred = Int_Xferred + 1 CALL MD_PackContState( Re_Buf, Db_Buf, Int_Buf, InData%xTemp, ErrStat2, ErrMsg2, OnlySize ) ! xTemp CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) @@ -8856,6 +8868,8 @@ SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) END DO END IF OutData%Nx = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%WaveTi = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 Buf_size=IntKiBuf( Int_Xferred ) Int_Xferred = Int_Xferred + 1 From 47f37c54e287df010be36e79dbf402db5fd892e5 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Sun, 21 Mar 2021 01:58:14 -0600 Subject: [PATCH 013/242] Fix-ups from merging StrucCtrl with MDv2 work. - Set up new control section in MoorDyn input file parsing. - A couple adjustments to stage-setting for future farm-level mooring capability with FAST.Farm. --- modules/moordyn/src/MoorDyn.f90 | 48 +++----- modules/moordyn/src/MoorDyn_Registry.txt | 7 +- modules/moordyn/src/MoorDyn_Types.f90 | 105 ++++++++++++++---- .../openfast-library/src/FAST_Registry.txt | 1 - modules/openfast-library/src/FAST_Types.f90 | 1 - modules/subdyn/src/FEM.f90 | 2 +- 6 files changed, 102 insertions(+), 62 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 9e1cb99d85..79aafdaa0f 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -861,12 +861,16 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! this is temporary for backwards compatibility >>>>> will need to update for more versatile coupling >>>> <<<<<<< this looks pretty good. Make sure it's done only once - either here or near end of init. Same for Rods and bodies. ! NOTE: second index would be used for multi-turbine couplings in FAST.Farm - CALL SmllRotTrans('PtfmInit', InitInp%PtfmInit(4,1),InitInp%PtfmInit(5,1),InitInp%PtfmInit(6,1), OrMat, '', ErrStat2, ErrMsg2) + CALL SmllRotTrans('PtfmInit', InitInp%PtfmInit(4),InitInp%PtfmInit(5),InitInp%PtfmInit(6), OrMat, '', ErrStat2, ErrMsg2) + !CALL SmllRotTrans('PtfmInit', InitInp%PtfmInit(4,1),InitInp%PtfmInit(5,1),InitInp%PtfmInit(6,1), OrMat, '', ErrStat2, ErrMsg2) ! set initial node position, including adjustments due to initial platform rotations and translations <<< could convert to array math - m%ConnectList(l)%r(1) = InitInp%PtfmInit(1,1) + OrMat(1,1)*tempArray(1) + OrMat(2,1)*tempArray(2) + OrMat(3,1)*tempArray(3) - m%ConnectList(l)%r(2) = InitInp%PtfmInit(2,1) + OrMat(1,2)*tempArray(1) + OrMat(2,2)*tempArray(2) + OrMat(3,2)*tempArray(3) - m%ConnectList(l)%r(3) = InitInp%PtfmInit(3,1) + OrMat(1,3)*tempArray(1) + OrMat(2,3)*tempArray(2) + OrMat(3,3)*tempArray(3) + m%ConnectList(l)%r(1) = InitInp%PtfmInit(1) + OrMat(1,1)*tempArray(1) + OrMat(2,1)*tempArray(2) + OrMat(3,1)*tempArray(3) + m%ConnectList(l)%r(2) = InitInp%PtfmInit(2) + OrMat(1,2)*tempArray(1) + OrMat(2,2)*tempArray(2) + OrMat(3,2)*tempArray(3) + m%ConnectList(l)%r(3) = InitInp%PtfmInit(3) + OrMat(1,3)*tempArray(1) + OrMat(2,3)*tempArray(2) + OrMat(3,3)*tempArray(3) + !m%ConnectList(l)%r(1) = InitInp%PtfmInit(1,1) + OrMat(1,1)*tempArray(1) + OrMat(2,1)*tempArray(2) + OrMat(3,1)*tempArray(3) + !m%ConnectList(l)%r(2) = InitInp%PtfmInit(2,1) + OrMat(1,2)*tempArray(1) + OrMat(2,2)*tempArray(2) + OrMat(3,2)*tempArray(3) + !m%ConnectList(l)%r(3) = InitInp%PtfmInit(3,1) + OrMat(1,3)*tempArray(1) + OrMat(2,3)*tempArray(2) + OrMat(3,3)*tempArray(3) else if ((let1 == "CONNECT") .or. (let1 == "CON") .or. (let1 == "FREE")) then m%ConnectList(l)%typeNum = 0 @@ -1114,22 +1118,18 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !read into a line read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - - ! parse out entries: CtrlChan, LineIdNums - IF (ErrStat2 == 0) THEN - READ(Line,*,IOSTAT=ErrStat2) Itemp, TempString5 - END IF - ! split the line IDs specified for this channel and apply to those lines - N = count(transfer(TempString5, 'a', len(TempString5)) == ",") + 1 ! number of line IDs given - !N = COUNT([(TempString5(i:i),i=1,len(TempString5))].eq.',') + 1 - read(TempString5, *) TempIDnums(1:N) ! parse out each line ID + ! count commas to determine how many line IDs specified for this channel + N = count(transfer(Line, 'a', len(Line)) == ",") + 1 ! number of line IDs given + + ! parse out entries: CtrlChan, LineIdNums + read(Line, *) Itemp, TempIDnums(1:N) ! parse out each line ID DO J = 1,N if (TempIDnums(J) <= p%nLines) then ! ensure line ID is in range if (m%LineList( TempIDnums(J) )%CtrlChan == 0) then ! ensure line doesn't already have a CtrlChan assigned m%LineList( TempIDnums(J) )%CtrlChan = Itemp - print *, 'Assigned Line ', TempIDnums(J), ' assigned control channel ', Itemp + print *, 'Assigned Line ', TempIDnums(J), ' to control channel ', Itemp else print *, 'Error: Line ', TempIDnums(J), ' already is assigned to control channel ', m%LineList( TempIDnums(J) )%CtrlChan, ' so cannot also be assigned to channel ', Itemp end if @@ -1141,26 +1141,6 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er END DO - -! character :: string*30 = "1,10,123,15,654,12" -! integer :: n, iarray(100) -! n = count(transfer(string, 'a', len(string)) == ",") -! read(string, *) iarray(1:n+1) ! N+1 because one more int than comma -! print *, 'nvalues=', n+1 -! print '(i10)', iarray(1:n+1) -! -! -! -! character(len=100) :: string = 'This;is;a test;hello;world!' -! integer :: n -! character(80), allocatable :: strarray(:) -! n = count(transfer(string, 'a', len(string)) == ";") -! allocate(strarray(n+1)) -! read(string, *) strarray(1:n+1) !N+1 because one more parts than semicolon -! print *, 'nvalues=', n+1 -! print '(a)', strarray(1:n+1) - - !------------------------------------------------------------------------------------------- else if (INDEX(Line, "FAILURE") > 0) then ! if failure conditions header diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 1228e79421..fa9f78e310 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -18,7 +18,8 @@ include Registry_NWTC_Library.txt typedef MoorDyn/MD InitInputType ReKi g - -999.9 - "gravity constant" "[m/s^2]" typedef ^ ^ ReKi rhoW - -999.9 - "sea density" "[kg/m^3]" typedef ^ ^ ReKi WtrDepth - -999.9 - "depth of water" "[m]" -typedef ^ ^ ReKi PtfmInit {:}{:} - - "initial position of platform(s) shape: 6, nTurbines" - +typedef ^ ^ ReKi PtfmInit {:} - - "initial position of platform(s) originally size 6" - +#typedef ^ ^ ReKi PtfmInit {:}{:} - - "initial position of platform(s) shape: 6, nTurbines" - typedef ^ ^ CHARACTER(1024) FileName - "" - "MoorDyn input file" typedef ^ ^ CHARACTER(1024) RootName - - - "RootName for writing output files" - typedef ^ ^ LOGICAL Echo - "" - "echo parameter - do we want to echo the header line describing the input file?" @@ -376,6 +377,6 @@ typedef ^ ^ ReKi DeltaLdot {:} ## ============================== Outputs ============================================================================================================================================ typedef ^ OutputType MeshType CoupledLoads - - - "point mesh for fairlead forces in X,Y,Z" "[N]" ->>> should this be an array? -#typedef ^ ^ DbKi rAll {:}{:} - - "Mesh of all point positions: bodies, rods, points, line internal nodes" - typedef ^ ^ ReKi WriteOutput {:} - - "output vector returned to glue code" "" +# should CoupledLoads be an array? +#typedef ^ ^ DbKi rAll {:}{:} - - "Mesh of all point positions: bodies, rods, points, line internal nodes" - diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index 5f23c83a8c..04bee87059 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -38,7 +38,7 @@ MODULE MoorDyn_Types REAL(ReKi) :: g = -999.9 !< gravity constant [[m/s^2]] REAL(ReKi) :: rhoW = -999.9 !< sea density [[kg/m^3]] REAL(ReKi) :: WtrDepth = -999.9 !< depth of water [[m]] - REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: PtfmInit !< initial position of platform(s) shape: 6, nTurbines [-] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: PtfmInit !< initial position of platform(s) originally size 6 [-] CHARACTER(1024) :: FileName !< MoorDyn input file [-] CHARACTER(1024) :: RootName !< RootName for writing output files [-] LOGICAL :: Echo !< echo parameter - do we want to echo the header line describing the input file? [-] @@ -394,6 +394,7 @@ MODULE MoorDyn_Types ! ========= MD_OutputType ======= TYPE, PUBLIC :: MD_OutputType TYPE(MeshType) :: CoupledLoads !< point mesh for fairlead forces in X,Y,Z [[N]] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: WriteOutput !< output vector returned to glue code [] END TYPE MD_OutputType ! ======================= CONTAINS @@ -421,10 +422,8 @@ SUBROUTINE MD_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, ErrSt IF (ALLOCATED(SrcInitInputData%PtfmInit)) THEN i1_l = LBOUND(SrcInitInputData%PtfmInit,1) i1_u = UBOUND(SrcInitInputData%PtfmInit,1) - i2_l = LBOUND(SrcInitInputData%PtfmInit,2) - i2_u = UBOUND(SrcInitInputData%PtfmInit,2) IF (.NOT. ALLOCATED(DstInitInputData%PtfmInit)) THEN - ALLOCATE(DstInitInputData%PtfmInit(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + ALLOCATE(DstInitInputData%PtfmInit(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitInputData%PtfmInit.', ErrStat, ErrMsg,RoutineName) RETURN @@ -582,7 +581,7 @@ SUBROUTINE MD_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg Re_BufSz = Re_BufSz + 1 ! WtrDepth Int_BufSz = Int_BufSz + 1 ! PtfmInit allocated yes/no IF ( ALLOCATED(InData%PtfmInit) ) THEN - Int_BufSz = Int_BufSz + 2*2 ! PtfmInit upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + 2*1 ! PtfmInit upper/lower bounds for each dimension Re_BufSz = Re_BufSz + SIZE(InData%PtfmInit) ! PtfmInit END IF Int_BufSz = Int_BufSz + 1*LEN(InData%FileName) ! FileName @@ -658,16 +657,11 @@ SUBROUTINE MD_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg Int_Xferred = Int_Xferred + 1 IntKiBuf( Int_Xferred ) = LBOUND(InData%PtfmInit,1) IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PtfmInit,1) - Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%PtfmInit,2) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PtfmInit,2) Int_Xferred = Int_Xferred + 2 - DO i2 = LBOUND(InData%PtfmInit,2), UBOUND(InData%PtfmInit,2) - DO i1 = LBOUND(InData%PtfmInit,1), UBOUND(InData%PtfmInit,1) - ReKiBuf(Re_Xferred) = InData%PtfmInit(i1,i2) - Re_Xferred = Re_Xferred + 1 - END DO + DO i1 = LBOUND(InData%PtfmInit,1), UBOUND(InData%PtfmInit,1) + ReKiBuf(Re_Xferred) = InData%PtfmInit(i1) + Re_Xferred = Re_Xferred + 1 END DO END IF DO I = 1, LEN(InData%FileName) @@ -838,20 +832,15 @@ SUBROUTINE MD_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Err i1_l = IntKiBuf( Int_Xferred ) i1_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 - i2_l = IntKiBuf( Int_Xferred ) - i2_u = IntKiBuf( Int_Xferred + 1) - Int_Xferred = Int_Xferred + 2 IF (ALLOCATED(OutData%PtfmInit)) DEALLOCATE(OutData%PtfmInit) - ALLOCATE(OutData%PtfmInit(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + ALLOCATE(OutData%PtfmInit(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%PtfmInit.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i2 = LBOUND(OutData%PtfmInit,2), UBOUND(OutData%PtfmInit,2) - DO i1 = LBOUND(OutData%PtfmInit,1), UBOUND(OutData%PtfmInit,1) - OutData%PtfmInit(i1,i2) = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 - END DO + DO i1 = LBOUND(OutData%PtfmInit,1), UBOUND(OutData%PtfmInit,1) + OutData%PtfmInit(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 END DO END IF DO I = 1, LEN(OutData%FileName) @@ -10780,6 +10769,7 @@ SUBROUTINE MD_CopyOutput( SrcOutputData, DstOutputData, CtrlCode, ErrStat, ErrMs CHARACTER(*), INTENT( OUT) :: ErrMsg ! Local INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyOutput' @@ -10789,6 +10779,18 @@ SUBROUTINE MD_CopyOutput( SrcOutputData, DstOutputData, CtrlCode, ErrStat, ErrMs CALL MeshCopy( SrcOutputData%CoupledLoads, DstOutputData%CoupledLoads, CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat>=AbortErrLev) RETURN +IF (ALLOCATED(SrcOutputData%WriteOutput)) THEN + i1_l = LBOUND(SrcOutputData%WriteOutput,1) + i1_u = UBOUND(SrcOutputData%WriteOutput,1) + IF (.NOT. ALLOCATED(DstOutputData%WriteOutput)) THEN + ALLOCATE(DstOutputData%WriteOutput(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstOutputData%WriteOutput.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstOutputData%WriteOutput = SrcOutputData%WriteOutput +ENDIF END SUBROUTINE MD_CopyOutput SUBROUTINE MD_DestroyOutput( OutputData, ErrStat, ErrMsg ) @@ -10801,6 +10803,9 @@ SUBROUTINE MD_DestroyOutput( OutputData, ErrStat, ErrMsg ) ErrStat = ErrID_None ErrMsg = "" CALL MeshDestroy( OutputData%CoupledLoads, ErrStat, ErrMsg ) +IF (ALLOCATED(OutputData%WriteOutput)) THEN + DEALLOCATE(OutputData%WriteOutput) +ENDIF END SUBROUTINE MD_DestroyOutput SUBROUTINE MD_PackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) @@ -10856,6 +10861,11 @@ SUBROUTINE MD_PackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, S Int_BufSz = Int_BufSz + SIZE( Int_Buf ) DEALLOCATE(Int_Buf) END IF + Int_BufSz = Int_BufSz + 1 ! WriteOutput allocated yes/no + IF ( ALLOCATED(InData%WriteOutput) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! WriteOutput upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%WriteOutput) ! WriteOutput + END IF IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) IF (ErrStat2 /= 0) THEN @@ -10911,6 +10921,21 @@ SUBROUTINE MD_PackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, S ELSE IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 ENDIF + IF ( .NOT. ALLOCATED(InData%WriteOutput) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%WriteOutput,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%WriteOutput,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%WriteOutput,1), UBOUND(InData%WriteOutput,1) + ReKiBuf(Re_Xferred) = InData%WriteOutput(i1) + Re_Xferred = Re_Xferred + 1 + END DO + END IF END SUBROUTINE MD_PackOutput SUBROUTINE MD_UnPackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) @@ -10926,6 +10951,7 @@ SUBROUTINE MD_UnPackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg INTEGER(IntKi) :: Db_Xferred INTEGER(IntKi) :: Int_Xferred INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackOutput' @@ -10979,6 +11005,24 @@ SUBROUTINE MD_UnPackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WriteOutput not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%WriteOutput)) DEALLOCATE(OutData%WriteOutput) + ALLOCATE(OutData%WriteOutput(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%WriteOutput.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%WriteOutput,1), UBOUND(OutData%WriteOutput,1) + OutData%WriteOutput(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END IF END SUBROUTINE MD_UnPackOutput @@ -11244,6 +11288,8 @@ SUBROUTINE MD_Output_ExtrapInterp1(y1, y2, tin, y_out, tin_out, ErrStat, ErrMsg REAL(DbKi) :: ScaleFactor ! temporary for extrapolation/interpolation INTEGER(IntKi) :: ErrStat2 ! local errors CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + INTEGER :: i01 ! dim1 level 0 counter variable for arrays of ddts + INTEGER :: i1 ! dim1 counter variable for arrays ! Initialize ErrStat ErrStat = ErrID_None ErrMsg = "" @@ -11260,6 +11306,12 @@ SUBROUTINE MD_Output_ExtrapInterp1(y1, y2, tin, y_out, tin_out, ErrStat, ErrMsg ScaleFactor = t_out / t(2) CALL MeshExtrapInterp1(y1%CoupledLoads, y2%CoupledLoads, tin, y_out%CoupledLoads, tin_out, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) +IF (ALLOCATED(y_out%WriteOutput) .AND. ALLOCATED(y1%WriteOutput)) THEN + DO i1 = LBOUND(y_out%WriteOutput,1),UBOUND(y_out%WriteOutput,1) + b = -(y1%WriteOutput(i1) - y2%WriteOutput(i1)) + y_out%WriteOutput(i1) = y1%WriteOutput(i1) + b * ScaleFactor + END DO +END IF ! check if allocated END SUBROUTINE MD_Output_ExtrapInterp1 @@ -11295,6 +11347,8 @@ SUBROUTINE MD_Output_ExtrapInterp2(y1, y2, y3, tin, y_out, tin_out, ErrStat, Err INTEGER(IntKi) :: ErrStat2 ! local errors CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors CHARACTER(*), PARAMETER :: RoutineName = 'MD_Output_ExtrapInterp2' + INTEGER :: i01 ! dim1 level 0 counter variable for arrays of ddts + INTEGER :: i1 ! dim1 counter variable for arrays ! Initialize ErrStat ErrStat = ErrID_None ErrMsg = "" @@ -11317,6 +11371,13 @@ SUBROUTINE MD_Output_ExtrapInterp2(y1, y2, y3, tin, y_out, tin_out, ErrStat, Err ScaleFactor = t_out / (t(2) * t(3) * (t(2) - t(3))) CALL MeshExtrapInterp2(y1%CoupledLoads, y2%CoupledLoads, y3%CoupledLoads, tin, y_out%CoupledLoads, tin_out, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) +IF (ALLOCATED(y_out%WriteOutput) .AND. ALLOCATED(y1%WriteOutput)) THEN + DO i1 = LBOUND(y_out%WriteOutput,1),UBOUND(y_out%WriteOutput,1) + b = (t(3)**2*(y1%WriteOutput(i1) - y2%WriteOutput(i1)) + t(2)**2*(-y1%WriteOutput(i1) + y3%WriteOutput(i1)))* scaleFactor + c = ( (t(2)-t(3))*y1%WriteOutput(i1) + t(3)*y2%WriteOutput(i1) - t(2)*y3%WriteOutput(i1) ) * scaleFactor + y_out%WriteOutput(i1) = y1%WriteOutput(i1) + b + c * t_out + END DO +END IF ! check if allocated END SUBROUTINE MD_Output_ExtrapInterp2 END MODULE MoorDyn_Types diff --git a/modules/openfast-library/src/FAST_Registry.txt b/modules/openfast-library/src/FAST_Registry.txt index 7a8b5d2dca..18eefe202b 100644 --- a/modules/openfast-library/src/FAST_Registry.txt +++ b/modules/openfast-library/src/FAST_Registry.txt @@ -714,7 +714,6 @@ typedef ^ FAST_ExternInitType CHARACTER(1024) RootName - - - "Root name of FAST typedef ^ FAST_ExternInitType IntKi NumActForcePtsBlade - - - "number of actuator line force points in blade" - typedef ^ FAST_ExternInitType IntKi NumActForcePtsTower - - - "number of actuator line force points in tower" - - # ..... FAST Turbine Data (one realization) ....................................................................................................... typedef ^ FAST_TurbineType IntKi TurbID - 1 - "Turbine ID Number" - typedef ^ FAST_TurbineType FAST_ParameterType p_FAST - - - "Parameters for the glue code" - diff --git a/modules/openfast-library/src/FAST_Types.f90 b/modules/openfast-library/src/FAST_Types.f90 index 1ec448768f..322d4d3e5f 100644 --- a/modules/openfast-library/src/FAST_Types.f90 +++ b/modules/openfast-library/src/FAST_Types.f90 @@ -72,7 +72,6 @@ MODULE FAST_Types USE MAP_Fortran_Types USE MAP_Types USE MoorDyn_Types -USE should_Types USE OrcaFlexInterface_Types USE ExtPtfm_MCKF_Types USE NWTC_Library diff --git a/modules/subdyn/src/FEM.f90 b/modules/subdyn/src/FEM.f90 index 2d5972e4d7..5a6f89b0ee 100644 --- a/modules/subdyn/src/FEM.f90 +++ b/modules/subdyn/src/FEM.f90 @@ -78,7 +78,7 @@ SUBROUTINE EigenSolve(K, M, N, bCheckSingularity, EigVect, Omega, ErrStat, ErrMs Omega2(:) =0.0_LaKi DO I=1,N !Initialize the key and calculate Omega KEY(I)=I - Omega2(I) = AlphaR(I)/Beta(I) + !Omega2(I) = AlphaR(I)/Beta(I) if ( EqualRealNos(real(Beta(I),ReKi),0.0_ReKi) ) then ! --- Beta =0 if (bCheckSingularity) call WrScr('[WARN] Large eigenvalue found, system may be ill-conditioned') From ee8f77222376ec2b13a9cd2597b53315c7c4f307 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Mon, 29 Mar 2021 09:35:04 -0600 Subject: [PATCH 014/242] Drafting shared moorings capability in FAST.Farm: - Created a MoorDyn instance in FAST.Farm that can do array-level moorings. - Modified MoorDyn to take an array of all platform-related inputs/outputs. - In normal MoorDyn use in FAST, only the first entry of these arrays is used. - in FAST.Farm use of the farm-level MoorDyn, the arrays are size nTurbines and they facilitate coupling with each turbine. - Modified FAST_Solver etc. to support the new array format - Various modifications in FAST.Farm to support the farm-level mooring capability. - New MoorDyn coupling functions are in FAST_Farm_Subs. - Made a separate workflow in FARM_UpdateStates to support the necessary substepping between all FAST instances and MoorDyn, at new introduced timestep DT_mooring, which is between DT_low and DT_FAST. Included a modified OpenMP nested parallel approach to keep the FAST instances in parallel while having a longer-term parallel AWAE call. - Some bugs still. Currently hitting a mesh issue with the farm-level MoorDyn mesh mapping I set up. "MD_2_FWrap:MeshMapCreate:MeshMap%MapLoads not allocated because no nodes were found to map." --- glue-codes/fast-farm/src/FASTWrapper.f90 | 32 +- .../fast-farm/src/FASTWrapper_Registry.txt | 1 + .../fast-farm/src/FASTWrapper_Types.f90 | 17 +- .../fast-farm/src/FAST_Farm_Registry.txt | 27 +- glue-codes/fast-farm/src/FAST_Farm_Subs.f90 | 455 +++++- glue-codes/fast-farm/src/FAST_Farm_Types.f90 | 1330 ++++++++++++++++- modules/hydrodyn/src/HydroDyn_Types.f90 | 2 +- modules/hydrodyn/src/Waves_Types.f90 | 2 +- modules/moordyn/src/MoorDyn.f90 | 266 ++-- modules/moordyn/src/MoorDyn_Registry.txt | 19 +- modules/moordyn/src/MoorDyn_Types.f90 | 479 +++++- modules/openfast-library/src/FAST_Solver.f90 | 32 +- modules/openfast-library/src/FAST_Subs.f90 | 16 +- modules/openfast-library/src/FAST_Types.f90 | 4 +- 14 files changed, 2431 insertions(+), 251 deletions(-) diff --git a/glue-codes/fast-farm/src/FASTWrapper.f90 b/glue-codes/fast-farm/src/FASTWrapper.f90 index fc41c60183..e0a78530c3 100644 --- a/glue-codes/fast-farm/src/FASTWrapper.f90 +++ b/glue-codes/fast-farm/src/FASTWrapper.f90 @@ -44,6 +44,8 @@ MODULE FASTWrapper PUBLIC :: FWrap_t0 ! call to compute outputs at t0 [and initialize some more variables] PUBLIC :: FWrap_Increment ! call to update states to n+1 and compute outputs at n+1 + PUBLIC :: FWrap_SetInputs + PUBLIC :: FWrap_CalcOutput CONTAINS @@ -287,11 +289,11 @@ end subroutine cleanup END SUBROUTINE FWrap_Init !---------------------------------------------------------------------------------------------------------------------------------- ! this routine sets the parameters for the FAST Wrapper module. It does not set p%n_FAST_low because we need to initialize FAST first. -subroutine FWrap_SetParameters(InitInp, p, dt_FAST, InitInp_dt_low, ErrStat, ErrMsg) +subroutine FWrap_SetParameters(InitInp, p, dt_FAST, dt_caller, ErrStat, ErrMsg) TYPE(FWrap_InitInputType), INTENT(IN ) :: InitInp !< Input data for initialization routine TYPE(FWrap_ParameterType), INTENT(INOUT) :: p !< Parameters REAL(DbKi), INTENT(IN ) :: dt_FAST !< time step for FAST - REAL(DbKi), INTENT(IN ) :: InitInp_dt_low !< time step for FAST.Farm + REAL(DbKi), INTENT(IN ) :: dt_caller !< time step that FWrap will be called at by FAST.Farm (if MooringMod>0, this will be smaller than DT_low) INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status of the operation CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message if ErrStat /= ErrID_None @@ -318,22 +320,22 @@ subroutine FWrap_SetParameters(InitInp, p, dt_FAST, InitInp_dt_low, ErrStat, Err ! p%n_FAST_low has to be set AFTER we initialize FAST, because we need to know what the FAST time step is going to be. - IF ( EqualRealNos( dt_FAST, InitInp_dt_low ) ) THEN + IF ( EqualRealNos( dt_FAST, dt_caller ) ) THEN p%n_FAST_low = 1 ELSE - IF ( dt_FAST > InitInp_dt_low ) THEN + IF ( dt_FAST > dt_caller ) THEN ErrStat = ErrID_Fatal ErrMsg = "The FAST time step ("//TRIM(Num2LStr(dt_FAST))// & - " s) cannot be larger than FAST.Farm time step ("//TRIM(Num2LStr(InitInp_dt_low))//" s)." + " s) cannot be larger than FAST.Farm time step ("//TRIM(Num2LStr(dt_caller))//" s)." ELSE ! calculate the number of subcycles: - p%n_FAST_low = NINT( InitInp_dt_low / dt_FAST ) + p%n_FAST_low = NINT( dt_caller / dt_FAST ) ! let's make sure the FAST DT is an exact integer divisor of the global (FAST.Farm) time step: - IF ( .NOT. EqualRealNos( InitInp_dt_low, dt_FAST * p%n_FAST_low ) ) THEN + IF ( .NOT. EqualRealNos( dt_caller, dt_FAST * p%n_FAST_low ) ) THEN ErrStat = ErrID_Fatal ErrMsg = "The FASTWrapper module time step ("//TRIM(Num2LStr(dt_FAST))// & - " s) must be an integer divisor of the FAST.Farm time step ("//TRIM(Num2LStr(InitInp_dt_low))//" s)." + " s) must be an integer divisor of the FAST.Farm or farm-level mooring time step ("//TRIM(Num2LStr(dt_caller))//" s)." END IF END IF @@ -412,7 +414,7 @@ END SUBROUTINE FWrap_End SUBROUTINE FWrap_Increment( t, n, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) !.................................................................................................................................. - REAL(DbKi), INTENT(IN ) :: t !< Current simulation time in seconds + REAL(DbKi), INTENT(IN ) :: t !< Current simulation time in seconds (no longer used, since inputs are set elsewhere) INTEGER(IntKi), INTENT(IN ) :: n !< Current step of the simulation: t = n*Interval TYPE(FWrap_InputType), INTENT(INOUT) :: u !< Inputs at t (not changed, but possibly copied) TYPE(FWrap_ParameterType), INTENT(IN ) :: p !< Parameters @@ -452,11 +454,11 @@ SUBROUTINE FWrap_Increment( t, n, u, p, x, xd, z, OtherState, y, m, ErrStat, Err !ELSE ! ! set the inputs needed for FAST - call FWrap_SetInputs(u, m, t) + !call FWrap_SetInputs(u, m, t) <<< moved up into FAST.Farm FARM_UpdateStates - ! call FAST p%n_FAST_low times: - do n_ss = 1, p%n_FAST_low - n_FAST = n*p%n_FAST_low + n_ss - 1 + ! call FAST p%n_FAST_low times (p%n_FAST_low is simply the number of steps to make per wrapper call. It is affected by MooringMod) + do n_ss = 1, p%n_FAST_low + n_FAST = n*p%n_FAST_low + n_ss - 1 CALL FAST_Solution_T( t_initial, n_FAST, m%Turbine, ErrStat2, ErrMsg2 ) call setErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) @@ -464,8 +466,8 @@ SUBROUTINE FWrap_Increment( t, n, u, p, x, xd, z, OtherState, y, m, ErrStat, Err end do ! n_ss - call FWrap_CalcOutput(p, u, y, m, ErrStat2, ErrMsg2) - call setErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + !call FWrap_CalcOutput(p, u, y, m, ErrStat2, ErrMsg2) <<< moved up into FAST.Farm FARM_UpdateStates + ! call setErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) !END IF diff --git a/glue-codes/fast-farm/src/FASTWrapper_Registry.txt b/glue-codes/fast-farm/src/FASTWrapper_Registry.txt index 1416eb4774..e494a34ea7 100644 --- a/glue-codes/fast-farm/src/FASTWrapper_Registry.txt +++ b/glue-codes/fast-farm/src/FASTWrapper_Registry.txt @@ -44,6 +44,7 @@ typedef ^ InitInputType SiKi fromSC # Define outputs from the initialization routine here: #typedef ^ InitOutputType CHARACTER(ChanLen) WriteOutputHdr {:} - - "Names of the output-to-file channels" - #typedef ^ InitOutputType CHARACTER(ChanLen) WriteOutputUnt {:} - - "Units of the output-to-file channels" - +typedef ^ InitOutputType DbKi PtfmInit {6} - - "Initial platform position/rotation vector - surge,sway,heave,roll,pitch,yaw - needed for mooring module initInp" - typedef ^ InitOutputType ProgDesc Ver - - - "This module's name, version, and date" - diff --git a/glue-codes/fast-farm/src/FASTWrapper_Types.f90 b/glue-codes/fast-farm/src/FASTWrapper_Types.f90 index 7b147138ef..24ab62d1cd 100644 --- a/glue-codes/fast-farm/src/FASTWrapper_Types.f90 +++ b/glue-codes/fast-farm/src/FASTWrapper_Types.f90 @@ -84,7 +84,7 @@ MODULE FASTWrapper_Types REAL(ReKi) :: dr !< Radial increment of radial finite-difference grid [m] REAL(DbKi) :: tmax !< Simulation length [s] REAL(ReKi) , DIMENSION(1:3) :: p_ref_Turbine !< Undisplaced global coordinates of this turbine [m] - INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 1: use individual HydroDyn inputs without adjustment, 2: adjust wave phases based on turbine offsets from farm origin [-] + INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 0: use individual HydroDyn inputs without adjustment, 1: adjust wave phases based on turbine offsets from farm origin [-] INTEGER(IntKi) :: n_high_low !< Number of high-resolution time steps per low-resolution time step [-] REAL(DbKi) :: dt_high !< High-resolution time step [s] REAL(ReKi) , DIMENSION(1:3) :: p_ref_high !< Position of the origin of the high-resolution spatial domain for this turbine [m] @@ -106,6 +106,7 @@ MODULE FASTWrapper_Types ! ======================= ! ========= FWrap_InitOutputType ======= TYPE, PUBLIC :: FWrap_InitOutputType + REAL(DbKi) , DIMENSION(1:6) :: PtfmInit !< Initial platform position/rotation vector - surge,sway,heave,roll,pitch,yaw - needed for mooring module initInp [-] TYPE(ProgDesc) :: Ver !< This module's name, version, and date [-] END TYPE FWrap_InitOutputType ! ======================= @@ -554,12 +555,14 @@ SUBROUTINE FWrap_CopyInitOutput( SrcInitOutputData, DstInitOutputData, CtrlCode, CHARACTER(*), INTENT( OUT) :: ErrMsg ! Local INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'FWrap_CopyInitOutput' ! ErrStat = ErrID_None ErrMsg = "" + DstInitOutputData%PtfmInit = SrcInitOutputData%PtfmInit CALL NWTC_Library_Copyprogdesc( SrcInitOutputData%Ver, DstInitOutputData%Ver, CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) IF (ErrStat>=AbortErrLev) RETURN @@ -612,6 +615,7 @@ SUBROUTINE FWrap_PackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, Er Re_BufSz = 0 Db_BufSz = 0 Int_BufSz = 0 + Db_BufSz = Db_BufSz + SIZE(InData%PtfmInit) ! PtfmInit ! Allocate buffers for subtypes, if any (we'll get sizes from these) Int_BufSz = Int_BufSz + 3 ! Ver: size of buffers for each call to pack subtype CALL NWTC_Library_Packprogdesc( Re_Buf, Db_Buf, Int_Buf, InData%Ver, ErrStat2, ErrMsg2, .TRUE. ) ! Ver @@ -657,6 +661,10 @@ SUBROUTINE FWrap_PackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, Er Db_Xferred = 1 Int_Xferred = 1 + DO i1 = LBOUND(InData%PtfmInit,1), UBOUND(InData%PtfmInit,1) + DbKiBuf(Db_Xferred) = InData%PtfmInit(i1) + Db_Xferred = Db_Xferred + 1 + END DO CALL NWTC_Library_Packprogdesc( Re_Buf, Db_Buf, Int_Buf, InData%Ver, ErrStat2, ErrMsg2, OnlySize ) ! Ver CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -700,6 +708,7 @@ SUBROUTINE FWrap_UnPackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, INTEGER(IntKi) :: Db_Xferred INTEGER(IntKi) :: Int_Xferred INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'FWrap_UnPackInitOutput' @@ -713,6 +722,12 @@ SUBROUTINE FWrap_UnPackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Re_Xferred = 1 Db_Xferred = 1 Int_Xferred = 1 + i1_l = LBOUND(OutData%PtfmInit,1) + i1_u = UBOUND(OutData%PtfmInit,1) + DO i1 = LBOUND(OutData%PtfmInit,1), UBOUND(OutData%PtfmInit,1) + OutData%PtfmInit(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO Buf_size=IntKiBuf( Int_Xferred ) Int_Xferred = Int_Xferred + 1 IF(Buf_size > 0) THEN diff --git a/glue-codes/fast-farm/src/FAST_Farm_Registry.txt b/glue-codes/fast-farm/src/FAST_Farm_Registry.txt index 74d9ca22ab..9a2e6ef3b8 100644 --- a/glue-codes/fast-farm/src/FAST_Farm_Registry.txt +++ b/glue-codes/fast-farm/src/FAST_Farm_Registry.txt @@ -17,12 +17,13 @@ usefrom WakeDynamics_Registry.txt usefrom AWAE_Registry.txt usefrom SuperController_Registry.txt -param FAST_Farm/Farm - INTEGER NumFFModules - 4 - "The number of modules available in FAST.Farm" - +param FAST_Farm/Farm - INTEGER NumFFModules - 5 - "The number of modules available in FAST.Farm" - param ^ - INTEGER ModuleFF_None - 0 - "No module selected" - param ^ - INTEGER ModuleFF_SC - 1 - "Super Controller" - param ^ - INTEGER ModuleFF_FWrap - 2 - "FAST Wrapper" - param ^ - INTEGER ModuleFF_WD - 3 - "Wake Dynamics" - param ^ - INTEGER ModuleFF_AWAE - 4 - "Ambient Wind and Array Effects" - +param ^ - INTEGER ModuleFF_MD - 5 - "Farm-level MoorDyn" - # ..... Parameters ................................................................................................................ typedef FAST_Farm/Farm ParameterType DbKi DT_low - - - "Time step for low-resolution wind data input files; will be used as the global FAST.Farm time step" seconds typedef ^ ParameterType DbKi DT_high - - - "High-resolution time step" seconds @@ -31,9 +32,13 @@ typedef ^ ParameterType IntKi n_high_low - typedef ^ ParameterType IntKi NumTurbines - - - "Number of turbines in the simulation" - typedef ^ ParameterType CHARACTER(1024) WindFilePath - - - "Path name of wind data files from ABLSolver precursor" - typedef ^ ParameterType CHARACTER(1024) SC_FileName - - - "Name/location of the dynamic library {.dll [Windows] or .so [Linux]} containing the Super Controller algorithms" - -typedef ^ ParameterType LOGICAL UseSC - - - "Use a super controller?" - +typedef ^ ParameterType LOGICAL UseSC - - - "Use a super controller?" - typedef ^ ParameterType ReKi WT_Position {:}{:} - - "X-Y-Z position of each wind turbine; index 1 = XYZ; index 2 = turbine number" meters -typedef ^ ParameterType IntKi WaveFieldMod - - - "Wave field handling (-) (switch) 0: use individual HydroDyn inputs without adjustment, 1: adjust wave phases based on turbine offsets from farm origin" - +typedef ^ ParameterType IntKi WaveFieldMod - - - "Wave field handling (-) (switch) {0: use individual HydroDyn inputs without adjustment, 1: adjust wave phases based on turbine offsets from farm origin}" - +typedef ^ ParameterType IntKi MooringMod - - - "Mod_SharedMooring is a flag for array-level mooring. (switch) {0: none, 3: yes/MoorDyn}" - +typedef ^ ParameterType CHARACTER(1024) MD_FileName - - - "Name/location of the farm-level MoorDyn input file" - +typedef ^ ParameterType DbKi DT_mooring - - - "Time step for farm-levem mooring coupling with each turbine [used only when Mod_SharedMooring > 0]" seconds +typedef ^ ParameterType IntKi n_mooring - - - "Number of FAST and MoorDyn time steps per FAST.Farm timestep when mooring > 0" - typedef ^ ParameterType CHARACTER(1024) WT_FASTInFile {:} - - "Name of input file for each turbine" - typedef ^ ParameterType CHARACTER(1024) FTitle - - - "The description line from the primary FAST.Farm input file" - typedef ^ ParameterType CHARACTER(1024) OutFileRoot - - - "The root name derived from the primary FAST.Farm input file" - @@ -80,6 +85,10 @@ typedef ^ ^ DbKi TimeData {:} - - "Array typedef ^ ^ ReKi AllOutData {:}{:} - - "Array to contain all the output data (time history of all outputs); Index 1 is NumOuts, Index 2 is Time step" typedef ^ ^ IntKi n_Out - - - "Time index into the AllOutData array" +typedef ^ MiscVarType MeshMapType FWrap_2_MD {:} - - "Map platform kinematics from each FAST instance to MD" +typedef ^ MiscVarType MeshMapType MD_2_FWrap {:} - - "Map MD loads at the array level to each FAST instance" + + # ..... FASTWrapper data ....................................................................................................... typedef ^ FASTWrapper_Data FWrap_ContinuousStateType x - - - "Continuous states" typedef ^ ^ FWrap_DiscreteStateType xd - - - "Discrete states" @@ -123,6 +132,17 @@ typedef ^ ^ DbKi utimes {1} - typedef ^ ^ SC_OutputType y - - - "System outputs" typedef ^ ^ SC_MiscVarType m - - - "Misc/optimization variables" typedef ^ ^ logical IsInitialized - .FALSE. - "Has SC_Init been called" +# ..... MD data ....................................................................................................... +typedef ^ MD_Data MD_ContinuousStateType x - - - "Continuous states" +typedef ^ ^ MD_DiscreteStateType xd - - - "Discrete states" +typedef ^ ^ MD_ConstraintStateType z - - - "Constraint states" +typedef ^ ^ MD_OtherStateType OtherSt - - - "Other states" +typedef ^ ^ MD_ParameterType p - - - "Parameters" +typedef ^ ^ MD_InputType u {2} - - "System inputs" +typedef ^ ^ DbKi utimes {2} - - "Current time" s +typedef ^ ^ MD_OutputType y - - - "System outputs" +typedef ^ ^ MD_MiscVarType m - - - "Misc/optimization variables" +typedef ^ ^ logical IsInitialized - .FALSE. - "Has MD_Init been called" # ..... All submodules' variables................................................................................................. typedef ^ All_FastFarm_Data Farm_ParameterType p - - - "FAST.Farm parameter data" - typedef ^ All_FastFarm_Data Farm_MiscVarType m - - - "FAST.Farm misc var data" - @@ -130,5 +150,6 @@ typedef ^ All_FastFarm_Data FASTWrapper_Data FWrap {:} - - typedef ^ All_FastFarm_Data WakeDynamics_Data WD {:} - - "WakeDynamics (WD) data" - typedef ^ All_FastFarm_Data AWAE_Data AWAE - - - "Ambient Wind & Array Effects (AWAE) data" - typedef ^ All_FastFarm_Data SC_Data SC - - - "Super Controller (SC) data" - +typedef ^ All_FastFarm_Data MD_Data MD - - - "Farm-level MoorDyn model data" - # ..... FAST.Farm data ................................................................................................................ # diff --git a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 index 8b1ebd9d64..096a0275f1 100644 --- a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 +++ b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 @@ -316,6 +316,19 @@ SUBROUTINE Farm_Initialize( farm, InputFile, ErrStat, ErrMsg ) CALL Cleanup() RETURN END IF + + !............................................................................................................................... + ! step 4.5: initialize farm-level MoorDyn if applicable + !............................................................................................................................... + + if (farm%p%MooringMod == 3) then + CALL Farm_InitMD( farm, ErrStat2, ErrMsg2) ! FAST instances must be initialized first so that turbine initial positions are known + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + IF (ErrStat >= AbortErrLev) THEN + CALL Cleanup() + RETURN + END IF + end if !............................................................................................................................... ! step 5: Open output file (or set up output file handling) @@ -346,6 +359,7 @@ SUBROUTINE Farm_Initialize( farm, InputFile, ErrStat, ErrMsg ) SUBROUTINE Cleanup() call WD_DestroyInitInput(WD_InitInput, ErrStat2, ErrMsg2) + ! add something for MD? call AWAE_DestroyInitInput(AWAE_InitInput, ErrStat2, ErrMsg2) call AWAE_DestroyInitOutput(AWAE_InitOutput, ErrStat2, ErrMsg2) @@ -544,6 +558,14 @@ SUBROUTINE Farm_ReadPrimaryFile( InputFile, p, WD_InitInp, AWAE_InitInp, SC_Init RETURN end if + ! Mod_SharedMooring - flag for array-level mooring. (switch) 0: none, 3: yes/MoorDyn + CALL ReadVar( UnIn, InputFile, p%MooringMod, "Mod_SharedMooring", "Array-level mooring handling (-) (switch) {0: none; 3: array-level MoorDyn model}", ErrStat2, ErrMsg2, UnEc) + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + if ( ErrStat >= AbortErrLev ) then + call cleanup() + RETURN + end if + !---------------------- SUPER CONTROLLER ------------------------------------------------------------------ CALL ReadCom( UnIn, InputFile, 'Section Header: Super Controller', ErrStat2, ErrMsg2, UnEc ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) @@ -561,6 +583,31 @@ SUBROUTINE Farm_ReadPrimaryFile( InputFile, p, WD_InitInp, AWAE_InitInp, SC_Init end if IF ( PathIsRelative( p%SC_FileName ) ) p%SC_FileName = TRIM(PriPath)//TRIM(p%SC_FileName) SC_InitInp%DLL_FileName = p%SC_FileName + + !---------------------- SHARED MOORING SYSTEM ------------------------------------------------------------------ + CALL ReadCom( UnIn, InputFile, 'Section Header: SHARED MOORING SYSTEM', ErrStat2, ErrMsg2, UnEc ) + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + if ( ErrStat >= AbortErrLev ) then + call cleanup() + RETURN + end if + + ! MD_FileName - Name/location of the farm-level MoorDyn input file (quoated string): + CALL ReadVar( UnIn, InputFile, p%MD_FileName, "MD_FileName", "Name/location of the dynamic library {.dll [Windows] or .so [Linux]} containing the Super Controller algorithms (quoated string)", ErrStat2, ErrMsg2, UnEc) + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + if ( ErrStat >= AbortErrLev ) then + call cleanup() + RETURN + end if + IF ( PathIsRelative( p%MD_FileName ) ) p%MD_FileName = TRIM(PriPath)//TRIM(p%MD_FileName) + + ! DT_Mooring - ime step for farm-levem mooring coupling with each turbine [used only when Mod_SharedMooring > 0] (s) [>0.0]: + CALL ReadVar( UnIn, InputFile, p%DT_mooring, "DT_Mooring", "Time step for farm-levem mooring coupling with each turbine [used only when Mod_SharedMooring > 0] (s) [>0.0]", ErrStat2, ErrMsg2, UnEc) + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + if ( ErrStat >= AbortErrLev ) then + call cleanup() + RETURN + end if !---------------------- AMBIENT WIND: PRECURSOR IN VTK FORMAT --------------------------------------------- CALL ReadCom( UnIn, InputFile, 'Section Header: Ambient Wind: Precursor in VTK Format', ErrStat2, ErrMsg2, UnEc ) @@ -1551,7 +1598,8 @@ SUBROUTINE Farm_InitFAST( farm, WD_InitInp, AWAE_InitOutput, SC_InitOutput, SC_y ! local variables type(FWrap_InitInputType) :: FWrap_InitInp - type(FWrap_InitOutputType) :: FWrap_InitOut + type(FWrap_InitOutputType) :: FWrap_InitOut + REAL(DbKi) :: FWrap_Interval !< Coupling interval that FWrap is called at (affected by MooringMod) INTEGER(IntKi) :: nt ! loop counter for rotor number INTEGER(IntKi) :: ErrStat2 ! Temporary Error status @@ -1589,6 +1637,11 @@ SUBROUTINE Farm_InitFAST( farm, WD_InitInp, AWAE_InitOutput, SC_InitOutput, SC_y allocate(FWrap_InitInp%fromSC(SC_InitOutput%NumSC2Ctrl)) + if (farm%p%MooringMod > 0) then + FWrap_Interval = farm%p%dt_mooring ! when there is a farm-level mooring model, FASTWrapper will be called at the mooring coupling time step + else + FWrap_Interval = farm%p%dt_low ! otherwise FASTWrapper will be called at the regular FAST.Farm time step + end if DO nt = 1,farm%p%NumTurbines !+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ @@ -1614,7 +1667,7 @@ SUBROUTINE Farm_InitFAST( farm, WD_InitInp, AWAE_InitOutput, SC_InitOutput, SC_y end if ! note that FWrap_Init has Interval as INTENT(IN) so, we don't need to worry about overwriting farm%p%dt_low here: call FWrap_Init( FWrap_InitInp, farm%FWrap(nt)%u, farm%FWrap(nt)%p, farm%FWrap(nt)%x, farm%FWrap(nt)%xd, farm%FWrap(nt)%z, & - farm%FWrap(nt)%OtherSt, farm%FWrap(nt)%y, farm%FWrap(nt)%m, farm%p%dt_low, FWrap_InitOut, ErrStat2, ErrMsg2 ) + farm%FWrap(nt)%OtherSt, farm%FWrap(nt)%y, farm%FWrap(nt)%m, FWrap_Interval, FWrap_InitOut, ErrStat2, ErrMsg2 ) farm%FWrap(nt)%IsInitialized = .true. @@ -1637,6 +1690,248 @@ subroutine cleanup() end subroutine cleanup END SUBROUTINE Farm_InitFAST !---------------------------------------------------------------------------------------------------------------------------------- +!> This routine initializes a farm-level instance of MoorDyn if applicable +SUBROUTINE Farm_InitMD( farm, ErrStat, ErrMsg ) + + ! Passed variables + type(All_FastFarm_Data), INTENT(INOUT) :: farm !< FAST.Farm data + INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status + CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message + + ! local variables + type(MD_InitInputType) :: MD_InitInp + type(MD_InitOutputType) :: MD_InitOut + + INTEGER(IntKi) :: nt ! loop counter for rotor number + INTEGER(IntKi) :: ErrStat2 ! Temporary Error status + CHARACTER(ErrMsgLen) :: ErrMsg2 ! Temporary Error message + CHARACTER(*), PARAMETER :: RoutineName = 'Farm_InitFAST' + + + ErrStat = ErrID_None + ErrMsg = "" + + CALL WrScr(" --------- in FARM_InitMD, to initiailze farm-level MoorDyn ------- ") + + + ! sort out how many times FASt and MoorDyn will be called per FAST.Farm time step based on DT_low and DT_mooring + IF ( EqualRealNos( farm%p%dt_mooring, farm%p%DT_low ) ) THEN + farm%p%n_mooring = 1 + ELSE + IF ( farm%p%dt_mooring > farm%p%DT_low ) THEN + ErrStat = ErrID_Fatal + ErrMsg = "The mooring coupling time step ("//TRIM(Num2LStr(farm%p%dt_mooring))// & + " s) cannot be larger than FAST.Farm time step ("//TRIM(Num2LStr(farm%p%DT_low))//" s)." + ELSE + ! calculate the number of FAST-MoorDyn subcycles: + farm%p%n_mooring = NINT( farm%p%DT_low / farm%p%dt_mooring ) + + ! let's make sure the FAST DT is an exact integer divisor of the global (FAST.Farm) time step: + IF ( .NOT. EqualRealNos( farm%p%DT_low, farm%p%dt_mooring * farm%p%n_mooring ) ) THEN + ErrStat = ErrID_Fatal + ErrMsg = "The MoorDyn coupling time step, DT_mooring ("//TRIM(Num2LStr(farm%p%dt_mooring))// & + " s) must be an integer divisor of the FAST.Farm time step ("//TRIM(Num2LStr(farm%p%DT_low))//" s)." + END IF + + END IF + END IF + + + !................. + ! MoorDyn initialization inputs... + !................ + !FWrap_InitInp%tmax = farm%p%TMax + !FWrap_InitInp%n_high_low = farm%p%n_high_low + 1 ! Add 1 because the FAST wrapper uses an index that starts at 1 + !FWrap_InitInp%dt_high = farm%p%dt_high + + + MD_InitInp%FileName = farm%p%MD_FileName ! input file name and path + MD_InitInp%RootName = trim(farm%p%OutFileRoot)//'.FarmMD' ! root of output files + MD_InitInp%FarmSize = farm%p%NumTurbines ! number of turbines in the array. >0 tells MoorDyn to operate in farm mode + + ALLOCATE( MD_InitInp%PtfmInit(6,farm%p%NumTurbines), MD_InitInp%TurbineRefPos(3,farm%p%NumTurbines), STAT = ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal,"Error allocating MoorDyn PtfmInit and TurbineRefPos initialization inputs in FAST.Farm.",ErrStat,ErrMsg,RoutineName) + CALL Cleanup() + RETURN + END IF + + ! gather spatial initialization inputs for Farm-level MoorDyn + DO nt = 1,farm%p%NumTurbines + MD_InitInp%PtfmInit(:,nt) = farm%FWrap(nt)%m%Turbine%MD%m%PtfmInit ! turbine PRP initial positions and rotations in their respective coordinate systems from each FAST/MD instance + MD_InitInp%TurbineRefPos(:,nt) = farm%p%WT_Position(:,nt) ! reference positions of each turbine in the farm global coordinate system + END DO + + ! These aren't currently handled at the FAST.Farm level, so just give the farm's MoorDyn default values, which can be overwridden by its input file + MD_InitInp%g = 9.81 + MD_InitInp%rhoW = 1025.0 + MD_InitInp%WtrDepth = 0.0 + + + !! allocate MoorDyn inputs (assuming size 2 for linear interpolation/extrapolation... > + !ALLOCATE( farm%MD%Input( 2 ), farm%MD%InputTimes( 2 ), STAT = ErrStat2 ) + !IF (ErrStat2 /= 0) THEN + ! CALL SetErrStat(ErrID_Fatal,"Error allocating MD%Input and MD%InputTimes.",ErrStat,ErrMsg,RoutineName) + ! CALL Cleanup() + ! RETURN + !END IF + + ! initialize MoorDyn + CALL MD_Init( MD_InitInp, farm%MD%u(1), farm%MD%p, farm%MD%x, farm%MD%xd, farm%MD%z, & + farm%MD%OtherSt, farm%MD%y, farm%MD%m, farm%p%DT_mooring, MD_InitOut, ErrStat2, ErrMsg2 ) + + farm%MD%IsInitialized = .true. + + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + if (ErrStat >= AbortErrLev) then + call cleanup() + return + end if + + + + ! Set up mesh maps between MoorDyn and floating platforms. + ! (for now assuming ElastoDyn - eventually could differentiate at the turbine level) + + ! allocate mesh mappings for coupling farm-level MoorDyn with OpenFAST instances + ALLOCATE( farm%m%MD_2_FWrap(farm%p%NumTurbines), farm%m%FWrap_2_MD(farm%p%NumTurbines), STAT = ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal,"Error allocating MD_2_FWrap and FWrap_2_MD.",ErrStat,ErrMsg,RoutineName) + CALL Cleanup() + RETURN + END IF + + ! MoorDyn point mesh to/from ElastoDyn (or SubDyn) point mesh + do nt = 1,farm%p%NumTurbines + ! loads + CALL MeshMapCreate( farm%MD%y%PtFairleadLoad(nt), & + farm%FWrap(nt)%m%Turbine%ED%Input(1)%PlatformPtMesh, farm%m%MD_2_FWrap(nt), ErrStat2, ErrMsg2 ) + + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':MD_2_FWrap' ) + + ! kinematics + CALL MeshMapCreate( farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh, & + farm%MD%u(1)%PtFairleadDisplacement(nt), farm%m%FWrap_2_MD(nt), ErrStat2, ErrMsg2 ) + + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':FWrap_2_MD' ) + + + ! SubDyn alternative: + !CALL MeshMapCreate( farm%MD%y%PtFairleadLoad(nt), & + ! farm%FWrap(nt)%m%Turbine%SD%Input(1)%LMesh, farm%m%MD_2_FWrap, ErrStat2, ErrMsg2 ) + ! + !CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':MD_2_FWrap' ) + ! + !CALL MeshMapCreate( farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh, & + ! farm%MD%u(1)%PtFairleadDisplacement(nt), farm%m%FWrap_2_MD, ErrStat2, ErrMsg2 ) + ! + !CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':FWrap_2_MD' ) + + end do + + + farm%p%Module_Ver( ModuleFF_MD) = MD_InitOut%Ver + + call cleanup() + +contains + subroutine cleanup() + call MD_DestroyInitInput( MD_InitInp, ErrStat2, ErrMsg2 ) + call MD_DestroyInitOutput( MD_InitOut, ErrStat2, ErrMsg2 ) + end subroutine cleanup +END SUBROUTINE Farm_InitMD +!---------------------------------------------------------------------------------------------------------------------------------- +!> This routine moves a farm-level MoorDyn simulation one step forward, to catch up with FWrap_Increment +subroutine FARM_MD_Increment(t, n, farm, ErrStat, ErrMsg) + REAL(DbKi), INTENT(IN ) :: t !< Current simulation time in seconds + INTEGER(IntKi), INTENT(IN ) :: n !< Current step of the simulation in FARM MoorDyn terms + type(All_FastFarm_Data), INTENT(INOUT) :: farm !< FAST.Farm data + INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status + CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message + + INTEGER(IntKi) :: nt + INTEGER(IntKi) :: n_ss + INTEGER(IntKi) :: n_FMD + REAL(DbKi) :: t_next ! time at next step after this one (s) + TYPE(MD_InputType) :: u_next + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'FARM_MD_Increment' + + + ! ----- extrapolate MD inputs ----- + + t_next = t + farm%p%DT_mooring + + ! Do a linear extrapolation to estimate MoorDyn inputs at time n_ss+1 + CALL MD_Input_ExtrapInterp(farm%MD%u, farm%MD%uTimes, u_next, t_next, ErrStat2, ErrMsg2) + CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName ) + + ! Shift "window" of MD%Input: move values of Input and InputTimes from index 1 to index 2 + CALL MD_CopyInput (farm%MD%u(1), farm%MD%u(2), MESH_UPDATECOPY, Errstat2, ErrMsg2) + CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName ) + farm%MD%uTimes(2) = farm%MD%uTimes(1) + + ! update index 1 entries with the new extrapolated values + CALL MD_CopyInput (u_next, farm%MD%u(1), MESH_UPDATECOPY, Errstat2, ErrMsg2) + CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName ) + farm%MD%uTimes(1) = t_next + + + ! ----- map substructure kinematics to MoorDyn inputs ----- (from mapping called at start of CalcOutputs Solve INputs) + + do nt = 1,farm%p%NumTurbines + + CALL Transfer_Point_to_Point( farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh, farm%MD%u(1)%PtFairleadDisplacement(nt), & + farm%m%FWrap_2_MD(nt), ErrStat2, ErrMsg2 ) + + CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat, ErrMsg,RoutineName//'u_MD%PtFairleadDisplacement' ) + + ! SubDyn alternative + !CALL Transfer_Point_to_Point( farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh, farm%MD%u(1)%PtFairleadDisplacement(nt), farm%m%FWrap_2_MD(nt), ErrStat, ErrMsg ) + end do + + + ! ----- update states and calculate outputs ----- + + CALL MD_UpdateStates( t, n_FMD, farm%MD%u, farm%MD%utimes, farm%MD%p, farm%MD%x, & + farm%MD%xd, farm%MD%z, farm%MD%OtherSt, farm%MD%m, ErrStat2, ErrMsg2 ) + + CALL SetErrStat( Errstat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + + CALL MD_CalcOutput( t, farm%MD%u(1), farm%MD%p, farm%MD%x, farm%MD%xd, farm%MD%z, & + farm%MD%OtherSt, farm%MD%y, farm%MD%m, ErrStat2, ErrMsg2 ) + + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + + + ! ----- map MD load outputs to each turbine's substructure ----- (taken from U FullOpt1...) + do nt = 1,farm%p%NumTurbines + + ! mapping + CALL Transfer_Point_to_Point( farm%MD%y%PtFairleadLoad(nt), farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_2, & + farm%m%MD_2_FWrap(nt), ErrStat2, ErrMsg2, & + farm%MD%u(1)%PtFairleadDisplacement(nt), farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh ) !u_MD and y_ED contain the displacements needed for moment calculations + + CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) + + ! adding to ElastoDyn mesh that will be a future input (I'm assuming MeshMapData%u_ED_PlatformPtMesh makes its way into ElastoDyn inputs somehow) + farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh%Force = farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh%Force & + + farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_2%Force + + farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh%Moment = farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh%Moment & + + farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_2%Moment + + ! SubDyn alternative + !CALL Transfer_Point_to_Point( farm%MD%y%PtFairleadLoad(nt), farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2, & + ! farm%m%MD_2_FWrap(nt), ErrStat2, ErrMsg2, & + ! farm%MD%u(1)%PtFairleadDisplacement(nt), farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh ) !u_MD and y_SD contain the displacements needed for moment calculations + ! + !farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Force = farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Force + farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2%Force + !farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Moment = farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Moment + farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2%Moment + end do +end subroutine Farm_MD_Increment +!---------------------------------------------------------------------------------------------------------------------------------- !> This routine performs the initial call to calculate outputs (at t=0). !! The Initial Calculate Output algorithm: \n !! - In parallel: @@ -1808,20 +2103,26 @@ subroutine FARM_UpdateStates(t, n, farm, ErrStat, ErrMsg) INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message - INTEGER(IntKi) :: nt - INTEGER(IntKi) :: ErrStatWD, ErrStat2 - INTEGER(IntKi), ALLOCATABLE :: ErrStatF(:) ! Temporary Error status + INTEGER(IntKi) :: nt + INTEGER(IntKi) :: n_ss + INTEGER(IntKi) :: n_FMD + REAL(DbKi) :: t2 ! time within the FAST-MoorDyn substepping loop for shared moorings + INTEGER(IntKi) :: ErrStatWD, ErrStatAWAE, ErrStatMD, ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(ErrMsgLen) :: ErrMsgWD - CHARACTER(ErrMsgLen), ALLOCATABLE :: ErrMsgF (:) ! Temporary Error message + CHARACTER(ErrMsgLen) :: ErrMsgAWAE + CHARACTER(ErrMsgLen) :: ErrMsgMD + INTEGER(IntKi), ALLOCATABLE :: ErrStatF(:) ! Temporary Error status for FAST + CHARACTER(ErrMsgLen), ALLOCATABLE :: ErrMsgF (:) ! Temporary Error message for FAST CHARACTER(*), PARAMETER :: RoutineName = 'FARM_UpdateStates' ! REAL(DbKi) :: tm1,tm2,tm3 ErrStat = ErrID_None ErrMsg = "" - allocate ( ErrStatF ( farm%p%NumTurbines + 1 ), STAT=errStat2 ) + allocate ( ErrStatF ( farm%p%NumTurbines+1 ), STAT=errStat2 ) if (errStat2 /= 0) call SetErrStat ( ErrID_Fatal, 'Could not allocate memory for ErrStatF.', errStat, errMsg, RoutineName ) - allocate ( ErrMsgF ( farm%p%NumTurbines + 1 ), STAT=errStat2 ) + allocate ( ErrMsgF ( farm%p%NumTurbines+1 ), STAT=errStat2 ) if (errStat2 /= 0) call SetErrStat ( ErrID_Fatal, 'Could not allocate memory for ErrMsgF.', errStat, errMsg, RoutineName ) if (ErrStat >= AbortErrLev) return @@ -1851,57 +2152,117 @@ subroutine FARM_UpdateStates(t, n, farm, ErrStat, ErrMsg) if (errStat >= AbortErrLev) return end if + !-------------------- - ! 3. CALL F_Increment and 4. CALL AWAE_UpdateStates -!#ifdef _OPENMP -! tm1 = omp_get_wtime() -!#endif - !$OMP PARALLEL DO DEFAULT(Shared) Private(nt) !Private(nt,tm2,tm3) - DO nt = 1,farm%p%NumTurbines+1 - if(nt.ne.farm%p%NumTurbines+1) then -!#ifdef _OPENMP -! tm3 = omp_get_wtime() -!#endif - call FWrap_Increment( t, n, farm%FWrap(nt)%u, farm%FWrap(nt)%p, farm%FWrap(nt)%x, farm%FWrap(nt)%xd, farm%FWrap(nt)%z, & - farm%FWrap(nt)%OtherSt, farm%FWrap(nt)%y, farm%FWrap(nt)%m, ErrStatF(nt), ErrMsgF(nt) ) + ! 3. CALL F_Increment (and FARM_MD_Increment) and 4. CALL AWAE_UpdateStates + + + ! set the inputs needed for FAST (these are slow-varying so can just be done once per farm time step) + do nt = 1,farm%p%NumTurbines + call FWrap_SetInputs(farm%FWrap(nt)%u, farm%FWrap(nt)%m, t) + end do + + + ! Original case: no shared moorings + if (farm%p%MooringMod == 0) then + + !#ifdef _OPENMP + ! tm1 = omp_get_wtime() + !#endif + !$OMP PARALLEL DO DEFAULT(Shared) Private(nt) !Private(nt,tm2,tm3) + DO nt = 1,farm%p%NumTurbines+1 + if(nt.ne.farm%p%NumTurbines+1) then + !#ifdef _OPENMP + ! tm3 = omp_get_wtime() + !#endif + call FWrap_Increment( t, n, farm%FWrap(nt)%u, farm%FWrap(nt)%p, farm%FWrap(nt)%x, farm%FWrap(nt)%xd, farm%FWrap(nt)%z, & + farm%FWrap(nt)%OtherSt, farm%FWrap(nt)%y, farm%FWrap(nt)%m, ErrStatF(nt), ErrMsgF(nt) ) + + !#ifdef _OPENMP + ! tm2 = omp_get_wtime() + ! write(*,*) ' FWrap_Increment for turbine #'//trim(num2lstr(nt))//' using thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' + !#endif + + else + !#ifdef _OPENMP + ! tm3 = omp_get_wtime() + !#endif + call AWAE_UpdateStates( t, n, farm%AWAE%u, farm%AWAE%p, farm%AWAE%x, farm%AWAE%xd, farm%AWAE%z, & + farm%AWAE%OtherSt, farm%AWAE%m, errStatF(nt), errMsgF(nt) ) + + !#ifdef _OPENMP + ! tm2 = omp_get_wtime() + ! write(*,*) ' AWAE_UpdateStates using thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' + !#endif + endif -!#ifdef _OPENMP -! tm2 = omp_get_wtime() -! write(*,*) ' FWrap_Increment for turbine #'//trim(num2lstr(nt))//' using thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' -!#endif + END DO + !$OMP END PARALLEL DO - else -!#ifdef _OPENMP -! tm3 = omp_get_wtime() -!#endif - call AWAE_UpdateStates( t, n, farm%AWAE%u, farm%AWAE%p, farm%AWAE%x, farm%AWAE%xd, farm%AWAE%z, & - farm%AWAE%OtherSt, farm%AWAE%m, errStatF(nt), errMsgF(nt) ) - -!#ifdef _OPENMP -! tm2 = omp_get_wtime() -! write(*,*) ' AWAE_UpdateStates using thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' -!#endif - endif + !#ifdef _OPENMP + ! tm2 = omp_get_wtime() + ! write(*,*) 'Total Farm_US took '//trim(num2lstr(tm2-tm1))//' seconds.' + !#endif + + + ! Farm-level moorings case using MoorDyn + else if (farm%p%MooringMod == 3) then - END DO - !$OMP END PARALLEL DO + ! Set up two parallel sections - one for FAST-MoorDyn steps (FAST portion in parallel for each step), and the other for AWAE. + !$OMP PARALLEL SECTIONS DEFAULT(Shared) + + ! The first section, for looping through FAST and farm-level MoorDyn time steps + !$OMP SECTION + + ! This is the FAST-MoorDyn farm-level substepping loop + do n_ss = 1, farm%p%n_mooring ! do n_mooring substeps (number of FAST/FarmMD steps per Farm time step) + + n_FMD = n*farm%p%n_mooring + n_ss - 1 ! number of the current time step of the call to FAST and MoorDyn + t2 = t + farm%p%DT_mooring*(n_ss - 1) ! current time in the loop + ! A nested parallel for loop to call each instance of OpenFAST in parallel + !$OMP PARALLEL DO DEFAULT(Shared) Private(nt) + DO nt = 1,farm%p%NumTurbines+1 + call FWrap_Increment( t2, n_FMD, farm%FWrap(nt)%u, farm%FWrap(nt)%p, farm%FWrap(nt)%x, farm%FWrap(nt)%xd, farm%FWrap(nt)%z, & + farm%FWrap(nt)%OtherSt, farm%FWrap(nt)%y, farm%FWrap(nt)%m, ErrStatF(nt), ErrMsgF(nt) ) + END DO + !$OMP END PARALLEL DO + + ! call farm-level MoorDyn time step here (can't multithread this with FAST since it needs inputs from all FAST instances) + call Farm_MD_Increment( t2, n_FMD, farm, ErrStatMD, ErrMsgMD) + call SetErrStat(ErrStatMD, ErrMsgMD, ErrStat, ErrMsg, 'FARM_UpdateStates') ! MD error status + + end do ! n_ss substepping + + ! The second section, for updating AWAE states on a separate thread in parallel with the FAST/MoorDyn time stepping + !$OMP SECTION + call AWAE_UpdateStates( t, n, farm%AWAE%u, farm%AWAE%p, farm%AWAE%x, farm%AWAE%xd, farm%AWAE%z, & + farm%AWAE%OtherSt, farm%AWAE%m, errStatF(nt), errMsgF(nt) ) + + !$OMP END PARALLEL SECTIONS + + else + CALL SetErrStat( ErrID_Fatal, 'MooringMod must be 0 or 3.', ErrStat, ErrMsg, RoutineName ) + end if + + + ! calculate outputs from FAST as needed by FAST.Farm + do nt = 1,farm%p%NumTurbines + call FWrap_CalcOutput(farm%FWrap(nt)%p, farm%FWrap(nt)%u, farm%FWrap(nt)%y, farm%FWrap(nt)%m, ErrStat2, ErrMsg2) + call setErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + end do + + ! update error messages for FAST and AWAE DO nt = 1,farm%p%NumTurbines - call SetErrStat(ErrStatF(nt), ErrMsgF(nt), ErrStat, ErrMsg, 'T'//trim(num2lstr(nt))//':FARM_UpdateStates') + call SetErrStat(ErrStatF(nt), ErrMsgF(nt), ErrStat, ErrMsg, 'T'//trim(num2lstr(nt))//':FARM_UpdateStates') ! FAST error status END DO - - call SetErrStat(ErrStatF(farm%p%NumTurbines+1), ErrMsgF(farm%p%NumTurbines+1), ErrStat, ErrMsg, 'FARM_UpdateStates') + call SetErrStat(ErrStatAWAE, ErrMsgAWAE, ErrStat, ErrMsg, 'FARM_UpdateStates') ! AWAE error status if (ErrStat >= AbortErrLev) return -!#ifdef _OPENMP -! tm2 = omp_get_wtime() -! write(*,*) 'Total Farm_US took '//trim(num2lstr(tm2-tm1))//' seconds.' -!#endif - end subroutine FARM_UpdateStates - +!---------------------------------------------------------------------------------------------------------------------------------- subroutine Farm_WriteOutput(n, t, farm, ErrStat, ErrMsg) INTEGER(IntKi), INTENT(IN ) :: n !< Time step increment number REAL(DbKi), INTENT(IN ) :: t !< Current simulation time in seconds diff --git a/glue-codes/fast-farm/src/FAST_Farm_Types.f90 b/glue-codes/fast-farm/src/FAST_Farm_Types.f90 index 667d0c2c3b..99d5e65512 100644 --- a/glue-codes/fast-farm/src/FAST_Farm_Types.f90 +++ b/glue-codes/fast-farm/src/FAST_Farm_Types.f90 @@ -81,12 +81,13 @@ MODULE FAST_Farm_Types USE SuperController_Types USE NWTC_Library IMPLICIT NONE - INTEGER(IntKi), PUBLIC, PARAMETER :: NumFFModules = 4 ! The number of modules available in FAST.Farm [-] + INTEGER(IntKi), PUBLIC, PARAMETER :: NumFFModules = 5 ! The number of modules available in FAST.Farm [-] INTEGER(IntKi), PUBLIC, PARAMETER :: ModuleFF_None = 0 ! No module selected [-] INTEGER(IntKi), PUBLIC, PARAMETER :: ModuleFF_SC = 1 ! Super Controller [-] INTEGER(IntKi), PUBLIC, PARAMETER :: ModuleFF_FWrap = 2 ! FAST Wrapper [-] INTEGER(IntKi), PUBLIC, PARAMETER :: ModuleFF_WD = 3 ! Wake Dynamics [-] INTEGER(IntKi), PUBLIC, PARAMETER :: ModuleFF_AWAE = 4 ! Ambient Wind and Array Effects [-] + INTEGER(IntKi), PUBLIC, PARAMETER :: ModuleFF_MD = 5 ! Farm-level MoorDyn [-] ! ========= Farm_ParameterType ======= TYPE, PUBLIC :: Farm_ParameterType REAL(DbKi) :: DT_low !< Time step for low-resolution wind data input files; will be used as the global FAST.Farm time step [seconds] @@ -98,7 +99,11 @@ MODULE FAST_Farm_Types CHARACTER(1024) :: SC_FileName !< Name/location of the dynamic library {.dll [Windows] or .so [Linux]} containing the Super Controller algorithms [-] LOGICAL :: UseSC !< Use a super controller? [-] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: WT_Position !< X-Y-Z position of each wind turbine; index 1 = XYZ; index 2 = turbine number [meters] - INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 1: use individual HydroDyn inputs without adjustment, 2: adjust wave phases based on turbine offsets from farm origin [-] + INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) {0: use individual HydroDyn inputs without adjustment, 1: adjust wave phases based on turbine offsets from farm origin} [-] + INTEGER(IntKi) :: MooringMod !< Mod_SharedMooring is a flag for array-level mooring. (switch) {0: none, 3: yes/MoorDyn} [-] + CHARACTER(1024) :: MD_FileName !< Name/location of the farm-level MoorDyn input file [-] + REAL(DbKi) :: DT_mooring !< Time step for farm-levem mooring coupling with each turbine [used only when Mod_SharedMooring > 0] [seconds] + INTEGER(IntKi) :: n_mooring !< Number of FAST and MoorDyn time steps per FAST.Farm timestep when mooring > 0 [-] CHARACTER(1024) , DIMENSION(:), ALLOCATABLE :: WT_FASTInFile !< Name of input file for each turbine [-] CHARACTER(1024) :: FTitle !< The description line from the primary FAST.Farm input file [-] CHARACTER(1024) :: OutFileRoot !< The root name derived from the primary FAST.Farm input file [-] @@ -145,6 +150,8 @@ MODULE FAST_Farm_Types REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: TimeData !< Array to contain the time output data for the binary file (first output time and a time [fixed] increment) [-] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: AllOutData !< Array to contain all the output data (time history of all outputs); Index 1 is NumOuts, Index 2 is Time step [-] INTEGER(IntKi) :: n_Out !< Time index into the AllOutData array [-] + TYPE(MeshMapType) , DIMENSION(:), ALLOCATABLE :: FWrap_2_MD !< Map platform kinematics from each FAST instance to MD [-] + TYPE(MeshMapType) , DIMENSION(:), ALLOCATABLE :: MD_2_FWrap !< Map MD loads at the array level to each FAST instance [-] END TYPE Farm_MiscVarType ! ======================= ! ========= FASTWrapper_Data ======= @@ -200,6 +207,20 @@ MODULE FAST_Farm_Types LOGICAL :: IsInitialized = .FALSE. !< Has SC_Init been called [-] END TYPE SC_Data ! ======================= +! ========= MD_Data ======= + TYPE, PUBLIC :: MD_Data + TYPE(MD_ContinuousStateType) :: x !< Continuous states [-] + TYPE(MD_DiscreteStateType) :: xd !< Discrete states [-] + TYPE(MD_ConstraintStateType) :: z !< Constraint states [-] + TYPE(MD_OtherStateType) :: OtherSt !< Other states [-] + TYPE(MD_ParameterType) :: p !< Parameters [-] + TYPE(MD_InputType) , DIMENSION(1:2) :: u !< System inputs [-] + REAL(DbKi) , DIMENSION(1:2) :: utimes !< Current time [s] + TYPE(MD_OutputType) :: y !< System outputs [-] + TYPE(MD_MiscVarType) :: m !< Misc/optimization variables [-] + LOGICAL :: IsInitialized = .FALSE. !< Has MD_Init been called [-] + END TYPE MD_Data +! ======================= ! ========= All_FastFarm_Data ======= TYPE, PUBLIC :: All_FastFarm_Data TYPE(Farm_ParameterType) :: p !< FAST.Farm parameter data [-] @@ -208,6 +229,7 @@ MODULE FAST_Farm_Types TYPE(WakeDynamics_Data) , DIMENSION(:), ALLOCATABLE :: WD !< WakeDynamics (WD) data [-] TYPE(AWAE_Data) :: AWAE !< Ambient Wind & Array Effects (AWAE) data [-] TYPE(SC_Data) :: SC !< Super Controller (SC) data [-] + TYPE(MD_Data) :: MD !< Farm-level MoorDyn model data [-] END TYPE All_FastFarm_Data ! ======================= CONTAINS @@ -250,6 +272,10 @@ SUBROUTINE Farm_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg DstParamData%WT_Position = SrcParamData%WT_Position ENDIF DstParamData%WaveFieldMod = SrcParamData%WaveFieldMod + DstParamData%MooringMod = SrcParamData%MooringMod + DstParamData%MD_FileName = SrcParamData%MD_FileName + DstParamData%DT_mooring = SrcParamData%DT_mooring + DstParamData%n_mooring = SrcParamData%n_mooring IF (ALLOCATED(SrcParamData%WT_FASTInFile)) THEN i1_l = LBOUND(SrcParamData%WT_FASTInFile,1) i1_u = UBOUND(SrcParamData%WT_FASTInFile,1) @@ -465,6 +491,10 @@ SUBROUTINE Farm_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Re_BufSz = Re_BufSz + SIZE(InData%WT_Position) ! WT_Position END IF Int_BufSz = Int_BufSz + 1 ! WaveFieldMod + Int_BufSz = Int_BufSz + 1 ! MooringMod + Int_BufSz = Int_BufSz + 1*LEN(InData%MD_FileName) ! MD_FileName + Db_BufSz = Db_BufSz + 1 ! DT_mooring + Int_BufSz = Int_BufSz + 1 ! n_mooring Int_BufSz = Int_BufSz + 1 ! WT_FASTInFile allocated yes/no IF ( ALLOCATED(InData%WT_FASTInFile) ) THEN Int_BufSz = Int_BufSz + 2*1 ! WT_FASTInFile upper/lower bounds for each dimension @@ -637,6 +667,16 @@ SUBROUTINE Farm_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, END IF IntKiBuf(Int_Xferred) = InData%WaveFieldMod Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%MooringMod + Int_Xferred = Int_Xferred + 1 + DO I = 1, LEN(InData%MD_FileName) + IntKiBuf(Int_Xferred) = ICHAR(InData%MD_FileName(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + DbKiBuf(Db_Xferred) = InData%DT_mooring + Db_Xferred = Db_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%n_mooring + Int_Xferred = Int_Xferred + 1 IF ( .NOT. ALLOCATED(InData%WT_FASTInFile) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -949,6 +989,16 @@ SUBROUTINE Farm_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMs END IF OutData%WaveFieldMod = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 + OutData%MooringMod = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + DO I = 1, LEN(OutData%MD_FileName) + OutData%MD_FileName(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + OutData%DT_mooring = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%n_mooring = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WT_FASTInFile not allocated Int_Xferred = Int_Xferred + 1 ELSE @@ -1238,7 +1288,7 @@ SUBROUTINE Farm_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMs END SUBROUTINE Farm_UnPackParam SUBROUTINE Farm_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) - TYPE(Farm_MiscVarType), INTENT(IN) :: SrcMiscData + TYPE(Farm_MiscVarType), INTENT(INOUT) :: SrcMiscData TYPE(Farm_MiscVarType), INTENT(INOUT) :: DstMiscData INTEGER(IntKi), INTENT(IN ) :: CtrlCode INTEGER(IntKi), INTENT( OUT) :: ErrStat @@ -1292,6 +1342,38 @@ SUBROUTINE Farm_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) DstMiscData%AllOutData = SrcMiscData%AllOutData ENDIF DstMiscData%n_Out = SrcMiscData%n_Out +IF (ALLOCATED(SrcMiscData%FWrap_2_MD)) THEN + i1_l = LBOUND(SrcMiscData%FWrap_2_MD,1) + i1_u = UBOUND(SrcMiscData%FWrap_2_MD,1) + IF (.NOT. ALLOCATED(DstMiscData%FWrap_2_MD)) THEN + ALLOCATE(DstMiscData%FWrap_2_MD(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%FWrap_2_MD.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcMiscData%FWrap_2_MD,1), UBOUND(SrcMiscData%FWrap_2_MD,1) + CALL NWTC_Library_Copymeshmaptype( SrcMiscData%FWrap_2_MD(i1), DstMiscData%FWrap_2_MD(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF +IF (ALLOCATED(SrcMiscData%MD_2_FWrap)) THEN + i1_l = LBOUND(SrcMiscData%MD_2_FWrap,1) + i1_u = UBOUND(SrcMiscData%MD_2_FWrap,1) + IF (.NOT. ALLOCATED(DstMiscData%MD_2_FWrap)) THEN + ALLOCATE(DstMiscData%MD_2_FWrap(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%MD_2_FWrap.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcMiscData%MD_2_FWrap,1), UBOUND(SrcMiscData%MD_2_FWrap,1) + CALL NWTC_Library_Copymeshmaptype( SrcMiscData%MD_2_FWrap(i1), DstMiscData%MD_2_FWrap(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF END SUBROUTINE Farm_CopyMisc SUBROUTINE Farm_DestroyMisc( MiscData, ErrStat, ErrMsg ) @@ -1311,6 +1393,18 @@ SUBROUTINE Farm_DestroyMisc( MiscData, ErrStat, ErrMsg ) ENDIF IF (ALLOCATED(MiscData%AllOutData)) THEN DEALLOCATE(MiscData%AllOutData) +ENDIF +IF (ALLOCATED(MiscData%FWrap_2_MD)) THEN +DO i1 = LBOUND(MiscData%FWrap_2_MD,1), UBOUND(MiscData%FWrap_2_MD,1) + CALL NWTC_Library_Destroymeshmaptype( MiscData%FWrap_2_MD(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(MiscData%FWrap_2_MD) +ENDIF +IF (ALLOCATED(MiscData%MD_2_FWrap)) THEN +DO i1 = LBOUND(MiscData%MD_2_FWrap,1), UBOUND(MiscData%MD_2_FWrap,1) + CALL NWTC_Library_Destroymeshmaptype( MiscData%MD_2_FWrap(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(MiscData%MD_2_FWrap) ENDIF END SUBROUTINE Farm_DestroyMisc @@ -1365,6 +1459,53 @@ SUBROUTINE Farm_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, S Re_BufSz = Re_BufSz + SIZE(InData%AllOutData) ! AllOutData END IF Int_BufSz = Int_BufSz + 1 ! n_Out + Int_BufSz = Int_BufSz + 1 ! FWrap_2_MD allocated yes/no + IF ( ALLOCATED(InData%FWrap_2_MD) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! FWrap_2_MD upper/lower bounds for each dimension + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + DO i1 = LBOUND(InData%FWrap_2_MD,1), UBOUND(InData%FWrap_2_MD,1) + Int_BufSz = Int_BufSz + 3 ! FWrap_2_MD: size of buffers for each call to pack subtype + CALL NWTC_Library_Packmeshmaptype( Re_Buf, Db_Buf, Int_Buf, InData%FWrap_2_MD(i1), ErrStat2, ErrMsg2, .TRUE. ) ! FWrap_2_MD + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! FWrap_2_MD + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! FWrap_2_MD + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! FWrap_2_MD + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 1 ! MD_2_FWrap allocated yes/no + IF ( ALLOCATED(InData%MD_2_FWrap) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! MD_2_FWrap upper/lower bounds for each dimension + DO i1 = LBOUND(InData%MD_2_FWrap,1), UBOUND(InData%MD_2_FWrap,1) + Int_BufSz = Int_BufSz + 3 ! MD_2_FWrap: size of buffers for each call to pack subtype + CALL NWTC_Library_Packmeshmaptype( Re_Buf, Db_Buf, Int_Buf, InData%MD_2_FWrap(i1), ErrStat2, ErrMsg2, .TRUE. ) ! MD_2_FWrap + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! MD_2_FWrap + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! MD_2_FWrap + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! MD_2_FWrap + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) IF (ErrStat2 /= 0) THEN @@ -1444,6 +1585,88 @@ SUBROUTINE Farm_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, S END IF IntKiBuf(Int_Xferred) = InData%n_Out Int_Xferred = Int_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%FWrap_2_MD) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%FWrap_2_MD,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%FWrap_2_MD,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%FWrap_2_MD,1), UBOUND(InData%FWrap_2_MD,1) + CALL NWTC_Library_Packmeshmaptype( Re_Buf, Db_Buf, Int_Buf, InData%FWrap_2_MD(i1), ErrStat2, ErrMsg2, OnlySize ) ! FWrap_2_MD + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF + IF ( .NOT. ALLOCATED(InData%MD_2_FWrap) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%MD_2_FWrap,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%MD_2_FWrap,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%MD_2_FWrap,1), UBOUND(InData%MD_2_FWrap,1) + CALL NWTC_Library_Packmeshmaptype( Re_Buf, Db_Buf, Int_Buf, InData%MD_2_FWrap(i1), ErrStat2, ErrMsg2, OnlySize ) ! MD_2_FWrap + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF END SUBROUTINE Farm_PackMisc SUBROUTINE Farm_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) @@ -1535,6 +1758,118 @@ SUBROUTINE Farm_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg END IF OutData%n_Out = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! FWrap_2_MD not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%FWrap_2_MD)) DEALLOCATE(OutData%FWrap_2_MD) + ALLOCATE(OutData%FWrap_2_MD(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%FWrap_2_MD.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%FWrap_2_MD,1), UBOUND(OutData%FWrap_2_MD,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL NWTC_Library_Unpackmeshmaptype( Re_Buf, Db_Buf, Int_Buf, OutData%FWrap_2_MD(i1), ErrStat2, ErrMsg2 ) ! FWrap_2_MD + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! MD_2_FWrap not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%MD_2_FWrap)) DEALLOCATE(OutData%MD_2_FWrap) + ALLOCATE(OutData%MD_2_FWrap(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%MD_2_FWrap.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%MD_2_FWrap,1), UBOUND(OutData%MD_2_FWrap,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL NWTC_Library_Unpackmeshmaptype( Re_Buf, Db_Buf, Int_Buf, OutData%MD_2_FWrap(i1), ErrStat2, ErrMsg2 ) ! MD_2_FWrap + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF END SUBROUTINE Farm_UnPackMisc SUBROUTINE Farm_CopyFASTWrapper_Data( SrcFASTWrapper_DataData, DstFASTWrapper_DataData, CtrlCode, ErrStat, ErrMsg ) @@ -4903,9 +5238,9 @@ SUBROUTINE Farm_UnPackSC_Data( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Err Int_Xferred = Int_Xferred + 1 END SUBROUTINE Farm_UnPackSC_Data - SUBROUTINE Farm_CopyAll_FastFarm_Data( SrcAll_FastFarm_DataData, DstAll_FastFarm_DataData, CtrlCode, ErrStat, ErrMsg ) - TYPE(All_FastFarm_Data), INTENT(INOUT) :: SrcAll_FastFarm_DataData - TYPE(All_FastFarm_Data), INTENT(INOUT) :: DstAll_FastFarm_DataData + SUBROUTINE Farm_CopyMD_Data( SrcMD_DataData, DstMD_DataData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_Data), INTENT(INOUT) :: SrcMD_DataData + TYPE(MD_Data), INTENT(INOUT) :: DstMD_DataData INTEGER(IntKi), INTENT(IN ) :: CtrlCode INTEGER(IntKi), INTENT( OUT) :: ErrStat CHARACTER(*), INTENT( OUT) :: ErrMsg @@ -4914,28 +5249,892 @@ SUBROUTINE Farm_CopyAll_FastFarm_Data( SrcAll_FastFarm_DataData, DstAll_FastFarm INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 - CHARACTER(*), PARAMETER :: RoutineName = 'Farm_CopyAll_FastFarm_Data' + CHARACTER(*), PARAMETER :: RoutineName = 'Farm_CopyMD_Data' ! ErrStat = ErrID_None ErrMsg = "" - CALL Farm_CopyParam( SrcAll_FastFarm_DataData%p, DstAll_FastFarm_DataData%p, CtrlCode, ErrStat2, ErrMsg2 ) + CALL MD_CopyContState( SrcMD_DataData%x, DstMD_DataData%x, CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) IF (ErrStat>=AbortErrLev) RETURN - CALL Farm_CopyMisc( SrcAll_FastFarm_DataData%m, DstAll_FastFarm_DataData%m, CtrlCode, ErrStat2, ErrMsg2 ) + CALL MD_CopyDiscState( SrcMD_DataData%xd, DstMD_DataData%xd, CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) IF (ErrStat>=AbortErrLev) RETURN -IF (ALLOCATED(SrcAll_FastFarm_DataData%FWrap)) THEN - i1_l = LBOUND(SrcAll_FastFarm_DataData%FWrap,1) - i1_u = UBOUND(SrcAll_FastFarm_DataData%FWrap,1) - IF (.NOT. ALLOCATED(DstAll_FastFarm_DataData%FWrap)) THEN - ALLOCATE(DstAll_FastFarm_DataData%FWrap(i1_l:i1_u),STAT=ErrStat2) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstAll_FastFarm_DataData%FWrap.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF - END IF - DO i1 = LBOUND(SrcAll_FastFarm_DataData%FWrap,1), UBOUND(SrcAll_FastFarm_DataData%FWrap,1) - CALL Farm_Copyfastwrapper_data( SrcAll_FastFarm_DataData%FWrap(i1), DstAll_FastFarm_DataData%FWrap(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL MD_CopyConstrState( SrcMD_DataData%z, DstMD_DataData%z, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + CALL MD_CopyOtherState( SrcMD_DataData%OtherSt, DstMD_DataData%OtherSt, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + CALL MD_CopyParam( SrcMD_DataData%p, DstMD_DataData%p, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + DO i1 = LBOUND(SrcMD_DataData%u,1), UBOUND(SrcMD_DataData%u,1) + CALL MD_CopyInput( SrcMD_DataData%u(i1), DstMD_DataData%u(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO + DstMD_DataData%utimes = SrcMD_DataData%utimes + CALL MD_CopyOutput( SrcMD_DataData%y, DstMD_DataData%y, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + CALL MD_CopyMisc( SrcMD_DataData%m, DstMD_DataData%m, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + DstMD_DataData%IsInitialized = SrcMD_DataData%IsInitialized + END SUBROUTINE Farm_CopyMD_Data + + SUBROUTINE Farm_DestroyMD_Data( MD_DataData, ErrStat, ErrMsg ) + TYPE(MD_Data), INTENT(INOUT) :: MD_DataData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'Farm_DestroyMD_Data' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + CALL MD_DestroyContState( MD_DataData%x, ErrStat, ErrMsg ) + CALL MD_DestroyDiscState( MD_DataData%xd, ErrStat, ErrMsg ) + CALL MD_DestroyConstrState( MD_DataData%z, ErrStat, ErrMsg ) + CALL MD_DestroyOtherState( MD_DataData%OtherSt, ErrStat, ErrMsg ) + CALL MD_DestroyParam( MD_DataData%p, ErrStat, ErrMsg ) +DO i1 = LBOUND(MD_DataData%u,1), UBOUND(MD_DataData%u,1) + CALL MD_DestroyInput( MD_DataData%u(i1), ErrStat, ErrMsg ) +ENDDO + CALL MD_DestroyOutput( MD_DataData%y, ErrStat, ErrMsg ) + CALL MD_DestroyMisc( MD_DataData%m, ErrStat, ErrMsg ) + END SUBROUTINE Farm_DestroyMD_Data + + SUBROUTINE Farm_PackMD_Data( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(MD_Data), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'Farm_PackMD_Data' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + Int_BufSz = Int_BufSz + 3 ! x: size of buffers for each call to pack subtype + CALL MD_PackContState( Re_Buf, Db_Buf, Int_Buf, InData%x, ErrStat2, ErrMsg2, .TRUE. ) ! x + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! x + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! x + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! x + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + Int_BufSz = Int_BufSz + 3 ! xd: size of buffers for each call to pack subtype + CALL MD_PackDiscState( Re_Buf, Db_Buf, Int_Buf, InData%xd, ErrStat2, ErrMsg2, .TRUE. ) ! xd + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! xd + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! xd + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! xd + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + Int_BufSz = Int_BufSz + 3 ! z: size of buffers for each call to pack subtype + CALL MD_PackConstrState( Re_Buf, Db_Buf, Int_Buf, InData%z, ErrStat2, ErrMsg2, .TRUE. ) ! z + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! z + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! z + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! z + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + Int_BufSz = Int_BufSz + 3 ! OtherSt: size of buffers for each call to pack subtype + CALL MD_PackOtherState( Re_Buf, Db_Buf, Int_Buf, InData%OtherSt, ErrStat2, ErrMsg2, .TRUE. ) ! OtherSt + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! OtherSt + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! OtherSt + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! OtherSt + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + Int_BufSz = Int_BufSz + 3 ! p: size of buffers for each call to pack subtype + CALL MD_PackParam( Re_Buf, Db_Buf, Int_Buf, InData%p, ErrStat2, ErrMsg2, .TRUE. ) ! p + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! p + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! p + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! p + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + DO i1 = LBOUND(InData%u,1), UBOUND(InData%u,1) + Int_BufSz = Int_BufSz + 3 ! u: size of buffers for each call to pack subtype + CALL MD_PackInput( Re_Buf, Db_Buf, Int_Buf, InData%u(i1), ErrStat2, ErrMsg2, .TRUE. ) ! u + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! u + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! u + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! u + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + Db_BufSz = Db_BufSz + SIZE(InData%utimes) ! utimes + Int_BufSz = Int_BufSz + 3 ! y: size of buffers for each call to pack subtype + CALL MD_PackOutput( Re_Buf, Db_Buf, Int_Buf, InData%y, ErrStat2, ErrMsg2, .TRUE. ) ! y + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! y + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! y + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! y + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + Int_BufSz = Int_BufSz + 3 ! m: size of buffers for each call to pack subtype + CALL MD_PackMisc( Re_Buf, Db_Buf, Int_Buf, InData%m, ErrStat2, ErrMsg2, .TRUE. ) ! m + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! m + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! m + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! m + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + Int_BufSz = Int_BufSz + 1 ! IsInitialized + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + CALL MD_PackContState( Re_Buf, Db_Buf, Int_Buf, InData%x, ErrStat2, ErrMsg2, OnlySize ) ! x + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + CALL MD_PackDiscState( Re_Buf, Db_Buf, Int_Buf, InData%xd, ErrStat2, ErrMsg2, OnlySize ) ! xd + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + CALL MD_PackConstrState( Re_Buf, Db_Buf, Int_Buf, InData%z, ErrStat2, ErrMsg2, OnlySize ) ! z + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + CALL MD_PackOtherState( Re_Buf, Db_Buf, Int_Buf, InData%OtherSt, ErrStat2, ErrMsg2, OnlySize ) ! OtherSt + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + CALL MD_PackParam( Re_Buf, Db_Buf, Int_Buf, InData%p, ErrStat2, ErrMsg2, OnlySize ) ! p + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + DO i1 = LBOUND(InData%u,1), UBOUND(InData%u,1) + CALL MD_PackInput( Re_Buf, Db_Buf, Int_Buf, InData%u(i1), ErrStat2, ErrMsg2, OnlySize ) ! u + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + DO i1 = LBOUND(InData%utimes,1), UBOUND(InData%utimes,1) + DbKiBuf(Db_Xferred) = InData%utimes(i1) + Db_Xferred = Db_Xferred + 1 + END DO + CALL MD_PackOutput( Re_Buf, Db_Buf, Int_Buf, InData%y, ErrStat2, ErrMsg2, OnlySize ) ! y + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + CALL MD_PackMisc( Re_Buf, Db_Buf, Int_Buf, InData%m, ErrStat2, ErrMsg2, OnlySize ) ! m + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IntKiBuf(Int_Xferred) = TRANSFER(InData%IsInitialized, IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + END SUBROUTINE Farm_PackMD_Data + + SUBROUTINE Farm_UnPackMD_Data( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_Data), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'Farm_UnPackMD_Data' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_UnpackContState( Re_Buf, Db_Buf, Int_Buf, OutData%x, ErrStat2, ErrMsg2 ) ! x + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_UnpackDiscState( Re_Buf, Db_Buf, Int_Buf, OutData%xd, ErrStat2, ErrMsg2 ) ! xd + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_UnpackConstrState( Re_Buf, Db_Buf, Int_Buf, OutData%z, ErrStat2, ErrMsg2 ) ! z + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_UnpackOtherState( Re_Buf, Db_Buf, Int_Buf, OutData%OtherSt, ErrStat2, ErrMsg2 ) ! OtherSt + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_UnpackParam( Re_Buf, Db_Buf, Int_Buf, OutData%p, ErrStat2, ErrMsg2 ) ! p + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + i1_l = LBOUND(OutData%u,1) + i1_u = UBOUND(OutData%u,1) + DO i1 = LBOUND(OutData%u,1), UBOUND(OutData%u,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_UnpackInput( Re_Buf, Db_Buf, Int_Buf, OutData%u(i1), ErrStat2, ErrMsg2 ) ! u + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + i1_l = LBOUND(OutData%utimes,1) + i1_u = UBOUND(OutData%utimes,1) + DO i1 = LBOUND(OutData%utimes,1), UBOUND(OutData%utimes,1) + OutData%utimes(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_UnpackOutput( Re_Buf, Db_Buf, Int_Buf, OutData%y, ErrStat2, ErrMsg2 ) ! y + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_UnpackMisc( Re_Buf, Db_Buf, Int_Buf, OutData%m, ErrStat2, ErrMsg2 ) ! m + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + OutData%IsInitialized = TRANSFER(IntKiBuf(Int_Xferred), OutData%IsInitialized) + Int_Xferred = Int_Xferred + 1 + END SUBROUTINE Farm_UnPackMD_Data + + SUBROUTINE Farm_CopyAll_FastFarm_Data( SrcAll_FastFarm_DataData, DstAll_FastFarm_DataData, CtrlCode, ErrStat, ErrMsg ) + TYPE(All_FastFarm_Data), INTENT(INOUT) :: SrcAll_FastFarm_DataData + TYPE(All_FastFarm_Data), INTENT(INOUT) :: DstAll_FastFarm_DataData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'Farm_CopyAll_FastFarm_Data' +! + ErrStat = ErrID_None + ErrMsg = "" + CALL Farm_CopyParam( SrcAll_FastFarm_DataData%p, DstAll_FastFarm_DataData%p, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + CALL Farm_CopyMisc( SrcAll_FastFarm_DataData%m, DstAll_FastFarm_DataData%m, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN +IF (ALLOCATED(SrcAll_FastFarm_DataData%FWrap)) THEN + i1_l = LBOUND(SrcAll_FastFarm_DataData%FWrap,1) + i1_u = UBOUND(SrcAll_FastFarm_DataData%FWrap,1) + IF (.NOT. ALLOCATED(DstAll_FastFarm_DataData%FWrap)) THEN + ALLOCATE(DstAll_FastFarm_DataData%FWrap(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstAll_FastFarm_DataData%FWrap.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcAll_FastFarm_DataData%FWrap,1), UBOUND(SrcAll_FastFarm_DataData%FWrap,1) + CALL Farm_Copyfastwrapper_data( SrcAll_FastFarm_DataData%FWrap(i1), DstAll_FastFarm_DataData%FWrap(i1), CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) IF (ErrStat>=AbortErrLev) RETURN ENDDO @@ -4962,6 +6161,9 @@ SUBROUTINE Farm_CopyAll_FastFarm_Data( SrcAll_FastFarm_DataData, DstAll_FastFarm CALL Farm_Copysc_data( SrcAll_FastFarm_DataData%SC, DstAll_FastFarm_DataData%SC, CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) IF (ErrStat>=AbortErrLev) RETURN + CALL Farm_Copymd_data( SrcAll_FastFarm_DataData%MD, DstAll_FastFarm_DataData%MD, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN END SUBROUTINE Farm_CopyAll_FastFarm_Data SUBROUTINE Farm_DestroyAll_FastFarm_Data( All_FastFarm_DataData, ErrStat, ErrMsg ) @@ -4989,6 +6191,7 @@ SUBROUTINE Farm_DestroyAll_FastFarm_Data( All_FastFarm_DataData, ErrStat, ErrMsg ENDIF CALL Farm_Destroyawae_data( All_FastFarm_DataData%AWAE, ErrStat, ErrMsg ) CALL Farm_Destroysc_data( All_FastFarm_DataData%SC, ErrStat, ErrMsg ) + CALL Farm_Destroymd_data( All_FastFarm_DataData%MD, ErrStat, ErrMsg ) END SUBROUTINE Farm_DestroyAll_FastFarm_Data SUBROUTINE Farm_PackAll_FastFarm_Data( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) @@ -5141,6 +6344,23 @@ SUBROUTINE Farm_PackAll_FastFarm_Data( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrSt Int_BufSz = Int_BufSz + SIZE( Int_Buf ) DEALLOCATE(Int_Buf) END IF + Int_BufSz = Int_BufSz + 3 ! MD: size of buffers for each call to pack subtype + CALL Farm_Packmd_data( Re_Buf, Db_Buf, Int_Buf, InData%MD, ErrStat2, ErrMsg2, .TRUE. ) ! MD + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! MD + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! MD + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! MD + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) IF (ErrStat2 /= 0) THEN @@ -5362,6 +6582,34 @@ SUBROUTINE Farm_PackAll_FastFarm_Data( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrSt ELSE IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 ENDIF + CALL Farm_Packmd_data( Re_Buf, Db_Buf, Int_Buf, InData%MD, ErrStat2, ErrMsg2, OnlySize ) ! MD + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF END SUBROUTINE Farm_PackAll_FastFarm_Data SUBROUTINE Farm_UnPackAll_FastFarm_Data( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) @@ -5663,6 +6911,46 @@ SUBROUTINE Farm_UnPackAll_FastFarm_Data( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, Er IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL Farm_Unpackmd_data( Re_Buf, Db_Buf, Int_Buf, OutData%MD, ErrStat2, ErrMsg2 ) ! MD + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) END SUBROUTINE Farm_UnPackAll_FastFarm_Data END MODULE FAST_Farm_Types diff --git a/modules/hydrodyn/src/HydroDyn_Types.f90 b/modules/hydrodyn/src/HydroDyn_Types.f90 index f33ac5f907..03c093227c 100644 --- a/modules/hydrodyn/src/HydroDyn_Types.f90 +++ b/modules/hydrodyn/src/HydroDyn_Types.f90 @@ -54,7 +54,7 @@ MODULE HydroDyn_Types REAL(DbKi) :: TMax !< Supplied by Driver: The total simulation time [(sec)] LOGICAL :: HasIce !< Supplied by Driver: Whether this simulation has ice loading (flag) [-] REAL(SiKi) , DIMENSION(:,:), ALLOCATABLE :: WaveElevXY !< Supplied by Driver: X-Y locations for WaveElevation output (for visualization). First dimension is the X (1) and Y (2) coordinate. Second dimension is the point number. [m,-] - INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 1: use individual HydroDyn inputs without adjustment, 2: adjust wave phases based on turbine offsets from farm origin [-] + INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 0: use individual HydroDyn inputs without adjustment, 1: adjust wave phases based on turbine offsets from farm origin [-] REAL(ReKi) :: PtfmLocationX !< Supplied by Driver: X coordinate of platform location in the wave field [m] REAL(ReKi) :: PtfmLocationY !< Supplied by Driver: Y coordinate of platform location in the wave field [m] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: AddF0 !< Additional pre-load forces and moments (N,N,N,N-m,N-m,N-m) [-] diff --git a/modules/hydrodyn/src/Waves_Types.f90 b/modules/hydrodyn/src/Waves_Types.f90 index a5bb1dad44..5bba94aa98 100644 --- a/modules/hydrodyn/src/Waves_Types.f90 +++ b/modules/hydrodyn/src/Waves_Types.f90 @@ -70,7 +70,7 @@ MODULE Waves_Types REAL(SiKi) , DIMENSION(:,:), ALLOCATABLE :: WaveElevXY !< Supplied by Driver: X-Y locations for WaveElevation output (for visualization). Index 1 corresponds to X or Y coordinate. Index 2 corresponds to point number. [-] REAL(ReKi) :: PtfmLocationX !< Copy of X coordinate of platform location in the wave field, used to offset/phase-shift all wave kinematics to account for location in the farm [m] REAL(ReKi) :: PtfmLocationY !< Copy of Y coordinate of platform location in the wave field, used to offset/phase-shift all wave kinematics to account for location in the farm [m] - INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 1: use individual HydroDyn inputs without adjustment, 2: adjust wave phases based on turbine offsets from farm origin [-] + INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 0: use individual HydroDyn inputs without adjustment, 1: adjust wave phases based on turbine offsets from farm origin [-] INTEGER(IntKi) :: NWaveKin !< Number of points where the incident wave kinematics will be computed [-] REAL(SiKi) , DIMENSION(:), ALLOCATABLE :: WaveKinxi !< xi-coordinates for points where the incident wave kinematics will be computed; these are relative to the mean sea level [(meters)] REAL(SiKi) , DIMENSION(:), ALLOCATABLE :: WaveKinyi !< yi-coordinates for points where the incident wave kinematics will be computed; these are relative to the mean sea level [(meters)] diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 4d1accbd50..1bf23a0b4d 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -27,7 +27,7 @@ MODULE MoorDyn PRIVATE - TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn', 'v1.01.02F', '8-Apr-2016' ) + TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn', 'v1.03.01FFarm', '28-Mar-2021' ) PUBLIC :: MD_Init @@ -62,9 +62,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er INTEGER(IntKi) :: I ! index INTEGER(IntKi) :: J ! index INTEGER(IntKi) :: K ! index + INTEGER(IntKi) :: iTurb ! index INTEGER(IntKi) :: Converged ! flag indicating whether the dynamic relaxation has converged INTEGER(IntKi) :: N ! convenience integer for readability: number of segments in the line - REAL(ReKi) :: Pos(3) ! array for setting absolute fairlead positions in mesh + REAL(ReKi) :: rPos(3) ! array for setting fairlead reference positions in mesh REAL(DbKi) :: TransMat(3,3) ! rotation matrix for setting fairlead positions correctly if there is initial platform rotation REAL(DbKi), ALLOCATABLE :: FairTensIC(:,:)! array of size Nfairs, 3 to store three latest fairlead tensions of each line CHARACTER(20) :: TempString ! temporary string for incidental use @@ -103,6 +104,33 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er p%RootName = TRIM(InitInp%RootName)//'.MD' ! all files written from this module will have this root name + ! Check if this MoorDyn instance is being run from FAST.Farm (indicated by FarmSize > 0) + p%nTurbines = InitInp%FarmSize ! 0 indicates regular FAST module mode + + if (p%nTurbines == 0) p%nTurbines = 1 ! if a regular FAST module mode, we treat it like a nTurbine=1 farm case + + allocate( p%NFairs( p%nTurbines)) ! allocate the array that will hold the number of fairleads for each turbine + allocate( p%TurbineRefPos( 3, p%nTurbines)) + + if (InitInp%FarmSize > 0) then + CALL WrScr(' >>> MoorDyn is running in array mode <<< ') + + ! could make sure the size of this is right: SIZE(InitInp%FarmCoupledKinematics) + + !InitInp%FarmNCpldCons + + ! copy over turbine reference positions for later use + p%TurbineRefPos = InitInp%TurbineRefPos + + else ! normal, FAST module mode + + m%PtfmInit = InitInp%PtfmInit(:,1) ! save a copy of PtfmInit so it's available at the farm level + p%TurbineRefPos = 0.0_DbKi ! for now assuming this is zero for FAST use + + END IF + + + ! call function that reads input file and creates cross-referenced Connect and Line objects CALL MDIO_ReadInput(InitInp, p, m, ErrStat2, ErrMsg2) CALL CheckError( ErrStat2, ErrMsg2 ) @@ -120,7 +148,9 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL WrNR( ' Creating mooring system. ' ) - p%NFairs = 0 ! this is the number of "vessel" type Connections. being consistent with MAP terminology + do iTurb = 1, p%nTurbines + p%NFairs(iTurb) = 0 ! this is the number of fairleads (for each turbine if in farm mode) + end do p%NConns = 0 ! this is the number of "connect" type Connections. not to be confused with NConnects, the number of Connections p%NAnchs = 0 ! this is the number of "fixed" type Connections. @@ -132,23 +162,39 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er if (TempString == 'FIXED') then m%ConnectList(I)%TypeNum = 0 p%NAnchs = p%NAnchs + 1 + else if (TempString == 'VESSEL') then - m%ConnectList(I)%TypeNum = 1 - p%NFairs = p%NFairs + 1 ! if a vessel connection, increment fairlead counter + + + m%ConnectList(I)%TypeNum = -1 ! changing fairlead typenum to -1 for FAST module operation + p%NFairs(1) = p%NFairs(1) + 1 ! if a vessel connection, increment fairlead counter + else if (TempString == 'CONNECT') then m%ConnectList(I)%TypeNum = 2 p%NConns = p%NConns + 1 + + else if (INDEX(TempString, "TURBINE") > 0 ) then ! turbine-coupled in FAST.Farm case + if (p%nTurbines > 0) then + K = scan(TempString , '1234567890' ) ! find index of first number in the string + READ(TempString(K:), *) iTurb ! READ(TRIM(TempString(K:)), *) iTurb ! convert to int, representing turbine index + m%ConnectList(I)%TypeNum = -iTurb ! typeNum < 0 indicates -turbine number + p%NFairs(iTurb) = p%NFairs(iTurb) + 1 ! increment fairlead counter + print *, ' added connection ', I, ' as fairlead for turbine ', iTurb + else + call CheckError( ErrID_Fatal, 'Error: Turbine[n] Connect types can only be used with FAST.Farm.' ) + return + end if else CALL CheckError( ErrID_Fatal, 'Error in provided Connect type. Must be fixed, vessel, or connect.' ) RETURN END IF END DO - CALL WrScr(trim(Num2LStr(p%NFairs))//' fairleads, '//trim(Num2LStr(p%NAnchs))//' anchors, '//trim(Num2LStr(p%NConns))//' connects.') + CALL WrScr(trim(Num2LStr(p%NFairs(1)))//' fairleads (T1), '//trim(Num2LStr(p%NAnchs))//' anchors, '//trim(Num2LStr(p%NConns))//' connects.') - ! allocate fairleads list - ALLOCATE ( m%FairIdList(p%NFairs), STAT = ErrStat ) + ! allocate fairleads list (size to the maximum number of fairleads on any given turbine - some entries may not be used) + ALLOCATE ( m%FairIdList(MAXVAL(p%NFairs),p%nTurbines), STAT = ErrStat ) IF ( ErrStat /= ErrID_None ) THEN CALL CheckError( ErrID_Fatal, 'Error allocating space for FairIdList array.') RETURN @@ -163,15 +209,16 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! now go back through and record the fairlead Id numbers (this is all the "connecting" that's required) - J = 1 ! counter for fairlead number - K = 1 ! counter for connect number + p%NFairs = 0 + p%NConns = 0 ! re-using this as a counter for connect number (should end back at same value) DO I = 1,p%NConnects - IF (m%ConnectList(I)%TypeNum == 1) THEN - m%FairIdList(J) = I ! if a vessel connection, add ID to list - J = J + 1 + IF (m%ConnectList(I)%TypeNum < 0) THEN + iTurb = -m%ConnectList(I)%TypeNum + p%NFairs(iTurb) = p%NFairs(iTurb) + 1 + m%FairIdList(p%NFairs(iTurb), iTurb) = I ! if a vessel connection, add ID to list of the appropriate turbine ELSE IF (m%ConnectList(I)%TypeNum == 2) THEN - m%ConnIdList(K) = I ! if a connect connection, add ID to list - K = K + 1 + p%NConns = p%NConns + 1 + m%ConnIdList(p%NConns) = I ! if a connect connection, add ID to list END IF END DO @@ -221,78 +268,93 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! create i/o meshes for fairlead positions and forces !-------------------------------------------------------------------------- - ! create input mesh for fairlead kinematics - CALL MeshCreate(BlankMesh=u%PtFairleadDisplacement , & - IOS= COMPONENT_INPUT , & - Nnodes=p%NFairs , & - TranslationDisp=.TRUE. , & - TranslationVel=.TRUE. , & - ErrStat=ErrStat2 , & - ErrMess=ErrMsg2) + ALLOCATE ( u%PtFairleadDisplacement(p%nTurbines), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) THEN + CALL CheckError(ErrID_Fatal, ' Error allocating PtFairleadDisplacement input array.') + RETURN + END IF + ALLOCATE ( y%PtFairleadLoad(p%nTurbines), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) THEN + CALL CheckError(ErrID_Fatal, ' Error allocating PtFairleadLoad output array.') + RETURN + END IF - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN + ! Go through each turbine and set up its mesh and initial fairlead positions + DO iTurb = 1,p%nTurbines + + ! create input mesh for fairlead kinematics + CALL MeshCreate(BlankMesh=u%PtFairleadDisplacement(iTurb) , & + IOS= COMPONENT_INPUT , & + Nnodes=p%NFairs(iTurb) , & + TranslationDisp=.TRUE. , & + TranslationVel=.TRUE. , & + ErrStat=ErrStat2 , & + ErrMess=ErrMsg2) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN - ! --------------------------- set up initial condition of each fairlead ------------------------------- - DO i = 1,p%NFairs - Pos(1) = m%ConnectList(m%FairIdList(i))%conX ! set relative position of each fairlead i (I'm pretty sure this is just relative to ptfm origin) - Pos(2) = m%ConnectList(m%FairIdList(i))%conY - Pos(3) = m%ConnectList(m%FairIdList(i))%conZ + ! --------------------------- set up initial condition of each fairlead ------------------------------- + DO i = 1,p%NFairs(iTurb) - CALL MeshPositionNode(u%PtFairleadDisplacement,i,Pos,ErrStat2,ErrMsg2)! "assign the coordinates of each node in the global coordinate space" + rPos(1) = m%ConnectList(m%FairIdList(i, iTurb))%conX ! reference position of each fairlead relative to each turbine's local reference position + rPos(2) = m%ConnectList(m%FairIdList(i, iTurb))%conY + rPos(3) = m%ConnectList(m%FairIdList(i, iTurb))%conZ - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN + CALL MeshPositionNode(u%PtFairleadDisplacement(iTurb), i, rPos, ErrStat2, ErrMsg2) ! set reference coordinates of each node in the respective turbine's 'global' coordinate space + + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN + ! set offset position of each node to according to initial platform position and rotation + CALL SmllRotTrans('PtfmInit', InitInp%PtfmInit(4,iTurb),InitInp%PtfmInit(5,iTurb),InitInp%PtfmInit(6,iTurb), TransMat, '', ErrStat2, ErrMsg2) - ! set offset position of each node to according to initial platform position - CALL SmllRotTrans('initial fairlead positions due to platform rotation', InitInp%PtfmInit(4),InitInp%PtfmInit(5),InitInp%PtfmInit(6), TransMat, '', ErrStat2, ErrMsg2) ! account for possible platform rotation + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN + ! Apply initial platform rotations and translations (fixed Jun 19, 2015) + u%PtFairleadDisplacement(iTurb)%TranslationDisp(1,i) = InitInp%PtfmInit(1,iTurb) + Transmat(1,1)*rPos(1) + Transmat(2,1)*rPos(2) + TransMat(3,1)*rPos(3) - rPos(1) + u%PtFairleadDisplacement(iTurb)%TranslationDisp(2,i) = InitInp%PtfmInit(2,iTurb) + Transmat(1,2)*rPos(1) + Transmat(2,2)*rPos(2) + TransMat(3,2)*rPos(3) - rPos(2) + u%PtFairleadDisplacement(iTurb)%TranslationDisp(3,i) = InitInp%PtfmInit(3,iTurb) + Transmat(1,3)*rPos(1) + Transmat(2,3)*rPos(2) + TransMat(3,3)*rPos(3) - rPos(3) - ! Apply initial platform rotations and translations (fixed Jun 19, 2015) - u%PtFairleadDisplacement%TranslationDisp(1,i) = InitInp%PtfmInit(1) + Transmat(1,1)*Pos(1) + Transmat(2,1)*Pos(2) + TransMat(3,1)*Pos(3) - Pos(1) - u%PtFairleadDisplacement%TranslationDisp(2,i) = InitInp%PtfmInit(2) + Transmat(1,2)*Pos(1) + Transmat(2,2)*Pos(2) + TransMat(3,2)*Pos(3) - Pos(2) - u%PtFairleadDisplacement%TranslationDisp(3,i) = InitInp%PtfmInit(3) + Transmat(1,3)*Pos(1) + Transmat(2,3)*Pos(2) + TransMat(3,3)*Pos(3) - Pos(3) + ! set velocity of each node to zero + u%PtFairleadDisplacement(iTurb)%TranslationVel(1,i) = 0.0_DbKi + u%PtFairleadDisplacement(iTurb)%TranslationVel(2,i) = 0.0_DbKi + u%PtFairleadDisplacement(iTurb)%TranslationVel(3,i) = 0.0_DbKi + + !print *, 'Fairlead ', i, ' z TranslationDisp at start is ', u%PtFairleadDisplacement(iTurb)%TranslationDisp(3,i) + !print *, 'Fairlead ', i, ' z Position at start is ', u%PtFairleadDisplacement(iTurb)%Position(3,i) - ! set velocity of each node to zero - u%PtFairleadDisplacement%TranslationVel(1,i) = 0.0_DbKi - u%PtFairleadDisplacement%TranslationVel(2,i) = 0.0_DbKi - u%PtFairleadDisplacement%TranslationVel(3,i) = 0.0_DbKi - - !print *, 'Fairlead ', i, ' z TranslationDisp at start is ', u%PtFairleadDisplacement%TranslationDisp(3,i) - !print *, 'Fairlead ', i, ' z Position at start is ', u%PtFairleadDisplacement%Position(3,i) + ! set each node as a point element + CALL MeshConstructElement(u%PtFairleadDisplacement(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, i) - ! set each node as a point element - CALL MeshConstructElement(u%PtFairleadDisplacement, ELEMENT_POINT, ErrStat2, ErrMsg2, i) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN + END DO ! I - END DO ! I + CALL MeshCommit ( u%PtFairleadDisplacement(iTurb), ErrStat, ErrMsg ) - CALL MeshCommit ( u%PtFairleadDisplacement, ErrStat, ErrMsg ) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN + ! copy the input fairlead kinematics mesh to make the output mesh for fairlead loads, PtFairleadLoad + CALL MeshCopy ( SrcMesh = u%PtFairleadDisplacement(iTurb), DestMesh = y%PtFairleadLoad(iTurb), & + CtrlCode = MESH_SIBLING, IOS = COMPONENT_OUTPUT, & + Force = .TRUE., ErrStat = ErrStat2, ErrMess=ErrMsg2 ) - ! copy the input fairlead kinematics mesh to make the output mesh for fairlead loads, PtFairleadLoad - CALL MeshCopy ( SrcMesh = u%PtFairleadDisplacement, DestMesh = y%PtFairleadLoad, & - CtrlCode = MESH_SIBLING, IOS = COMPONENT_OUTPUT, & - Force = .TRUE., ErrStat = ErrStat2, ErrMess=ErrMsg2 ) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN + end do ! iTurb ! -------------------------------------------------------------------- - ! go through all Connects and set position based on input file + ! go through all Connections and set position based on input file ! -------------------------------------------------------------------- ! first do it for all connections (connect and anchor types will be saved) @@ -305,13 +367,19 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er m%ConnectList(I)%rd(3) = 0.0_DbKi END DO - ! then do it for fairlead types - DO I = 1,p%NFairs - DO J = 1, 3 - m%ConnectList(m%FairIdList(I))%r(J) = u%PtFairleadDisplacement%Position(J,I) + u%PtFairleadDisplacement%TranslationDisp(J,I) - m%ConnectList(m%FairIdList(I))%rd(J) = 0.0_DbKi + ! then redo it for fairlead types (remember that for FAST.Farm these need to be offset by turbine positions) + do iTurb = 1,p%nTurbines + DO I = 1,p%NFairs(iTurb) + ! get values in turbine's 'global' reference frame from its mesh + DO J = 1, 3 + m%ConnectList(m%FairIdList(I,iTurb))%r(J) = u%PtFairleadDisplacement(iTurb)%Position(J,I) + u%PtFairleadDisplacement(iTurb)%TranslationDisp(J,I) + m%ConnectList(m%FairIdList(I,iTurb))%rd(J) = 0.0_DbKi + END DO + ! offset into farm's true global reference based on turbine X and Y reference positions (these should be 0 for regular FAST use) + m%ConnectList(m%FairIdList(I,iTurb))%r(1) = m%ConnectList(m%FairIdList(I,iTurb))%r(1) + p%TurbineRefPos(1,iTurb) + m%ConnectList(m%FairIdList(I,iTurb))%r(2) = m%ConnectList(m%FairIdList(I,iTurb))%r(2) + p%TurbineRefPos(2,iTurb) END DO - END DO + end do ! for connect types, write the coordinates to the state vector DO I = 1,p%NConns @@ -397,15 +465,15 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er m%LineTypeList(I)%Cdt = m%LineTypeList(I)%Cdt * InitInp%CdScaleIC END DO - ! allocate array holding three latest fairlead tensions - ALLOCATE ( FairTensIC(p%NFairs,3), STAT = ErrStat ) + ! allocate array holding three latest fairlead tensions *of first turbine only* + ALLOCATE ( FairTensIC(p%NFairs(1),3), STAT = ErrStat ) IF ( ErrStat /= ErrID_None ) THEN CALL CheckError( ErrID_Fatal, ErrMsg2 ) RETURN END IF ! initialize fairlead tension memory at zero - DO J = 1,p%NFairs + DO J = 1,p%NFairs(1) DO I = 1, 3 FairTensIC(J,I) = 0.0_DbKi END DO @@ -425,10 +493,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF (ErrStat >= AbortErrLev) RETURN ! store new fairlead tension (and previous fairlead tensions for comparison) - DO J = 1, p%NFairs + DO J = 1, p%NFairs(1) FairTensIC(J,3) = FairTensIC(J,2) FairTensIC(J,2) = FairTensIC(J,1) - FairTensIC(J,1) = TwoNorm(m%ConnectList(m%FairIdList(J))%Ftot(:)) + FairTensIC(J,1) = TwoNorm(m%ConnectList(m%FairIdList(J,1))%Ftot(:)) END DO ! provide status message @@ -440,7 +508,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! check for convergence (compare current tension at each fairlead with previous two values) IF (I > 2) THEN Converged = 1 - DO J = 1, p%NFairs ! check for non-convergence + DO J = 1, p%NFairs(1) ! check for non-convergence IF (( abs( FairTensIC(J,1)/FairTensIC(J,2) - 1.0 ) > InitInp%threshIC ) .OR. ( abs( FairTensIC(J,1)/FairTensIC(J,3) - 1.0 ) > InitInp%threshIC ) ) THEN Converged = 0 EXIT @@ -560,8 +628,8 @@ SUBROUTINE MD_UpdateStates( t, n, u, utimes, p, x, xd, z, other, m, ErrStat, Err ! ! go through fairleads and apply motions from driver ! DO I = 1, p%NFairs ! DO J = 1,3 -! m%ConnectList(m%FairIdList(I))%r(J) = u_interp%PtFairleadDisplacement%Position(J,I) + u_interp%PtFairleadDisplacement%TranslationDisp(J,I) -! m%ConnectList(m%FairIdList(I))%rd(J) = u_interp%PtFairleadDisplacement%TranslationVel(J,I) ! is this right? <<< +! m%ConnectList(m%FairIdList(I))%r(J) = u_interp%PtFairleadDisplacement(iTurb)%Position(J,I) + u_interp%PtFairleadDisplacement(iTurb)%TranslationDisp(J,I) +! m%ConnectList(m%FairIdList(I))%rd(J) = u_interp%PtFairleadDisplacement(iTurb)%TranslationVel(J,I) ! is this right? <<< ! END DO ! END DO ! @@ -623,6 +691,7 @@ SUBROUTINE MD_CalcOutput( t, u, p, x, xd, z, other, y, m, ErrStat, ErrMsg ) TYPE(MD_ContinuousStateType) :: dxdt ! time derivatives of continuous states (initialized in CalcContStateDeriv) INTEGER(IntKi) :: I ! counter + INTEGER(IntKi) :: iTurb ! counter INTEGER(IntKi) :: J ! counter INTEGER(IntKi) :: ErrStat2 ! Error status of the operation @@ -632,24 +701,30 @@ SUBROUTINE MD_CalcOutput( t, u, p, x, xd, z, other, y, m, ErrStat, ErrMsg ) ! below updated to make sure outputs are current (based on provided x and u) - similar to what's in UpdateStates ! go through fairleads and apply motions from driver - DO I = 1, p%NFairs - DO J = 1,3 - m%ConnectList(m%FairIdList(I))%r(J) = u%PtFairleadDisplacement%Position(J,I) + u%PtFairleadDisplacement%TranslationDisp(J,I) - m%ConnectList(m%FairIdList(I))%rd(J) = u%PtFairleadDisplacement%TranslationVel(J,I) ! is this right? <<< + do iTurb = 1,p%nTurbines + DO I = 1,p%NFairs(iTurb) + DO J = 1,3 + m%ConnectList(m%FairIdList(I,iTurb))%r(J) = u%PtFairleadDisplacement(iTurb)%Position(J,I) + u%PtFairleadDisplacement(iTurb)%TranslationDisp(J,I) + m%ConnectList(m%FairIdList(I,iTurb))%rd(J) = u%PtFairleadDisplacement(iTurb)%TranslationVel(J,I) + END DO + ! offset into farm's true global reference based on turbine X and Y reference positions (these should be 0 for regular FAST use) + m%ConnectList(m%FairIdList(I,iTurb))%r(1) = m%ConnectList(m%FairIdList(I,iTurb))%r(1) + p%TurbineRefPos(1,iTurb) + m%ConnectList(m%FairIdList(I,iTurb))%r(2) = m%ConnectList(m%FairIdList(I,iTurb))%r(2) + p%TurbineRefPos(2,iTurb) END DO - END DO + end do ! call CalcContStateDeriv in order to run model and calculate dynamics with provided x and u CALL MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, ErrMsg ) ! assign net force on fairlead Connects to the output mesh - DO i = 1, p%NFairs - DO J=1,3 - y%PtFairleadLoad%Force(J,I) = m%ConnectList(m%FairIdList(I))%Ftot(J) + do iTurb = 1,p%nTurbines + DO i = 1, p%NFairs(iTurb) + DO J=1,3 + y%PtFairleadLoad(iTurb)%Force(J,I) = m%ConnectList(m%FairIdList(I,iTurb))%Ftot(J) + END DO END DO - END DO - + end do ! calculate outputs (y%WriteOutput) for glue code and write any m outputs to MoorDyn output files CALL MDIO_WriteOutputs(t, p, m, y, ErrStat2, ErrMsg2) @@ -712,6 +787,8 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er INTEGER(IntKi) :: L ! index INTEGER(IntKi) :: J ! index + INTEGER(IntKi) :: iTurb ! index + INTEGER(IntKi) :: I ! index INTEGER(IntKi) :: K ! index INTEGER(IntKi) :: Istart ! start index of line/connect in state vector INTEGER(IntKi) :: Iend ! end index of line/connect in state vector @@ -742,12 +819,17 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er END DO ! update fairlead positions for instantaneous values (fixed 2015-06-22) - DO K = 1, p%NFairs - DO J = 1,3 - m%ConnectList(m%FairIdList(K))%r(J) = u%PtFairleadDisplacement%Position(J,K) + u%PtFairleadDisplacement%TranslationDisp(J,K) - m%ConnectList(m%FairIdList(K))%rd(J) = u%PtFairleadDisplacement%TranslationVel(J,K) ! is this right? <<< + do iTurb = 1,p%nTurbines + DO I = 1, p%NFairs(iTurb) + DO J = 1,3 + m%ConnectList(m%FairIdList(I,iTurb))%r(J) = u%PtFairleadDisplacement(iTurb)%Position(J,I) + u%PtFairleadDisplacement(iTurb)%TranslationDisp(J,I) + m%ConnectList(m%FairIdList(I,iTurb))%rd(J) = u%PtFairleadDisplacement(iTurb)%TranslationVel(J,I) + END DO + ! offset into farm's true global reference based on turbine X and Y reference positions (these should be 0 for regular FAST use) + m%ConnectList(m%FairIdList(I,iTurb))%r(1) = m%ConnectList(m%FairIdList(I,iTurb))%r(1) + p%TurbineRefPos(1,iTurb) + m%ConnectList(m%FairIdList(I,iTurb))%r(2) = m%ConnectList(m%FairIdList(I,iTurb))%r(2) + p%TurbineRefPos(2,iTurb) END DO - END DO + end do ! apply line length changes from active tensioning if applicable DO L = 1, p%NLines diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 6fb1e729db..68af14ef82 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -18,7 +18,9 @@ include Registry_NWTC_Library.txt typedef MoorDyn/MD InitInputType ReKi g - -999.9 - "gravity constant" "[m/s^2]" typedef ^ ^ ReKi rhoW - -999.9 - "sea density" "[kg/m^3]" typedef ^ ^ ReKi WtrDepth - -999.9 - "depth of water" "[m]" -typedef ^ ^ ReKi PtfmInit {6} - - "initial position of platform" - +typedef ^ ^ ReKi PtfmInit {:}{:} - - "initial position of platform(s) shape: 6, nTurbines" - +typedef ^ ^ IntKi FarmSize - 0 - "Indicates normal FAST module mode if 0, FAST.Farm coupled mode and =nTurbines if >0" - +typedef ^ ^ ReKi TurbineRefPos {:}{:} - - "reference position of turbines in farm, shape: 3, nTurbines" - typedef ^ ^ CHARACTER(1024) FileName - "" - "MoorDyn input file" typedef ^ ^ CHARACTER(1024) RootName - - - "RootName for writing output files" - typedef ^ ^ LOGICAL Echo - "" - "echo parameter - do we want to echo the header line describing the input file?" @@ -26,7 +28,7 @@ typedef ^ ^ ReKi DTIC - - typedef ^ ^ ReKi TMaxIC - 120 - "maximum time to allow for getting converged ICs" "[s]" typedef ^ ^ ReKi CdScaleIC - 1 - "factor to scale drag coefficients by during dynamic relaxation" "[]" typedef ^ ^ ReKi threshIC - 0.01 - "convergence tolerance for ICs (0.01 means 1%)" "[]" -typedef ^ ^ CHARACTER(ChanLen) OutList {:} "" - "string containing list of output channels requested in input file" +typedef ^ ^ CHARACTER(ChanLen) OutList {:} "" - "string containing list of output channels requested in input file" # ====================================== Internal data types ======================================================================== @@ -134,16 +136,16 @@ typedef ^ OtherStateType SiKi dummy - typedef ^ MiscVarType MD_LineProp LineTypeList {:} - - "array of properties for each line type" - typedef ^ ^ MD_Connect ConnectList {:} - - "array of connection properties" - typedef ^ ^ MD_Line LineList {:} - - "array of line properties" - -typedef ^ ^ IntKi FairIdList {:} - - "array of size NFairs listing the ID of each fairlead (index of ConnectList)" "" +typedef ^ ^ IntKi FairIdList {:}{:} - - "array of size max(NFairs) X nTurbines listing the ID of each fairlead (index of ConnectList)" "" typedef ^ ^ IntKi ConnIdList {:} - - "array of size NConnss listing the ID of each connect type connection (index of ConnectList)" "" typedef ^ ^ IntKi LineStateIndList {:} - - "starting index of each line's states in state vector" "" typedef ^ ^ ReKi MDWrOutput {:} - - "Data from time step to be written to a MoorDyn output file" - +typedef ^ ^ ReKi PtfmInit {6} - - "initial position of platform - saving here to get around FAST(wrapper)'s lack of an InitOut" - ## ============================== Parameters ============================================================================================================================================ typedef ^ ParameterType IntKi NTypes - - - "number of line types" "" typedef ^ ^ IntKi NConnects - - - "number of Connection objects" "" -typedef ^ ^ IntKi NFairs - - - "number of Fairlead Connections" "" +typedef ^ ^ IntKi NFairs {:} - - "number of Fairlead Connections FOR EACH TURBINE" "" typedef ^ ^ IntKi NConns - - - "number of Connect type Connections - not to be confused with NConnects" "" typedef ^ ^ IntKi NAnchs - - - "number of Anchor type Connections" "" typedef ^ ^ IntKi NLines - - - "number of Line objects" "" @@ -159,16 +161,17 @@ typedef ^ ^ CHARACTER(1024) RootName - typedef ^ ^ MD_OutParmType OutParam {:} - - "Names and units (and other characteristics) of all requested output parameters" - typedef ^ ^ CHARACTER(1) Delim - - - "Column delimiter for output text files" - typedef ^ ^ IntKi MDUnOut - - - "Unit number of main output file" - +typedef ^ ^ IntKi nTurbines - - - "Number of turbines if MoorDyn is performing an array-level simulation with FAST.Farm, otherwise 0" +typedef ^ ^ ReKi TurbineRefPos {:}{:} - - "turbine reference positions, 0 for regular FAST use, shape: 3, nTurbines" - # ============================== Inputs ============================================================================================================================================ -typedef ^ InputType MeshType PtFairleadDisplacement - - - "mesh for position AND VELOCITY of each fairlead in X,Y,Z" "[m, m/s]" +typedef ^ InputType MeshType PtFairleadDisplacement {:} - - "mesh for position AND VELOCITY of each fairlead in X,Y,Z" "[m, m/s]" # typedef ^ ^ MeshType HydroForceLineMesh - - - "Meshed input data" - typedef ^ ^ ReKi DeltaL {:} - - "change in line length command for each channel" "[m]" typedef ^ ^ ReKi DeltaLdot {:} - - "rate of change of line length command for each channel" "[m]" ## ============================== Outputs ============================================================================================================================================ -typedef ^ OutputType MeshType PtFairleadLoad - - - "point mesh for fairlead forces in X,Y,Z" "[N]" +typedef ^ OutputType MeshType PtFairleadLoad {:} - - "point mesh for fairlead forces in X,Y,Z" "[N]" typedef ^ ^ ReKi WriteOutput {:} - - "output vector returned to glue code" "" # typedef ^ ^ MeshType LineMeshPosition - - - "Meshed output data" - diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index dc9c2bff15..c6503a7a26 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -38,7 +38,9 @@ MODULE MoorDyn_Types REAL(ReKi) :: g = -999.9 !< gravity constant [[m/s^2]] REAL(ReKi) :: rhoW = -999.9 !< sea density [[kg/m^3]] REAL(ReKi) :: WtrDepth = -999.9 !< depth of water [[m]] - REAL(ReKi) , DIMENSION(1:6) :: PtfmInit !< initial position of platform [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: PtfmInit !< initial position of platform(s) shape: 6, nTurbines [-] + INTEGER(IntKi) :: FarmSize = 0 !< Indicates normal FAST module mode if 0, FAST.Farm coupled mode and =nTurbines if >0 [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: TurbineRefPos !< reference position of turbines in farm, shape: 3, nTurbines [-] CHARACTER(1024) :: FileName !< MoorDyn input file [-] CHARACTER(1024) :: RootName !< RootName for writing output files [-] LOGICAL :: Echo !< echo parameter - do we want to echo the header line describing the input file? [-] @@ -164,17 +166,18 @@ MODULE MoorDyn_Types TYPE(MD_LineProp) , DIMENSION(:), ALLOCATABLE :: LineTypeList !< array of properties for each line type [-] TYPE(MD_Connect) , DIMENSION(:), ALLOCATABLE :: ConnectList !< array of connection properties [-] TYPE(MD_Line) , DIMENSION(:), ALLOCATABLE :: LineList !< array of line properties [-] - INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: FairIdList !< array of size NFairs listing the ID of each fairlead (index of ConnectList) [] + INTEGER(IntKi) , DIMENSION(:,:), ALLOCATABLE :: FairIdList !< array of size max(NFairs) X nTurbines listing the ID of each fairlead (index of ConnectList) [] INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: ConnIdList !< array of size NConnss listing the ID of each connect type connection (index of ConnectList) [] INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: LineStateIndList !< starting index of each line's states in state vector [] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: MDWrOutput !< Data from time step to be written to a MoorDyn output file [-] + REAL(ReKi) , DIMENSION(1:6) :: PtfmInit !< initial position of platform - saving here to get around FAST(wrapper)'s lack of an InitOut [-] END TYPE MD_MiscVarType ! ======================= ! ========= MD_ParameterType ======= TYPE, PUBLIC :: MD_ParameterType INTEGER(IntKi) :: NTypes !< number of line types [] INTEGER(IntKi) :: NConnects !< number of Connection objects [] - INTEGER(IntKi) :: NFairs !< number of Fairlead Connections [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: NFairs !< number of Fairlead Connections FOR EACH TURBINE [] INTEGER(IntKi) :: NConns !< number of Connect type Connections - not to be confused with NConnects [] INTEGER(IntKi) :: NAnchs !< number of Anchor type Connections [] INTEGER(IntKi) :: NLines !< number of Line objects [] @@ -190,18 +193,20 @@ MODULE MoorDyn_Types TYPE(MD_OutParmType) , DIMENSION(:), ALLOCATABLE :: OutParam !< Names and units (and other characteristics) of all requested output parameters [-] CHARACTER(1) :: Delim !< Column delimiter for output text files [-] INTEGER(IntKi) :: MDUnOut !< Unit number of main output file [-] + INTEGER(IntKi) :: nTurbines !< Number of turbines if MoorDyn is performing an array-level simulation with FAST.Farm, otherwise 0 [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: TurbineRefPos !< turbine reference positions, 0 for regular FAST use, shape: 3, nTurbines [-] END TYPE MD_ParameterType ! ======================= ! ========= MD_InputType ======= TYPE, PUBLIC :: MD_InputType - TYPE(MeshType) :: PtFairleadDisplacement !< mesh for position AND VELOCITY of each fairlead in X,Y,Z [[m, m/s]] + TYPE(MeshType) , DIMENSION(:), ALLOCATABLE :: PtFairleadDisplacement !< mesh for position AND VELOCITY of each fairlead in X,Y,Z [[m, m/s]] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: DeltaL !< change in line length command for each channel [[m]] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: DeltaLdot !< rate of change of line length command for each channel [[m]] END TYPE MD_InputType ! ======================= ! ========= MD_OutputType ======= TYPE, PUBLIC :: MD_OutputType - TYPE(MeshType) :: PtFairleadLoad !< point mesh for fairlead forces in X,Y,Z [[N]] + TYPE(MeshType) , DIMENSION(:), ALLOCATABLE :: PtFairleadLoad !< point mesh for fairlead forces in X,Y,Z [[N]] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: WriteOutput !< output vector returned to glue code [] END TYPE MD_OutputType ! ======================= @@ -226,7 +231,35 @@ SUBROUTINE MD_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, ErrSt DstInitInputData%g = SrcInitInputData%g DstInitInputData%rhoW = SrcInitInputData%rhoW DstInitInputData%WtrDepth = SrcInitInputData%WtrDepth +IF (ALLOCATED(SrcInitInputData%PtfmInit)) THEN + i1_l = LBOUND(SrcInitInputData%PtfmInit,1) + i1_u = UBOUND(SrcInitInputData%PtfmInit,1) + i2_l = LBOUND(SrcInitInputData%PtfmInit,2) + i2_u = UBOUND(SrcInitInputData%PtfmInit,2) + IF (.NOT. ALLOCATED(DstInitInputData%PtfmInit)) THEN + ALLOCATE(DstInitInputData%PtfmInit(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitInputData%PtfmInit.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF DstInitInputData%PtfmInit = SrcInitInputData%PtfmInit +ENDIF + DstInitInputData%FarmSize = SrcInitInputData%FarmSize +IF (ALLOCATED(SrcInitInputData%TurbineRefPos)) THEN + i1_l = LBOUND(SrcInitInputData%TurbineRefPos,1) + i1_u = UBOUND(SrcInitInputData%TurbineRefPos,1) + i2_l = LBOUND(SrcInitInputData%TurbineRefPos,2) + i2_u = UBOUND(SrcInitInputData%TurbineRefPos,2) + IF (.NOT. ALLOCATED(DstInitInputData%TurbineRefPos)) THEN + ALLOCATE(DstInitInputData%TurbineRefPos(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitInputData%TurbineRefPos.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitInputData%TurbineRefPos = SrcInitInputData%TurbineRefPos +ENDIF DstInitInputData%FileName = SrcInitInputData%FileName DstInitInputData%RootName = SrcInitInputData%RootName DstInitInputData%Echo = SrcInitInputData%Echo @@ -257,6 +290,12 @@ SUBROUTINE MD_DestroyInitInput( InitInputData, ErrStat, ErrMsg ) ! ErrStat = ErrID_None ErrMsg = "" +IF (ALLOCATED(InitInputData%PtfmInit)) THEN + DEALLOCATE(InitInputData%PtfmInit) +ENDIF +IF (ALLOCATED(InitInputData%TurbineRefPos)) THEN + DEALLOCATE(InitInputData%TurbineRefPos) +ENDIF IF (ALLOCATED(InitInputData%OutList)) THEN DEALLOCATE(InitInputData%OutList) ENDIF @@ -300,7 +339,17 @@ SUBROUTINE MD_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg Re_BufSz = Re_BufSz + 1 ! g Re_BufSz = Re_BufSz + 1 ! rhoW Re_BufSz = Re_BufSz + 1 ! WtrDepth + Int_BufSz = Int_BufSz + 1 ! PtfmInit allocated yes/no + IF ( ALLOCATED(InData%PtfmInit) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! PtfmInit upper/lower bounds for each dimension Re_BufSz = Re_BufSz + SIZE(InData%PtfmInit) ! PtfmInit + END IF + Int_BufSz = Int_BufSz + 1 ! FarmSize + Int_BufSz = Int_BufSz + 1 ! TurbineRefPos allocated yes/no + IF ( ALLOCATED(InData%TurbineRefPos) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! TurbineRefPos upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%TurbineRefPos) ! TurbineRefPos + END IF Int_BufSz = Int_BufSz + 1*LEN(InData%FileName) ! FileName Int_BufSz = Int_BufSz + 1*LEN(InData%RootName) ! RootName Int_BufSz = Int_BufSz + 1 ! Echo @@ -346,10 +395,48 @@ SUBROUTINE MD_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg Re_Xferred = Re_Xferred + 1 ReKiBuf(Re_Xferred) = InData%WtrDepth Re_Xferred = Re_Xferred + 1 - DO i1 = LBOUND(InData%PtfmInit,1), UBOUND(InData%PtfmInit,1) - ReKiBuf(Re_Xferred) = InData%PtfmInit(i1) - Re_Xferred = Re_Xferred + 1 - END DO + IF ( .NOT. ALLOCATED(InData%PtfmInit) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%PtfmInit,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PtfmInit,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%PtfmInit,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PtfmInit,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%PtfmInit,2), UBOUND(InData%PtfmInit,2) + DO i1 = LBOUND(InData%PtfmInit,1), UBOUND(InData%PtfmInit,1) + ReKiBuf(Re_Xferred) = InData%PtfmInit(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IntKiBuf(Int_Xferred) = InData%FarmSize + Int_Xferred = Int_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%TurbineRefPos) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%TurbineRefPos,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%TurbineRefPos,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%TurbineRefPos,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%TurbineRefPos,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%TurbineRefPos,2), UBOUND(InData%TurbineRefPos,2) + DO i1 = LBOUND(InData%TurbineRefPos,1), UBOUND(InData%TurbineRefPos,1) + ReKiBuf(Re_Xferred) = InData%TurbineRefPos(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF DO I = 1, LEN(InData%FileName) IntKiBuf(Int_Xferred) = ICHAR(InData%FileName(I:I), IntKi) Int_Xferred = Int_Xferred + 1 @@ -422,12 +509,54 @@ SUBROUTINE MD_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Err Re_Xferred = Re_Xferred + 1 OutData%WtrDepth = ReKiBuf(Re_Xferred) Re_Xferred = Re_Xferred + 1 - i1_l = LBOUND(OutData%PtfmInit,1) - i1_u = UBOUND(OutData%PtfmInit,1) - DO i1 = LBOUND(OutData%PtfmInit,1), UBOUND(OutData%PtfmInit,1) - OutData%PtfmInit(i1) = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 - END DO + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! PtfmInit not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%PtfmInit)) DEALLOCATE(OutData%PtfmInit) + ALLOCATE(OutData%PtfmInit(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%PtfmInit.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%PtfmInit,2), UBOUND(OutData%PtfmInit,2) + DO i1 = LBOUND(OutData%PtfmInit,1), UBOUND(OutData%PtfmInit,1) + OutData%PtfmInit(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + OutData%FarmSize = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! TurbineRefPos not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%TurbineRefPos)) DEALLOCATE(OutData%TurbineRefPos) + ALLOCATE(OutData%TurbineRefPos(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%TurbineRefPos.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%TurbineRefPos,2), UBOUND(OutData%TurbineRefPos,2) + DO i1 = LBOUND(OutData%TurbineRefPos,1), UBOUND(OutData%TurbineRefPos,1) + OutData%TurbineRefPos(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF DO I = 1, LEN(OutData%FileName) OutData%FileName(I:I) = CHAR(IntKiBuf(Int_Xferred)) Int_Xferred = Int_Xferred + 1 @@ -3533,6 +3662,7 @@ SUBROUTINE MD_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) ! Local INTEGER(IntKi) :: i,j,k INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyMisc' @@ -3590,8 +3720,10 @@ SUBROUTINE MD_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) IF (ALLOCATED(SrcMiscData%FairIdList)) THEN i1_l = LBOUND(SrcMiscData%FairIdList,1) i1_u = UBOUND(SrcMiscData%FairIdList,1) + i2_l = LBOUND(SrcMiscData%FairIdList,2) + i2_u = UBOUND(SrcMiscData%FairIdList,2) IF (.NOT. ALLOCATED(DstMiscData%FairIdList)) THEN - ALLOCATE(DstMiscData%FairIdList(i1_l:i1_u),STAT=ErrStat2) + ALLOCATE(DstMiscData%FairIdList(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%FairIdList.', ErrStat, ErrMsg,RoutineName) RETURN @@ -3635,6 +3767,7 @@ SUBROUTINE MD_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) END IF DstMiscData%MDWrOutput = SrcMiscData%MDWrOutput ENDIF + DstMiscData%PtfmInit = SrcMiscData%PtfmInit END SUBROUTINE MD_CopyMisc SUBROUTINE MD_DestroyMisc( MiscData, ErrStat, ErrMsg ) @@ -3785,7 +3918,7 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz END IF Int_BufSz = Int_BufSz + 1 ! FairIdList allocated yes/no IF ( ALLOCATED(InData%FairIdList) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! FairIdList upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + 2*2 ! FairIdList upper/lower bounds for each dimension Int_BufSz = Int_BufSz + SIZE(InData%FairIdList) ! FairIdList END IF Int_BufSz = Int_BufSz + 1 ! ConnIdList allocated yes/no @@ -3803,6 +3936,7 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_BufSz = Int_BufSz + 2*1 ! MDWrOutput upper/lower bounds for each dimension Re_BufSz = Re_BufSz + SIZE(InData%MDWrOutput) ! MDWrOutput END IF + Re_BufSz = Re_BufSz + SIZE(InData%PtfmInit) ! PtfmInit IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) IF (ErrStat2 /= 0) THEN @@ -3961,11 +4095,16 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_Xferred = Int_Xferred + 1 IntKiBuf( Int_Xferred ) = LBOUND(InData%FairIdList,1) IntKiBuf( Int_Xferred + 1) = UBOUND(InData%FairIdList,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%FairIdList,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%FairIdList,2) Int_Xferred = Int_Xferred + 2 - DO i1 = LBOUND(InData%FairIdList,1), UBOUND(InData%FairIdList,1) - IntKiBuf(Int_Xferred) = InData%FairIdList(i1) - Int_Xferred = Int_Xferred + 1 + DO i2 = LBOUND(InData%FairIdList,2), UBOUND(InData%FairIdList,2) + DO i1 = LBOUND(InData%FairIdList,1), UBOUND(InData%FairIdList,1) + IntKiBuf(Int_Xferred) = InData%FairIdList(i1,i2) + Int_Xferred = Int_Xferred + 1 + END DO END DO END IF IF ( .NOT. ALLOCATED(InData%ConnIdList) ) THEN @@ -4013,6 +4152,10 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Re_Xferred = Re_Xferred + 1 END DO END IF + DO i1 = LBOUND(InData%PtfmInit,1), UBOUND(InData%PtfmInit,1) + ReKiBuf(Re_Xferred) = InData%PtfmInit(i1) + Re_Xferred = Re_Xferred + 1 + END DO END SUBROUTINE MD_PackMisc SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) @@ -4029,6 +4172,7 @@ SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) INTEGER(IntKi) :: Int_Xferred INTEGER(IntKi) :: i INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackMisc' @@ -4217,15 +4361,20 @@ SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) i1_l = IntKiBuf( Int_Xferred ) i1_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 IF (ALLOCATED(OutData%FairIdList)) DEALLOCATE(OutData%FairIdList) - ALLOCATE(OutData%FairIdList(i1_l:i1_u),STAT=ErrStat2) + ALLOCATE(OutData%FairIdList(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%FairIdList.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i1 = LBOUND(OutData%FairIdList,1), UBOUND(OutData%FairIdList,1) - OutData%FairIdList(i1) = IntKiBuf(Int_Xferred) - Int_Xferred = Int_Xferred + 1 + DO i2 = LBOUND(OutData%FairIdList,2), UBOUND(OutData%FairIdList,2) + DO i1 = LBOUND(OutData%FairIdList,1), UBOUND(OutData%FairIdList,1) + OutData%FairIdList(i1,i2) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO END DO END IF IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ConnIdList not allocated @@ -4282,6 +4431,12 @@ SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) Re_Xferred = Re_Xferred + 1 END DO END IF + i1_l = LBOUND(OutData%PtfmInit,1) + i1_u = UBOUND(OutData%PtfmInit,1) + DO i1 = LBOUND(OutData%PtfmInit,1), UBOUND(OutData%PtfmInit,1) + OutData%PtfmInit(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO END SUBROUTINE MD_UnPackMisc SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) @@ -4293,6 +4448,7 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) ! Local INTEGER(IntKi) :: i,j,k INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyParam' @@ -4301,7 +4457,18 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) ErrMsg = "" DstParamData%NTypes = SrcParamData%NTypes DstParamData%NConnects = SrcParamData%NConnects +IF (ALLOCATED(SrcParamData%NFairs)) THEN + i1_l = LBOUND(SrcParamData%NFairs,1) + i1_u = UBOUND(SrcParamData%NFairs,1) + IF (.NOT. ALLOCATED(DstParamData%NFairs)) THEN + ALLOCATE(DstParamData%NFairs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%NFairs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF DstParamData%NFairs = SrcParamData%NFairs +ENDIF DstParamData%NConns = SrcParamData%NConns DstParamData%NAnchs = SrcParamData%NAnchs DstParamData%NLines = SrcParamData%NLines @@ -4332,6 +4499,21 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) ENDIF DstParamData%Delim = SrcParamData%Delim DstParamData%MDUnOut = SrcParamData%MDUnOut + DstParamData%nTurbines = SrcParamData%nTurbines +IF (ALLOCATED(SrcParamData%TurbineRefPos)) THEN + i1_l = LBOUND(SrcParamData%TurbineRefPos,1) + i1_u = UBOUND(SrcParamData%TurbineRefPos,1) + i2_l = LBOUND(SrcParamData%TurbineRefPos,2) + i2_u = UBOUND(SrcParamData%TurbineRefPos,2) + IF (.NOT. ALLOCATED(DstParamData%TurbineRefPos)) THEN + ALLOCATE(DstParamData%TurbineRefPos(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%TurbineRefPos.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%TurbineRefPos = SrcParamData%TurbineRefPos +ENDIF END SUBROUTINE MD_CopyParam SUBROUTINE MD_DestroyParam( ParamData, ErrStat, ErrMsg ) @@ -4343,11 +4525,17 @@ SUBROUTINE MD_DestroyParam( ParamData, ErrStat, ErrMsg ) ! ErrStat = ErrID_None ErrMsg = "" +IF (ALLOCATED(ParamData%NFairs)) THEN + DEALLOCATE(ParamData%NFairs) +ENDIF IF (ALLOCATED(ParamData%OutParam)) THEN DO i1 = LBOUND(ParamData%OutParam,1), UBOUND(ParamData%OutParam,1) CALL MD_Destroyoutparmtype( ParamData%OutParam(i1), ErrStat, ErrMsg ) ENDDO DEALLOCATE(ParamData%OutParam) +ENDIF +IF (ALLOCATED(ParamData%TurbineRefPos)) THEN + DEALLOCATE(ParamData%TurbineRefPos) ENDIF END SUBROUTINE MD_DestroyParam @@ -4388,7 +4576,11 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Int_BufSz = 0 Int_BufSz = Int_BufSz + 1 ! NTypes Int_BufSz = Int_BufSz + 1 ! NConnects - Int_BufSz = Int_BufSz + 1 ! NFairs + Int_BufSz = Int_BufSz + 1 ! NFairs allocated yes/no + IF ( ALLOCATED(InData%NFairs) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! NFairs upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%NFairs) ! NFairs + END IF Int_BufSz = Int_BufSz + 1 ! NConns Int_BufSz = Int_BufSz + 1 ! NAnchs Int_BufSz = Int_BufSz + 1 ! NLines @@ -4427,6 +4619,12 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si END IF Int_BufSz = Int_BufSz + 1*LEN(InData%Delim) ! Delim Int_BufSz = Int_BufSz + 1 ! MDUnOut + Int_BufSz = Int_BufSz + 1 ! nTurbines + Int_BufSz = Int_BufSz + 1 ! TurbineRefPos allocated yes/no + IF ( ALLOCATED(InData%TurbineRefPos) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! TurbineRefPos upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%TurbineRefPos) ! TurbineRefPos + END IF IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) IF (ErrStat2 /= 0) THEN @@ -4458,8 +4656,21 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%NConnects Int_Xferred = Int_Xferred + 1 - IntKiBuf(Int_Xferred) = InData%NFairs + IF ( .NOT. ALLOCATED(InData%NFairs) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%NFairs,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%NFairs,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%NFairs,1), UBOUND(InData%NFairs,1) + IntKiBuf(Int_Xferred) = InData%NFairs(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF IntKiBuf(Int_Xferred) = InData%NConns Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%NAnchs @@ -4533,6 +4744,28 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si END DO ! I IntKiBuf(Int_Xferred) = InData%MDUnOut Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nTurbines + Int_Xferred = Int_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%TurbineRefPos) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%TurbineRefPos,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%TurbineRefPos,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%TurbineRefPos,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%TurbineRefPos,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%TurbineRefPos,2), UBOUND(InData%TurbineRefPos,2) + DO i1 = LBOUND(InData%TurbineRefPos,1), UBOUND(InData%TurbineRefPos,1) + ReKiBuf(Re_Xferred) = InData%TurbineRefPos(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF END SUBROUTINE MD_PackParam SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) @@ -4549,6 +4782,7 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg INTEGER(IntKi) :: Int_Xferred INTEGER(IntKi) :: i INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackParam' @@ -4566,8 +4800,24 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg Int_Xferred = Int_Xferred + 1 OutData%NConnects = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 - OutData%NFairs = IntKiBuf(Int_Xferred) + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! NFairs not allocated + Int_Xferred = Int_Xferred + 1 + ELSE Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%NFairs)) DEALLOCATE(OutData%NFairs) + ALLOCATE(OutData%NFairs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%NFairs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%NFairs,1), UBOUND(OutData%NFairs,1) + OutData%NFairs(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF OutData%NConns = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 OutData%NAnchs = IntKiBuf(Int_Xferred) @@ -4656,6 +4906,31 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg END DO ! I OutData%MDUnOut = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 + OutData%nTurbines = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! TurbineRefPos not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%TurbineRefPos)) DEALLOCATE(OutData%TurbineRefPos) + ALLOCATE(OutData%TurbineRefPos(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%TurbineRefPos.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%TurbineRefPos,2), UBOUND(OutData%TurbineRefPos,2) + DO i1 = LBOUND(OutData%TurbineRefPos,1), UBOUND(OutData%TurbineRefPos,1) + OutData%TurbineRefPos(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF END SUBROUTINE MD_UnPackParam SUBROUTINE MD_CopyInput( SrcInputData, DstInputData, CtrlCode, ErrStat, ErrMsg ) @@ -4673,9 +4948,22 @@ SUBROUTINE MD_CopyInput( SrcInputData, DstInputData, CtrlCode, ErrStat, ErrMsg ) ! ErrStat = ErrID_None ErrMsg = "" - CALL MeshCopy( SrcInputData%PtFairleadDisplacement, DstInputData%PtFairleadDisplacement, CtrlCode, ErrStat2, ErrMsg2 ) +IF (ALLOCATED(SrcInputData%PtFairleadDisplacement)) THEN + i1_l = LBOUND(SrcInputData%PtFairleadDisplacement,1) + i1_u = UBOUND(SrcInputData%PtFairleadDisplacement,1) + IF (.NOT. ALLOCATED(DstInputData%PtFairleadDisplacement)) THEN + ALLOCATE(DstInputData%PtFairleadDisplacement(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInputData%PtFairleadDisplacement.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcInputData%PtFairleadDisplacement,1), UBOUND(SrcInputData%PtFairleadDisplacement,1) + CALL MeshCopy( SrcInputData%PtFairleadDisplacement(i1), DstInputData%PtFairleadDisplacement(i1), CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF IF (ALLOCATED(SrcInputData%DeltaL)) THEN i1_l = LBOUND(SrcInputData%DeltaL,1) i1_u = UBOUND(SrcInputData%DeltaL,1) @@ -4711,7 +4999,12 @@ SUBROUTINE MD_DestroyInput( InputData, ErrStat, ErrMsg ) ! ErrStat = ErrID_None ErrMsg = "" - CALL MeshDestroy( InputData%PtFairleadDisplacement, ErrStat, ErrMsg ) +IF (ALLOCATED(InputData%PtFairleadDisplacement)) THEN +DO i1 = LBOUND(InputData%PtFairleadDisplacement,1), UBOUND(InputData%PtFairleadDisplacement,1) + CALL MeshDestroy( InputData%PtFairleadDisplacement(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(InputData%PtFairleadDisplacement) +ENDIF IF (ALLOCATED(InputData%DeltaL)) THEN DEALLOCATE(InputData%DeltaL) ENDIF @@ -4755,9 +5048,13 @@ SUBROUTINE MD_PackInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Re_BufSz = 0 Db_BufSz = 0 Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! PtFairleadDisplacement allocated yes/no + IF ( ALLOCATED(InData%PtFairleadDisplacement) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! PtFairleadDisplacement upper/lower bounds for each dimension ! Allocate buffers for subtypes, if any (we'll get sizes from these) + DO i1 = LBOUND(InData%PtFairleadDisplacement,1), UBOUND(InData%PtFairleadDisplacement,1) Int_BufSz = Int_BufSz + 3 ! PtFairleadDisplacement: size of buffers for each call to pack subtype - CALL MeshPack( InData%PtFairleadDisplacement, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, .TRUE. ) ! PtFairleadDisplacement + CALL MeshPack( InData%PtFairleadDisplacement(i1), Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, .TRUE. ) ! PtFairleadDisplacement CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -4773,6 +5070,8 @@ SUBROUTINE MD_PackInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Int_BufSz = Int_BufSz + SIZE( Int_Buf ) DEALLOCATE(Int_Buf) END IF + END DO + END IF Int_BufSz = Int_BufSz + 1 ! DeltaL allocated yes/no IF ( ALLOCATED(InData%DeltaL) ) THEN Int_BufSz = Int_BufSz + 2*1 ! DeltaL upper/lower bounds for each dimension @@ -4810,7 +5109,18 @@ SUBROUTINE MD_PackInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Db_Xferred = 1 Int_Xferred = 1 - CALL MeshPack( InData%PtFairleadDisplacement, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, OnlySize ) ! PtFairleadDisplacement + IF ( .NOT. ALLOCATED(InData%PtFairleadDisplacement) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%PtFairleadDisplacement,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PtFairleadDisplacement,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%PtFairleadDisplacement,1), UBOUND(InData%PtFairleadDisplacement,1) + CALL MeshPack( InData%PtFairleadDisplacement(i1), Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, OnlySize ) ! PtFairleadDisplacement CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -4838,6 +5148,8 @@ SUBROUTINE MD_PackInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si ELSE IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 ENDIF + END DO + END IF IF ( .NOT. ALLOCATED(InData%DeltaL) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -4897,6 +5209,20 @@ SUBROUTINE MD_UnPackInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg Re_Xferred = 1 Db_Xferred = 1 Int_Xferred = 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! PtFairleadDisplacement not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%PtFairleadDisplacement)) DEALLOCATE(OutData%PtFairleadDisplacement) + ALLOCATE(OutData%PtFairleadDisplacement(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%PtFairleadDisplacement.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%PtFairleadDisplacement,1), UBOUND(OutData%PtFairleadDisplacement,1) Buf_size=IntKiBuf( Int_Xferred ) Int_Xferred = Int_Xferred + 1 IF(Buf_size > 0) THEN @@ -4930,13 +5256,15 @@ SUBROUTINE MD_UnPackInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) Int_Xferred = Int_Xferred + Buf_size END IF - CALL MeshUnpack( OutData%PtFairleadDisplacement, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2 ) ! PtFairleadDisplacement + CALL MeshUnpack( OutData%PtFairleadDisplacement(i1), Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2 ) ! PtFairleadDisplacement CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! DeltaL not allocated Int_Xferred = Int_Xferred + 1 ELSE @@ -4990,9 +5318,22 @@ SUBROUTINE MD_CopyOutput( SrcOutputData, DstOutputData, CtrlCode, ErrStat, ErrMs ! ErrStat = ErrID_None ErrMsg = "" - CALL MeshCopy( SrcOutputData%PtFairleadLoad, DstOutputData%PtFairleadLoad, CtrlCode, ErrStat2, ErrMsg2 ) +IF (ALLOCATED(SrcOutputData%PtFairleadLoad)) THEN + i1_l = LBOUND(SrcOutputData%PtFairleadLoad,1) + i1_u = UBOUND(SrcOutputData%PtFairleadLoad,1) + IF (.NOT. ALLOCATED(DstOutputData%PtFairleadLoad)) THEN + ALLOCATE(DstOutputData%PtFairleadLoad(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstOutputData%PtFairleadLoad.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcOutputData%PtFairleadLoad,1), UBOUND(SrcOutputData%PtFairleadLoad,1) + CALL MeshCopy( SrcOutputData%PtFairleadLoad(i1), DstOutputData%PtFairleadLoad(i1), CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF IF (ALLOCATED(SrcOutputData%WriteOutput)) THEN i1_l = LBOUND(SrcOutputData%WriteOutput,1) i1_u = UBOUND(SrcOutputData%WriteOutput,1) @@ -5016,7 +5357,12 @@ SUBROUTINE MD_DestroyOutput( OutputData, ErrStat, ErrMsg ) ! ErrStat = ErrID_None ErrMsg = "" - CALL MeshDestroy( OutputData%PtFairleadLoad, ErrStat, ErrMsg ) +IF (ALLOCATED(OutputData%PtFairleadLoad)) THEN +DO i1 = LBOUND(OutputData%PtFairleadLoad,1), UBOUND(OutputData%PtFairleadLoad,1) + CALL MeshDestroy( OutputData%PtFairleadLoad(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(OutputData%PtFairleadLoad) +ENDIF IF (ALLOCATED(OutputData%WriteOutput)) THEN DEALLOCATE(OutputData%WriteOutput) ENDIF @@ -5057,9 +5403,13 @@ SUBROUTINE MD_PackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, S Re_BufSz = 0 Db_BufSz = 0 Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! PtFairleadLoad allocated yes/no + IF ( ALLOCATED(InData%PtFairleadLoad) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! PtFairleadLoad upper/lower bounds for each dimension ! Allocate buffers for subtypes, if any (we'll get sizes from these) + DO i1 = LBOUND(InData%PtFairleadLoad,1), UBOUND(InData%PtFairleadLoad,1) Int_BufSz = Int_BufSz + 3 ! PtFairleadLoad: size of buffers for each call to pack subtype - CALL MeshPack( InData%PtFairleadLoad, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, .TRUE. ) ! PtFairleadLoad + CALL MeshPack( InData%PtFairleadLoad(i1), Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, .TRUE. ) ! PtFairleadLoad CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -5075,6 +5425,8 @@ SUBROUTINE MD_PackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, S Int_BufSz = Int_BufSz + SIZE( Int_Buf ) DEALLOCATE(Int_Buf) END IF + END DO + END IF Int_BufSz = Int_BufSz + 1 ! WriteOutput allocated yes/no IF ( ALLOCATED(InData%WriteOutput) ) THEN Int_BufSz = Int_BufSz + 2*1 ! WriteOutput upper/lower bounds for each dimension @@ -5107,7 +5459,18 @@ SUBROUTINE MD_PackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, S Db_Xferred = 1 Int_Xferred = 1 - CALL MeshPack( InData%PtFairleadLoad, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, OnlySize ) ! PtFairleadLoad + IF ( .NOT. ALLOCATED(InData%PtFairleadLoad) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%PtFairleadLoad,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%PtFairleadLoad,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%PtFairleadLoad,1), UBOUND(InData%PtFairleadLoad,1) + CALL MeshPack( InData%PtFairleadLoad(i1), Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, OnlySize ) ! PtFairleadLoad CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -5135,6 +5498,8 @@ SUBROUTINE MD_PackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, S ELSE IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 ENDIF + END DO + END IF IF ( .NOT. ALLOCATED(InData%WriteOutput) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -5179,6 +5544,20 @@ SUBROUTINE MD_UnPackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg Re_Xferred = 1 Db_Xferred = 1 Int_Xferred = 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! PtFairleadLoad not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%PtFairleadLoad)) DEALLOCATE(OutData%PtFairleadLoad) + ALLOCATE(OutData%PtFairleadLoad(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%PtFairleadLoad.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%PtFairleadLoad,1), UBOUND(OutData%PtFairleadLoad,1) Buf_size=IntKiBuf( Int_Xferred ) Int_Xferred = Int_Xferred + 1 IF(Buf_size > 0) THEN @@ -5212,13 +5591,15 @@ SUBROUTINE MD_UnPackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) Int_Xferred = Int_Xferred + Buf_size END IF - CALL MeshUnpack( OutData%PtFairleadLoad, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2 ) ! PtFairleadLoad + CALL MeshUnpack( OutData%PtFairleadLoad(i1), Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2 ) ! PtFairleadLoad CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WriteOutput not allocated Int_Xferred = Int_Xferred + 1 ELSE @@ -5334,8 +5715,12 @@ SUBROUTINE MD_Input_ExtrapInterp1(u1, u2, tin, u_out, tin_out, ErrStat, ErrMsg ) END IF ScaleFactor = t_out / t(2) - CALL MeshExtrapInterp1(u1%PtFairleadDisplacement, u2%PtFairleadDisplacement, tin, u_out%PtFairleadDisplacement, tin_out, ErrStat2, ErrMsg2 ) +IF (ALLOCATED(u_out%PtFairleadDisplacement) .AND. ALLOCATED(u1%PtFairleadDisplacement)) THEN + DO i1 = LBOUND(u_out%PtFairleadDisplacement,1),UBOUND(u_out%PtFairleadDisplacement,1) + CALL MeshExtrapInterp1(u1%PtFairleadDisplacement(i1), u2%PtFairleadDisplacement(i1), tin, u_out%PtFairleadDisplacement(i1), tin_out, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ENDDO +END IF ! check if allocated IF (ALLOCATED(u_out%DeltaL) .AND. ALLOCATED(u1%DeltaL)) THEN DO i1 = LBOUND(u_out%DeltaL,1),UBOUND(u_out%DeltaL,1) b = -(u1%DeltaL(i1) - u2%DeltaL(i1)) @@ -5405,8 +5790,12 @@ SUBROUTINE MD_Input_ExtrapInterp2(u1, u2, u3, tin, u_out, tin_out, ErrStat, ErrM END IF ScaleFactor = t_out / (t(2) * t(3) * (t(2) - t(3))) - CALL MeshExtrapInterp2(u1%PtFairleadDisplacement, u2%PtFairleadDisplacement, u3%PtFairleadDisplacement, tin, u_out%PtFairleadDisplacement, tin_out, ErrStat2, ErrMsg2 ) +IF (ALLOCATED(u_out%PtFairleadDisplacement) .AND. ALLOCATED(u1%PtFairleadDisplacement)) THEN + DO i1 = LBOUND(u_out%PtFairleadDisplacement,1),UBOUND(u_out%PtFairleadDisplacement,1) + CALL MeshExtrapInterp2(u1%PtFairleadDisplacement(i1), u2%PtFairleadDisplacement(i1), u3%PtFairleadDisplacement(i1), tin, u_out%PtFairleadDisplacement(i1), tin_out, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ENDDO +END IF ! check if allocated IF (ALLOCATED(u_out%DeltaL) .AND. ALLOCATED(u1%DeltaL)) THEN DO i1 = LBOUND(u_out%DeltaL,1),UBOUND(u_out%DeltaL,1) b = (t(3)**2*(u1%DeltaL(i1) - u2%DeltaL(i1)) + t(2)**2*(-u1%DeltaL(i1) + u3%DeltaL(i1)))* scaleFactor @@ -5518,8 +5907,12 @@ SUBROUTINE MD_Output_ExtrapInterp1(y1, y2, tin, y_out, tin_out, ErrStat, ErrMsg END IF ScaleFactor = t_out / t(2) - CALL MeshExtrapInterp1(y1%PtFairleadLoad, y2%PtFairleadLoad, tin, y_out%PtFairleadLoad, tin_out, ErrStat2, ErrMsg2 ) +IF (ALLOCATED(y_out%PtFairleadLoad) .AND. ALLOCATED(y1%PtFairleadLoad)) THEN + DO i1 = LBOUND(y_out%PtFairleadLoad,1),UBOUND(y_out%PtFairleadLoad,1) + CALL MeshExtrapInterp1(y1%PtFairleadLoad(i1), y2%PtFairleadLoad(i1), tin, y_out%PtFairleadLoad(i1), tin_out, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ENDDO +END IF ! check if allocated IF (ALLOCATED(y_out%WriteOutput) .AND. ALLOCATED(y1%WriteOutput)) THEN DO i1 = LBOUND(y_out%WriteOutput,1),UBOUND(y_out%WriteOutput,1) b = -(y1%WriteOutput(i1) - y2%WriteOutput(i1)) @@ -5583,8 +5976,12 @@ SUBROUTINE MD_Output_ExtrapInterp2(y1, y2, y3, tin, y_out, tin_out, ErrStat, Err END IF ScaleFactor = t_out / (t(2) * t(3) * (t(2) - t(3))) - CALL MeshExtrapInterp2(y1%PtFairleadLoad, y2%PtFairleadLoad, y3%PtFairleadLoad, tin, y_out%PtFairleadLoad, tin_out, ErrStat2, ErrMsg2 ) +IF (ALLOCATED(y_out%PtFairleadLoad) .AND. ALLOCATED(y1%PtFairleadLoad)) THEN + DO i1 = LBOUND(y_out%PtFairleadLoad,1),UBOUND(y_out%PtFairleadLoad,1) + CALL MeshExtrapInterp2(y1%PtFairleadLoad(i1), y2%PtFairleadLoad(i1), y3%PtFairleadLoad(i1), tin, y_out%PtFairleadLoad(i1), tin_out, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ENDDO +END IF ! check if allocated IF (ALLOCATED(y_out%WriteOutput) .AND. ALLOCATED(y1%WriteOutput)) THEN DO i1 = LBOUND(y_out%WriteOutput,1),UBOUND(y_out%WriteOutput,1) b = (t(3)**2*(y1%WriteOutput(i1) - y2%WriteOutput(i1)) + t(2)**2*(-y1%WriteOutput(i1) + y3%WriteOutput(i1)))* scaleFactor diff --git a/modules/openfast-library/src/FAST_Solver.f90 b/modules/openfast-library/src/FAST_Solver.f90 index e8768bc046..c123895542 100644 --- a/modules/openfast-library/src/FAST_Solver.f90 +++ b/modules/openfast-library/src/FAST_Solver.f90 @@ -1351,7 +1351,7 @@ SUBROUTINE Transfer_ED_to_HD_SD_BD_Mooring( p_FAST, y_ED, u_HD, u_SD, u_ExtPtfm, ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN ! motions: - CALL Transfer_Point_to_Point( y_ED%PlatformPtMesh, u_MD%PtFairleadDisplacement, MeshMapData%ED_P_2_Mooring_P, ErrStat2, ErrMsg2 ) + CALL Transfer_Point_to_Point( y_ED%PlatformPtMesh, u_MD%PtFairleadDisplacement(1), MeshMapData%ED_P_2_Mooring_P, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat, ErrMsg,RoutineName//'u_MD%PtFairleadDisplacement' ) ELSEIF ( p_FAST%CompMooring == Module_FEAM ) THEN @@ -2001,10 +2001,10 @@ SUBROUTINE U_ED_HD_Residual( y_ED2, y_HD2, u_IN, U_Resid) ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN ! note: MD_InputSolve must be called before setting ED loads inputs (so that motions are known for loads [moment] mapping) - CALL Transfer_Point_to_Point( y_ED2%PlatformPtMesh, u_MD%PtFairleadDisplacement, MeshMapData%ED_P_2_Mooring_P, ErrStat, ErrMsg ) + CALL Transfer_Point_to_Point( y_ED2%PlatformPtMesh, u_MD%PtFairleadDisplacement(1), MeshMapData%ED_P_2_Mooring_P, ErrStat, ErrMsg ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - CALL Transfer_Point_to_Point( y_MD%PtFairleadLoad, MeshMapData%u_ED_PlatformPtMesh, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2, u_MD%PtFairleadDisplacement, PlatformMotions ) !u_MD and y_ED contain the displacements needed for moment calculations + CALL Transfer_Point_to_Point( y_MD%PtFairleadLoad(1), MeshMapData%u_ED_PlatformPtMesh, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2, u_MD%PtFairleadDisplacement(1), PlatformMotions ) !u_MD and y_ED contain the displacements needed for moment calculations CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) ELSEIF ( p_FAST%CompMooring == Module_FEAM ) THEN @@ -2934,10 +2934,10 @@ SUBROUTINE U_FullOpt1_Residual( y_ED2, y_SD2, y_HD2, y_BD2, y_Orca2, y_ExtPtfm2, ! note: MD_InputSolve must be called before setting ED loads inputs (so that motions are known for loads [moment] mapping) if ( p_FAST%CompSub == Module_SD ) then - CALL Transfer_Point_to_Point( y_SD2%y2Mesh, u_MD%PtFairleadDisplacement, MeshMapData%SD_P_2_Mooring_P, ErrStat, ErrMsg ) + CALL Transfer_Point_to_Point( y_SD2%y2Mesh, u_MD%PtFairleadDisplacement(1), MeshMapData%SD_P_2_Mooring_P, ErrStat, ErrMsg ) CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) else - CALL Transfer_Point_to_Point( y_ED2%PlatformPtMesh, u_MD%PtFairleadDisplacement, MeshMapData%ED_P_2_Mooring_P, ErrStat, ErrMsg ) + CALL Transfer_Point_to_Point( y_ED2%PlatformPtMesh, u_MD%PtFairleadDisplacement(1), MeshMapData%ED_P_2_Mooring_P, ErrStat, ErrMsg ) CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) end if @@ -3223,13 +3223,13 @@ SUBROUTINE U_FullOpt1_Residual( y_ED2, y_SD2, y_HD2, y_BD2, y_Orca2, y_ExtPtfm2, ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN if ( p_FAST%CompSub == Module_SD ) then - CALL Transfer_Point_to_Point( y_MD%PtFairleadLoad, MeshMapData%u_SD_LMesh_2, MeshMapData%Mooring_P_2_SD_P, ErrStat2, ErrMsg2, u_MD%PtFairleadDisplacement, y_SD2%Y2Mesh ) !u_MD and y_SD contain the displacements needed for moment calculations + CALL Transfer_Point_to_Point( y_MD%PtFairleadLoad(1), MeshMapData%u_SD_LMesh_2, MeshMapData%Mooring_P_2_SD_P, ErrStat2, ErrMsg2, u_MD%PtFairleadDisplacement(1), y_SD2%Y2Mesh ) !u_MD and y_SD contain the displacements needed for moment calculations CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) MeshMapData%u_SD_LMesh%Force = MeshMapData%u_SD_LMesh%Force + MeshMapData%u_SD_LMesh_2%Force MeshMapData%u_SD_LMesh%Moment = MeshMapData%u_SD_LMesh%Moment + MeshMapData%u_SD_LMesh_2%Moment else - CALL Transfer_Point_to_Point( y_MD%PtFairleadLoad, MeshMapData%u_ED_PlatformPtMesh_2, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2, u_MD%PtFairleadDisplacement, PlatformMotions ) !u_MD and y_ED contain the displacements needed for moment calculations + CALL Transfer_Point_to_Point( y_MD%PtFairleadLoad(1), MeshMapData%u_ED_PlatformPtMesh_2, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2, u_MD%PtFairleadDisplacement(1), PlatformMotions ) !u_MD and y_ED contain the displacements needed for moment calculations CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) MeshMapData%u_ED_PlatformPtMesh%Force = MeshMapData%u_ED_PlatformPtMesh%Force + MeshMapData%u_ED_PlatformPtMesh_2%Force @@ -4205,8 +4205,8 @@ SUBROUTINE ResetRemapFlags(p_FAST, ED, BD, AD14, AD, HD, SD, ExtPtfm, SrvD, MAPp MAPp%Input(1)%PtFairDisplacement%RemapFlag = .FALSE. MAPp%y%PtFairleadLoad%RemapFlag = .FALSE. ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN - MD%Input(1)%PtFairleadDisplacement%RemapFlag = .FALSE. - MD%y%PtFairleadLoad%RemapFlag = .FALSE. + MD%Input(1)%PtFairleadDisplacement(1)%RemapFlag = .FALSE. + MD%y%PtFairleadLoad(1)%RemapFlag = .FALSE. ELSEIF ( p_FAST%CompMooring == Module_FEAM ) THEN FEAM%Input(1)%PtFairleadDisplacement%RemapFlag = .FALSE. FEAM%y%PtFairleadLoad%RemapFlag = .FALSE. @@ -4735,18 +4735,18 @@ SUBROUTINE InitModuleMappings(p_FAST, ED, BD, AD14, AD, HD, SD, ExtPtfm, SrvD, M ! SubDyn <-> MoorDyn !------------------------- ! MoorDyn point mesh to/from SubDyn point mesh - CALL MeshMapCreate( MD%y%PtFairleadLoad, SD%Input(1)%LMesh, MeshMapData%Mooring_P_2_SD_P, ErrStat2, ErrMsg2 ) + CALL MeshMapCreate( MD%y%PtFairleadLoad(1), SD%Input(1)%LMesh, MeshMapData%Mooring_P_2_SD_P, ErrStat2, ErrMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':Mooring_P_2_SD_P' ) - CALL MeshMapCreate( SD%y%y2Mesh, MD%Input(1)%PtFairleadDisplacement, MeshMapData%SD_P_2_Mooring_P, ErrStat2, ErrMsg2 ) + CALL MeshMapCreate( SD%y%y2Mesh, MD%Input(1)%PtFairleadDisplacement(1), MeshMapData%SD_P_2_Mooring_P, ErrStat2, ErrMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':SD_P_2_Mooring_P' ) ELSE !------------------------- ! ElastoDyn <-> MoorDyn !------------------------- ! MoorDyn point mesh to/from ElastoDyn point mesh - CALL MeshMapCreate( MD%y%PtFairleadLoad, PlatformLoads, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2 ) + CALL MeshMapCreate( MD%y%PtFairleadLoad(1), PlatformLoads, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':Mooring_P_2_Ptfm' ) - CALL MeshMapCreate( PlatformMotion, MD%Input(1)%PtFairleadDisplacement, MeshMapData%ED_P_2_Mooring_P, ErrStat2, ErrMsg2 ) + CALL MeshMapCreate( PlatformMotion, MD%Input(1)%PtFairleadDisplacement(1), MeshMapData%ED_P_2_Mooring_P, ErrStat2, ErrMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':Ptfm_2_Mooring_P' ) END IF ! p_FAST%CompSub == Module_SD @@ -5065,7 +5065,7 @@ SUBROUTINE CalcOutputs_And_SolveForInputs( n_t_global, this_time, this_state, ca CALL Transfer_Point_to_Point( SD%y%y2Mesh, MAPp%Input(1)%PtFairDisplacement, MeshMapData%SD_P_2_Mooring_P, ErrStat, ErrMsg ) CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN - CALL Transfer_Point_to_Point( SD%y%y2Mesh, MD%Input(1)%PtFairleadDisplacement, MeshMapData%SD_P_2_Mooring_P, ErrStat, ErrMsg ) + CALL Transfer_Point_to_Point( SD%y%y2Mesh, MD%Input(1)%PtFairleadDisplacement(1), MeshMapData%SD_P_2_Mooring_P, ErrStat, ErrMsg ) CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) ELSEIF ( p_FAST%CompMooring == Module_FEAM ) THEN CALL Transfer_Point_to_Point( SD%y%y2Mesh, FEAM%Input(1)%PtFairleadDisplacement, MeshMapData%SD_P_2_Mooring_P, ErrStat, ErrMsg ) @@ -5282,10 +5282,10 @@ SUBROUTINE SolveOption1(this_time, this_state, calcJacobian, p_FAST, ED, BD, HD, ! note: MD_InputSolve must be called before setting ED loads inputs (so that motions are known for loads [moment] mapping) if ( p_FAST%CompSub == Module_SD ) then - CALL Transfer_Point_to_Point( SD%y%y2Mesh, MD%Input(1)%PtFairleadDisplacement, MeshMapData%SD_P_2_Mooring_P, ErrStat, ErrMsg ) + CALL Transfer_Point_to_Point( SD%y%y2Mesh, MD%Input(1)%PtFairleadDisplacement(1), MeshMapData%SD_P_2_Mooring_P, ErrStat, ErrMsg ) CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) else - CALL Transfer_Point_to_Point( ED%y%PlatformPtMesh, MD%Input(1)%PtFairleadDisplacement, MeshMapData%ED_P_2_Mooring_P, ErrStat, ErrMsg ) + CALL Transfer_Point_to_Point( ED%y%PlatformPtMesh, MD%Input(1)%PtFairleadDisplacement(1), MeshMapData%ED_P_2_Mooring_P, ErrStat, ErrMsg ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) endif diff --git a/modules/openfast-library/src/FAST_Subs.f90 b/modules/openfast-library/src/FAST_Subs.f90 index 1ec4d04608..d9a94e77e0 100644 --- a/modules/openfast-library/src/FAST_Subs.f90 +++ b/modules/openfast-library/src/FAST_Subs.f90 @@ -31,7 +31,7 @@ MODULE FAST_Subs !++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ! INITIALIZATION ROUTINES !++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -!> a wrapper routine to call FAST_Initialize a the full-turbine simulation level (makes easier to write top-level driver) +!> a wrapper routine to call FAST_Initialize at the full-turbine simulation level (makes easier to write top-level driver) SUBROUTINE FAST_InitializeAll_T( t_initial, TurbID, Turbine, ErrStat, ErrMsg, InFile, ExternInitData ) REAL(DbKi), INTENT(IN ) :: t_initial !< initial time @@ -1050,11 +1050,21 @@ SUBROUTINE FAST_InitializeAll( t_initial, p_FAST, y_FAST, m_FAST, ED, BD, SrvD, ! initialize MoorDyn ! ........................ ELSEIF (p_FAST%CompMooring == Module_MD) THEN + + ! some new allocations needed with version that's compatible with farm-level use + ALLOCATE( Init%InData_MD%PtfmInit(6,1), Init%InData_MD%TurbineRefPos(3,1), STAT = ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal,"Error allocating MoorDyn PtfmInit and TurbineRefPos initialization inputs.",ErrStat,ErrMsg,RoutineName) + CALL Cleanup() + RETURN + END IF Init%InData_MD%FileName = p_FAST%MooringFile ! This needs to be set according to what is in the FAST input file. Init%InData_MD%RootName = p_FAST%OutFileRoot - Init%InData_MD%PtfmInit = Init%OutData_ED%PlatformPos !ED%x(STATE_CURR)%QT(1:6) ! initial position of the platform !bjj: this should come from Init%OutData_ED, not x_ED + Init%InData_MD%PtfmInit(:,1) = Init%OutData_ED%PlatformPos ! initial position of the platform (when a FAST module, MoorDyn just takes one row in this matrix) + Init%InData_MD%FarmSize = 0 ! 0 here indicates normal FAST module use of MoorDyn, for a single turbine + Init%InData_MD%TurbineRefPos(:,1) = 0.0_DbKi ! for normal FAST use, the global reference frame is at 0,0,0 Init%InData_MD%g = Init%OutData_ED%Gravity ! This need to be according to g used in ElastoDyn Init%InData_MD%rhoW = Init%OutData_HD%WtrDens ! This needs to be set according to seawater density in HydroDyn Init%InData_MD%WtrDepth = Init%OutData_HD%WtrDpth ! This need to be set according to the water depth in HydroDyn @@ -5336,7 +5346,7 @@ SUBROUTINE WrVTK_AllMeshes(p_FAST, y_FAST, MeshMapData, ED, BD, AD, IfW, OpFM, H ! MoorDyn ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN if (allocated(MD%Input)) then - call MeshWrVTK(p_FAST%TurbinePos, MD%y%PtFairleadLoad, trim(p_FAST%VTK_OutFileRoot)//'.MD_PtFairlead', y_FAST%VTK_count, p_FAST%VTK_fields, ErrStat2, ErrMsg2, p_FAST%VTK_tWidth, MD%Input(1)%PtFairleadDisplacement ) + call MeshWrVTK(p_FAST%TurbinePos, MD%y%PtFairleadLoad(1), trim(p_FAST%VTK_OutFileRoot)//'.MD_PtFairlead', y_FAST%VTK_count, p_FAST%VTK_fields, ErrStat2, ErrMsg2, p_FAST%VTK_tWidth, MD%Input(1)%PtFairleadDisplacement(1) ) !call MeshWrVTK(p_FAST%TurbinePos, MD%Input(1)%PtFairleadDisplacement, trim(p_FAST%VTK_OutFileRoot)//'.MD_PtFair_motion', y_FAST%VTK_count, p_FAST%VTK_fields, ErrStat2, ErrMsg2, p_FAST%VTK_tWidth ) end if diff --git a/modules/openfast-library/src/FAST_Types.f90 b/modules/openfast-library/src/FAST_Types.f90 index 31d99e3110..79d6a691c3 100644 --- a/modules/openfast-library/src/FAST_Types.f90 +++ b/modules/openfast-library/src/FAST_Types.f90 @@ -161,7 +161,7 @@ MODULE FAST_Types INTEGER(IntKi) :: CompIce !< Compute ice loading (switch) {Module_None; Module_IceF, Module_IceD} [-] LOGICAL :: UseDWM !< Use the DWM module in AeroDyn [-] LOGICAL :: Linearize !< Linearization analysis (flag) [-] - INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 1: use individual HydroDyn inputs without adjustment, 2: adjust wave phases based on turbine offsets from farm origin [-] + INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 0: use individual HydroDyn inputs without adjustment, 1: adjust wave phases based on turbine offsets from farm origin [-] CHARACTER(1024) :: EDFile !< The name of the ElastoDyn input file [-] CHARACTER(1024) , DIMENSION(MaxNBlades) :: BDBldFile !< Name of files containing BeamDyn inputs for each blade [-] CHARACTER(1024) :: InflowFile !< Name of file containing inflow wind input parameters [-] @@ -754,7 +754,7 @@ MODULE FAST_Types LOGICAL :: LidRadialVel !< TRUE => return radial component, FALSE => return 'x' direction estimate [-] INTEGER(IntKi) :: TurbineID = 0 !< ID number for turbine (used to create output file naming convention) [-] REAL(ReKi) , DIMENSION(1:3) :: TurbinePos !< Initial position of turbine base (origin used for graphics or in FAST.Farm) [m] - INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 1: use individual HydroDyn inputs without adjustment, 2: adjust wave phases based on turbine offsets from farm origin [-] + INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 0: use individual HydroDyn inputs without adjustment, 1: adjust wave phases based on turbine offsets from farm origin [-] INTEGER(IntKi) :: NumSC2CtrlGlob !< number of global controller inputs [from supercontroller] [-] INTEGER(IntKi) :: NumSC2Ctrl !< number of turbine specific controller inputs [from supercontroller] [-] INTEGER(IntKi) :: NumCtrl2SC !< number of controller outputs [to supercontroller] [-] From e4f93cfbf295f4049e8ae177515473c1d64e9104 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Mon, 29 Mar 2021 09:45:56 -0600 Subject: [PATCH 015/242] Fix routineName of Farm_InitMD --- glue-codes/fast-farm/src/FAST_Farm_Subs.f90 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 index 096a0275f1..b169c5de87 100644 --- a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 +++ b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 @@ -1705,7 +1705,7 @@ SUBROUTINE Farm_InitMD( farm, ErrStat, ErrMsg ) INTEGER(IntKi) :: nt ! loop counter for rotor number INTEGER(IntKi) :: ErrStat2 ! Temporary Error status CHARACTER(ErrMsgLen) :: ErrMsg2 ! Temporary Error message - CHARACTER(*), PARAMETER :: RoutineName = 'Farm_InitFAST' + CHARACTER(*), PARAMETER :: RoutineName = 'Farm_InitMD' ErrStat = ErrID_None From 7d126cdd253cae35cb771850635b07c9b89e1106 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Mon, 29 Mar 2021 15:35:40 -0600 Subject: [PATCH 016/242] Handling uncoupled turbines with a dummy node in the MD meshes --- glue-codes/fast-farm/src/FAST_Farm_Subs.f90 | 69 ++++++++++++--------- modules/moordyn/src/MoorDyn.f90 | 14 ++++- 2 files changed, 51 insertions(+), 32 deletions(-) diff --git a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 index b169c5de87..3c52f219ad 100644 --- a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 +++ b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 @@ -1802,7 +1802,9 @@ SUBROUTINE Farm_InitMD( farm, ErrStat, ErrMsg ) END IF ! MoorDyn point mesh to/from ElastoDyn (or SubDyn) point mesh - do nt = 1,farm%p%NumTurbines + do nt = 1,farm%p%NumTurbines + !if (farm%MD%p%NFairs(nt) > 0 ) then ! only set up a mesh map if MoorDyn has connections to this turbine + ! loads CALL MeshMapCreate( farm%MD%y%PtFairleadLoad(nt), & farm%FWrap(nt)%m%Turbine%ED%Input(1)%PlatformPtMesh, farm%m%MD_2_FWrap(nt), ErrStat2, ErrMsg2 ) @@ -1826,7 +1828,7 @@ SUBROUTINE Farm_InitMD( farm, ErrStat, ErrMsg ) ! farm%MD%u(1)%PtFairleadDisplacement(nt), farm%m%FWrap_2_MD, ErrStat2, ErrMsg2 ) ! !CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':FWrap_2_MD' ) - + !end if end do @@ -1881,14 +1883,16 @@ subroutine FARM_MD_Increment(t, n, farm, ErrStat, ErrMsg) ! ----- map substructure kinematics to MoorDyn inputs ----- (from mapping called at start of CalcOutputs Solve INputs) do nt = 1,farm%p%NumTurbines - - CALL Transfer_Point_to_Point( farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh, farm%MD%u(1)%PtFairleadDisplacement(nt), & - farm%m%FWrap_2_MD(nt), ErrStat2, ErrMsg2 ) - - CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat, ErrMsg,RoutineName//'u_MD%PtFairleadDisplacement' ) - - ! SubDyn alternative - !CALL Transfer_Point_to_Point( farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh, farm%MD%u(1)%PtFairleadDisplacement(nt), farm%m%FWrap_2_MD(nt), ErrStat, ErrMsg ) + !if (farm%MD%p%NFairs(nt) > 0 ) then + + CALL Transfer_Point_to_Point( farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh, farm%MD%u(1)%PtFairleadDisplacement(nt), & + farm%m%FWrap_2_MD(nt), ErrStat2, ErrMsg2 ) + + CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat, ErrMsg,RoutineName//'u_MD%PtFairleadDisplacement' ) + + ! SubDyn alternative + !CALL Transfer_Point_to_Point( farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh, farm%MD%u(1)%PtFairleadDisplacement(nt), farm%m%FWrap_2_MD(nt), ErrStat, ErrMsg ) + !end if end do @@ -1908,27 +1912,30 @@ subroutine FARM_MD_Increment(t, n, farm, ErrStat, ErrMsg) ! ----- map MD load outputs to each turbine's substructure ----- (taken from U FullOpt1...) do nt = 1,farm%p%NumTurbines - ! mapping - CALL Transfer_Point_to_Point( farm%MD%y%PtFairleadLoad(nt), farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_2, & - farm%m%MD_2_FWrap(nt), ErrStat2, ErrMsg2, & - farm%MD%u(1)%PtFairleadDisplacement(nt), farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh ) !u_MD and y_ED contain the displacements needed for moment calculations - - CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) + if (farm%MD%p%NFairs(nt) > 0 ) then ! only map loads if MoorDyn has connections to this turbine + + ! mapping + CALL Transfer_Point_to_Point( farm%MD%y%PtFairleadLoad(nt), farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_2, & + farm%m%MD_2_FWrap(nt), ErrStat2, ErrMsg2, & + farm%MD%u(1)%PtFairleadDisplacement(nt), farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh ) !u_MD and y_ED contain the displacements needed for moment calculations - ! adding to ElastoDyn mesh that will be a future input (I'm assuming MeshMapData%u_ED_PlatformPtMesh makes its way into ElastoDyn inputs somehow) - farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh%Force = farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh%Force & - + farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_2%Force - - farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh%Moment = farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh%Moment & - + farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_2%Moment - - ! SubDyn alternative - !CALL Transfer_Point_to_Point( farm%MD%y%PtFairleadLoad(nt), farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2, & - ! farm%m%MD_2_FWrap(nt), ErrStat2, ErrMsg2, & - ! farm%MD%u(1)%PtFairleadDisplacement(nt), farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh ) !u_MD and y_SD contain the displacements needed for moment calculations - ! - !farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Force = farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Force + farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2%Force - !farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Moment = farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Moment + farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2%Moment + CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) + + ! adding to ElastoDyn mesh that will be a future input (I'm assuming MeshMapData%u_ED_PlatformPtMesh makes its way into ElastoDyn inputs somehow) + farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh%Force = farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh%Force & + + farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_2%Force + + farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh%Moment = farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh%Moment & + + farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_2%Moment + + ! SubDyn alternative + !CALL Transfer_Point_to_Point( farm%MD%y%PtFairleadLoad(nt), farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2, & + ! farm%m%MD_2_FWrap(nt), ErrStat2, ErrMsg2, & + ! farm%MD%u(1)%PtFairleadDisplacement(nt), farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh ) !u_MD and y_SD contain the displacements needed for moment calculations + ! + !farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Force = farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Force + farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2%Force + !farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Moment = farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Moment + farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2%Moment + end if end do end subroutine Farm_MD_Increment !---------------------------------------------------------------------------------------------------------------------------------- @@ -2222,7 +2229,7 @@ subroutine FARM_UpdateStates(t, n, farm, ErrStat, ErrMsg) ! A nested parallel for loop to call each instance of OpenFAST in parallel !$OMP PARALLEL DO DEFAULT(Shared) Private(nt) - DO nt = 1,farm%p%NumTurbines+1 + DO nt = 1,farm%p%NumTurbines call FWrap_Increment( t2, n_FMD, farm%FWrap(nt)%u, farm%FWrap(nt)%p, farm%FWrap(nt)%x, farm%FWrap(nt)%xd, farm%FWrap(nt)%z, & farm%FWrap(nt)%OtherSt, farm%FWrap(nt)%y, farm%FWrap(nt)%m, ErrStatF(nt), ErrMsgF(nt) ) END DO diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 1bf23a0b4d..394c886dee 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -282,10 +282,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! Go through each turbine and set up its mesh and initial fairlead positions DO iTurb = 1,p%nTurbines + ! Always have at least one node (it will be a dummy node if no fairleads are attached) + K = p%NFairs(iTurb) + if (K == 0) K = 1 + ! create input mesh for fairlead kinematics CALL MeshCreate(BlankMesh=u%PtFairleadDisplacement(iTurb) , & IOS= COMPONENT_INPUT , & - Nnodes=p%NFairs(iTurb) , & + Nnodes= K , & TranslationDisp=.TRUE. , & TranslationVel=.TRUE. , & ErrStat=ErrStat2 , & @@ -335,6 +339,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er END DO ! I + ! add a single dummy element for turbines that aren't coupled with, to keep I/O interp/extrap routines happy + if (p%NFairs(iTurb) == 0) then + rPos = 0.0_DbKi ! position at PRP + CALL MeshPositionNode(u%PtFairleadDisplacement(iTurb), 1, rPos, ErrStat2, ErrMsg2) + CALL MeshConstructElement(u%PtFairleadDisplacement(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, 1) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN + end if CALL MeshCommit ( u%PtFairleadDisplacement(iTurb), ErrStat, ErrMsg ) From 2fd0973f5cfac785c64e0a2430c89f2f7a6b16c3 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Mon, 29 Mar 2021 17:27:09 -0600 Subject: [PATCH 017/242] Fixed up MD input prep in FAST.Farm --- .../fast-farm/src/FAST_Farm_Registry.txt | 5 +- glue-codes/fast-farm/src/FAST_Farm_Subs.f90 | 53 +++-- glue-codes/fast-farm/src/FAST_Farm_Types.f90 | 224 ++++++++++++++++-- 3 files changed, 231 insertions(+), 51 deletions(-) diff --git a/glue-codes/fast-farm/src/FAST_Farm_Registry.txt b/glue-codes/fast-farm/src/FAST_Farm_Registry.txt index 9a2e6ef3b8..9f595b55c5 100644 --- a/glue-codes/fast-farm/src/FAST_Farm_Registry.txt +++ b/glue-codes/fast-farm/src/FAST_Farm_Registry.txt @@ -138,8 +138,9 @@ typedef ^ ^ MD_DiscreteStateType xd - - typedef ^ ^ MD_ConstraintStateType z - - - "Constraint states" typedef ^ ^ MD_OtherStateType OtherSt - - - "Other states" typedef ^ ^ MD_ParameterType p - - - "Parameters" -typedef ^ ^ MD_InputType u {2} - - "System inputs" -typedef ^ ^ DbKi utimes {2} - - "Current time" s +typedef ^ ^ MD_InputType u - - - "Extrapolated system inputs" +typedef ^ ^ MD_InputType Input {:} - - "System inputs" +typedef ^ ^ DbKi InputTimes {:} - - "Current time" s typedef ^ ^ MD_OutputType y - - - "System outputs" typedef ^ ^ MD_MiscVarType m - - - "Misc/optimization variables" typedef ^ ^ logical IsInitialized - .FALSE. - "Has MD_Init been called" diff --git a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 index 3c52f219ad..33dff9a311 100644 --- a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 +++ b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 @@ -1768,16 +1768,16 @@ SUBROUTINE Farm_InitMD( farm, ErrStat, ErrMsg ) MD_InitInp%WtrDepth = 0.0 - !! allocate MoorDyn inputs (assuming size 2 for linear interpolation/extrapolation... > - !ALLOCATE( farm%MD%Input( 2 ), farm%MD%InputTimes( 2 ), STAT = ErrStat2 ) - !IF (ErrStat2 /= 0) THEN - ! CALL SetErrStat(ErrID_Fatal,"Error allocating MD%Input and MD%InputTimes.",ErrStat,ErrMsg,RoutineName) - ! CALL Cleanup() - ! RETURN - !END IF + ! allocate MoorDyn inputs (assuming size 2 for linear interpolation/extrapolation... > + ALLOCATE( farm%MD%Input( 2 ), farm%MD%InputTimes( 2 ), STAT = ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal,"Error allocating MD%Input and MD%InputTimes.",ErrStat,ErrMsg,RoutineName) + CALL Cleanup() + RETURN + END IF ! initialize MoorDyn - CALL MD_Init( MD_InitInp, farm%MD%u(1), farm%MD%p, farm%MD%x, farm%MD%xd, farm%MD%z, & + CALL MD_Init( MD_InitInp, farm%MD%Input(1), farm%MD%p, farm%MD%x, farm%MD%xd, farm%MD%z, & farm%MD%OtherSt, farm%MD%y, farm%MD%m, farm%p%DT_mooring, MD_InitOut, ErrStat2, ErrMsg2 ) farm%MD%IsInitialized = .true. @@ -1789,6 +1789,15 @@ SUBROUTINE Farm_InitMD( farm, ErrStat, ErrMsg ) end if + ! Copy MD inputs over into the 2nd entry of the input array, to allow the first extrapolation in FARM_MD_Increment + CALL MD_CopyInput (farm%MD%Input(1), farm%MD%Input(2), MESH_NEWCOPY, Errstat2, ErrMsg2) + CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName ) + farm%MD%InputTimes(2) = -0.1_DbKi + + CALL MD_CopyInput (farm%MD%Input(1), farm%MD%u, MESH_NEWCOPY, Errstat2, ErrMsg2) ! do this to initialize meshes/allocatable arrays for output of ExtrapInterp routine + CALL SetErrStat( Errstat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + + ! Set up mesh maps between MoorDyn and floating platforms. ! (for now assuming ElastoDyn - eventually could differentiate at the turbine level) @@ -1813,7 +1822,7 @@ SUBROUTINE Farm_InitMD( farm, ErrStat, ErrMsg ) ! kinematics CALL MeshMapCreate( farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh, & - farm%MD%u(1)%PtFairleadDisplacement(nt), farm%m%FWrap_2_MD(nt), ErrStat2, ErrMsg2 ) + farm%MD%Input(1)%PtFairleadDisplacement(nt), farm%m%FWrap_2_MD(nt), ErrStat2, ErrMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':FWrap_2_MD' ) @@ -1825,7 +1834,7 @@ SUBROUTINE Farm_InitMD( farm, ErrStat, ErrMsg ) !CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':MD_2_FWrap' ) ! !CALL MeshMapCreate( farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh, & - ! farm%MD%u(1)%PtFairleadDisplacement(nt), farm%m%FWrap_2_MD, ErrStat2, ErrMsg2 ) + ! farm%MD%Input(1)%PtFairleadDisplacement(nt), farm%m%FWrap_2_MD, ErrStat2, ErrMsg2 ) ! !CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':FWrap_2_MD' ) !end if @@ -1855,29 +1864,27 @@ subroutine FARM_MD_Increment(t, n, farm, ErrStat, ErrMsg) INTEGER(IntKi) :: n_ss INTEGER(IntKi) :: n_FMD REAL(DbKi) :: t_next ! time at next step after this one (s) - TYPE(MD_InputType) :: u_next INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'FARM_MD_Increment' ! ----- extrapolate MD inputs ----- - t_next = t + farm%p%DT_mooring ! Do a linear extrapolation to estimate MoorDyn inputs at time n_ss+1 - CALL MD_Input_ExtrapInterp(farm%MD%u, farm%MD%uTimes, u_next, t_next, ErrStat2, ErrMsg2) + CALL MD_Input_ExtrapInterp(farm%MD%Input, farm%MD%InputTimes, farm%MD%u, t_next, ErrStat2, ErrMsg2) CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName ) ! Shift "window" of MD%Input: move values of Input and InputTimes from index 1 to index 2 - CALL MD_CopyInput (farm%MD%u(1), farm%MD%u(2), MESH_UPDATECOPY, Errstat2, ErrMsg2) + CALL MD_CopyInput (farm%MD%Input(1), farm%MD%Input(2), MESH_UPDATECOPY, Errstat2, ErrMsg2) CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName ) - farm%MD%uTimes(2) = farm%MD%uTimes(1) + farm%MD%InputTimes(2) = farm%MD%InputTimes(1) ! update index 1 entries with the new extrapolated values - CALL MD_CopyInput (u_next, farm%MD%u(1), MESH_UPDATECOPY, Errstat2, ErrMsg2) + CALL MD_CopyInput (farm%MD%u, farm%MD%Input(1), MESH_UPDATECOPY, Errstat2, ErrMsg2) CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName ) - farm%MD%uTimes(1) = t_next + farm%MD%InputTimes(1) = t_next ! ----- map substructure kinematics to MoorDyn inputs ----- (from mapping called at start of CalcOutputs Solve INputs) @@ -1885,25 +1892,25 @@ subroutine FARM_MD_Increment(t, n, farm, ErrStat, ErrMsg) do nt = 1,farm%p%NumTurbines !if (farm%MD%p%NFairs(nt) > 0 ) then - CALL Transfer_Point_to_Point( farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh, farm%MD%u(1)%PtFairleadDisplacement(nt), & + CALL Transfer_Point_to_Point( farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh, farm%MD%Input(1)%PtFairleadDisplacement(nt), & farm%m%FWrap_2_MD(nt), ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat, ErrMsg,RoutineName//'u_MD%PtFairleadDisplacement' ) ! SubDyn alternative - !CALL Transfer_Point_to_Point( farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh, farm%MD%u(1)%PtFairleadDisplacement(nt), farm%m%FWrap_2_MD(nt), ErrStat, ErrMsg ) + !CALL Transfer_Point_to_Point( farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh, farm%MD%Input(1)%PtFairleadDisplacement(nt), farm%m%FWrap_2_MD(nt), ErrStat, ErrMsg ) !end if end do ! ----- update states and calculate outputs ----- - CALL MD_UpdateStates( t, n_FMD, farm%MD%u, farm%MD%utimes, farm%MD%p, farm%MD%x, & + CALL MD_UpdateStates( t, n_FMD, farm%MD%Input, farm%MD%InputTimes, farm%MD%p, farm%MD%x, & farm%MD%xd, farm%MD%z, farm%MD%OtherSt, farm%MD%m, ErrStat2, ErrMsg2 ) CALL SetErrStat( Errstat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - CALL MD_CalcOutput( t, farm%MD%u(1), farm%MD%p, farm%MD%x, farm%MD%xd, farm%MD%z, & + CALL MD_CalcOutput( t, farm%MD%Input(1), farm%MD%p, farm%MD%x, farm%MD%xd, farm%MD%z, & farm%MD%OtherSt, farm%MD%y, farm%MD%m, ErrStat2, ErrMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) @@ -1917,7 +1924,7 @@ subroutine FARM_MD_Increment(t, n, farm, ErrStat, ErrMsg) ! mapping CALL Transfer_Point_to_Point( farm%MD%y%PtFairleadLoad(nt), farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_2, & farm%m%MD_2_FWrap(nt), ErrStat2, ErrMsg2, & - farm%MD%u(1)%PtFairleadDisplacement(nt), farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh ) !u_MD and y_ED contain the displacements needed for moment calculations + farm%MD%Input(1)%PtFairleadDisplacement(nt), farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh ) !u_MD and y_ED contain the displacements needed for moment calculations CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) @@ -1931,7 +1938,7 @@ subroutine FARM_MD_Increment(t, n, farm, ErrStat, ErrMsg) ! SubDyn alternative !CALL Transfer_Point_to_Point( farm%MD%y%PtFairleadLoad(nt), farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2, & ! farm%m%MD_2_FWrap(nt), ErrStat2, ErrMsg2, & - ! farm%MD%u(1)%PtFairleadDisplacement(nt), farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh ) !u_MD and y_SD contain the displacements needed for moment calculations + ! farm%MD%Input(1)%PtFairleadDisplacement(nt), farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh ) !u_MD and y_SD contain the displacements needed for moment calculations ! !farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Force = farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Force + farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2%Force !farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Moment = farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Moment + farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2%Moment diff --git a/glue-codes/fast-farm/src/FAST_Farm_Types.f90 b/glue-codes/fast-farm/src/FAST_Farm_Types.f90 index 99d5e65512..81ab190e37 100644 --- a/glue-codes/fast-farm/src/FAST_Farm_Types.f90 +++ b/glue-codes/fast-farm/src/FAST_Farm_Types.f90 @@ -214,8 +214,9 @@ MODULE FAST_Farm_Types TYPE(MD_ConstraintStateType) :: z !< Constraint states [-] TYPE(MD_OtherStateType) :: OtherSt !< Other states [-] TYPE(MD_ParameterType) :: p !< Parameters [-] - TYPE(MD_InputType) , DIMENSION(1:2) :: u !< System inputs [-] - REAL(DbKi) , DIMENSION(1:2) :: utimes !< Current time [s] + TYPE(MD_InputType) :: u !< Extrapolated system inputs [-] + TYPE(MD_InputType) , DIMENSION(:), ALLOCATABLE :: Input !< System inputs [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: InputTimes !< Current time [s] TYPE(MD_OutputType) :: y !< System outputs [-] TYPE(MD_MiscVarType) :: m !< Misc/optimization variables [-] LOGICAL :: IsInitialized = .FALSE. !< Has MD_Init been called [-] @@ -5268,12 +5269,37 @@ SUBROUTINE Farm_CopyMD_Data( SrcMD_DataData, DstMD_DataData, CtrlCode, ErrStat, CALL MD_CopyParam( SrcMD_DataData%p, DstMD_DataData%p, CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) IF (ErrStat>=AbortErrLev) RETURN - DO i1 = LBOUND(SrcMD_DataData%u,1), UBOUND(SrcMD_DataData%u,1) - CALL MD_CopyInput( SrcMD_DataData%u(i1), DstMD_DataData%u(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL MD_CopyInput( SrcMD_DataData%u, DstMD_DataData%u, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN +IF (ALLOCATED(SrcMD_DataData%Input)) THEN + i1_l = LBOUND(SrcMD_DataData%Input,1) + i1_u = UBOUND(SrcMD_DataData%Input,1) + IF (.NOT. ALLOCATED(DstMD_DataData%Input)) THEN + ALLOCATE(DstMD_DataData%Input(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMD_DataData%Input.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcMD_DataData%Input,1), UBOUND(SrcMD_DataData%Input,1) + CALL MD_CopyInput( SrcMD_DataData%Input(i1), DstMD_DataData%Input(i1), CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) IF (ErrStat>=AbortErrLev) RETURN ENDDO - DstMD_DataData%utimes = SrcMD_DataData%utimes +ENDIF +IF (ALLOCATED(SrcMD_DataData%InputTimes)) THEN + i1_l = LBOUND(SrcMD_DataData%InputTimes,1) + i1_u = UBOUND(SrcMD_DataData%InputTimes,1) + IF (.NOT. ALLOCATED(DstMD_DataData%InputTimes)) THEN + ALLOCATE(DstMD_DataData%InputTimes(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMD_DataData%InputTimes.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMD_DataData%InputTimes = SrcMD_DataData%InputTimes +ENDIF CALL MD_CopyOutput( SrcMD_DataData%y, DstMD_DataData%y, CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) IF (ErrStat>=AbortErrLev) RETURN @@ -5297,9 +5323,16 @@ SUBROUTINE Farm_DestroyMD_Data( MD_DataData, ErrStat, ErrMsg ) CALL MD_DestroyConstrState( MD_DataData%z, ErrStat, ErrMsg ) CALL MD_DestroyOtherState( MD_DataData%OtherSt, ErrStat, ErrMsg ) CALL MD_DestroyParam( MD_DataData%p, ErrStat, ErrMsg ) -DO i1 = LBOUND(MD_DataData%u,1), UBOUND(MD_DataData%u,1) - CALL MD_DestroyInput( MD_DataData%u(i1), ErrStat, ErrMsg ) + CALL MD_DestroyInput( MD_DataData%u, ErrStat, ErrMsg ) +IF (ALLOCATED(MD_DataData%Input)) THEN +DO i1 = LBOUND(MD_DataData%Input,1), UBOUND(MD_DataData%Input,1) + CALL MD_DestroyInput( MD_DataData%Input(i1), ErrStat, ErrMsg ) ENDDO + DEALLOCATE(MD_DataData%Input) +ENDIF +IF (ALLOCATED(MD_DataData%InputTimes)) THEN + DEALLOCATE(MD_DataData%InputTimes) +ENDIF CALL MD_DestroyOutput( MD_DataData%y, ErrStat, ErrMsg ) CALL MD_DestroyMisc( MD_DataData%m, ErrStat, ErrMsg ) END SUBROUTINE Farm_DestroyMD_Data @@ -5425,9 +5458,8 @@ SUBROUTINE Farm_PackMD_Data( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg Int_BufSz = Int_BufSz + SIZE( Int_Buf ) DEALLOCATE(Int_Buf) END IF - DO i1 = LBOUND(InData%u,1), UBOUND(InData%u,1) Int_BufSz = Int_BufSz + 3 ! u: size of buffers for each call to pack subtype - CALL MD_PackInput( Re_Buf, Db_Buf, Int_Buf, InData%u(i1), ErrStat2, ErrMsg2, .TRUE. ) ! u + CALL MD_PackInput( Re_Buf, Db_Buf, Int_Buf, InData%u, ErrStat2, ErrMsg2, .TRUE. ) ! u CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -5443,8 +5475,34 @@ SUBROUTINE Farm_PackMD_Data( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg Int_BufSz = Int_BufSz + SIZE( Int_Buf ) DEALLOCATE(Int_Buf) END IF + Int_BufSz = Int_BufSz + 1 ! Input allocated yes/no + IF ( ALLOCATED(InData%Input) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! Input upper/lower bounds for each dimension + DO i1 = LBOUND(InData%Input,1), UBOUND(InData%Input,1) + Int_BufSz = Int_BufSz + 3 ! Input: size of buffers for each call to pack subtype + CALL MD_PackInput( Re_Buf, Db_Buf, Int_Buf, InData%Input(i1), ErrStat2, ErrMsg2, .TRUE. ) ! Input + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! Input + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! Input + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! Input + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF END DO - Db_BufSz = Db_BufSz + SIZE(InData%utimes) ! utimes + END IF + Int_BufSz = Int_BufSz + 1 ! InputTimes allocated yes/no + IF ( ALLOCATED(InData%InputTimes) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! InputTimes upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%InputTimes) ! InputTimes + END IF Int_BufSz = Int_BufSz + 3 ! y: size of buffers for each call to pack subtype CALL MD_PackOutput( Re_Buf, Db_Buf, Int_Buf, InData%y, ErrStat2, ErrMsg2, .TRUE. ) ! y CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) @@ -5647,8 +5705,46 @@ SUBROUTINE Farm_PackMD_Data( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg ELSE IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 ENDIF - DO i1 = LBOUND(InData%u,1), UBOUND(InData%u,1) - CALL MD_PackInput( Re_Buf, Db_Buf, Int_Buf, InData%u(i1), ErrStat2, ErrMsg2, OnlySize ) ! u + CALL MD_PackInput( Re_Buf, Db_Buf, Int_Buf, InData%u, ErrStat2, ErrMsg2, OnlySize ) ! u + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF ( .NOT. ALLOCATED(InData%Input) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Input,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Input,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%Input,1), UBOUND(InData%Input,1) + CALL MD_PackInput( Re_Buf, Db_Buf, Int_Buf, InData%Input(i1), ErrStat2, ErrMsg2, OnlySize ) ! Input CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -5677,10 +5773,22 @@ SUBROUTINE Farm_PackMD_Data( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 ENDIF END DO - DO i1 = LBOUND(InData%utimes,1), UBOUND(InData%utimes,1) - DbKiBuf(Db_Xferred) = InData%utimes(i1) - Db_Xferred = Db_Xferred + 1 - END DO + END IF + IF ( .NOT. ALLOCATED(InData%InputTimes) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%InputTimes,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%InputTimes,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%InputTimes,1), UBOUND(InData%InputTimes,1) + DbKiBuf(Db_Xferred) = InData%InputTimes(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF CALL MD_PackOutput( Re_Buf, Db_Buf, Int_Buf, InData%y, ErrStat2, ErrMsg2, OnlySize ) ! y CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -5968,9 +6076,6 @@ SUBROUTINE Farm_UnPackMD_Data( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Err IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) - i1_l = LBOUND(OutData%u,1) - i1_u = UBOUND(OutData%u,1) - DO i1 = LBOUND(OutData%u,1), UBOUND(OutData%u,1) Buf_size=IntKiBuf( Int_Xferred ) Int_Xferred = Int_Xferred + 1 IF(Buf_size > 0) THEN @@ -6004,7 +6109,61 @@ SUBROUTINE Farm_UnPackMD_Data( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Err Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) Int_Xferred = Int_Xferred + Buf_size END IF - CALL MD_UnpackInput( Re_Buf, Db_Buf, Int_Buf, OutData%u(i1), ErrStat2, ErrMsg2 ) ! u + CALL MD_UnpackInput( Re_Buf, Db_Buf, Int_Buf, OutData%u, ErrStat2, ErrMsg2 ) ! u + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Input not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Input)) DEALLOCATE(OutData%Input) + ALLOCATE(OutData%Input(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Input.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%Input,1), UBOUND(OutData%Input,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_UnpackInput( Re_Buf, Db_Buf, Int_Buf, OutData%Input(i1), ErrStat2, ErrMsg2 ) ! Input CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -6012,12 +6171,25 @@ SUBROUTINE Farm_UnPackMD_Data( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Err IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) END DO - i1_l = LBOUND(OutData%utimes,1) - i1_u = UBOUND(OutData%utimes,1) - DO i1 = LBOUND(OutData%utimes,1), UBOUND(OutData%utimes,1) - OutData%utimes(i1) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 - END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! InputTimes not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%InputTimes)) DEALLOCATE(OutData%InputTimes) + ALLOCATE(OutData%InputTimes(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%InputTimes.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%InputTimes,1), UBOUND(OutData%InputTimes,1) + OutData%InputTimes(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF Buf_size=IntKiBuf( Int_Xferred ) Int_Xferred = Int_Xferred + 1 IF(Buf_size > 0) THEN From 1e22dcee1de6eb7d1e61defd67d72b17ebdd1303 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Fri, 2 Apr 2021 14:49:15 -0600 Subject: [PATCH 018/242] Updating MD version number --- modules/moordyn/src/MoorDyn.f90 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 79aafdaa0f..f02060c8e6 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -29,7 +29,7 @@ MODULE MoorDyn PRIVATE - TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a8', '15 Feb. 2021' ) + TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a9', '24 March 2021' ) INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output From 743b10454987f4cf7cde7c69838809ceee2d52b3 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Tue, 6 Apr 2021 15:59:17 -0600 Subject: [PATCH 019/242] Add some _Types.f90 files back in. Deleted in prior commit???? --- modules/aerodyn/src/DBEMT_Types.f90 | 3199 +++++++++++ modules/inflowwind/src/IfW_UserWind_Types.f90 | 646 +++ modules/servodyn/src/StrucCtrl_Types.f90 | 4723 +++++++++++++++++ 3 files changed, 8568 insertions(+) create mode 100644 modules/aerodyn/src/DBEMT_Types.f90 create mode 100644 modules/inflowwind/src/IfW_UserWind_Types.f90 create mode 100644 modules/servodyn/src/StrucCtrl_Types.f90 diff --git a/modules/aerodyn/src/DBEMT_Types.f90 b/modules/aerodyn/src/DBEMT_Types.f90 new file mode 100644 index 0000000000..04f0fb3af0 --- /dev/null +++ b/modules/aerodyn/src/DBEMT_Types.f90 @@ -0,0 +1,3199 @@ +!STARTOFREGISTRYGENERATEDFILE 'DBEMT_Types.f90' +! +! WARNING This file is generated automatically by the FAST registry. +! Do not edit. Your changes to this file will be lost. +! +! FAST Registry +!********************************************************************************************************************************* +! DBEMT_Types +!................................................................................................................................. +! This file is part of DBEMT. +! +! Copyright (C) 2012-2016 National Renewable Energy Laboratory +! +! Licensed under the Apache License, Version 2.0 (the "License"); +! you may not use this file except in compliance with the License. +! You may obtain a copy of the License at +! +! http://www.apache.org/licenses/LICENSE-2.0 +! +! Unless required by applicable law or agreed to in writing, software +! distributed under the License is distributed on an "AS IS" BASIS, +! WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +! See the License for the specific language governing permissions and +! limitations under the License. +! +! +! W A R N I N G : This file was automatically generated from the FAST registry. Changes made to this file may be lost. +! +!********************************************************************************************************************************* +!> This module contains the user-defined types needed in DBEMT. It also contains copy, destroy, pack, and +!! unpack routines associated with each defined data type. This code is automatically generated by the FAST Registry. +MODULE DBEMT_Types +!--------------------------------------------------------------------------------------------------------------------------------- +USE NWTC_Library +IMPLICIT NONE + INTEGER(IntKi), PUBLIC, PARAMETER :: DBEMT_none = 0 ! use BEMT instead (not DBEMT) [-] + INTEGER(IntKi), PUBLIC, PARAMETER :: DBEMT_tauConst = 1 ! use constant tau1 [-] + INTEGER(IntKi), PUBLIC, PARAMETER :: DBEMT_tauVaries = 2 ! use time-dependent tau1 [-] + INTEGER(IntKi), PUBLIC, PARAMETER :: DBEMT_cont_tauConst = 3 ! use continuous formulation with constant tau1 [-] +! ========= DBEMT_InitInputType ======= + TYPE, PUBLIC :: DBEMT_InitInputType + INTEGER(IntKi) :: NumBlades !< Number of blades on the turbine [-] + INTEGER(IntKi) :: NumNodes !< Number of nodes on each blade [-] + REAL(ReKi) :: tau1_const !< delay value based on disk-averaged quantities [-] + INTEGER(IntKi) :: DBEMT_Mod !< DBEMT Model. 1 = constant tau1, 2 = time dependent tau1, 3=continuous form with constant tau1 [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: rLocal !< Radial distance to blade node from the center of rotation, measured in the rotor plane, needed for DBEMT [m] + END TYPE DBEMT_InitInputType +! ======================= +! ========= DBEMT_InitOutputType ======= + TYPE, PUBLIC :: DBEMT_InitOutputType + TYPE(ProgDesc) :: Ver !< This module's name, version, and date [-] + END TYPE DBEMT_InitOutputType +! ======================= +! ========= DBEMT_ElementContinuousStateType ======= + TYPE, PUBLIC :: DBEMT_ElementContinuousStateType + REAL(R8Ki) , DIMENSION(1:2) :: vind !< The filtered induced velocity, [1,i,j] is the axial induced velocity (-Vx*a) at node i on blade j and [2,i,j] is the tantential induced velocity (Vy*a') [m/s] + REAL(R8Ki) , DIMENSION(1:2) :: vind_dot !< Time derivative of the filtered induced velocity, x%vind in CCSD [m/s^2] + REAL(R8Ki) , DIMENSION(1:2) :: vind_1 !< The filtered intermediate induced velocity [m/s] + END TYPE DBEMT_ElementContinuousStateType +! ======================= +! ========= DBEMT_ContinuousStateType ======= + TYPE, PUBLIC :: DBEMT_ContinuousStateType + TYPE(DBEMT_ElementContinuousStateType) , DIMENSION(:,:), ALLOCATABLE :: element !< The filtered induced velocity [1,i,j] is the axial induced velocity (-Vx*a) at node i on blade j and [2,i,j] is the tantential induced velocity (Vy*a') at node i on blade j [m/s] + END TYPE DBEMT_ContinuousStateType +! ======================= +! ========= DBEMT_DiscreteStateType ======= + TYPE, PUBLIC :: DBEMT_DiscreteStateType + REAL(SiKi) :: DummyState !< Remove this variable if you have continuous states [-] + END TYPE DBEMT_DiscreteStateType +! ======================= +! ========= DBEMT_ConstraintStateType ======= + TYPE, PUBLIC :: DBEMT_ConstraintStateType + REAL(SiKi) :: DummyState !< Remove this variable if you have constraint states [-] + END TYPE DBEMT_ConstraintStateType +! ======================= +! ========= DBEMT_OtherStateType ======= + TYPE, PUBLIC :: DBEMT_OtherStateType + LOGICAL , DIMENSION(:,:), ALLOCATABLE :: areStatesInitialized !< Flag indicating whether the module's states have been initialized properly [-] + REAL(ReKi) :: tau1 !< value of tau1 used in updateStates (for output-to-file only) [-] + REAL(ReKi) :: tau2 !< value of tau2 used in updateStates (equal to k_tau * tau1, not used between time steps) [-] + INTEGER(IntKi) , DIMENSION(:,:), ALLOCATABLE :: n !< time step value used for continuous state integrator [-] + TYPE(DBEMT_ContinuousStateType) , DIMENSION(1:4) :: xdot !< derivative history for continuous state integrators [-] + END TYPE DBEMT_OtherStateType +! ======================= +! ========= DBEMT_MiscVarType ======= + TYPE, PUBLIC :: DBEMT_MiscVarType + LOGICAL :: FirstWarn_tau1 !< flag so tau1 limit warning doesn't get repeated forever [-] + END TYPE DBEMT_MiscVarType +! ======================= +! ========= DBEMT_ParameterType ======= + TYPE, PUBLIC :: DBEMT_ParameterType + REAL(DbKi) :: DT !< Time step for continuous state integration & discrete state update [seconds] + INTEGER(IntKi) :: lin_nx = 0 !< Number of continuous states for linearization [-] + INTEGER(IntKi) :: NumBlades !< Number of blades on the turbine [-] + INTEGER(IntKi) :: NumNodes !< Number of nodes on each blade [-] + REAL(ReKi) :: k_0ye !< Filter dynamics constant [default = 0.6 ] [-] + REAL(ReKi) :: tau1_const !< constant version of the delay value [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: spanRatio !< static span ratio of each blade node [-] + INTEGER(IntKi) :: DBEMT_Mod !< DBEMT Model. 1 = constant tau1, 2 = time dependent tau1, 3=continuous form of constant tau1 [-] + END TYPE DBEMT_ParameterType +! ======================= +! ========= DBEMT_ElementInputType ======= + TYPE, PUBLIC :: DBEMT_ElementInputType + REAL(ReKi) , DIMENSION(1:2) :: vind_s !< The unfiltered induced velocity, [1] is the axial induced velocity (-Vx*a) and [2] is the tangential induced velocity (Vy*a') at node i on blade j. Note that the inputs are used only operated on at a particular node and blade, so we don't store all elements [m/s] + REAL(ReKi) , DIMENSION(1:2) :: vind_s_dot !< The first time derivative of the unfiltered induced velocity, u%vind_s [m/s^2] + REAL(ReKi) :: spanRatio !< Normalized span location of blade node [-] + END TYPE DBEMT_ElementInputType +! ======================= +! ========= DBEMT_InputType ======= + TYPE, PUBLIC :: DBEMT_InputType + REAL(ReKi) :: AxInd_disk !< Disk-averaged axial induction (for time-varying tau) [-] + REAL(ReKi) :: Un_disk !< Disk-averaged normal relative inflow velocity (for time-varying tau) [m/s] + REAL(ReKi) :: R_disk !< Disk-averaged rotor radius (for time-varying tau) [m] + TYPE(DBEMT_ElementInputType) , DIMENSION(:,:), ALLOCATABLE :: element !< The element-level inputs at each blade node [-] + END TYPE DBEMT_InputType +! ======================= +! ========= DBEMT_OutputType ======= + TYPE, PUBLIC :: DBEMT_OutputType + REAL(ReKi) , DIMENSION(:,:,:), ALLOCATABLE :: vind !< The filtered induced velocity, [1,i,j] is the axial induced velocity (-Vx*a) at node i on blade j and [2,i,j] is the tangential induced velocity (Vy*a') at node i on blade j [m/s] + END TYPE DBEMT_OutputType +! ======================= +CONTAINS + SUBROUTINE DBEMT_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, ErrStat, ErrMsg ) + TYPE(DBEMT_InitInputType), INTENT(IN) :: SrcInitInputData + TYPE(DBEMT_InitInputType), INTENT(INOUT) :: DstInitInputData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_CopyInitInput' +! + ErrStat = ErrID_None + ErrMsg = "" + DstInitInputData%NumBlades = SrcInitInputData%NumBlades + DstInitInputData%NumNodes = SrcInitInputData%NumNodes + DstInitInputData%tau1_const = SrcInitInputData%tau1_const + DstInitInputData%DBEMT_Mod = SrcInitInputData%DBEMT_Mod +IF (ALLOCATED(SrcInitInputData%rLocal)) THEN + i1_l = LBOUND(SrcInitInputData%rLocal,1) + i1_u = UBOUND(SrcInitInputData%rLocal,1) + i2_l = LBOUND(SrcInitInputData%rLocal,2) + i2_u = UBOUND(SrcInitInputData%rLocal,2) + IF (.NOT. ALLOCATED(DstInitInputData%rLocal)) THEN + ALLOCATE(DstInitInputData%rLocal(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitInputData%rLocal.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitInputData%rLocal = SrcInitInputData%rLocal +ENDIF + END SUBROUTINE DBEMT_CopyInitInput + + SUBROUTINE DBEMT_DestroyInitInput( InitInputData, ErrStat, ErrMsg ) + TYPE(DBEMT_InitInputType), INTENT(INOUT) :: InitInputData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_DestroyInitInput' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(InitInputData%rLocal)) THEN + DEALLOCATE(InitInputData%rLocal) +ENDIF + END SUBROUTINE DBEMT_DestroyInitInput + + SUBROUTINE DBEMT_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(DBEMT_InitInputType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_PackInitInput' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! NumBlades + Int_BufSz = Int_BufSz + 1 ! NumNodes + Re_BufSz = Re_BufSz + 1 ! tau1_const + Int_BufSz = Int_BufSz + 1 ! DBEMT_Mod + Int_BufSz = Int_BufSz + 1 ! rLocal allocated yes/no + IF ( ALLOCATED(InData%rLocal) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! rLocal upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%rLocal) ! rLocal + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IntKiBuf(Int_Xferred) = InData%NumBlades + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%NumNodes + Int_Xferred = Int_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%tau1_const + Re_Xferred = Re_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%DBEMT_Mod + Int_Xferred = Int_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%rLocal) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%rLocal,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%rLocal,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%rLocal,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%rLocal,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%rLocal,2), UBOUND(InData%rLocal,2) + DO i1 = LBOUND(InData%rLocal,1), UBOUND(InData%rLocal,1) + ReKiBuf(Re_Xferred) = InData%rLocal(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + END SUBROUTINE DBEMT_PackInitInput + + SUBROUTINE DBEMT_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(DBEMT_InitInputType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_UnPackInitInput' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%NumBlades = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%NumNodes = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%tau1_const = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%DBEMT_Mod = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! rLocal not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%rLocal)) DEALLOCATE(OutData%rLocal) + ALLOCATE(OutData%rLocal(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%rLocal.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%rLocal,2), UBOUND(OutData%rLocal,2) + DO i1 = LBOUND(OutData%rLocal,1), UBOUND(OutData%rLocal,1) + OutData%rLocal(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + END SUBROUTINE DBEMT_UnPackInitInput + + SUBROUTINE DBEMT_CopyInitOutput( SrcInitOutputData, DstInitOutputData, CtrlCode, ErrStat, ErrMsg ) + TYPE(DBEMT_InitOutputType), INTENT(IN) :: SrcInitOutputData + TYPE(DBEMT_InitOutputType), INTENT(INOUT) :: DstInitOutputData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_CopyInitOutput' +! + ErrStat = ErrID_None + ErrMsg = "" + CALL NWTC_Library_Copyprogdesc( SrcInitOutputData%Ver, DstInitOutputData%Ver, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + END SUBROUTINE DBEMT_CopyInitOutput + + SUBROUTINE DBEMT_DestroyInitOutput( InitOutputData, ErrStat, ErrMsg ) + TYPE(DBEMT_InitOutputType), INTENT(INOUT) :: InitOutputData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_DestroyInitOutput' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + CALL NWTC_Library_Destroyprogdesc( InitOutputData%Ver, ErrStat, ErrMsg ) + END SUBROUTINE DBEMT_DestroyInitOutput + + SUBROUTINE DBEMT_PackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(DBEMT_InitOutputType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_PackInitOutput' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + Int_BufSz = Int_BufSz + 3 ! Ver: size of buffers for each call to pack subtype + CALL NWTC_Library_Packprogdesc( Re_Buf, Db_Buf, Int_Buf, InData%Ver, ErrStat2, ErrMsg2, .TRUE. ) ! Ver + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! Ver + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! Ver + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! Ver + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + CALL NWTC_Library_Packprogdesc( Re_Buf, Db_Buf, Int_Buf, InData%Ver, ErrStat2, ErrMsg2, OnlySize ) ! Ver + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END SUBROUTINE DBEMT_PackInitOutput + + SUBROUTINE DBEMT_UnPackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(DBEMT_InitOutputType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_UnPackInitOutput' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL NWTC_Library_Unpackprogdesc( Re_Buf, Db_Buf, Int_Buf, OutData%Ver, ErrStat2, ErrMsg2 ) ! Ver + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END SUBROUTINE DBEMT_UnPackInitOutput + + SUBROUTINE DBEMT_CopyElementContinuousStateType( SrcElementContinuousStateTypeData, DstElementContinuousStateTypeData, CtrlCode, ErrStat, ErrMsg ) + TYPE(DBEMT_ElementContinuousStateType), INTENT(IN) :: SrcElementContinuousStateTypeData + TYPE(DBEMT_ElementContinuousStateType), INTENT(INOUT) :: DstElementContinuousStateTypeData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_CopyElementContinuousStateType' +! + ErrStat = ErrID_None + ErrMsg = "" + DstElementContinuousStateTypeData%vind = SrcElementContinuousStateTypeData%vind + DstElementContinuousStateTypeData%vind_dot = SrcElementContinuousStateTypeData%vind_dot + DstElementContinuousStateTypeData%vind_1 = SrcElementContinuousStateTypeData%vind_1 + END SUBROUTINE DBEMT_CopyElementContinuousStateType + + SUBROUTINE DBEMT_DestroyElementContinuousStateType( ElementContinuousStateTypeData, ErrStat, ErrMsg ) + TYPE(DBEMT_ElementContinuousStateType), INTENT(INOUT) :: ElementContinuousStateTypeData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_DestroyElementContinuousStateType' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE DBEMT_DestroyElementContinuousStateType + + SUBROUTINE DBEMT_PackElementContinuousStateType( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(DBEMT_ElementContinuousStateType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_PackElementContinuousStateType' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Db_BufSz = Db_BufSz + SIZE(InData%vind) ! vind + Db_BufSz = Db_BufSz + SIZE(InData%vind_dot) ! vind_dot + Db_BufSz = Db_BufSz + SIZE(InData%vind_1) ! vind_1 + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + DO i1 = LBOUND(InData%vind,1), UBOUND(InData%vind,1) + DbKiBuf(Db_Xferred) = InData%vind(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%vind_dot,1), UBOUND(InData%vind_dot,1) + DbKiBuf(Db_Xferred) = InData%vind_dot(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%vind_1,1), UBOUND(InData%vind_1,1) + DbKiBuf(Db_Xferred) = InData%vind_1(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END SUBROUTINE DBEMT_PackElementContinuousStateType + + SUBROUTINE DBEMT_UnPackElementContinuousStateType( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(DBEMT_ElementContinuousStateType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_UnPackElementContinuousStateType' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + i1_l = LBOUND(OutData%vind,1) + i1_u = UBOUND(OutData%vind,1) + DO i1 = LBOUND(OutData%vind,1), UBOUND(OutData%vind,1) + OutData%vind(i1) = REAL(DbKiBuf(Db_Xferred), R8Ki) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%vind_dot,1) + i1_u = UBOUND(OutData%vind_dot,1) + DO i1 = LBOUND(OutData%vind_dot,1), UBOUND(OutData%vind_dot,1) + OutData%vind_dot(i1) = REAL(DbKiBuf(Db_Xferred), R8Ki) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%vind_1,1) + i1_u = UBOUND(OutData%vind_1,1) + DO i1 = LBOUND(OutData%vind_1,1), UBOUND(OutData%vind_1,1) + OutData%vind_1(i1) = REAL(DbKiBuf(Db_Xferred), R8Ki) + Db_Xferred = Db_Xferred + 1 + END DO + END SUBROUTINE DBEMT_UnPackElementContinuousStateType + + SUBROUTINE DBEMT_CopyContState( SrcContStateData, DstContStateData, CtrlCode, ErrStat, ErrMsg ) + TYPE(DBEMT_ContinuousStateType), INTENT(IN) :: SrcContStateData + TYPE(DBEMT_ContinuousStateType), INTENT(INOUT) :: DstContStateData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_CopyContState' +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(SrcContStateData%element)) THEN + i1_l = LBOUND(SrcContStateData%element,1) + i1_u = UBOUND(SrcContStateData%element,1) + i2_l = LBOUND(SrcContStateData%element,2) + i2_u = UBOUND(SrcContStateData%element,2) + IF (.NOT. ALLOCATED(DstContStateData%element)) THEN + ALLOCATE(DstContStateData%element(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstContStateData%element.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i2 = LBOUND(SrcContStateData%element,2), UBOUND(SrcContStateData%element,2) + DO i1 = LBOUND(SrcContStateData%element,1), UBOUND(SrcContStateData%element,1) + CALL DBEMT_Copyelementcontinuousstatetype( SrcContStateData%element(i1,i2), DstContStateData%element(i1,i2), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO + ENDDO +ENDIF + END SUBROUTINE DBEMT_CopyContState + + SUBROUTINE DBEMT_DestroyContState( ContStateData, ErrStat, ErrMsg ) + TYPE(DBEMT_ContinuousStateType), INTENT(INOUT) :: ContStateData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_DestroyContState' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(ContStateData%element)) THEN +DO i2 = LBOUND(ContStateData%element,2), UBOUND(ContStateData%element,2) +DO i1 = LBOUND(ContStateData%element,1), UBOUND(ContStateData%element,1) + CALL DBEMT_Destroyelementcontinuousstatetype( ContStateData%element(i1,i2), ErrStat, ErrMsg ) +ENDDO +ENDDO + DEALLOCATE(ContStateData%element) +ENDIF + END SUBROUTINE DBEMT_DestroyContState + + SUBROUTINE DBEMT_PackContState( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(DBEMT_ContinuousStateType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_PackContState' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! element allocated yes/no + IF ( ALLOCATED(InData%element) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! element upper/lower bounds for each dimension + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + DO i2 = LBOUND(InData%element,2), UBOUND(InData%element,2) + DO i1 = LBOUND(InData%element,1), UBOUND(InData%element,1) + Int_BufSz = Int_BufSz + 3 ! element: size of buffers for each call to pack subtype + CALL DBEMT_Packelementcontinuousstatetype( Re_Buf, Db_Buf, Int_Buf, InData%element(i1,i2), ErrStat2, ErrMsg2, .TRUE. ) ! element + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! element + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! element + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! element + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END DO + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IF ( .NOT. ALLOCATED(InData%element) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%element,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%element,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%element,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%element,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%element,2), UBOUND(InData%element,2) + DO i1 = LBOUND(InData%element,1), UBOUND(InData%element,1) + CALL DBEMT_Packelementcontinuousstatetype( Re_Buf, Db_Buf, Int_Buf, InData%element(i1,i2), ErrStat2, ErrMsg2, OnlySize ) ! element + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END DO + END IF + END SUBROUTINE DBEMT_PackContState + + SUBROUTINE DBEMT_UnPackContState( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(DBEMT_ContinuousStateType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_UnPackContState' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! element not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%element)) DEALLOCATE(OutData%element) + ALLOCATE(OutData%element(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%element.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%element,2), UBOUND(OutData%element,2) + DO i1 = LBOUND(OutData%element,1), UBOUND(OutData%element,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL DBEMT_Unpackelementcontinuousstatetype( Re_Buf, Db_Buf, Int_Buf, OutData%element(i1,i2), ErrStat2, ErrMsg2 ) ! element + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END DO + END IF + END SUBROUTINE DBEMT_UnPackContState + + SUBROUTINE DBEMT_CopyDiscState( SrcDiscStateData, DstDiscStateData, CtrlCode, ErrStat, ErrMsg ) + TYPE(DBEMT_DiscreteStateType), INTENT(IN) :: SrcDiscStateData + TYPE(DBEMT_DiscreteStateType), INTENT(INOUT) :: DstDiscStateData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_CopyDiscState' +! + ErrStat = ErrID_None + ErrMsg = "" + DstDiscStateData%DummyState = SrcDiscStateData%DummyState + END SUBROUTINE DBEMT_CopyDiscState + + SUBROUTINE DBEMT_DestroyDiscState( DiscStateData, ErrStat, ErrMsg ) + TYPE(DBEMT_DiscreteStateType), INTENT(INOUT) :: DiscStateData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_DestroyDiscState' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE DBEMT_DestroyDiscState + + SUBROUTINE DBEMT_PackDiscState( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(DBEMT_DiscreteStateType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_PackDiscState' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Re_BufSz = Re_BufSz + 1 ! DummyState + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + ReKiBuf(Re_Xferred) = InData%DummyState + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE DBEMT_PackDiscState + + SUBROUTINE DBEMT_UnPackDiscState( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(DBEMT_DiscreteStateType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_UnPackDiscState' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%DummyState = REAL(ReKiBuf(Re_Xferred), SiKi) + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE DBEMT_UnPackDiscState + + SUBROUTINE DBEMT_CopyConstrState( SrcConstrStateData, DstConstrStateData, CtrlCode, ErrStat, ErrMsg ) + TYPE(DBEMT_ConstraintStateType), INTENT(IN) :: SrcConstrStateData + TYPE(DBEMT_ConstraintStateType), INTENT(INOUT) :: DstConstrStateData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_CopyConstrState' +! + ErrStat = ErrID_None + ErrMsg = "" + DstConstrStateData%DummyState = SrcConstrStateData%DummyState + END SUBROUTINE DBEMT_CopyConstrState + + SUBROUTINE DBEMT_DestroyConstrState( ConstrStateData, ErrStat, ErrMsg ) + TYPE(DBEMT_ConstraintStateType), INTENT(INOUT) :: ConstrStateData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_DestroyConstrState' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE DBEMT_DestroyConstrState + + SUBROUTINE DBEMT_PackConstrState( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(DBEMT_ConstraintStateType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_PackConstrState' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Re_BufSz = Re_BufSz + 1 ! DummyState + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + ReKiBuf(Re_Xferred) = InData%DummyState + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE DBEMT_PackConstrState + + SUBROUTINE DBEMT_UnPackConstrState( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(DBEMT_ConstraintStateType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_UnPackConstrState' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%DummyState = REAL(ReKiBuf(Re_Xferred), SiKi) + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE DBEMT_UnPackConstrState + + SUBROUTINE DBEMT_CopyOtherState( SrcOtherStateData, DstOtherStateData, CtrlCode, ErrStat, ErrMsg ) + TYPE(DBEMT_OtherStateType), INTENT(IN) :: SrcOtherStateData + TYPE(DBEMT_OtherStateType), INTENT(INOUT) :: DstOtherStateData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_CopyOtherState' +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(SrcOtherStateData%areStatesInitialized)) THEN + i1_l = LBOUND(SrcOtherStateData%areStatesInitialized,1) + i1_u = UBOUND(SrcOtherStateData%areStatesInitialized,1) + i2_l = LBOUND(SrcOtherStateData%areStatesInitialized,2) + i2_u = UBOUND(SrcOtherStateData%areStatesInitialized,2) + IF (.NOT. ALLOCATED(DstOtherStateData%areStatesInitialized)) THEN + ALLOCATE(DstOtherStateData%areStatesInitialized(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstOtherStateData%areStatesInitialized.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstOtherStateData%areStatesInitialized = SrcOtherStateData%areStatesInitialized +ENDIF + DstOtherStateData%tau1 = SrcOtherStateData%tau1 + DstOtherStateData%tau2 = SrcOtherStateData%tau2 +IF (ALLOCATED(SrcOtherStateData%n)) THEN + i1_l = LBOUND(SrcOtherStateData%n,1) + i1_u = UBOUND(SrcOtherStateData%n,1) + i2_l = LBOUND(SrcOtherStateData%n,2) + i2_u = UBOUND(SrcOtherStateData%n,2) + IF (.NOT. ALLOCATED(DstOtherStateData%n)) THEN + ALLOCATE(DstOtherStateData%n(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstOtherStateData%n.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstOtherStateData%n = SrcOtherStateData%n +ENDIF + DO i1 = LBOUND(SrcOtherStateData%xdot,1), UBOUND(SrcOtherStateData%xdot,1) + CALL DBEMT_CopyContState( SrcOtherStateData%xdot(i1), DstOtherStateData%xdot(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO + END SUBROUTINE DBEMT_CopyOtherState + + SUBROUTINE DBEMT_DestroyOtherState( OtherStateData, ErrStat, ErrMsg ) + TYPE(DBEMT_OtherStateType), INTENT(INOUT) :: OtherStateData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_DestroyOtherState' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(OtherStateData%areStatesInitialized)) THEN + DEALLOCATE(OtherStateData%areStatesInitialized) +ENDIF +IF (ALLOCATED(OtherStateData%n)) THEN + DEALLOCATE(OtherStateData%n) +ENDIF +DO i1 = LBOUND(OtherStateData%xdot,1), UBOUND(OtherStateData%xdot,1) + CALL DBEMT_DestroyContState( OtherStateData%xdot(i1), ErrStat, ErrMsg ) +ENDDO + END SUBROUTINE DBEMT_DestroyOtherState + + SUBROUTINE DBEMT_PackOtherState( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(DBEMT_OtherStateType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_PackOtherState' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! areStatesInitialized allocated yes/no + IF ( ALLOCATED(InData%areStatesInitialized) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! areStatesInitialized upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%areStatesInitialized) ! areStatesInitialized + END IF + Re_BufSz = Re_BufSz + 1 ! tau1 + Re_BufSz = Re_BufSz + 1 ! tau2 + Int_BufSz = Int_BufSz + 1 ! n allocated yes/no + IF ( ALLOCATED(InData%n) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! n upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%n) ! n + END IF + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + DO i1 = LBOUND(InData%xdot,1), UBOUND(InData%xdot,1) + Int_BufSz = Int_BufSz + 3 ! xdot: size of buffers for each call to pack subtype + CALL DBEMT_PackContState( Re_Buf, Db_Buf, Int_Buf, InData%xdot(i1), ErrStat2, ErrMsg2, .TRUE. ) ! xdot + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! xdot + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! xdot + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! xdot + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IF ( .NOT. ALLOCATED(InData%areStatesInitialized) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%areStatesInitialized,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%areStatesInitialized,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%areStatesInitialized,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%areStatesInitialized,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%areStatesInitialized,2), UBOUND(InData%areStatesInitialized,2) + DO i1 = LBOUND(InData%areStatesInitialized,1), UBOUND(InData%areStatesInitialized,1) + IntKiBuf(Int_Xferred) = TRANSFER(InData%areStatesInitialized(i1,i2), IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + END DO + END DO + END IF + ReKiBuf(Re_Xferred) = InData%tau1 + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%tau2 + Re_Xferred = Re_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%n) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%n,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%n,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%n,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%n,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%n,2), UBOUND(InData%n,2) + DO i1 = LBOUND(InData%n,1), UBOUND(InData%n,1) + IntKiBuf(Int_Xferred) = InData%n(i1,i2) + Int_Xferred = Int_Xferred + 1 + END DO + END DO + END IF + DO i1 = LBOUND(InData%xdot,1), UBOUND(InData%xdot,1) + CALL DBEMT_PackContState( Re_Buf, Db_Buf, Int_Buf, InData%xdot(i1), ErrStat2, ErrMsg2, OnlySize ) ! xdot + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END SUBROUTINE DBEMT_PackOtherState + + SUBROUTINE DBEMT_UnPackOtherState( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(DBEMT_OtherStateType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_UnPackOtherState' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! areStatesInitialized not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%areStatesInitialized)) DEALLOCATE(OutData%areStatesInitialized) + ALLOCATE(OutData%areStatesInitialized(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%areStatesInitialized.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%areStatesInitialized,2), UBOUND(OutData%areStatesInitialized,2) + DO i1 = LBOUND(OutData%areStatesInitialized,1), UBOUND(OutData%areStatesInitialized,1) + OutData%areStatesInitialized(i1,i2) = TRANSFER(IntKiBuf(Int_Xferred), OutData%areStatesInitialized(i1,i2)) + Int_Xferred = Int_Xferred + 1 + END DO + END DO + END IF + OutData%tau1 = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%tau2 = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! n not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%n)) DEALLOCATE(OutData%n) + ALLOCATE(OutData%n(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%n.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%n,2), UBOUND(OutData%n,2) + DO i1 = LBOUND(OutData%n,1), UBOUND(OutData%n,1) + OutData%n(i1,i2) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END DO + END IF + i1_l = LBOUND(OutData%xdot,1) + i1_u = UBOUND(OutData%xdot,1) + DO i1 = LBOUND(OutData%xdot,1), UBOUND(OutData%xdot,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL DBEMT_UnpackContState( Re_Buf, Db_Buf, Int_Buf, OutData%xdot(i1), ErrStat2, ErrMsg2 ) ! xdot + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END SUBROUTINE DBEMT_UnPackOtherState + + SUBROUTINE DBEMT_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) + TYPE(DBEMT_MiscVarType), INTENT(IN) :: SrcMiscData + TYPE(DBEMT_MiscVarType), INTENT(INOUT) :: DstMiscData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_CopyMisc' +! + ErrStat = ErrID_None + ErrMsg = "" + DstMiscData%FirstWarn_tau1 = SrcMiscData%FirstWarn_tau1 + END SUBROUTINE DBEMT_CopyMisc + + SUBROUTINE DBEMT_DestroyMisc( MiscData, ErrStat, ErrMsg ) + TYPE(DBEMT_MiscVarType), INTENT(INOUT) :: MiscData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_DestroyMisc' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE DBEMT_DestroyMisc + + SUBROUTINE DBEMT_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(DBEMT_MiscVarType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_PackMisc' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! FirstWarn_tau1 + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IntKiBuf(Int_Xferred) = TRANSFER(InData%FirstWarn_tau1, IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + END SUBROUTINE DBEMT_PackMisc + + SUBROUTINE DBEMT_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(DBEMT_MiscVarType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_UnPackMisc' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%FirstWarn_tau1 = TRANSFER(IntKiBuf(Int_Xferred), OutData%FirstWarn_tau1) + Int_Xferred = Int_Xferred + 1 + END SUBROUTINE DBEMT_UnPackMisc + + SUBROUTINE DBEMT_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) + TYPE(DBEMT_ParameterType), INTENT(IN) :: SrcParamData + TYPE(DBEMT_ParameterType), INTENT(INOUT) :: DstParamData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_CopyParam' +! + ErrStat = ErrID_None + ErrMsg = "" + DstParamData%DT = SrcParamData%DT + DstParamData%lin_nx = SrcParamData%lin_nx + DstParamData%NumBlades = SrcParamData%NumBlades + DstParamData%NumNodes = SrcParamData%NumNodes + DstParamData%k_0ye = SrcParamData%k_0ye + DstParamData%tau1_const = SrcParamData%tau1_const +IF (ALLOCATED(SrcParamData%spanRatio)) THEN + i1_l = LBOUND(SrcParamData%spanRatio,1) + i1_u = UBOUND(SrcParamData%spanRatio,1) + i2_l = LBOUND(SrcParamData%spanRatio,2) + i2_u = UBOUND(SrcParamData%spanRatio,2) + IF (.NOT. ALLOCATED(DstParamData%spanRatio)) THEN + ALLOCATE(DstParamData%spanRatio(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%spanRatio.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%spanRatio = SrcParamData%spanRatio +ENDIF + DstParamData%DBEMT_Mod = SrcParamData%DBEMT_Mod + END SUBROUTINE DBEMT_CopyParam + + SUBROUTINE DBEMT_DestroyParam( ParamData, ErrStat, ErrMsg ) + TYPE(DBEMT_ParameterType), INTENT(INOUT) :: ParamData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_DestroyParam' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(ParamData%spanRatio)) THEN + DEALLOCATE(ParamData%spanRatio) +ENDIF + END SUBROUTINE DBEMT_DestroyParam + + SUBROUTINE DBEMT_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(DBEMT_ParameterType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_PackParam' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Db_BufSz = Db_BufSz + 1 ! DT + Int_BufSz = Int_BufSz + 1 ! lin_nx + Int_BufSz = Int_BufSz + 1 ! NumBlades + Int_BufSz = Int_BufSz + 1 ! NumNodes + Re_BufSz = Re_BufSz + 1 ! k_0ye + Re_BufSz = Re_BufSz + 1 ! tau1_const + Int_BufSz = Int_BufSz + 1 ! spanRatio allocated yes/no + IF ( ALLOCATED(InData%spanRatio) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! spanRatio upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%spanRatio) ! spanRatio + END IF + Int_BufSz = Int_BufSz + 1 ! DBEMT_Mod + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + DbKiBuf(Db_Xferred) = InData%DT + Db_Xferred = Db_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%lin_nx + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%NumBlades + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%NumNodes + Int_Xferred = Int_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%k_0ye + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%tau1_const + Re_Xferred = Re_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%spanRatio) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%spanRatio,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%spanRatio,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%spanRatio,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%spanRatio,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%spanRatio,2), UBOUND(InData%spanRatio,2) + DO i1 = LBOUND(InData%spanRatio,1), UBOUND(InData%spanRatio,1) + ReKiBuf(Re_Xferred) = InData%spanRatio(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IntKiBuf(Int_Xferred) = InData%DBEMT_Mod + Int_Xferred = Int_Xferred + 1 + END SUBROUTINE DBEMT_PackParam + + SUBROUTINE DBEMT_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(DBEMT_ParameterType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_UnPackParam' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%DT = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%lin_nx = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%NumBlades = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%NumNodes = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%k_0ye = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%tau1_const = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! spanRatio not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%spanRatio)) DEALLOCATE(OutData%spanRatio) + ALLOCATE(OutData%spanRatio(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%spanRatio.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%spanRatio,2), UBOUND(OutData%spanRatio,2) + DO i1 = LBOUND(OutData%spanRatio,1), UBOUND(OutData%spanRatio,1) + OutData%spanRatio(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + OutData%DBEMT_Mod = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END SUBROUTINE DBEMT_UnPackParam + + SUBROUTINE DBEMT_CopyElementInputType( SrcElementInputTypeData, DstElementInputTypeData, CtrlCode, ErrStat, ErrMsg ) + TYPE(DBEMT_ElementInputType), INTENT(IN) :: SrcElementInputTypeData + TYPE(DBEMT_ElementInputType), INTENT(INOUT) :: DstElementInputTypeData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_CopyElementInputType' +! + ErrStat = ErrID_None + ErrMsg = "" + DstElementInputTypeData%vind_s = SrcElementInputTypeData%vind_s + DstElementInputTypeData%vind_s_dot = SrcElementInputTypeData%vind_s_dot + DstElementInputTypeData%spanRatio = SrcElementInputTypeData%spanRatio + END SUBROUTINE DBEMT_CopyElementInputType + + SUBROUTINE DBEMT_DestroyElementInputType( ElementInputTypeData, ErrStat, ErrMsg ) + TYPE(DBEMT_ElementInputType), INTENT(INOUT) :: ElementInputTypeData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_DestroyElementInputType' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE DBEMT_DestroyElementInputType + + SUBROUTINE DBEMT_PackElementInputType( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(DBEMT_ElementInputType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_PackElementInputType' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Re_BufSz = Re_BufSz + SIZE(InData%vind_s) ! vind_s + Re_BufSz = Re_BufSz + SIZE(InData%vind_s_dot) ! vind_s_dot + Re_BufSz = Re_BufSz + 1 ! spanRatio + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + DO i1 = LBOUND(InData%vind_s,1), UBOUND(InData%vind_s,1) + ReKiBuf(Re_Xferred) = InData%vind_s(i1) + Re_Xferred = Re_Xferred + 1 + END DO + DO i1 = LBOUND(InData%vind_s_dot,1), UBOUND(InData%vind_s_dot,1) + ReKiBuf(Re_Xferred) = InData%vind_s_dot(i1) + Re_Xferred = Re_Xferred + 1 + END DO + ReKiBuf(Re_Xferred) = InData%spanRatio + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE DBEMT_PackElementInputType + + SUBROUTINE DBEMT_UnPackElementInputType( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(DBEMT_ElementInputType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_UnPackElementInputType' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + i1_l = LBOUND(OutData%vind_s,1) + i1_u = UBOUND(OutData%vind_s,1) + DO i1 = LBOUND(OutData%vind_s,1), UBOUND(OutData%vind_s,1) + OutData%vind_s(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + i1_l = LBOUND(OutData%vind_s_dot,1) + i1_u = UBOUND(OutData%vind_s_dot,1) + DO i1 = LBOUND(OutData%vind_s_dot,1), UBOUND(OutData%vind_s_dot,1) + OutData%vind_s_dot(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + OutData%spanRatio = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE DBEMT_UnPackElementInputType + + SUBROUTINE DBEMT_CopyInput( SrcInputData, DstInputData, CtrlCode, ErrStat, ErrMsg ) + TYPE(DBEMT_InputType), INTENT(IN) :: SrcInputData + TYPE(DBEMT_InputType), INTENT(INOUT) :: DstInputData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_CopyInput' +! + ErrStat = ErrID_None + ErrMsg = "" + DstInputData%AxInd_disk = SrcInputData%AxInd_disk + DstInputData%Un_disk = SrcInputData%Un_disk + DstInputData%R_disk = SrcInputData%R_disk +IF (ALLOCATED(SrcInputData%element)) THEN + i1_l = LBOUND(SrcInputData%element,1) + i1_u = UBOUND(SrcInputData%element,1) + i2_l = LBOUND(SrcInputData%element,2) + i2_u = UBOUND(SrcInputData%element,2) + IF (.NOT. ALLOCATED(DstInputData%element)) THEN + ALLOCATE(DstInputData%element(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInputData%element.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i2 = LBOUND(SrcInputData%element,2), UBOUND(SrcInputData%element,2) + DO i1 = LBOUND(SrcInputData%element,1), UBOUND(SrcInputData%element,1) + CALL DBEMT_Copyelementinputtype( SrcInputData%element(i1,i2), DstInputData%element(i1,i2), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO + ENDDO +ENDIF + END SUBROUTINE DBEMT_CopyInput + + SUBROUTINE DBEMT_DestroyInput( InputData, ErrStat, ErrMsg ) + TYPE(DBEMT_InputType), INTENT(INOUT) :: InputData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_DestroyInput' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(InputData%element)) THEN +DO i2 = LBOUND(InputData%element,2), UBOUND(InputData%element,2) +DO i1 = LBOUND(InputData%element,1), UBOUND(InputData%element,1) + CALL DBEMT_Destroyelementinputtype( InputData%element(i1,i2), ErrStat, ErrMsg ) +ENDDO +ENDDO + DEALLOCATE(InputData%element) +ENDIF + END SUBROUTINE DBEMT_DestroyInput + + SUBROUTINE DBEMT_PackInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(DBEMT_InputType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_PackInput' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Re_BufSz = Re_BufSz + 1 ! AxInd_disk + Re_BufSz = Re_BufSz + 1 ! Un_disk + Re_BufSz = Re_BufSz + 1 ! R_disk + Int_BufSz = Int_BufSz + 1 ! element allocated yes/no + IF ( ALLOCATED(InData%element) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! element upper/lower bounds for each dimension + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + DO i2 = LBOUND(InData%element,2), UBOUND(InData%element,2) + DO i1 = LBOUND(InData%element,1), UBOUND(InData%element,1) + Int_BufSz = Int_BufSz + 3 ! element: size of buffers for each call to pack subtype + CALL DBEMT_Packelementinputtype( Re_Buf, Db_Buf, Int_Buf, InData%element(i1,i2), ErrStat2, ErrMsg2, .TRUE. ) ! element + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! element + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! element + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! element + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END DO + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + ReKiBuf(Re_Xferred) = InData%AxInd_disk + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%Un_disk + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%R_disk + Re_Xferred = Re_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%element) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%element,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%element,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%element,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%element,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%element,2), UBOUND(InData%element,2) + DO i1 = LBOUND(InData%element,1), UBOUND(InData%element,1) + CALL DBEMT_Packelementinputtype( Re_Buf, Db_Buf, Int_Buf, InData%element(i1,i2), ErrStat2, ErrMsg2, OnlySize ) ! element + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END DO + END IF + END SUBROUTINE DBEMT_PackInput + + SUBROUTINE DBEMT_UnPackInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(DBEMT_InputType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_UnPackInput' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%AxInd_disk = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%Un_disk = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%R_disk = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! element not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%element)) DEALLOCATE(OutData%element) + ALLOCATE(OutData%element(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%element.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%element,2), UBOUND(OutData%element,2) + DO i1 = LBOUND(OutData%element,1), UBOUND(OutData%element,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL DBEMT_Unpackelementinputtype( Re_Buf, Db_Buf, Int_Buf, OutData%element(i1,i2), ErrStat2, ErrMsg2 ) ! element + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END DO + END IF + END SUBROUTINE DBEMT_UnPackInput + + SUBROUTINE DBEMT_CopyOutput( SrcOutputData, DstOutputData, CtrlCode, ErrStat, ErrMsg ) + TYPE(DBEMT_OutputType), INTENT(IN) :: SrcOutputData + TYPE(DBEMT_OutputType), INTENT(INOUT) :: DstOutputData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_CopyOutput' +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(SrcOutputData%vind)) THEN + i1_l = LBOUND(SrcOutputData%vind,1) + i1_u = UBOUND(SrcOutputData%vind,1) + i2_l = LBOUND(SrcOutputData%vind,2) + i2_u = UBOUND(SrcOutputData%vind,2) + i3_l = LBOUND(SrcOutputData%vind,3) + i3_u = UBOUND(SrcOutputData%vind,3) + IF (.NOT. ALLOCATED(DstOutputData%vind)) THEN + ALLOCATE(DstOutputData%vind(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstOutputData%vind.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstOutputData%vind = SrcOutputData%vind +ENDIF + END SUBROUTINE DBEMT_CopyOutput + + SUBROUTINE DBEMT_DestroyOutput( OutputData, ErrStat, ErrMsg ) + TYPE(DBEMT_OutputType), INTENT(INOUT) :: OutputData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_DestroyOutput' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(OutputData%vind)) THEN + DEALLOCATE(OutputData%vind) +ENDIF + END SUBROUTINE DBEMT_DestroyOutput + + SUBROUTINE DBEMT_PackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(DBEMT_OutputType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_PackOutput' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! vind allocated yes/no + IF ( ALLOCATED(InData%vind) ) THEN + Int_BufSz = Int_BufSz + 2*3 ! vind upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%vind) ! vind + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IF ( .NOT. ALLOCATED(InData%vind) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%vind,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%vind,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%vind,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%vind,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%vind,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%vind,3) + Int_Xferred = Int_Xferred + 2 + + DO i3 = LBOUND(InData%vind,3), UBOUND(InData%vind,3) + DO i2 = LBOUND(InData%vind,2), UBOUND(InData%vind,2) + DO i1 = LBOUND(InData%vind,1), UBOUND(InData%vind,1) + ReKiBuf(Re_Xferred) = InData%vind(i1,i2,i3) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END DO + END IF + END SUBROUTINE DBEMT_PackOutput + + SUBROUTINE DBEMT_UnPackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(DBEMT_OutputType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_UnPackOutput' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! vind not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%vind)) DEALLOCATE(OutData%vind) + ALLOCATE(OutData%vind(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%vind.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i3 = LBOUND(OutData%vind,3), UBOUND(OutData%vind,3) + DO i2 = LBOUND(OutData%vind,2), UBOUND(OutData%vind,2) + DO i1 = LBOUND(OutData%vind,1), UBOUND(OutData%vind,1) + OutData%vind(i1,i2,i3) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END DO + END IF + END SUBROUTINE DBEMT_UnPackOutput + + + SUBROUTINE DBEMT_ElementInputType_ExtrapInterp(u, t, u_out, t_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) ElementInputType u_out at time t_out, from previous/future time +! values of u (which has values associated with times in t). Order of the interpolation is given by the size of u +! +! expressions below based on either +! +! f(t) = a +! f(t) = a + b * t, or +! f(t) = a + b * t + c * t**2 +! +! where a, b and c are determined as the solution to +! f(t1) = u1, f(t2) = u2, f(t3) = u3 (as appropriate) +! +!.................................................................................................................................. + + TYPE(DBEMT_ElementInputType), INTENT(IN) :: u(:) ! ElementInputType at t1 > t2 > t3 + REAL(DbKi), INTENT(IN ) :: t(:) ! Times associated with the ElementInputTypes + TYPE(DBEMT_ElementInputType), INTENT(INOUT) :: u_out ! ElementInputType at tin_out + REAL(DbKi), INTENT(IN ) :: t_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + INTEGER(IntKi) :: order ! order of polynomial fit (max 2) + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_ElementInputType_ExtrapInterp' + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + if ( size(t) .ne. size(u)) then + CALL SetErrStat(ErrID_Fatal,'size(t) must equal size(u)',ErrStat,ErrMsg,RoutineName) + RETURN + endif + order = SIZE(u) - 1 + IF ( order .eq. 0 ) THEN + CALL DBEMT_CopyElementInputType(u(1), u_out, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE IF ( order .eq. 1 ) THEN + CALL DBEMT_ElementInputType_ExtrapInterp1(u(1), u(2), t, u_out, t_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE IF ( order .eq. 2 ) THEN + CALL DBEMT_ElementInputType_ExtrapInterp2(u(1), u(2), u(3), t, u_out, t_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE + CALL SetErrStat(ErrID_Fatal,'size(u) must be less than 4 (order must be less than 3).',ErrStat,ErrMsg,RoutineName) + RETURN + ENDIF + END SUBROUTINE DBEMT_ElementInputType_ExtrapInterp + + + SUBROUTINE DBEMT_ElementInputType_ExtrapInterp1(u1, u2, tin, u_out, tin_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) ElementInputType u_out at time t_out, from previous/future time +! values of u (which has values associated with times in t). Order of the interpolation is 1. +! +! f(t) = a + b * t, or +! +! where a and b are determined as the solution to +! f(t1) = u1, f(t2) = u2 +! +!.................................................................................................................................. + + TYPE(DBEMT_ElementInputType), INTENT(IN) :: u1 ! ElementInputType at t1 > t2 + TYPE(DBEMT_ElementInputType), INTENT(IN) :: u2 ! ElementInputType at t2 + REAL(DbKi), INTENT(IN ) :: tin(2) ! Times associated with the ElementInputTypes + TYPE(DBEMT_ElementInputType), INTENT(INOUT) :: u_out ! ElementInputType at tin_out + REAL(DbKi), INTENT(IN ) :: tin_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + REAL(DbKi) :: t(2) ! Times associated with the ElementInputTypes + REAL(DbKi) :: t_out ! Time to which to be extrap/interpd + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_ElementInputType_ExtrapInterp1' + REAL(DbKi) :: b ! temporary for extrapolation/interpolation + REAL(DbKi) :: ScaleFactor ! temporary for extrapolation/interpolation + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + INTEGER :: i01 ! dim1 level 0 counter variable for arrays of ddts + INTEGER :: i1 ! dim1 counter variable for arrays + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + ! we'll subtract a constant from the times to resolve some + ! numerical issues when t gets large (and to simplify the equations) + t = tin - tin(1) + t_out = tin_out - tin(1) + + IF ( EqualRealNos( t(1), t(2) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(2) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + + ScaleFactor = t_out / t(2) + DO i1 = LBOUND(u_out%vind_s,1),UBOUND(u_out%vind_s,1) + b = -(u1%vind_s(i1) - u2%vind_s(i1)) + u_out%vind_s(i1) = u1%vind_s(i1) + b * ScaleFactor + END DO + DO i1 = LBOUND(u_out%vind_s_dot,1),UBOUND(u_out%vind_s_dot,1) + b = -(u1%vind_s_dot(i1) - u2%vind_s_dot(i1)) + u_out%vind_s_dot(i1) = u1%vind_s_dot(i1) + b * ScaleFactor + END DO + b = -(u1%spanRatio - u2%spanRatio) + u_out%spanRatio = u1%spanRatio + b * ScaleFactor + END SUBROUTINE DBEMT_ElementInputType_ExtrapInterp1 + + + SUBROUTINE DBEMT_ElementInputType_ExtrapInterp2(u1, u2, u3, tin, u_out, tin_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) ElementInputType u_out at time t_out, from previous/future time +! values of u (which has values associated with times in t). Order of the interpolation is 2. +! +! expressions below based on either +! +! f(t) = a + b * t + c * t**2 +! +! where a, b and c are determined as the solution to +! f(t1) = u1, f(t2) = u2, f(t3) = u3 +! +!.................................................................................................................................. + + TYPE(DBEMT_ElementInputType), INTENT(IN) :: u1 ! ElementInputType at t1 > t2 > t3 + TYPE(DBEMT_ElementInputType), INTENT(IN) :: u2 ! ElementInputType at t2 > t3 + TYPE(DBEMT_ElementInputType), INTENT(IN) :: u3 ! ElementInputType at t3 + REAL(DbKi), INTENT(IN ) :: tin(3) ! Times associated with the ElementInputTypes + TYPE(DBEMT_ElementInputType), INTENT(INOUT) :: u_out ! ElementInputType at tin_out + REAL(DbKi), INTENT(IN ) :: tin_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + REAL(DbKi) :: t(3) ! Times associated with the ElementInputTypes + REAL(DbKi) :: t_out ! Time to which to be extrap/interpd + INTEGER(IntKi) :: order ! order of polynomial fit (max 2) + REAL(DbKi) :: b ! temporary for extrapolation/interpolation + REAL(DbKi) :: c ! temporary for extrapolation/interpolation + REAL(DbKi) :: ScaleFactor ! temporary for extrapolation/interpolation + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_ElementInputType_ExtrapInterp2' + INTEGER :: i01 ! dim1 level 0 counter variable for arrays of ddts + INTEGER :: i1 ! dim1 counter variable for arrays + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + ! we'll subtract a constant from the times to resolve some + ! numerical issues when t gets large (and to simplify the equations) + t = tin - tin(1) + t_out = tin_out - tin(1) + + IF ( EqualRealNos( t(1), t(2) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(2) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + ELSE IF ( EqualRealNos( t(2), t(3) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(2) must not equal t(3) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + ELSE IF ( EqualRealNos( t(1), t(3) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(3) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + + ScaleFactor = t_out / (t(2) * t(3) * (t(2) - t(3))) + DO i1 = LBOUND(u_out%vind_s,1),UBOUND(u_out%vind_s,1) + b = (t(3)**2*(u1%vind_s(i1) - u2%vind_s(i1)) + t(2)**2*(-u1%vind_s(i1) + u3%vind_s(i1)))* scaleFactor + c = ( (t(2)-t(3))*u1%vind_s(i1) + t(3)*u2%vind_s(i1) - t(2)*u3%vind_s(i1) ) * scaleFactor + u_out%vind_s(i1) = u1%vind_s(i1) + b + c * t_out + END DO + DO i1 = LBOUND(u_out%vind_s_dot,1),UBOUND(u_out%vind_s_dot,1) + b = (t(3)**2*(u1%vind_s_dot(i1) - u2%vind_s_dot(i1)) + t(2)**2*(-u1%vind_s_dot(i1) + u3%vind_s_dot(i1)))* scaleFactor + c = ( (t(2)-t(3))*u1%vind_s_dot(i1) + t(3)*u2%vind_s_dot(i1) - t(2)*u3%vind_s_dot(i1) ) * scaleFactor + u_out%vind_s_dot(i1) = u1%vind_s_dot(i1) + b + c * t_out + END DO + b = (t(3)**2*(u1%spanRatio - u2%spanRatio) + t(2)**2*(-u1%spanRatio + u3%spanRatio))* scaleFactor + c = ( (t(2)-t(3))*u1%spanRatio + t(3)*u2%spanRatio - t(2)*u3%spanRatio ) * scaleFactor + u_out%spanRatio = u1%spanRatio + b + c * t_out + END SUBROUTINE DBEMT_ElementInputType_ExtrapInterp2 + + + SUBROUTINE DBEMT_Input_ExtrapInterp(u, t, u_out, t_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Input u_out at time t_out, from previous/future time +! values of u (which has values associated with times in t). Order of the interpolation is given by the size of u +! +! expressions below based on either +! +! f(t) = a +! f(t) = a + b * t, or +! f(t) = a + b * t + c * t**2 +! +! where a, b and c are determined as the solution to +! f(t1) = u1, f(t2) = u2, f(t3) = u3 (as appropriate) +! +!.................................................................................................................................. + + TYPE(DBEMT_InputType), INTENT(IN) :: u(:) ! Input at t1 > t2 > t3 + REAL(DbKi), INTENT(IN ) :: t(:) ! Times associated with the Inputs + TYPE(DBEMT_InputType), INTENT(INOUT) :: u_out ! Input at tin_out + REAL(DbKi), INTENT(IN ) :: t_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + INTEGER(IntKi) :: order ! order of polynomial fit (max 2) + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_Input_ExtrapInterp' + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + if ( size(t) .ne. size(u)) then + CALL SetErrStat(ErrID_Fatal,'size(t) must equal size(u)',ErrStat,ErrMsg,RoutineName) + RETURN + endif + order = SIZE(u) - 1 + IF ( order .eq. 0 ) THEN + CALL DBEMT_CopyInput(u(1), u_out, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE IF ( order .eq. 1 ) THEN + CALL DBEMT_Input_ExtrapInterp1(u(1), u(2), t, u_out, t_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE IF ( order .eq. 2 ) THEN + CALL DBEMT_Input_ExtrapInterp2(u(1), u(2), u(3), t, u_out, t_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE + CALL SetErrStat(ErrID_Fatal,'size(u) must be less than 4 (order must be less than 3).',ErrStat,ErrMsg,RoutineName) + RETURN + ENDIF + END SUBROUTINE DBEMT_Input_ExtrapInterp + + + SUBROUTINE DBEMT_Input_ExtrapInterp1(u1, u2, tin, u_out, tin_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Input u_out at time t_out, from previous/future time +! values of u (which has values associated with times in t). Order of the interpolation is 1. +! +! f(t) = a + b * t, or +! +! where a and b are determined as the solution to +! f(t1) = u1, f(t2) = u2 +! +!.................................................................................................................................. + + TYPE(DBEMT_InputType), INTENT(IN) :: u1 ! Input at t1 > t2 + TYPE(DBEMT_InputType), INTENT(IN) :: u2 ! Input at t2 + REAL(DbKi), INTENT(IN ) :: tin(2) ! Times associated with the Inputs + TYPE(DBEMT_InputType), INTENT(INOUT) :: u_out ! Input at tin_out + REAL(DbKi), INTENT(IN ) :: tin_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + REAL(DbKi) :: t(2) ! Times associated with the Inputs + REAL(DbKi) :: t_out ! Time to which to be extrap/interpd + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_Input_ExtrapInterp1' + REAL(DbKi) :: b ! temporary for extrapolation/interpolation + REAL(DbKi) :: ScaleFactor ! temporary for extrapolation/interpolation + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + INTEGER :: i01 ! dim1 level 0 counter variable for arrays of ddts + INTEGER :: i02 ! dim2 level 0 counter variable for arrays of ddts + INTEGER :: i1 ! dim1 counter variable for arrays + INTEGER :: i2 ! dim2 counter variable for arrays + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + ! we'll subtract a constant from the times to resolve some + ! numerical issues when t gets large (and to simplify the equations) + t = tin - tin(1) + t_out = tin_out - tin(1) + + IF ( EqualRealNos( t(1), t(2) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(2) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + + ScaleFactor = t_out / t(2) + b = -(u1%AxInd_disk - u2%AxInd_disk) + u_out%AxInd_disk = u1%AxInd_disk + b * ScaleFactor + b = -(u1%Un_disk - u2%Un_disk) + u_out%Un_disk = u1%Un_disk + b * ScaleFactor + b = -(u1%R_disk - u2%R_disk) + u_out%R_disk = u1%R_disk + b * ScaleFactor +IF (ALLOCATED(u_out%element) .AND. ALLOCATED(u1%element)) THEN + DO i02 = LBOUND(u_out%element,2),UBOUND(u_out%element,2) + DO i01 = LBOUND(u_out%element,1),UBOUND(u_out%element,1) + DO i1 = LBOUND(u_out%element(i01,i02)%vind_s,1),UBOUND(u_out%element(i01,i02)%vind_s,1) + b = -(u1%element(i01,i02)%vind_s(i1) - u2%element(i01,i02)%vind_s(i1)) + u_out%element(i01,i02)%vind_s(i1) = u1%element(i01,i02)%vind_s(i1) + b * ScaleFactor + END DO + ENDDO + ENDDO + DO i02 = LBOUND(u_out%element,2),UBOUND(u_out%element,2) + DO i01 = LBOUND(u_out%element,1),UBOUND(u_out%element,1) + DO i1 = LBOUND(u_out%element(i01,i02)%vind_s_dot,1),UBOUND(u_out%element(i01,i02)%vind_s_dot,1) + b = -(u1%element(i01,i02)%vind_s_dot(i1) - u2%element(i01,i02)%vind_s_dot(i1)) + u_out%element(i01,i02)%vind_s_dot(i1) = u1%element(i01,i02)%vind_s_dot(i1) + b * ScaleFactor + END DO + ENDDO + ENDDO + DO i02 = LBOUND(u_out%element,2),UBOUND(u_out%element,2) + DO i01 = LBOUND(u_out%element,1),UBOUND(u_out%element,1) + b = -(u1%element(i01,i02)%spanRatio - u2%element(i01,i02)%spanRatio) + u_out%element(i01,i02)%spanRatio = u1%element(i01,i02)%spanRatio + b * ScaleFactor + ENDDO + ENDDO +END IF ! check if allocated + END SUBROUTINE DBEMT_Input_ExtrapInterp1 + + + SUBROUTINE DBEMT_Input_ExtrapInterp2(u1, u2, u3, tin, u_out, tin_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Input u_out at time t_out, from previous/future time +! values of u (which has values associated with times in t). Order of the interpolation is 2. +! +! expressions below based on either +! +! f(t) = a + b * t + c * t**2 +! +! where a, b and c are determined as the solution to +! f(t1) = u1, f(t2) = u2, f(t3) = u3 +! +!.................................................................................................................................. + + TYPE(DBEMT_InputType), INTENT(IN) :: u1 ! Input at t1 > t2 > t3 + TYPE(DBEMT_InputType), INTENT(IN) :: u2 ! Input at t2 > t3 + TYPE(DBEMT_InputType), INTENT(IN) :: u3 ! Input at t3 + REAL(DbKi), INTENT(IN ) :: tin(3) ! Times associated with the Inputs + TYPE(DBEMT_InputType), INTENT(INOUT) :: u_out ! Input at tin_out + REAL(DbKi), INTENT(IN ) :: tin_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + REAL(DbKi) :: t(3) ! Times associated with the Inputs + REAL(DbKi) :: t_out ! Time to which to be extrap/interpd + INTEGER(IntKi) :: order ! order of polynomial fit (max 2) + REAL(DbKi) :: b ! temporary for extrapolation/interpolation + REAL(DbKi) :: c ! temporary for extrapolation/interpolation + REAL(DbKi) :: ScaleFactor ! temporary for extrapolation/interpolation + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_Input_ExtrapInterp2' + INTEGER :: i01 ! dim1 level 0 counter variable for arrays of ddts + INTEGER :: i02 ! dim2 level 0 counter variable for arrays of ddts + INTEGER :: i1 ! dim1 counter variable for arrays + INTEGER :: i2 ! dim2 counter variable for arrays + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + ! we'll subtract a constant from the times to resolve some + ! numerical issues when t gets large (and to simplify the equations) + t = tin - tin(1) + t_out = tin_out - tin(1) + + IF ( EqualRealNos( t(1), t(2) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(2) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + ELSE IF ( EqualRealNos( t(2), t(3) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(2) must not equal t(3) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + ELSE IF ( EqualRealNos( t(1), t(3) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(3) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + + ScaleFactor = t_out / (t(2) * t(3) * (t(2) - t(3))) + b = (t(3)**2*(u1%AxInd_disk - u2%AxInd_disk) + t(2)**2*(-u1%AxInd_disk + u3%AxInd_disk))* scaleFactor + c = ( (t(2)-t(3))*u1%AxInd_disk + t(3)*u2%AxInd_disk - t(2)*u3%AxInd_disk ) * scaleFactor + u_out%AxInd_disk = u1%AxInd_disk + b + c * t_out + b = (t(3)**2*(u1%Un_disk - u2%Un_disk) + t(2)**2*(-u1%Un_disk + u3%Un_disk))* scaleFactor + c = ( (t(2)-t(3))*u1%Un_disk + t(3)*u2%Un_disk - t(2)*u3%Un_disk ) * scaleFactor + u_out%Un_disk = u1%Un_disk + b + c * t_out + b = (t(3)**2*(u1%R_disk - u2%R_disk) + t(2)**2*(-u1%R_disk + u3%R_disk))* scaleFactor + c = ( (t(2)-t(3))*u1%R_disk + t(3)*u2%R_disk - t(2)*u3%R_disk ) * scaleFactor + u_out%R_disk = u1%R_disk + b + c * t_out +IF (ALLOCATED(u_out%element) .AND. ALLOCATED(u1%element)) THEN + DO i02 = LBOUND(u_out%element,2),UBOUND(u_out%element,2) + DO i01 = LBOUND(u_out%element,1),UBOUND(u_out%element,1) + DO i1 = LBOUND(u_out%element(i01,i02)%vind_s,1),UBOUND(u_out%element(i01,i02)%vind_s,1) + b = (t(3)**2*(u1%element(i01,i02)%vind_s(i1) - u2%element(i01,i02)%vind_s(i1)) + t(2)**2*(-u1%element(i01,i02)%vind_s(i1) + u3%element(i01,i02)%vind_s(i1)))* scaleFactor + c = ( (t(2)-t(3))*u1%element(i01,i02)%vind_s(i1) + t(3)*u2%element(i01,i02)%vind_s(i1) - t(2)*u3%element(i01,i02)%vind_s(i1) ) * scaleFactor + u_out%element(i01,i02)%vind_s(i1) = u1%element(i01,i02)%vind_s(i1) + b + c * t_out + END DO + ENDDO + ENDDO + DO i02 = LBOUND(u_out%element,2),UBOUND(u_out%element,2) + DO i01 = LBOUND(u_out%element,1),UBOUND(u_out%element,1) + DO i1 = LBOUND(u_out%element(i01,i02)%vind_s_dot,1),UBOUND(u_out%element(i01,i02)%vind_s_dot,1) + b = (t(3)**2*(u1%element(i01,i02)%vind_s_dot(i1) - u2%element(i01,i02)%vind_s_dot(i1)) + t(2)**2*(-u1%element(i01,i02)%vind_s_dot(i1) + u3%element(i01,i02)%vind_s_dot(i1)))* scaleFactor + c = ( (t(2)-t(3))*u1%element(i01,i02)%vind_s_dot(i1) + t(3)*u2%element(i01,i02)%vind_s_dot(i1) - t(2)*u3%element(i01,i02)%vind_s_dot(i1) ) * scaleFactor + u_out%element(i01,i02)%vind_s_dot(i1) = u1%element(i01,i02)%vind_s_dot(i1) + b + c * t_out + END DO + ENDDO + ENDDO + DO i02 = LBOUND(u_out%element,2),UBOUND(u_out%element,2) + DO i01 = LBOUND(u_out%element,1),UBOUND(u_out%element,1) + b = (t(3)**2*(u1%element(i01,i02)%spanRatio - u2%element(i01,i02)%spanRatio) + t(2)**2*(-u1%element(i01,i02)%spanRatio + u3%element(i01,i02)%spanRatio))* scaleFactor + c = ( (t(2)-t(3))*u1%element(i01,i02)%spanRatio + t(3)*u2%element(i01,i02)%spanRatio - t(2)*u3%element(i01,i02)%spanRatio ) * scaleFactor + u_out%element(i01,i02)%spanRatio = u1%element(i01,i02)%spanRatio + b + c * t_out + ENDDO + ENDDO +END IF ! check if allocated + END SUBROUTINE DBEMT_Input_ExtrapInterp2 + + + SUBROUTINE DBEMT_Output_ExtrapInterp(y, t, y_out, t_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Output y_out at time t_out, from previous/future time +! values of y (which has values associated with times in t). Order of the interpolation is given by the size of y +! +! expressions below based on either +! +! f(t) = a +! f(t) = a + b * t, or +! f(t) = a + b * t + c * t**2 +! +! where a, b and c are determined as the solution to +! f(t1) = y1, f(t2) = y2, f(t3) = y3 (as appropriate) +! +!.................................................................................................................................. + + TYPE(DBEMT_OutputType), INTENT(IN) :: y(:) ! Output at t1 > t2 > t3 + REAL(DbKi), INTENT(IN ) :: t(:) ! Times associated with the Outputs + TYPE(DBEMT_OutputType), INTENT(INOUT) :: y_out ! Output at tin_out + REAL(DbKi), INTENT(IN ) :: t_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + INTEGER(IntKi) :: order ! order of polynomial fit (max 2) + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_Output_ExtrapInterp' + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + if ( size(t) .ne. size(y)) then + CALL SetErrStat(ErrID_Fatal,'size(t) must equal size(y)',ErrStat,ErrMsg,RoutineName) + RETURN + endif + order = SIZE(y) - 1 + IF ( order .eq. 0 ) THEN + CALL DBEMT_CopyOutput(y(1), y_out, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE IF ( order .eq. 1 ) THEN + CALL DBEMT_Output_ExtrapInterp1(y(1), y(2), t, y_out, t_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE IF ( order .eq. 2 ) THEN + CALL DBEMT_Output_ExtrapInterp2(y(1), y(2), y(3), t, y_out, t_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE + CALL SetErrStat(ErrID_Fatal,'size(y) must be less than 4 (order must be less than 3).',ErrStat,ErrMsg,RoutineName) + RETURN + ENDIF + END SUBROUTINE DBEMT_Output_ExtrapInterp + + + SUBROUTINE DBEMT_Output_ExtrapInterp1(y1, y2, tin, y_out, tin_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Output y_out at time t_out, from previous/future time +! values of y (which has values associated with times in t). Order of the interpolation is 1. +! +! f(t) = a + b * t, or +! +! where a and b are determined as the solution to +! f(t1) = y1, f(t2) = y2 +! +!.................................................................................................................................. + + TYPE(DBEMT_OutputType), INTENT(IN) :: y1 ! Output at t1 > t2 + TYPE(DBEMT_OutputType), INTENT(IN) :: y2 ! Output at t2 + REAL(DbKi), INTENT(IN ) :: tin(2) ! Times associated with the Outputs + TYPE(DBEMT_OutputType), INTENT(INOUT) :: y_out ! Output at tin_out + REAL(DbKi), INTENT(IN ) :: tin_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + REAL(DbKi) :: t(2) ! Times associated with the Outputs + REAL(DbKi) :: t_out ! Time to which to be extrap/interpd + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_Output_ExtrapInterp1' + REAL(DbKi) :: b ! temporary for extrapolation/interpolation + REAL(DbKi) :: ScaleFactor ! temporary for extrapolation/interpolation + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + INTEGER :: i01 ! dim1 level 0 counter variable for arrays of ddts + INTEGER :: i02 ! dim2 level 0 counter variable for arrays of ddts + INTEGER :: i03 ! dim3 level 0 counter variable for arrays of ddts + INTEGER :: i1 ! dim1 counter variable for arrays + INTEGER :: i2 ! dim2 counter variable for arrays + INTEGER :: i3 ! dim3 counter variable for arrays + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + ! we'll subtract a constant from the times to resolve some + ! numerical issues when t gets large (and to simplify the equations) + t = tin - tin(1) + t_out = tin_out - tin(1) + + IF ( EqualRealNos( t(1), t(2) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(2) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + + ScaleFactor = t_out / t(2) +IF (ALLOCATED(y_out%vind) .AND. ALLOCATED(y1%vind)) THEN + DO i3 = LBOUND(y_out%vind,3),UBOUND(y_out%vind,3) + DO i2 = LBOUND(y_out%vind,2),UBOUND(y_out%vind,2) + DO i1 = LBOUND(y_out%vind,1),UBOUND(y_out%vind,1) + b = -(y1%vind(i1,i2,i3) - y2%vind(i1,i2,i3)) + y_out%vind(i1,i2,i3) = y1%vind(i1,i2,i3) + b * ScaleFactor + END DO + END DO + END DO +END IF ! check if allocated + END SUBROUTINE DBEMT_Output_ExtrapInterp1 + + + SUBROUTINE DBEMT_Output_ExtrapInterp2(y1, y2, y3, tin, y_out, tin_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Output y_out at time t_out, from previous/future time +! values of y (which has values associated with times in t). Order of the interpolation is 2. +! +! expressions below based on either +! +! f(t) = a + b * t + c * t**2 +! +! where a, b and c are determined as the solution to +! f(t1) = y1, f(t2) = y2, f(t3) = y3 +! +!.................................................................................................................................. + + TYPE(DBEMT_OutputType), INTENT(IN) :: y1 ! Output at t1 > t2 > t3 + TYPE(DBEMT_OutputType), INTENT(IN) :: y2 ! Output at t2 > t3 + TYPE(DBEMT_OutputType), INTENT(IN) :: y3 ! Output at t3 + REAL(DbKi), INTENT(IN ) :: tin(3) ! Times associated with the Outputs + TYPE(DBEMT_OutputType), INTENT(INOUT) :: y_out ! Output at tin_out + REAL(DbKi), INTENT(IN ) :: tin_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + REAL(DbKi) :: t(3) ! Times associated with the Outputs + REAL(DbKi) :: t_out ! Time to which to be extrap/interpd + INTEGER(IntKi) :: order ! order of polynomial fit (max 2) + REAL(DbKi) :: b ! temporary for extrapolation/interpolation + REAL(DbKi) :: c ! temporary for extrapolation/interpolation + REAL(DbKi) :: ScaleFactor ! temporary for extrapolation/interpolation + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_Output_ExtrapInterp2' + INTEGER :: i01 ! dim1 level 0 counter variable for arrays of ddts + INTEGER :: i02 ! dim2 level 0 counter variable for arrays of ddts + INTEGER :: i03 ! dim3 level 0 counter variable for arrays of ddts + INTEGER :: i1 ! dim1 counter variable for arrays + INTEGER :: i2 ! dim2 counter variable for arrays + INTEGER :: i3 ! dim3 counter variable for arrays + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + ! we'll subtract a constant from the times to resolve some + ! numerical issues when t gets large (and to simplify the equations) + t = tin - tin(1) + t_out = tin_out - tin(1) + + IF ( EqualRealNos( t(1), t(2) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(2) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + ELSE IF ( EqualRealNos( t(2), t(3) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(2) must not equal t(3) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + ELSE IF ( EqualRealNos( t(1), t(3) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(3) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + + ScaleFactor = t_out / (t(2) * t(3) * (t(2) - t(3))) +IF (ALLOCATED(y_out%vind) .AND. ALLOCATED(y1%vind)) THEN + DO i3 = LBOUND(y_out%vind,3),UBOUND(y_out%vind,3) + DO i2 = LBOUND(y_out%vind,2),UBOUND(y_out%vind,2) + DO i1 = LBOUND(y_out%vind,1),UBOUND(y_out%vind,1) + b = (t(3)**2*(y1%vind(i1,i2,i3) - y2%vind(i1,i2,i3)) + t(2)**2*(-y1%vind(i1,i2,i3) + y3%vind(i1,i2,i3)))* scaleFactor + c = ( (t(2)-t(3))*y1%vind(i1,i2,i3) + t(3)*y2%vind(i1,i2,i3) - t(2)*y3%vind(i1,i2,i3) ) * scaleFactor + y_out%vind(i1,i2,i3) = y1%vind(i1,i2,i3) + b + c * t_out + END DO + END DO + END DO +END IF ! check if allocated + END SUBROUTINE DBEMT_Output_ExtrapInterp2 + +END MODULE DBEMT_Types +!ENDOFREGISTRYGENERATEDFILE diff --git a/modules/inflowwind/src/IfW_UserWind_Types.f90 b/modules/inflowwind/src/IfW_UserWind_Types.f90 new file mode 100644 index 0000000000..1fa5c484ac --- /dev/null +++ b/modules/inflowwind/src/IfW_UserWind_Types.f90 @@ -0,0 +1,646 @@ +!STARTOFREGISTRYGENERATEDFILE 'IfW_UserWind_Types.f90' +! +! WARNING This file is generated automatically by the FAST registry. +! Do not edit. Your changes to this file will be lost. +! +! FAST Registry +!********************************************************************************************************************************* +! IfW_UserWind_Types +!................................................................................................................................. +! This file is part of IfW_UserWind. +! +! Copyright (C) 2012-2016 National Renewable Energy Laboratory +! +! Licensed under the Apache License, Version 2.0 (the "License"); +! you may not use this file except in compliance with the License. +! You may obtain a copy of the License at +! +! http://www.apache.org/licenses/LICENSE-2.0 +! +! Unless required by applicable law or agreed to in writing, software +! distributed under the License is distributed on an "AS IS" BASIS, +! WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +! See the License for the specific language governing permissions and +! limitations under the License. +! +! +! W A R N I N G : This file was automatically generated from the FAST registry. Changes made to this file may be lost. +! +!********************************************************************************************************************************* +!> This module contains the user-defined types needed in IfW_UserWind. It also contains copy, destroy, pack, and +!! unpack routines associated with each defined data type. This code is automatically generated by the FAST Registry. +MODULE IfW_UserWind_Types +!--------------------------------------------------------------------------------------------------------------------------------- +USE NWTC_Library +IMPLICIT NONE +! ========= IfW_UserWind_InitInputType ======= + TYPE, PUBLIC :: IfW_UserWind_InitInputType + CHARACTER(1024) :: WindFileName !< Name of the wind file to use [-] + END TYPE IfW_UserWind_InitInputType +! ======================= +! ========= IfW_UserWind_InitOutputType ======= + TYPE, PUBLIC :: IfW_UserWind_InitOutputType + TYPE(ProgDesc) :: Ver !< Version information off HHWind submodule [-] + END TYPE IfW_UserWind_InitOutputType +! ======================= +! ========= IfW_UserWind_MiscVarType ======= + TYPE, PUBLIC :: IfW_UserWind_MiscVarType + REAL(ReKi) :: DummyMiscVar !< Remove this variable if you have misc variables [-] + END TYPE IfW_UserWind_MiscVarType +! ======================= +! ========= IfW_UserWind_ParameterType ======= + TYPE, PUBLIC :: IfW_UserWind_ParameterType + REAL(SiKi) :: dummy !< remove if you have parameters [-] + END TYPE IfW_UserWind_ParameterType +! ======================= +CONTAINS + SUBROUTINE IfW_UserWind_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, ErrStat, ErrMsg ) + TYPE(IfW_UserWind_InitInputType), INTENT(IN) :: SrcInitInputData + TYPE(IfW_UserWind_InitInputType), INTENT(INOUT) :: DstInitInputData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'IfW_UserWind_CopyInitInput' +! + ErrStat = ErrID_None + ErrMsg = "" + DstInitInputData%WindFileName = SrcInitInputData%WindFileName + END SUBROUTINE IfW_UserWind_CopyInitInput + + SUBROUTINE IfW_UserWind_DestroyInitInput( InitInputData, ErrStat, ErrMsg ) + TYPE(IfW_UserWind_InitInputType), INTENT(INOUT) :: InitInputData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'IfW_UserWind_DestroyInitInput' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE IfW_UserWind_DestroyInitInput + + SUBROUTINE IfW_UserWind_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(IfW_UserWind_InitInputType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'IfW_UserWind_PackInitInput' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1*LEN(InData%WindFileName) ! WindFileName + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + DO I = 1, LEN(InData%WindFileName) + IntKiBuf(Int_Xferred) = ICHAR(InData%WindFileName(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + END SUBROUTINE IfW_UserWind_PackInitInput + + SUBROUTINE IfW_UserWind_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(IfW_UserWind_InitInputType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'IfW_UserWind_UnPackInitInput' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + DO I = 1, LEN(OutData%WindFileName) + OutData%WindFileName(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + END SUBROUTINE IfW_UserWind_UnPackInitInput + + SUBROUTINE IfW_UserWind_CopyInitOutput( SrcInitOutputData, DstInitOutputData, CtrlCode, ErrStat, ErrMsg ) + TYPE(IfW_UserWind_InitOutputType), INTENT(IN) :: SrcInitOutputData + TYPE(IfW_UserWind_InitOutputType), INTENT(INOUT) :: DstInitOutputData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'IfW_UserWind_CopyInitOutput' +! + ErrStat = ErrID_None + ErrMsg = "" + CALL NWTC_Library_Copyprogdesc( SrcInitOutputData%Ver, DstInitOutputData%Ver, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + END SUBROUTINE IfW_UserWind_CopyInitOutput + + SUBROUTINE IfW_UserWind_DestroyInitOutput( InitOutputData, ErrStat, ErrMsg ) + TYPE(IfW_UserWind_InitOutputType), INTENT(INOUT) :: InitOutputData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'IfW_UserWind_DestroyInitOutput' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + CALL NWTC_Library_Destroyprogdesc( InitOutputData%Ver, ErrStat, ErrMsg ) + END SUBROUTINE IfW_UserWind_DestroyInitOutput + + SUBROUTINE IfW_UserWind_PackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(IfW_UserWind_InitOutputType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'IfW_UserWind_PackInitOutput' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + Int_BufSz = Int_BufSz + 3 ! Ver: size of buffers for each call to pack subtype + CALL NWTC_Library_Packprogdesc( Re_Buf, Db_Buf, Int_Buf, InData%Ver, ErrStat2, ErrMsg2, .TRUE. ) ! Ver + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! Ver + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! Ver + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! Ver + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + CALL NWTC_Library_Packprogdesc( Re_Buf, Db_Buf, Int_Buf, InData%Ver, ErrStat2, ErrMsg2, OnlySize ) ! Ver + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END SUBROUTINE IfW_UserWind_PackInitOutput + + SUBROUTINE IfW_UserWind_UnPackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(IfW_UserWind_InitOutputType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'IfW_UserWind_UnPackInitOutput' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL NWTC_Library_Unpackprogdesc( Re_Buf, Db_Buf, Int_Buf, OutData%Ver, ErrStat2, ErrMsg2 ) ! Ver + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END SUBROUTINE IfW_UserWind_UnPackInitOutput + + SUBROUTINE IfW_UserWind_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) + TYPE(IfW_UserWind_MiscVarType), INTENT(IN) :: SrcMiscData + TYPE(IfW_UserWind_MiscVarType), INTENT(INOUT) :: DstMiscData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'IfW_UserWind_CopyMisc' +! + ErrStat = ErrID_None + ErrMsg = "" + DstMiscData%DummyMiscVar = SrcMiscData%DummyMiscVar + END SUBROUTINE IfW_UserWind_CopyMisc + + SUBROUTINE IfW_UserWind_DestroyMisc( MiscData, ErrStat, ErrMsg ) + TYPE(IfW_UserWind_MiscVarType), INTENT(INOUT) :: MiscData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'IfW_UserWind_DestroyMisc' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE IfW_UserWind_DestroyMisc + + SUBROUTINE IfW_UserWind_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(IfW_UserWind_MiscVarType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'IfW_UserWind_PackMisc' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Re_BufSz = Re_BufSz + 1 ! DummyMiscVar + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + ReKiBuf(Re_Xferred) = InData%DummyMiscVar + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE IfW_UserWind_PackMisc + + SUBROUTINE IfW_UserWind_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(IfW_UserWind_MiscVarType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'IfW_UserWind_UnPackMisc' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%DummyMiscVar = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE IfW_UserWind_UnPackMisc + + SUBROUTINE IfW_UserWind_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) + TYPE(IfW_UserWind_ParameterType), INTENT(IN) :: SrcParamData + TYPE(IfW_UserWind_ParameterType), INTENT(INOUT) :: DstParamData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'IfW_UserWind_CopyParam' +! + ErrStat = ErrID_None + ErrMsg = "" + DstParamData%dummy = SrcParamData%dummy + END SUBROUTINE IfW_UserWind_CopyParam + + SUBROUTINE IfW_UserWind_DestroyParam( ParamData, ErrStat, ErrMsg ) + TYPE(IfW_UserWind_ParameterType), INTENT(INOUT) :: ParamData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'IfW_UserWind_DestroyParam' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE IfW_UserWind_DestroyParam + + SUBROUTINE IfW_UserWind_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(IfW_UserWind_ParameterType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'IfW_UserWind_PackParam' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Re_BufSz = Re_BufSz + 1 ! dummy + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + ReKiBuf(Re_Xferred) = InData%dummy + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE IfW_UserWind_PackParam + + SUBROUTINE IfW_UserWind_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(IfW_UserWind_ParameterType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'IfW_UserWind_UnPackParam' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%dummy = REAL(ReKiBuf(Re_Xferred), SiKi) + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE IfW_UserWind_UnPackParam + +END MODULE IfW_UserWind_Types +!ENDOFREGISTRYGENERATEDFILE diff --git a/modules/servodyn/src/StrucCtrl_Types.f90 b/modules/servodyn/src/StrucCtrl_Types.f90 new file mode 100644 index 0000000000..6e56561efa --- /dev/null +++ b/modules/servodyn/src/StrucCtrl_Types.f90 @@ -0,0 +1,4723 @@ +!STARTOFREGISTRYGENERATEDFILE 'StrucCtrl_Types.f90' +! +! WARNING This file is generated automatically by the FAST registry. +! Do not edit. Your changes to this file will be lost. +! +! FAST Registry +!********************************************************************************************************************************* +! StrucCtrl_Types +!................................................................................................................................. +! This file is part of StrucCtrl. +! +! Copyright (C) 2012-2016 National Renewable Energy Laboratory +! +! Licensed under the Apache License, Version 2.0 (the "License"); +! you may not use this file except in compliance with the License. +! You may obtain a copy of the License at +! +! http://www.apache.org/licenses/LICENSE-2.0 +! +! Unless required by applicable law or agreed to in writing, software +! distributed under the License is distributed on an "AS IS" BASIS, +! WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +! See the License for the specific language governing permissions and +! limitations under the License. +! +! +! W A R N I N G : This file was automatically generated from the FAST registry. Changes made to this file may be lost. +! +!********************************************************************************************************************************* +!> This module contains the user-defined types needed in StrucCtrl. It also contains copy, destroy, pack, and +!! unpack routines associated with each defined data type. This code is automatically generated by the FAST Registry. +MODULE StrucCtrl_Types +!--------------------------------------------------------------------------------------------------------------------------------- +USE NWTC_Library +IMPLICIT NONE +! ========= StC_InputFile ======= + TYPE, PUBLIC :: StC_InputFile + CHARACTER(1024) :: StCFileName !< Name of the input file; remove if there is no file [-] + LOGICAL :: Echo !< Echo input file to echo file [-] + INTEGER(IntKi) :: StC_CMODE !< control mode {0:none; 1: Semi-Active Control Mode; 2: Active Control Mode;} [-] + INTEGER(IntKi) :: StC_SA_MODE !< Semi-Active control mode {1: velocity-based ground hook control; 2: Inverse velocity-based ground hook control; 3: displacement-based ground hook control 4: Phase difference Algorithm with Friction Force 5: Phase difference Algorithm with Damping Force} [-] + INTEGER(IntKi) :: StC_DOF_MODE !< DOF mode {0: NO StC_DOF; 1: StC_X_DOF and StC_Y_DOF; 2: StC_XY_DOF; 3: TLCD; 4: Prescribed force/moment time series} [-] + LOGICAL :: StC_X_DOF !< DOF on or off [-] + LOGICAL :: StC_Y_DOF !< DOF on or off [-] + LOGICAL :: StC_Z_DOF !< DOF on or off [-] + REAL(ReKi) :: StC_X_DSP !< StC_X initial displacement [m] + REAL(ReKi) :: StC_Y_DSP !< StC_Y initial displacement [m] + REAL(ReKi) :: StC_Z_DSP !< StC_Z initial displacement [m] + REAL(ReKi) :: StC_X_M !< StC X mass [kg] + REAL(ReKi) :: StC_Y_M !< StC Y mass [kg] + REAL(ReKi) :: StC_Z_M !< StC Z mass [kg] + REAL(ReKi) :: StC_XY_M !< StC XY mass [kg] + REAL(ReKi) :: StC_X_K !< StC X stiffness [N/m] + REAL(ReKi) :: StC_Y_K !< StC Y stiffness [N/m] + REAL(ReKi) :: StC_Z_K !< StC Y stiffness [N/m] + REAL(ReKi) :: StC_X_C !< StC X damping [N/(m/s)] + REAL(ReKi) :: StC_Y_C !< StC Y damping [N/(m/s)] + REAL(ReKi) :: StC_Z_C !< StC Z damping [N/(m/s)] + REAL(ReKi) :: StC_X_PSP !< Positive stop position (maximum X mass displacement) [m] + REAL(ReKi) :: StC_X_NSP !< Negative stop position (minimum X mass displacement) [m] + REAL(ReKi) :: StC_Y_PSP !< Positive stop position (maximum Y mass displacement) [m] + REAL(ReKi) :: StC_Y_NSP !< Negative stop position (minimum Y mass displacement) [m] + REAL(ReKi) :: StC_Z_PSP !< Positive stop position (maximum Z mass displacement) [m] + REAL(ReKi) :: StC_Z_NSP !< Negative stop position (minimum Z mass displacement) [m] + REAL(ReKi) :: StC_X_KS !< Stop spring X stiffness [N/m] + REAL(ReKi) :: StC_X_CS !< Stop spring X damping [N/(m/s)] + REAL(ReKi) :: StC_Y_KS !< Stop spring Y stiffness [N/m] + REAL(ReKi) :: StC_Y_CS !< Stop spring Y damping [N/(m/s)] + REAL(ReKi) :: StC_Z_KS !< Stop spring Z stiffness [used only when StC_DOF_MODE=1 and StC_Z_DOF=TRUE] [N/m] + REAL(ReKi) :: StC_Z_CS !< Stop spring Z damping [used only when StC_DOF_MODE=1 and StC_Z_DOF=TRUE] [N/(m/s)] + REAL(ReKi) :: StC_P_X !< StC X initial displacement (m) [relative to at rest position] [m] + REAL(ReKi) :: StC_P_Y !< StC Y initial displacement (m) [relative to at rest position] [m] + REAL(ReKi) :: StC_P_Z !< StC Z initial displacement (m) [relative to at rest position; used only when StC_DOF_MODE=1 and StC_Z_DOF=TRUE] [m] + REAL(ReKi) :: StC_X_C_HIGH !< StC X high damping for ground hook control [N/(m/s)] + REAL(ReKi) :: StC_X_C_LOW !< StC X low damping for ground hook control [N/(m/s)] + REAL(ReKi) :: StC_Y_C_HIGH !< StC Y high damping for ground hook control [N/(m/s)] + REAL(ReKi) :: StC_Y_C_LOW !< StC Y low damping for ground hook control [N/(m/s)] + REAL(ReKi) :: StC_Z_C_HIGH !< StC Z high damping for ground hook control [N/(m/s)] + REAL(ReKi) :: StC_Z_C_LOW !< StC Z low damping for ground hook control [N/(m/s)] + REAL(ReKi) :: StC_X_C_BRAKE !< StC X high damping for braking the StC [N/(m/s)] + REAL(ReKi) :: StC_Y_C_BRAKE !< StC Y high damping for braking the StC [N/(m/s)] + REAL(ReKi) :: StC_Z_C_BRAKE !< StC Z high damping for braking the StC [N/(m/s)] + REAL(ReKi) :: L_X !< X TLCD total length [m] + REAL(ReKi) :: B_X !< X TLCD horizontal length [m] + REAL(ReKi) :: area_X !< X TLCD cross-sectional area of vertical column [m^2] + REAL(ReKi) :: area_ratio_X !< X TLCD cross-sectional area ratio (vertical column area divided by horizontal column area) [-] + REAL(ReKi) :: headLossCoeff_X !< X TLCD head loss coeff [-] + REAL(ReKi) :: rho_X !< X TLCD liquid density [kg/m^3] + REAL(ReKi) :: L_Y !< Y TLCD total length [m] + REAL(ReKi) :: B_Y !< Y TLCD horizontal length [m] + REAL(ReKi) :: area_Y !< Side-Side TLCD cross-sectional area of vertical column [m] + REAL(ReKi) :: area_ratio_Y !< Side-Side TLCD cross-sectional area ratio (vertical column area divided by horizontal column area) [-] + REAL(ReKi) :: headLossCoeff_Y !< Side-Side TLCD head loss coeff [-] + REAL(ReKi) :: rho_Y !< Side-Side TLCD liquid density [kg/m^3] + LOGICAL :: USE_F_TBL !< use spring force from user-defined table (flag) [-] + INTEGER(IntKi) :: NKInpSt !< Number of input spring force rows in table [-] + CHARACTER(1024) :: StC_F_TBL_FILE !< user-defined spring table filename [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: F_TBL !< user-defined spring force [N] + INTEGER(IntKi) :: PrescribedForcesCoordSys !< Prescribed forces coordinate system {0: global; 1: local} [-] + CHARACTER(1024) :: PrescribedForcesFile !< Prescribed force time-series filename [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: StC_PrescribedForce !< StC prescribed force time-series info [(s,N,N-m)] + END TYPE StC_InputFile +! ======================= +! ========= StC_InitInputType ======= + TYPE, PUBLIC :: StC_InitInputType + CHARACTER(1024) :: InputFile !< Name of the input file; remove if there is no file [-] + CHARACTER(1024) :: RootName !< RootName for writing output files [-] + REAL(ReKi) , DIMENSION(1:3) :: Gravity !< Gravitational acceleration vector [m/s^2] + INTEGER(IntKi) :: NumMeshPts !< Number of mesh points [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: InitPosition !< X-Y-Z reference position of point: i.e. each blade root (3 x NumBlades) [m] + REAL(R8Ki) , DIMENSION(:,:,:), ALLOCATABLE :: InitOrientation !< DCM reference orientation of point: i.e. each blade root (3x3 x NumBlades) [-] + LOGICAL :: UseInputFile = .TRUE. !< Read from the input file. If false, must parse the string info passed [-] + TYPE(FileInfoType) :: PassedPrimaryInputData !< Primary input file as FileInfoType (set by driver/glue code) [-] + LOGICAL :: UseInputFile_PrescribeFrc = .TRUE. !< Read from the input file. If false, must parse the string info passed [-] + TYPE(FileInfoType) :: PassedPrescribeFrcData !< Prescribed forces input file as FileInfoType (set by driver/glue code) [-] + END TYPE StC_InitInputType +! ======================= +! ========= StC_InitOutputType ======= + TYPE, PUBLIC :: StC_InitOutputType + REAL(SiKi) :: DummyInitOut !< dummy init output [-] + END TYPE StC_InitOutputType +! ======================= +! ========= StC_ContinuousStateType ======= + TYPE, PUBLIC :: StC_ContinuousStateType + REAL(ReKi) :: DummyContState !< Remove this variable if you have continuous states [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: StC_x !< Continuous States -- StrucCtrl x [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: StC_xdot !< Continuous States -- StrucCtrl xdot [-] + END TYPE StC_ContinuousStateType +! ======================= +! ========= StC_DiscreteStateType ======= + TYPE, PUBLIC :: StC_DiscreteStateType + REAL(ReKi) :: DummyDiscState !< Remove this variable if you have discrete states [-] + END TYPE StC_DiscreteStateType +! ======================= +! ========= StC_ConstraintStateType ======= + TYPE, PUBLIC :: StC_ConstraintStateType + REAL(ReKi) :: DummyConstrState !< Remove this variable if you have constraint states [-] + END TYPE StC_ConstraintStateType +! ======================= +! ========= StC_OtherStateType ======= + TYPE, PUBLIC :: StC_OtherStateType + REAL(ReKi) :: DummyOtherState !< Remove this variable if you have other/logical states [-] + END TYPE StC_OtherStateType +! ======================= +! ========= StC_MiscVarType ======= + TYPE, PUBLIC :: StC_MiscVarType + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: F_stop !< Stop forces [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: F_ext !< External forces (user defined) [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: F_fr !< Friction forces [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: C_ctrl !< Controlled Damping (On/Off) [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: C_Brake !< Braking Damping [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: F_table !< Tabled Stiffness [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: F_k !< Factor for x and y-component stiffness force [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: a_G !< Gravitational acceleration vector, local coordinates for point [m/s^2] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: rdisp_P !< Translational displacement vector, local coordinates for point [m] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: rdot_P !< Translational velocity vector, local coordinates for point [m/s] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: rddot_P !< Translational acceleration vector, local coordinates for point [m/s^2] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: omega_P !< Rotational velocity vector, local coordinates for point [rad/s] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: alpha_P !< Rotational aceeleration vector, local coordinates for point [rad/s^2] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: F_P !< StC force vector, local coordinates for point [N] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: M_P !< StC moment vector, local coordinates for point [N-m] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: Acc !< StC aggregated acceleration in X,Y local coordinates for point [m/s^2] + INTEGER(IntKi) :: PrescribedInterpIdx !< Index for interpolation of Prescribed force array [-] + END TYPE StC_MiscVarType +! ======================= +! ========= StC_ParameterType ======= + TYPE, PUBLIC :: StC_ParameterType + REAL(DbKi) :: DT !< Time step for cont. state integration & disc. state update [seconds] + CHARACTER(1024) :: RootName !< RootName for writing output files [-] + INTEGER(IntKi) :: StC_DOF_MODE !< DOF mode {0: NO StC_DOF; 1: StC_X_DOF and StC_Y_DOF; 2: StC_XY_DOF; 3: TLCD; 4: Prescribed force/moment time series} [-] + LOGICAL :: StC_X_DOF !< DOF on or off [-] + LOGICAL :: StC_Y_DOF !< DOF on or off [-] + LOGICAL :: StC_Z_DOF !< DOF on or off [-] + REAL(ReKi) :: M_X !< StC mass [kg] + REAL(ReKi) :: M_Y !< StC mass [kg] + REAL(ReKi) :: M_Z !< StC mass [kg] + REAL(ReKi) :: M_XY !< StCXY mass [kg] + REAL(ReKi) :: K_X !< StC stiffness [N/m] + REAL(ReKi) :: K_Y !< StC stiffness [N/m] + REAL(ReKi) :: K_Z !< StC stiffness [N/m] + REAL(ReKi) :: C_X !< StC damping [N/(m/s)] + REAL(ReKi) :: C_Y !< StC damping [N/(m/s)] + REAL(ReKi) :: C_Z !< StC damping [N/(m/s)] + REAL(ReKi) , DIMENSION(1:3) :: K_S !< StC stop stiffness [N/m] + REAL(ReKi) , DIMENSION(1:3) :: C_S !< StC stop damping [N/(m/s)] + REAL(ReKi) , DIMENSION(1:3) :: P_SP !< Positive stop position (maximum mass displacement) [m] + REAL(ReKi) , DIMENSION(1:3) :: N_SP !< Negative stop position (minimum X mass displacement) [m] + REAL(ReKi) , DIMENSION(1:3) :: Gravity !< Gravitational acceleration vector [m/s^2] + INTEGER(IntKi) :: StC_CMODE !< control mode {0:none; 1: Semi-Active Control Mode; 2: Active Control Mode;} [-] + INTEGER(IntKi) :: StC_SA_MODE !< Semi-Active control mode {1: velocity-based ground hook control; 2: Inverse velocity-based ground hook control; 3: displacement-based ground hook control 4: Phase difference Algorithm with Friction Force 5: Phase difference Algorithm with Damping Force} [-] + REAL(ReKi) :: StC_X_C_HIGH !< StC X high damping for ground hook control [N/(m/s)] + REAL(ReKi) :: StC_X_C_LOW !< StC X low damping for ground hook control [N/(m/s)] + REAL(ReKi) :: StC_Y_C_HIGH !< StC Y high damping for ground hook control [N/(m/s)] + REAL(ReKi) :: StC_Y_C_LOW !< StC Y low damping for ground hook control [N/(m/s)] + REAL(ReKi) :: StC_Z_C_HIGH !< StC Z high damping for ground hook control [N/(m/s)] + REAL(ReKi) :: StC_Z_C_LOW !< StC Z low damping for ground hook control [N/(m/s)] + REAL(ReKi) :: StC_X_C_BRAKE !< StC X high damping for braking the StC [N/(m/s)] + REAL(ReKi) :: StC_Y_C_BRAKE !< StC Y high damping for braking the StC [N/(m/s)] + REAL(ReKi) :: StC_Z_C_BRAKE !< StC Y high damping for braking the StC [N/(m/s)] + REAL(ReKi) :: L_X !< X TLCD total length [m] + REAL(ReKi) :: B_X !< X TLCD horizontal length [m] + REAL(ReKi) :: area_X !< X TLCD cross-sectional area of vertical column [m^2] + REAL(ReKi) :: area_ratio_X !< X TLCD cross-sectional area ratio (vertical column area divided by horizontal column area) [-] + REAL(ReKi) :: headLossCoeff_X !< X TLCD head loss coeff [-] + REAL(ReKi) :: rho_X !< X TLCD liquid density [kg/m^3] + REAL(ReKi) :: L_Y !< Y TLCD total length [m] + REAL(ReKi) :: B_Y !< Y TLCD horizontal length [m] + REAL(ReKi) :: area_Y !< Side-Side TLCD cross-sectional area of vertical column [m] + REAL(ReKi) :: area_ratio_Y !< Side-Side TLCD cross-sectional area ratio (vertical column area divided by horizontal column area) [-] + REAL(ReKi) :: headLossCoeff_Y !< Side-Side TLCD head loss coeff [-] + REAL(ReKi) :: rho_Y !< Side-Side TLCD liquid density [kg/m^3] + LOGICAL :: Use_F_TBL !< use spring force from user-defined table (flag) [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: F_TBL !< user-defined spring force [N] + INTEGER(IntKi) :: NumMeshPts !< Number of mesh points [-] + INTEGER(IntKi) :: PrescribedForcesCoordSys !< Prescribed forces coordinate system {0: global; 1: local} [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: StC_PrescribedForce !< StC prescribed force time-series info [(s,N,N-m)] + END TYPE StC_ParameterType +! ======================= +! ========= StC_InputType ======= + TYPE, PUBLIC :: StC_InputType + TYPE(MeshType) , DIMENSION(:), ALLOCATABLE :: Mesh !< Displacements at the StC reference point(s) P in the inertial frame [-] + END TYPE StC_InputType +! ======================= +! ========= StC_OutputType ======= + TYPE, PUBLIC :: StC_OutputType + TYPE(MeshType) , DIMENSION(:), ALLOCATABLE :: Mesh !< Loads at the StC reference points in the inertial frame [-] + END TYPE StC_OutputType +! ======================= +CONTAINS + SUBROUTINE StC_CopyInputFile( SrcInputFileData, DstInputFileData, CtrlCode, ErrStat, ErrMsg ) + TYPE(StC_InputFile), INTENT(IN) :: SrcInputFileData + TYPE(StC_InputFile), INTENT(INOUT) :: DstInputFileData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_CopyInputFile' +! + ErrStat = ErrID_None + ErrMsg = "" + DstInputFileData%StCFileName = SrcInputFileData%StCFileName + DstInputFileData%Echo = SrcInputFileData%Echo + DstInputFileData%StC_CMODE = SrcInputFileData%StC_CMODE + DstInputFileData%StC_SA_MODE = SrcInputFileData%StC_SA_MODE + DstInputFileData%StC_DOF_MODE = SrcInputFileData%StC_DOF_MODE + DstInputFileData%StC_X_DOF = SrcInputFileData%StC_X_DOF + DstInputFileData%StC_Y_DOF = SrcInputFileData%StC_Y_DOF + DstInputFileData%StC_Z_DOF = SrcInputFileData%StC_Z_DOF + DstInputFileData%StC_X_DSP = SrcInputFileData%StC_X_DSP + DstInputFileData%StC_Y_DSP = SrcInputFileData%StC_Y_DSP + DstInputFileData%StC_Z_DSP = SrcInputFileData%StC_Z_DSP + DstInputFileData%StC_X_M = SrcInputFileData%StC_X_M + DstInputFileData%StC_Y_M = SrcInputFileData%StC_Y_M + DstInputFileData%StC_Z_M = SrcInputFileData%StC_Z_M + DstInputFileData%StC_XY_M = SrcInputFileData%StC_XY_M + DstInputFileData%StC_X_K = SrcInputFileData%StC_X_K + DstInputFileData%StC_Y_K = SrcInputFileData%StC_Y_K + DstInputFileData%StC_Z_K = SrcInputFileData%StC_Z_K + DstInputFileData%StC_X_C = SrcInputFileData%StC_X_C + DstInputFileData%StC_Y_C = SrcInputFileData%StC_Y_C + DstInputFileData%StC_Z_C = SrcInputFileData%StC_Z_C + DstInputFileData%StC_X_PSP = SrcInputFileData%StC_X_PSP + DstInputFileData%StC_X_NSP = SrcInputFileData%StC_X_NSP + DstInputFileData%StC_Y_PSP = SrcInputFileData%StC_Y_PSP + DstInputFileData%StC_Y_NSP = SrcInputFileData%StC_Y_NSP + DstInputFileData%StC_Z_PSP = SrcInputFileData%StC_Z_PSP + DstInputFileData%StC_Z_NSP = SrcInputFileData%StC_Z_NSP + DstInputFileData%StC_X_KS = SrcInputFileData%StC_X_KS + DstInputFileData%StC_X_CS = SrcInputFileData%StC_X_CS + DstInputFileData%StC_Y_KS = SrcInputFileData%StC_Y_KS + DstInputFileData%StC_Y_CS = SrcInputFileData%StC_Y_CS + DstInputFileData%StC_Z_KS = SrcInputFileData%StC_Z_KS + DstInputFileData%StC_Z_CS = SrcInputFileData%StC_Z_CS + DstInputFileData%StC_P_X = SrcInputFileData%StC_P_X + DstInputFileData%StC_P_Y = SrcInputFileData%StC_P_Y + DstInputFileData%StC_P_Z = SrcInputFileData%StC_P_Z + DstInputFileData%StC_X_C_HIGH = SrcInputFileData%StC_X_C_HIGH + DstInputFileData%StC_X_C_LOW = SrcInputFileData%StC_X_C_LOW + DstInputFileData%StC_Y_C_HIGH = SrcInputFileData%StC_Y_C_HIGH + DstInputFileData%StC_Y_C_LOW = SrcInputFileData%StC_Y_C_LOW + DstInputFileData%StC_Z_C_HIGH = SrcInputFileData%StC_Z_C_HIGH + DstInputFileData%StC_Z_C_LOW = SrcInputFileData%StC_Z_C_LOW + DstInputFileData%StC_X_C_BRAKE = SrcInputFileData%StC_X_C_BRAKE + DstInputFileData%StC_Y_C_BRAKE = SrcInputFileData%StC_Y_C_BRAKE + DstInputFileData%StC_Z_C_BRAKE = SrcInputFileData%StC_Z_C_BRAKE + DstInputFileData%L_X = SrcInputFileData%L_X + DstInputFileData%B_X = SrcInputFileData%B_X + DstInputFileData%area_X = SrcInputFileData%area_X + DstInputFileData%area_ratio_X = SrcInputFileData%area_ratio_X + DstInputFileData%headLossCoeff_X = SrcInputFileData%headLossCoeff_X + DstInputFileData%rho_X = SrcInputFileData%rho_X + DstInputFileData%L_Y = SrcInputFileData%L_Y + DstInputFileData%B_Y = SrcInputFileData%B_Y + DstInputFileData%area_Y = SrcInputFileData%area_Y + DstInputFileData%area_ratio_Y = SrcInputFileData%area_ratio_Y + DstInputFileData%headLossCoeff_Y = SrcInputFileData%headLossCoeff_Y + DstInputFileData%rho_Y = SrcInputFileData%rho_Y + DstInputFileData%USE_F_TBL = SrcInputFileData%USE_F_TBL + DstInputFileData%NKInpSt = SrcInputFileData%NKInpSt + DstInputFileData%StC_F_TBL_FILE = SrcInputFileData%StC_F_TBL_FILE +IF (ALLOCATED(SrcInputFileData%F_TBL)) THEN + i1_l = LBOUND(SrcInputFileData%F_TBL,1) + i1_u = UBOUND(SrcInputFileData%F_TBL,1) + i2_l = LBOUND(SrcInputFileData%F_TBL,2) + i2_u = UBOUND(SrcInputFileData%F_TBL,2) + IF (.NOT. ALLOCATED(DstInputFileData%F_TBL)) THEN + ALLOCATE(DstInputFileData%F_TBL(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInputFileData%F_TBL.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInputFileData%F_TBL = SrcInputFileData%F_TBL +ENDIF + DstInputFileData%PrescribedForcesCoordSys = SrcInputFileData%PrescribedForcesCoordSys + DstInputFileData%PrescribedForcesFile = SrcInputFileData%PrescribedForcesFile +IF (ALLOCATED(SrcInputFileData%StC_PrescribedForce)) THEN + i1_l = LBOUND(SrcInputFileData%StC_PrescribedForce,1) + i1_u = UBOUND(SrcInputFileData%StC_PrescribedForce,1) + i2_l = LBOUND(SrcInputFileData%StC_PrescribedForce,2) + i2_u = UBOUND(SrcInputFileData%StC_PrescribedForce,2) + IF (.NOT. ALLOCATED(DstInputFileData%StC_PrescribedForce)) THEN + ALLOCATE(DstInputFileData%StC_PrescribedForce(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInputFileData%StC_PrescribedForce.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInputFileData%StC_PrescribedForce = SrcInputFileData%StC_PrescribedForce +ENDIF + END SUBROUTINE StC_CopyInputFile + + SUBROUTINE StC_DestroyInputFile( InputFileData, ErrStat, ErrMsg ) + TYPE(StC_InputFile), INTENT(INOUT) :: InputFileData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'StC_DestroyInputFile' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(InputFileData%F_TBL)) THEN + DEALLOCATE(InputFileData%F_TBL) +ENDIF +IF (ALLOCATED(InputFileData%StC_PrescribedForce)) THEN + DEALLOCATE(InputFileData%StC_PrescribedForce) +ENDIF + END SUBROUTINE StC_DestroyInputFile + + SUBROUTINE StC_PackInputFile( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(StC_InputFile), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_PackInputFile' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1*LEN(InData%StCFileName) ! StCFileName + Int_BufSz = Int_BufSz + 1 ! Echo + Int_BufSz = Int_BufSz + 1 ! StC_CMODE + Int_BufSz = Int_BufSz + 1 ! StC_SA_MODE + Int_BufSz = Int_BufSz + 1 ! StC_DOF_MODE + Int_BufSz = Int_BufSz + 1 ! StC_X_DOF + Int_BufSz = Int_BufSz + 1 ! StC_Y_DOF + Int_BufSz = Int_BufSz + 1 ! StC_Z_DOF + Re_BufSz = Re_BufSz + 1 ! StC_X_DSP + Re_BufSz = Re_BufSz + 1 ! StC_Y_DSP + Re_BufSz = Re_BufSz + 1 ! StC_Z_DSP + Re_BufSz = Re_BufSz + 1 ! StC_X_M + Re_BufSz = Re_BufSz + 1 ! StC_Y_M + Re_BufSz = Re_BufSz + 1 ! StC_Z_M + Re_BufSz = Re_BufSz + 1 ! StC_XY_M + Re_BufSz = Re_BufSz + 1 ! StC_X_K + Re_BufSz = Re_BufSz + 1 ! StC_Y_K + Re_BufSz = Re_BufSz + 1 ! StC_Z_K + Re_BufSz = Re_BufSz + 1 ! StC_X_C + Re_BufSz = Re_BufSz + 1 ! StC_Y_C + Re_BufSz = Re_BufSz + 1 ! StC_Z_C + Re_BufSz = Re_BufSz + 1 ! StC_X_PSP + Re_BufSz = Re_BufSz + 1 ! StC_X_NSP + Re_BufSz = Re_BufSz + 1 ! StC_Y_PSP + Re_BufSz = Re_BufSz + 1 ! StC_Y_NSP + Re_BufSz = Re_BufSz + 1 ! StC_Z_PSP + Re_BufSz = Re_BufSz + 1 ! StC_Z_NSP + Re_BufSz = Re_BufSz + 1 ! StC_X_KS + Re_BufSz = Re_BufSz + 1 ! StC_X_CS + Re_BufSz = Re_BufSz + 1 ! StC_Y_KS + Re_BufSz = Re_BufSz + 1 ! StC_Y_CS + Re_BufSz = Re_BufSz + 1 ! StC_Z_KS + Re_BufSz = Re_BufSz + 1 ! StC_Z_CS + Re_BufSz = Re_BufSz + 1 ! StC_P_X + Re_BufSz = Re_BufSz + 1 ! StC_P_Y + Re_BufSz = Re_BufSz + 1 ! StC_P_Z + Re_BufSz = Re_BufSz + 1 ! StC_X_C_HIGH + Re_BufSz = Re_BufSz + 1 ! StC_X_C_LOW + Re_BufSz = Re_BufSz + 1 ! StC_Y_C_HIGH + Re_BufSz = Re_BufSz + 1 ! StC_Y_C_LOW + Re_BufSz = Re_BufSz + 1 ! StC_Z_C_HIGH + Re_BufSz = Re_BufSz + 1 ! StC_Z_C_LOW + Re_BufSz = Re_BufSz + 1 ! StC_X_C_BRAKE + Re_BufSz = Re_BufSz + 1 ! StC_Y_C_BRAKE + Re_BufSz = Re_BufSz + 1 ! StC_Z_C_BRAKE + Re_BufSz = Re_BufSz + 1 ! L_X + Re_BufSz = Re_BufSz + 1 ! B_X + Re_BufSz = Re_BufSz + 1 ! area_X + Re_BufSz = Re_BufSz + 1 ! area_ratio_X + Re_BufSz = Re_BufSz + 1 ! headLossCoeff_X + Re_BufSz = Re_BufSz + 1 ! rho_X + Re_BufSz = Re_BufSz + 1 ! L_Y + Re_BufSz = Re_BufSz + 1 ! B_Y + Re_BufSz = Re_BufSz + 1 ! area_Y + Re_BufSz = Re_BufSz + 1 ! area_ratio_Y + Re_BufSz = Re_BufSz + 1 ! headLossCoeff_Y + Re_BufSz = Re_BufSz + 1 ! rho_Y + Int_BufSz = Int_BufSz + 1 ! USE_F_TBL + Int_BufSz = Int_BufSz + 1 ! NKInpSt + Int_BufSz = Int_BufSz + 1*LEN(InData%StC_F_TBL_FILE) ! StC_F_TBL_FILE + Int_BufSz = Int_BufSz + 1 ! F_TBL allocated yes/no + IF ( ALLOCATED(InData%F_TBL) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! F_TBL upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%F_TBL) ! F_TBL + END IF + Int_BufSz = Int_BufSz + 1 ! PrescribedForcesCoordSys + Int_BufSz = Int_BufSz + 1*LEN(InData%PrescribedForcesFile) ! PrescribedForcesFile + Int_BufSz = Int_BufSz + 1 ! StC_PrescribedForce allocated yes/no + IF ( ALLOCATED(InData%StC_PrescribedForce) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! StC_PrescribedForce upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%StC_PrescribedForce) ! StC_PrescribedForce + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + DO I = 1, LEN(InData%StCFileName) + IntKiBuf(Int_Xferred) = ICHAR(InData%StCFileName(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + IntKiBuf(Int_Xferred) = TRANSFER(InData%Echo, IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%StC_CMODE + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%StC_SA_MODE + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%StC_DOF_MODE + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = TRANSFER(InData%StC_X_DOF, IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = TRANSFER(InData%StC_Y_DOF, IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = TRANSFER(InData%StC_Z_DOF, IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_X_DSP + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Y_DSP + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Z_DSP + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_X_M + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Y_M + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Z_M + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_XY_M + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_X_K + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Y_K + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Z_K + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_X_C + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Y_C + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Z_C + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_X_PSP + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_X_NSP + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Y_PSP + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Y_NSP + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Z_PSP + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Z_NSP + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_X_KS + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_X_CS + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Y_KS + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Y_CS + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Z_KS + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Z_CS + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_P_X + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_P_Y + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_P_Z + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_X_C_HIGH + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_X_C_LOW + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Y_C_HIGH + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Y_C_LOW + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Z_C_HIGH + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Z_C_LOW + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_X_C_BRAKE + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Y_C_BRAKE + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Z_C_BRAKE + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%L_X + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%B_X + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%area_X + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%area_ratio_X + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%headLossCoeff_X + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%rho_X + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%L_Y + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%B_Y + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%area_Y + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%area_ratio_Y + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%headLossCoeff_Y + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%rho_Y + Re_Xferred = Re_Xferred + 1 + IntKiBuf(Int_Xferred) = TRANSFER(InData%USE_F_TBL, IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%NKInpSt + Int_Xferred = Int_Xferred + 1 + DO I = 1, LEN(InData%StC_F_TBL_FILE) + IntKiBuf(Int_Xferred) = ICHAR(InData%StC_F_TBL_FILE(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + IF ( .NOT. ALLOCATED(InData%F_TBL) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%F_TBL,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F_TBL,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%F_TBL,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F_TBL,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%F_TBL,2), UBOUND(InData%F_TBL,2) + DO i1 = LBOUND(InData%F_TBL,1), UBOUND(InData%F_TBL,1) + ReKiBuf(Re_Xferred) = InData%F_TBL(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IntKiBuf(Int_Xferred) = InData%PrescribedForcesCoordSys + Int_Xferred = Int_Xferred + 1 + DO I = 1, LEN(InData%PrescribedForcesFile) + IntKiBuf(Int_Xferred) = ICHAR(InData%PrescribedForcesFile(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + IF ( .NOT. ALLOCATED(InData%StC_PrescribedForce) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%StC_PrescribedForce,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%StC_PrescribedForce,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%StC_PrescribedForce,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%StC_PrescribedForce,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%StC_PrescribedForce,2), UBOUND(InData%StC_PrescribedForce,2) + DO i1 = LBOUND(InData%StC_PrescribedForce,1), UBOUND(InData%StC_PrescribedForce,1) + ReKiBuf(Re_Xferred) = InData%StC_PrescribedForce(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + END SUBROUTINE StC_PackInputFile + + SUBROUTINE StC_UnPackInputFile( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(StC_InputFile), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_UnPackInputFile' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + DO I = 1, LEN(OutData%StCFileName) + OutData%StCFileName(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + OutData%Echo = TRANSFER(IntKiBuf(Int_Xferred), OutData%Echo) + Int_Xferred = Int_Xferred + 1 + OutData%StC_CMODE = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%StC_SA_MODE = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%StC_DOF_MODE = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%StC_X_DOF = TRANSFER(IntKiBuf(Int_Xferred), OutData%StC_X_DOF) + Int_Xferred = Int_Xferred + 1 + OutData%StC_Y_DOF = TRANSFER(IntKiBuf(Int_Xferred), OutData%StC_Y_DOF) + Int_Xferred = Int_Xferred + 1 + OutData%StC_Z_DOF = TRANSFER(IntKiBuf(Int_Xferred), OutData%StC_Z_DOF) + Int_Xferred = Int_Xferred + 1 + OutData%StC_X_DSP = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Y_DSP = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Z_DSP = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_X_M = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Y_M = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Z_M = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_XY_M = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_X_K = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Y_K = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Z_K = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_X_C = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Y_C = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Z_C = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_X_PSP = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_X_NSP = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Y_PSP = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Y_NSP = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Z_PSP = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Z_NSP = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_X_KS = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_X_CS = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Y_KS = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Y_CS = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Z_KS = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Z_CS = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_P_X = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_P_Y = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_P_Z = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_X_C_HIGH = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_X_C_LOW = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Y_C_HIGH = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Y_C_LOW = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Z_C_HIGH = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Z_C_LOW = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_X_C_BRAKE = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Y_C_BRAKE = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Z_C_BRAKE = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%L_X = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%B_X = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%area_X = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%area_ratio_X = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%headLossCoeff_X = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%rho_X = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%L_Y = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%B_Y = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%area_Y = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%area_ratio_Y = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%headLossCoeff_Y = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%rho_Y = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%USE_F_TBL = TRANSFER(IntKiBuf(Int_Xferred), OutData%USE_F_TBL) + Int_Xferred = Int_Xferred + 1 + OutData%NKInpSt = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + DO I = 1, LEN(OutData%StC_F_TBL_FILE) + OutData%StC_F_TBL_FILE(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! F_TBL not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%F_TBL)) DEALLOCATE(OutData%F_TBL) + ALLOCATE(OutData%F_TBL(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%F_TBL.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%F_TBL,2), UBOUND(OutData%F_TBL,2) + DO i1 = LBOUND(OutData%F_TBL,1), UBOUND(OutData%F_TBL,1) + OutData%F_TBL(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + OutData%PrescribedForcesCoordSys = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + DO I = 1, LEN(OutData%PrescribedForcesFile) + OutData%PrescribedForcesFile(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! StC_PrescribedForce not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%StC_PrescribedForce)) DEALLOCATE(OutData%StC_PrescribedForce) + ALLOCATE(OutData%StC_PrescribedForce(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%StC_PrescribedForce.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%StC_PrescribedForce,2), UBOUND(OutData%StC_PrescribedForce,2) + DO i1 = LBOUND(OutData%StC_PrescribedForce,1), UBOUND(OutData%StC_PrescribedForce,1) + OutData%StC_PrescribedForce(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + END SUBROUTINE StC_UnPackInputFile + + SUBROUTINE StC_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, ErrStat, ErrMsg ) + TYPE(StC_InitInputType), INTENT(IN) :: SrcInitInputData + TYPE(StC_InitInputType), INTENT(INOUT) :: DstInitInputData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_CopyInitInput' +! + ErrStat = ErrID_None + ErrMsg = "" + DstInitInputData%InputFile = SrcInitInputData%InputFile + DstInitInputData%RootName = SrcInitInputData%RootName + DstInitInputData%Gravity = SrcInitInputData%Gravity + DstInitInputData%NumMeshPts = SrcInitInputData%NumMeshPts +IF (ALLOCATED(SrcInitInputData%InitPosition)) THEN + i1_l = LBOUND(SrcInitInputData%InitPosition,1) + i1_u = UBOUND(SrcInitInputData%InitPosition,1) + i2_l = LBOUND(SrcInitInputData%InitPosition,2) + i2_u = UBOUND(SrcInitInputData%InitPosition,2) + IF (.NOT. ALLOCATED(DstInitInputData%InitPosition)) THEN + ALLOCATE(DstInitInputData%InitPosition(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitInputData%InitPosition.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitInputData%InitPosition = SrcInitInputData%InitPosition +ENDIF +IF (ALLOCATED(SrcInitInputData%InitOrientation)) THEN + i1_l = LBOUND(SrcInitInputData%InitOrientation,1) + i1_u = UBOUND(SrcInitInputData%InitOrientation,1) + i2_l = LBOUND(SrcInitInputData%InitOrientation,2) + i2_u = UBOUND(SrcInitInputData%InitOrientation,2) + i3_l = LBOUND(SrcInitInputData%InitOrientation,3) + i3_u = UBOUND(SrcInitInputData%InitOrientation,3) + IF (.NOT. ALLOCATED(DstInitInputData%InitOrientation)) THEN + ALLOCATE(DstInitInputData%InitOrientation(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitInputData%InitOrientation.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitInputData%InitOrientation = SrcInitInputData%InitOrientation +ENDIF + DstInitInputData%UseInputFile = SrcInitInputData%UseInputFile + CALL NWTC_Library_Copyfileinfotype( SrcInitInputData%PassedPrimaryInputData, DstInitInputData%PassedPrimaryInputData, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + DstInitInputData%UseInputFile_PrescribeFrc = SrcInitInputData%UseInputFile_PrescribeFrc + CALL NWTC_Library_Copyfileinfotype( SrcInitInputData%PassedPrescribeFrcData, DstInitInputData%PassedPrescribeFrcData, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + END SUBROUTINE StC_CopyInitInput + + SUBROUTINE StC_DestroyInitInput( InitInputData, ErrStat, ErrMsg ) + TYPE(StC_InitInputType), INTENT(INOUT) :: InitInputData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'StC_DestroyInitInput' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(InitInputData%InitPosition)) THEN + DEALLOCATE(InitInputData%InitPosition) +ENDIF +IF (ALLOCATED(InitInputData%InitOrientation)) THEN + DEALLOCATE(InitInputData%InitOrientation) +ENDIF + CALL NWTC_Library_Destroyfileinfotype( InitInputData%PassedPrimaryInputData, ErrStat, ErrMsg ) + CALL NWTC_Library_Destroyfileinfotype( InitInputData%PassedPrescribeFrcData, ErrStat, ErrMsg ) + END SUBROUTINE StC_DestroyInitInput + + SUBROUTINE StC_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(StC_InitInputType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_PackInitInput' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1*LEN(InData%InputFile) ! InputFile + Int_BufSz = Int_BufSz + 1*LEN(InData%RootName) ! RootName + Re_BufSz = Re_BufSz + SIZE(InData%Gravity) ! Gravity + Int_BufSz = Int_BufSz + 1 ! NumMeshPts + Int_BufSz = Int_BufSz + 1 ! InitPosition allocated yes/no + IF ( ALLOCATED(InData%InitPosition) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! InitPosition upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%InitPosition) ! InitPosition + END IF + Int_BufSz = Int_BufSz + 1 ! InitOrientation allocated yes/no + IF ( ALLOCATED(InData%InitOrientation) ) THEN + Int_BufSz = Int_BufSz + 2*3 ! InitOrientation upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%InitOrientation) ! InitOrientation + END IF + Int_BufSz = Int_BufSz + 1 ! UseInputFile + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + Int_BufSz = Int_BufSz + 3 ! PassedPrimaryInputData: size of buffers for each call to pack subtype + CALL NWTC_Library_Packfileinfotype( Re_Buf, Db_Buf, Int_Buf, InData%PassedPrimaryInputData, ErrStat2, ErrMsg2, .TRUE. ) ! PassedPrimaryInputData + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! PassedPrimaryInputData + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! PassedPrimaryInputData + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! PassedPrimaryInputData + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + Int_BufSz = Int_BufSz + 1 ! UseInputFile_PrescribeFrc + Int_BufSz = Int_BufSz + 3 ! PassedPrescribeFrcData: size of buffers for each call to pack subtype + CALL NWTC_Library_Packfileinfotype( Re_Buf, Db_Buf, Int_Buf, InData%PassedPrescribeFrcData, ErrStat2, ErrMsg2, .TRUE. ) ! PassedPrescribeFrcData + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! PassedPrescribeFrcData + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! PassedPrescribeFrcData + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! PassedPrescribeFrcData + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + DO I = 1, LEN(InData%InputFile) + IntKiBuf(Int_Xferred) = ICHAR(InData%InputFile(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + DO I = 1, LEN(InData%RootName) + IntKiBuf(Int_Xferred) = ICHAR(InData%RootName(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + DO i1 = LBOUND(InData%Gravity,1), UBOUND(InData%Gravity,1) + ReKiBuf(Re_Xferred) = InData%Gravity(i1) + Re_Xferred = Re_Xferred + 1 + END DO + IntKiBuf(Int_Xferred) = InData%NumMeshPts + Int_Xferred = Int_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%InitPosition) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%InitPosition,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%InitPosition,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%InitPosition,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%InitPosition,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%InitPosition,2), UBOUND(InData%InitPosition,2) + DO i1 = LBOUND(InData%InitPosition,1), UBOUND(InData%InitPosition,1) + ReKiBuf(Re_Xferred) = InData%InitPosition(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%InitOrientation) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%InitOrientation,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%InitOrientation,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%InitOrientation,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%InitOrientation,2) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%InitOrientation,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%InitOrientation,3) + Int_Xferred = Int_Xferred + 2 + + DO i3 = LBOUND(InData%InitOrientation,3), UBOUND(InData%InitOrientation,3) + DO i2 = LBOUND(InData%InitOrientation,2), UBOUND(InData%InitOrientation,2) + DO i1 = LBOUND(InData%InitOrientation,1), UBOUND(InData%InitOrientation,1) + DbKiBuf(Db_Xferred) = InData%InitOrientation(i1,i2,i3) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END IF + IntKiBuf(Int_Xferred) = TRANSFER(InData%UseInputFile, IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + CALL NWTC_Library_Packfileinfotype( Re_Buf, Db_Buf, Int_Buf, InData%PassedPrimaryInputData, ErrStat2, ErrMsg2, OnlySize ) ! PassedPrimaryInputData + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IntKiBuf(Int_Xferred) = TRANSFER(InData%UseInputFile_PrescribeFrc, IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + CALL NWTC_Library_Packfileinfotype( Re_Buf, Db_Buf, Int_Buf, InData%PassedPrescribeFrcData, ErrStat2, ErrMsg2, OnlySize ) ! PassedPrescribeFrcData + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END SUBROUTINE StC_PackInitInput + + SUBROUTINE StC_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(StC_InitInputType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_UnPackInitInput' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + DO I = 1, LEN(OutData%InputFile) + OutData%InputFile(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + DO I = 1, LEN(OutData%RootName) + OutData%RootName(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + i1_l = LBOUND(OutData%Gravity,1) + i1_u = UBOUND(OutData%Gravity,1) + DO i1 = LBOUND(OutData%Gravity,1), UBOUND(OutData%Gravity,1) + OutData%Gravity(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + OutData%NumMeshPts = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! InitPosition not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%InitPosition)) DEALLOCATE(OutData%InitPosition) + ALLOCATE(OutData%InitPosition(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%InitPosition.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%InitPosition,2), UBOUND(OutData%InitPosition,2) + DO i1 = LBOUND(OutData%InitPosition,1), UBOUND(OutData%InitPosition,1) + OutData%InitPosition(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! InitOrientation not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i3_l = IntKiBuf( Int_Xferred ) + i3_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%InitOrientation)) DEALLOCATE(OutData%InitOrientation) + ALLOCATE(OutData%InitOrientation(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%InitOrientation.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i3 = LBOUND(OutData%InitOrientation,3), UBOUND(OutData%InitOrientation,3) + DO i2 = LBOUND(OutData%InitOrientation,2), UBOUND(OutData%InitOrientation,2) + DO i1 = LBOUND(OutData%InitOrientation,1), UBOUND(OutData%InitOrientation,1) + OutData%InitOrientation(i1,i2,i3) = REAL(DbKiBuf(Db_Xferred), R8Ki) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END DO + END IF + OutData%UseInputFile = TRANSFER(IntKiBuf(Int_Xferred), OutData%UseInputFile) + Int_Xferred = Int_Xferred + 1 + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL NWTC_Library_Unpackfileinfotype( Re_Buf, Db_Buf, Int_Buf, OutData%PassedPrimaryInputData, ErrStat2, ErrMsg2 ) ! PassedPrimaryInputData + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + OutData%UseInputFile_PrescribeFrc = TRANSFER(IntKiBuf(Int_Xferred), OutData%UseInputFile_PrescribeFrc) + Int_Xferred = Int_Xferred + 1 + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL NWTC_Library_Unpackfileinfotype( Re_Buf, Db_Buf, Int_Buf, OutData%PassedPrescribeFrcData, ErrStat2, ErrMsg2 ) ! PassedPrescribeFrcData + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END SUBROUTINE StC_UnPackInitInput + + SUBROUTINE StC_CopyInitOutput( SrcInitOutputData, DstInitOutputData, CtrlCode, ErrStat, ErrMsg ) + TYPE(StC_InitOutputType), INTENT(IN) :: SrcInitOutputData + TYPE(StC_InitOutputType), INTENT(INOUT) :: DstInitOutputData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_CopyInitOutput' +! + ErrStat = ErrID_None + ErrMsg = "" + DstInitOutputData%DummyInitOut = SrcInitOutputData%DummyInitOut + END SUBROUTINE StC_CopyInitOutput + + SUBROUTINE StC_DestroyInitOutput( InitOutputData, ErrStat, ErrMsg ) + TYPE(StC_InitOutputType), INTENT(INOUT) :: InitOutputData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'StC_DestroyInitOutput' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE StC_DestroyInitOutput + + SUBROUTINE StC_PackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(StC_InitOutputType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_PackInitOutput' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Re_BufSz = Re_BufSz + 1 ! DummyInitOut + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + ReKiBuf(Re_Xferred) = InData%DummyInitOut + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE StC_PackInitOutput + + SUBROUTINE StC_UnPackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(StC_InitOutputType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_UnPackInitOutput' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%DummyInitOut = REAL(ReKiBuf(Re_Xferred), SiKi) + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE StC_UnPackInitOutput + + SUBROUTINE StC_CopyContState( SrcContStateData, DstContStateData, CtrlCode, ErrStat, ErrMsg ) + TYPE(StC_ContinuousStateType), INTENT(IN) :: SrcContStateData + TYPE(StC_ContinuousStateType), INTENT(INOUT) :: DstContStateData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_CopyContState' +! + ErrStat = ErrID_None + ErrMsg = "" + DstContStateData%DummyContState = SrcContStateData%DummyContState +IF (ALLOCATED(SrcContStateData%StC_x)) THEN + i1_l = LBOUND(SrcContStateData%StC_x,1) + i1_u = UBOUND(SrcContStateData%StC_x,1) + i2_l = LBOUND(SrcContStateData%StC_x,2) + i2_u = UBOUND(SrcContStateData%StC_x,2) + IF (.NOT. ALLOCATED(DstContStateData%StC_x)) THEN + ALLOCATE(DstContStateData%StC_x(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstContStateData%StC_x.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstContStateData%StC_x = SrcContStateData%StC_x +ENDIF +IF (ALLOCATED(SrcContStateData%StC_xdot)) THEN + i1_l = LBOUND(SrcContStateData%StC_xdot,1) + i1_u = UBOUND(SrcContStateData%StC_xdot,1) + i2_l = LBOUND(SrcContStateData%StC_xdot,2) + i2_u = UBOUND(SrcContStateData%StC_xdot,2) + IF (.NOT. ALLOCATED(DstContStateData%StC_xdot)) THEN + ALLOCATE(DstContStateData%StC_xdot(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstContStateData%StC_xdot.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstContStateData%StC_xdot = SrcContStateData%StC_xdot +ENDIF + END SUBROUTINE StC_CopyContState + + SUBROUTINE StC_DestroyContState( ContStateData, ErrStat, ErrMsg ) + TYPE(StC_ContinuousStateType), INTENT(INOUT) :: ContStateData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'StC_DestroyContState' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(ContStateData%StC_x)) THEN + DEALLOCATE(ContStateData%StC_x) +ENDIF +IF (ALLOCATED(ContStateData%StC_xdot)) THEN + DEALLOCATE(ContStateData%StC_xdot) +ENDIF + END SUBROUTINE StC_DestroyContState + + SUBROUTINE StC_PackContState( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(StC_ContinuousStateType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_PackContState' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Re_BufSz = Re_BufSz + 1 ! DummyContState + Int_BufSz = Int_BufSz + 1 ! StC_x allocated yes/no + IF ( ALLOCATED(InData%StC_x) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! StC_x upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%StC_x) ! StC_x + END IF + Int_BufSz = Int_BufSz + 1 ! StC_xdot allocated yes/no + IF ( ALLOCATED(InData%StC_xdot) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! StC_xdot upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%StC_xdot) ! StC_xdot + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + ReKiBuf(Re_Xferred) = InData%DummyContState + Re_Xferred = Re_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%StC_x) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%StC_x,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%StC_x,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%StC_x,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%StC_x,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%StC_x,2), UBOUND(InData%StC_x,2) + DO i1 = LBOUND(InData%StC_x,1), UBOUND(InData%StC_x,1) + ReKiBuf(Re_Xferred) = InData%StC_x(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%StC_xdot) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%StC_xdot,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%StC_xdot,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%StC_xdot,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%StC_xdot,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%StC_xdot,2), UBOUND(InData%StC_xdot,2) + DO i1 = LBOUND(InData%StC_xdot,1), UBOUND(InData%StC_xdot,1) + ReKiBuf(Re_Xferred) = InData%StC_xdot(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + END SUBROUTINE StC_PackContState + + SUBROUTINE StC_UnPackContState( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(StC_ContinuousStateType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_UnPackContState' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%DummyContState = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! StC_x not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%StC_x)) DEALLOCATE(OutData%StC_x) + ALLOCATE(OutData%StC_x(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%StC_x.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%StC_x,2), UBOUND(OutData%StC_x,2) + DO i1 = LBOUND(OutData%StC_x,1), UBOUND(OutData%StC_x,1) + OutData%StC_x(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! StC_xdot not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%StC_xdot)) DEALLOCATE(OutData%StC_xdot) + ALLOCATE(OutData%StC_xdot(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%StC_xdot.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%StC_xdot,2), UBOUND(OutData%StC_xdot,2) + DO i1 = LBOUND(OutData%StC_xdot,1), UBOUND(OutData%StC_xdot,1) + OutData%StC_xdot(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + END SUBROUTINE StC_UnPackContState + + SUBROUTINE StC_CopyDiscState( SrcDiscStateData, DstDiscStateData, CtrlCode, ErrStat, ErrMsg ) + TYPE(StC_DiscreteStateType), INTENT(IN) :: SrcDiscStateData + TYPE(StC_DiscreteStateType), INTENT(INOUT) :: DstDiscStateData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_CopyDiscState' +! + ErrStat = ErrID_None + ErrMsg = "" + DstDiscStateData%DummyDiscState = SrcDiscStateData%DummyDiscState + END SUBROUTINE StC_CopyDiscState + + SUBROUTINE StC_DestroyDiscState( DiscStateData, ErrStat, ErrMsg ) + TYPE(StC_DiscreteStateType), INTENT(INOUT) :: DiscStateData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'StC_DestroyDiscState' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE StC_DestroyDiscState + + SUBROUTINE StC_PackDiscState( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(StC_DiscreteStateType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_PackDiscState' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Re_BufSz = Re_BufSz + 1 ! DummyDiscState + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + ReKiBuf(Re_Xferred) = InData%DummyDiscState + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE StC_PackDiscState + + SUBROUTINE StC_UnPackDiscState( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(StC_DiscreteStateType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_UnPackDiscState' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%DummyDiscState = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE StC_UnPackDiscState + + SUBROUTINE StC_CopyConstrState( SrcConstrStateData, DstConstrStateData, CtrlCode, ErrStat, ErrMsg ) + TYPE(StC_ConstraintStateType), INTENT(IN) :: SrcConstrStateData + TYPE(StC_ConstraintStateType), INTENT(INOUT) :: DstConstrStateData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_CopyConstrState' +! + ErrStat = ErrID_None + ErrMsg = "" + DstConstrStateData%DummyConstrState = SrcConstrStateData%DummyConstrState + END SUBROUTINE StC_CopyConstrState + + SUBROUTINE StC_DestroyConstrState( ConstrStateData, ErrStat, ErrMsg ) + TYPE(StC_ConstraintStateType), INTENT(INOUT) :: ConstrStateData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'StC_DestroyConstrState' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE StC_DestroyConstrState + + SUBROUTINE StC_PackConstrState( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(StC_ConstraintStateType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_PackConstrState' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Re_BufSz = Re_BufSz + 1 ! DummyConstrState + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + ReKiBuf(Re_Xferred) = InData%DummyConstrState + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE StC_PackConstrState + + SUBROUTINE StC_UnPackConstrState( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(StC_ConstraintStateType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_UnPackConstrState' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%DummyConstrState = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE StC_UnPackConstrState + + SUBROUTINE StC_CopyOtherState( SrcOtherStateData, DstOtherStateData, CtrlCode, ErrStat, ErrMsg ) + TYPE(StC_OtherStateType), INTENT(IN) :: SrcOtherStateData + TYPE(StC_OtherStateType), INTENT(INOUT) :: DstOtherStateData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_CopyOtherState' +! + ErrStat = ErrID_None + ErrMsg = "" + DstOtherStateData%DummyOtherState = SrcOtherStateData%DummyOtherState + END SUBROUTINE StC_CopyOtherState + + SUBROUTINE StC_DestroyOtherState( OtherStateData, ErrStat, ErrMsg ) + TYPE(StC_OtherStateType), INTENT(INOUT) :: OtherStateData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'StC_DestroyOtherState' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE StC_DestroyOtherState + + SUBROUTINE StC_PackOtherState( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(StC_OtherStateType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_PackOtherState' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Re_BufSz = Re_BufSz + 1 ! DummyOtherState + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + ReKiBuf(Re_Xferred) = InData%DummyOtherState + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE StC_PackOtherState + + SUBROUTINE StC_UnPackOtherState( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(StC_OtherStateType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_UnPackOtherState' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%DummyOtherState = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE StC_UnPackOtherState + + SUBROUTINE StC_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) + TYPE(StC_MiscVarType), INTENT(IN) :: SrcMiscData + TYPE(StC_MiscVarType), INTENT(INOUT) :: DstMiscData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_CopyMisc' +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(SrcMiscData%F_stop)) THEN + i1_l = LBOUND(SrcMiscData%F_stop,1) + i1_u = UBOUND(SrcMiscData%F_stop,1) + i2_l = LBOUND(SrcMiscData%F_stop,2) + i2_u = UBOUND(SrcMiscData%F_stop,2) + IF (.NOT. ALLOCATED(DstMiscData%F_stop)) THEN + ALLOCATE(DstMiscData%F_stop(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%F_stop.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%F_stop = SrcMiscData%F_stop +ENDIF +IF (ALLOCATED(SrcMiscData%F_ext)) THEN + i1_l = LBOUND(SrcMiscData%F_ext,1) + i1_u = UBOUND(SrcMiscData%F_ext,1) + i2_l = LBOUND(SrcMiscData%F_ext,2) + i2_u = UBOUND(SrcMiscData%F_ext,2) + IF (.NOT. ALLOCATED(DstMiscData%F_ext)) THEN + ALLOCATE(DstMiscData%F_ext(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%F_ext.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%F_ext = SrcMiscData%F_ext +ENDIF +IF (ALLOCATED(SrcMiscData%F_fr)) THEN + i1_l = LBOUND(SrcMiscData%F_fr,1) + i1_u = UBOUND(SrcMiscData%F_fr,1) + i2_l = LBOUND(SrcMiscData%F_fr,2) + i2_u = UBOUND(SrcMiscData%F_fr,2) + IF (.NOT. ALLOCATED(DstMiscData%F_fr)) THEN + ALLOCATE(DstMiscData%F_fr(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%F_fr.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%F_fr = SrcMiscData%F_fr +ENDIF +IF (ALLOCATED(SrcMiscData%C_ctrl)) THEN + i1_l = LBOUND(SrcMiscData%C_ctrl,1) + i1_u = UBOUND(SrcMiscData%C_ctrl,1) + i2_l = LBOUND(SrcMiscData%C_ctrl,2) + i2_u = UBOUND(SrcMiscData%C_ctrl,2) + IF (.NOT. ALLOCATED(DstMiscData%C_ctrl)) THEN + ALLOCATE(DstMiscData%C_ctrl(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%C_ctrl.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%C_ctrl = SrcMiscData%C_ctrl +ENDIF +IF (ALLOCATED(SrcMiscData%C_Brake)) THEN + i1_l = LBOUND(SrcMiscData%C_Brake,1) + i1_u = UBOUND(SrcMiscData%C_Brake,1) + i2_l = LBOUND(SrcMiscData%C_Brake,2) + i2_u = UBOUND(SrcMiscData%C_Brake,2) + IF (.NOT. ALLOCATED(DstMiscData%C_Brake)) THEN + ALLOCATE(DstMiscData%C_Brake(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%C_Brake.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%C_Brake = SrcMiscData%C_Brake +ENDIF +IF (ALLOCATED(SrcMiscData%F_table)) THEN + i1_l = LBOUND(SrcMiscData%F_table,1) + i1_u = UBOUND(SrcMiscData%F_table,1) + i2_l = LBOUND(SrcMiscData%F_table,2) + i2_u = UBOUND(SrcMiscData%F_table,2) + IF (.NOT. ALLOCATED(DstMiscData%F_table)) THEN + ALLOCATE(DstMiscData%F_table(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%F_table.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%F_table = SrcMiscData%F_table +ENDIF +IF (ALLOCATED(SrcMiscData%F_k)) THEN + i1_l = LBOUND(SrcMiscData%F_k,1) + i1_u = UBOUND(SrcMiscData%F_k,1) + i2_l = LBOUND(SrcMiscData%F_k,2) + i2_u = UBOUND(SrcMiscData%F_k,2) + IF (.NOT. ALLOCATED(DstMiscData%F_k)) THEN + ALLOCATE(DstMiscData%F_k(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%F_k.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%F_k = SrcMiscData%F_k +ENDIF +IF (ALLOCATED(SrcMiscData%a_G)) THEN + i1_l = LBOUND(SrcMiscData%a_G,1) + i1_u = UBOUND(SrcMiscData%a_G,1) + i2_l = LBOUND(SrcMiscData%a_G,2) + i2_u = UBOUND(SrcMiscData%a_G,2) + IF (.NOT. ALLOCATED(DstMiscData%a_G)) THEN + ALLOCATE(DstMiscData%a_G(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%a_G.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%a_G = SrcMiscData%a_G +ENDIF +IF (ALLOCATED(SrcMiscData%rdisp_P)) THEN + i1_l = LBOUND(SrcMiscData%rdisp_P,1) + i1_u = UBOUND(SrcMiscData%rdisp_P,1) + i2_l = LBOUND(SrcMiscData%rdisp_P,2) + i2_u = UBOUND(SrcMiscData%rdisp_P,2) + IF (.NOT. ALLOCATED(DstMiscData%rdisp_P)) THEN + ALLOCATE(DstMiscData%rdisp_P(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%rdisp_P.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%rdisp_P = SrcMiscData%rdisp_P +ENDIF +IF (ALLOCATED(SrcMiscData%rdot_P)) THEN + i1_l = LBOUND(SrcMiscData%rdot_P,1) + i1_u = UBOUND(SrcMiscData%rdot_P,1) + i2_l = LBOUND(SrcMiscData%rdot_P,2) + i2_u = UBOUND(SrcMiscData%rdot_P,2) + IF (.NOT. ALLOCATED(DstMiscData%rdot_P)) THEN + ALLOCATE(DstMiscData%rdot_P(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%rdot_P.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%rdot_P = SrcMiscData%rdot_P +ENDIF +IF (ALLOCATED(SrcMiscData%rddot_P)) THEN + i1_l = LBOUND(SrcMiscData%rddot_P,1) + i1_u = UBOUND(SrcMiscData%rddot_P,1) + i2_l = LBOUND(SrcMiscData%rddot_P,2) + i2_u = UBOUND(SrcMiscData%rddot_P,2) + IF (.NOT. ALLOCATED(DstMiscData%rddot_P)) THEN + ALLOCATE(DstMiscData%rddot_P(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%rddot_P.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%rddot_P = SrcMiscData%rddot_P +ENDIF +IF (ALLOCATED(SrcMiscData%omega_P)) THEN + i1_l = LBOUND(SrcMiscData%omega_P,1) + i1_u = UBOUND(SrcMiscData%omega_P,1) + i2_l = LBOUND(SrcMiscData%omega_P,2) + i2_u = UBOUND(SrcMiscData%omega_P,2) + IF (.NOT. ALLOCATED(DstMiscData%omega_P)) THEN + ALLOCATE(DstMiscData%omega_P(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%omega_P.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%omega_P = SrcMiscData%omega_P +ENDIF +IF (ALLOCATED(SrcMiscData%alpha_P)) THEN + i1_l = LBOUND(SrcMiscData%alpha_P,1) + i1_u = UBOUND(SrcMiscData%alpha_P,1) + i2_l = LBOUND(SrcMiscData%alpha_P,2) + i2_u = UBOUND(SrcMiscData%alpha_P,2) + IF (.NOT. ALLOCATED(DstMiscData%alpha_P)) THEN + ALLOCATE(DstMiscData%alpha_P(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%alpha_P.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%alpha_P = SrcMiscData%alpha_P +ENDIF +IF (ALLOCATED(SrcMiscData%F_P)) THEN + i1_l = LBOUND(SrcMiscData%F_P,1) + i1_u = UBOUND(SrcMiscData%F_P,1) + i2_l = LBOUND(SrcMiscData%F_P,2) + i2_u = UBOUND(SrcMiscData%F_P,2) + IF (.NOT. ALLOCATED(DstMiscData%F_P)) THEN + ALLOCATE(DstMiscData%F_P(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%F_P.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%F_P = SrcMiscData%F_P +ENDIF +IF (ALLOCATED(SrcMiscData%M_P)) THEN + i1_l = LBOUND(SrcMiscData%M_P,1) + i1_u = UBOUND(SrcMiscData%M_P,1) + i2_l = LBOUND(SrcMiscData%M_P,2) + i2_u = UBOUND(SrcMiscData%M_P,2) + IF (.NOT. ALLOCATED(DstMiscData%M_P)) THEN + ALLOCATE(DstMiscData%M_P(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%M_P.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%M_P = SrcMiscData%M_P +ENDIF +IF (ALLOCATED(SrcMiscData%Acc)) THEN + i1_l = LBOUND(SrcMiscData%Acc,1) + i1_u = UBOUND(SrcMiscData%Acc,1) + i2_l = LBOUND(SrcMiscData%Acc,2) + i2_u = UBOUND(SrcMiscData%Acc,2) + IF (.NOT. ALLOCATED(DstMiscData%Acc)) THEN + ALLOCATE(DstMiscData%Acc(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%Acc.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%Acc = SrcMiscData%Acc +ENDIF + DstMiscData%PrescribedInterpIdx = SrcMiscData%PrescribedInterpIdx + END SUBROUTINE StC_CopyMisc + + SUBROUTINE StC_DestroyMisc( MiscData, ErrStat, ErrMsg ) + TYPE(StC_MiscVarType), INTENT(INOUT) :: MiscData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'StC_DestroyMisc' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(MiscData%F_stop)) THEN + DEALLOCATE(MiscData%F_stop) +ENDIF +IF (ALLOCATED(MiscData%F_ext)) THEN + DEALLOCATE(MiscData%F_ext) +ENDIF +IF (ALLOCATED(MiscData%F_fr)) THEN + DEALLOCATE(MiscData%F_fr) +ENDIF +IF (ALLOCATED(MiscData%C_ctrl)) THEN + DEALLOCATE(MiscData%C_ctrl) +ENDIF +IF (ALLOCATED(MiscData%C_Brake)) THEN + DEALLOCATE(MiscData%C_Brake) +ENDIF +IF (ALLOCATED(MiscData%F_table)) THEN + DEALLOCATE(MiscData%F_table) +ENDIF +IF (ALLOCATED(MiscData%F_k)) THEN + DEALLOCATE(MiscData%F_k) +ENDIF +IF (ALLOCATED(MiscData%a_G)) THEN + DEALLOCATE(MiscData%a_G) +ENDIF +IF (ALLOCATED(MiscData%rdisp_P)) THEN + DEALLOCATE(MiscData%rdisp_P) +ENDIF +IF (ALLOCATED(MiscData%rdot_P)) THEN + DEALLOCATE(MiscData%rdot_P) +ENDIF +IF (ALLOCATED(MiscData%rddot_P)) THEN + DEALLOCATE(MiscData%rddot_P) +ENDIF +IF (ALLOCATED(MiscData%omega_P)) THEN + DEALLOCATE(MiscData%omega_P) +ENDIF +IF (ALLOCATED(MiscData%alpha_P)) THEN + DEALLOCATE(MiscData%alpha_P) +ENDIF +IF (ALLOCATED(MiscData%F_P)) THEN + DEALLOCATE(MiscData%F_P) +ENDIF +IF (ALLOCATED(MiscData%M_P)) THEN + DEALLOCATE(MiscData%M_P) +ENDIF +IF (ALLOCATED(MiscData%Acc)) THEN + DEALLOCATE(MiscData%Acc) +ENDIF + END SUBROUTINE StC_DestroyMisc + + SUBROUTINE StC_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(StC_MiscVarType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_PackMisc' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! F_stop allocated yes/no + IF ( ALLOCATED(InData%F_stop) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! F_stop upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%F_stop) ! F_stop + END IF + Int_BufSz = Int_BufSz + 1 ! F_ext allocated yes/no + IF ( ALLOCATED(InData%F_ext) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! F_ext upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%F_ext) ! F_ext + END IF + Int_BufSz = Int_BufSz + 1 ! F_fr allocated yes/no + IF ( ALLOCATED(InData%F_fr) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! F_fr upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%F_fr) ! F_fr + END IF + Int_BufSz = Int_BufSz + 1 ! C_ctrl allocated yes/no + IF ( ALLOCATED(InData%C_ctrl) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! C_ctrl upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%C_ctrl) ! C_ctrl + END IF + Int_BufSz = Int_BufSz + 1 ! C_Brake allocated yes/no + IF ( ALLOCATED(InData%C_Brake) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! C_Brake upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%C_Brake) ! C_Brake + END IF + Int_BufSz = Int_BufSz + 1 ! F_table allocated yes/no + IF ( ALLOCATED(InData%F_table) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! F_table upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%F_table) ! F_table + END IF + Int_BufSz = Int_BufSz + 1 ! F_k allocated yes/no + IF ( ALLOCATED(InData%F_k) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! F_k upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%F_k) ! F_k + END IF + Int_BufSz = Int_BufSz + 1 ! a_G allocated yes/no + IF ( ALLOCATED(InData%a_G) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! a_G upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%a_G) ! a_G + END IF + Int_BufSz = Int_BufSz + 1 ! rdisp_P allocated yes/no + IF ( ALLOCATED(InData%rdisp_P) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! rdisp_P upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%rdisp_P) ! rdisp_P + END IF + Int_BufSz = Int_BufSz + 1 ! rdot_P allocated yes/no + IF ( ALLOCATED(InData%rdot_P) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! rdot_P upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%rdot_P) ! rdot_P + END IF + Int_BufSz = Int_BufSz + 1 ! rddot_P allocated yes/no + IF ( ALLOCATED(InData%rddot_P) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! rddot_P upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%rddot_P) ! rddot_P + END IF + Int_BufSz = Int_BufSz + 1 ! omega_P allocated yes/no + IF ( ALLOCATED(InData%omega_P) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! omega_P upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%omega_P) ! omega_P + END IF + Int_BufSz = Int_BufSz + 1 ! alpha_P allocated yes/no + IF ( ALLOCATED(InData%alpha_P) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! alpha_P upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%alpha_P) ! alpha_P + END IF + Int_BufSz = Int_BufSz + 1 ! F_P allocated yes/no + IF ( ALLOCATED(InData%F_P) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! F_P upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%F_P) ! F_P + END IF + Int_BufSz = Int_BufSz + 1 ! M_P allocated yes/no + IF ( ALLOCATED(InData%M_P) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! M_P upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%M_P) ! M_P + END IF + Int_BufSz = Int_BufSz + 1 ! Acc allocated yes/no + IF ( ALLOCATED(InData%Acc) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Acc upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%Acc) ! Acc + END IF + Int_BufSz = Int_BufSz + 1 ! PrescribedInterpIdx + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IF ( .NOT. ALLOCATED(InData%F_stop) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%F_stop,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F_stop,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%F_stop,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F_stop,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%F_stop,2), UBOUND(InData%F_stop,2) + DO i1 = LBOUND(InData%F_stop,1), UBOUND(InData%F_stop,1) + ReKiBuf(Re_Xferred) = InData%F_stop(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%F_ext) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%F_ext,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F_ext,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%F_ext,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F_ext,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%F_ext,2), UBOUND(InData%F_ext,2) + DO i1 = LBOUND(InData%F_ext,1), UBOUND(InData%F_ext,1) + ReKiBuf(Re_Xferred) = InData%F_ext(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%F_fr) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%F_fr,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F_fr,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%F_fr,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F_fr,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%F_fr,2), UBOUND(InData%F_fr,2) + DO i1 = LBOUND(InData%F_fr,1), UBOUND(InData%F_fr,1) + ReKiBuf(Re_Xferred) = InData%F_fr(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%C_ctrl) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%C_ctrl,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%C_ctrl,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%C_ctrl,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%C_ctrl,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%C_ctrl,2), UBOUND(InData%C_ctrl,2) + DO i1 = LBOUND(InData%C_ctrl,1), UBOUND(InData%C_ctrl,1) + ReKiBuf(Re_Xferred) = InData%C_ctrl(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%C_Brake) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%C_Brake,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%C_Brake,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%C_Brake,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%C_Brake,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%C_Brake,2), UBOUND(InData%C_Brake,2) + DO i1 = LBOUND(InData%C_Brake,1), UBOUND(InData%C_Brake,1) + ReKiBuf(Re_Xferred) = InData%C_Brake(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%F_table) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%F_table,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F_table,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%F_table,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F_table,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%F_table,2), UBOUND(InData%F_table,2) + DO i1 = LBOUND(InData%F_table,1), UBOUND(InData%F_table,1) + ReKiBuf(Re_Xferred) = InData%F_table(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%F_k) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%F_k,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F_k,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%F_k,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F_k,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%F_k,2), UBOUND(InData%F_k,2) + DO i1 = LBOUND(InData%F_k,1), UBOUND(InData%F_k,1) + ReKiBuf(Re_Xferred) = InData%F_k(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%a_G) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%a_G,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%a_G,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%a_G,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%a_G,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%a_G,2), UBOUND(InData%a_G,2) + DO i1 = LBOUND(InData%a_G,1), UBOUND(InData%a_G,1) + ReKiBuf(Re_Xferred) = InData%a_G(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%rdisp_P) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%rdisp_P,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%rdisp_P,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%rdisp_P,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%rdisp_P,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%rdisp_P,2), UBOUND(InData%rdisp_P,2) + DO i1 = LBOUND(InData%rdisp_P,1), UBOUND(InData%rdisp_P,1) + ReKiBuf(Re_Xferred) = InData%rdisp_P(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%rdot_P) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%rdot_P,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%rdot_P,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%rdot_P,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%rdot_P,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%rdot_P,2), UBOUND(InData%rdot_P,2) + DO i1 = LBOUND(InData%rdot_P,1), UBOUND(InData%rdot_P,1) + ReKiBuf(Re_Xferred) = InData%rdot_P(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%rddot_P) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%rddot_P,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%rddot_P,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%rddot_P,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%rddot_P,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%rddot_P,2), UBOUND(InData%rddot_P,2) + DO i1 = LBOUND(InData%rddot_P,1), UBOUND(InData%rddot_P,1) + ReKiBuf(Re_Xferred) = InData%rddot_P(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%omega_P) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%omega_P,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%omega_P,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%omega_P,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%omega_P,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%omega_P,2), UBOUND(InData%omega_P,2) + DO i1 = LBOUND(InData%omega_P,1), UBOUND(InData%omega_P,1) + ReKiBuf(Re_Xferred) = InData%omega_P(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%alpha_P) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%alpha_P,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%alpha_P,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%alpha_P,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%alpha_P,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%alpha_P,2), UBOUND(InData%alpha_P,2) + DO i1 = LBOUND(InData%alpha_P,1), UBOUND(InData%alpha_P,1) + ReKiBuf(Re_Xferred) = InData%alpha_P(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%F_P) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%F_P,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F_P,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%F_P,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F_P,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%F_P,2), UBOUND(InData%F_P,2) + DO i1 = LBOUND(InData%F_P,1), UBOUND(InData%F_P,1) + ReKiBuf(Re_Xferred) = InData%F_P(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%M_P) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%M_P,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%M_P,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%M_P,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%M_P,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%M_P,2), UBOUND(InData%M_P,2) + DO i1 = LBOUND(InData%M_P,1), UBOUND(InData%M_P,1) + ReKiBuf(Re_Xferred) = InData%M_P(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%Acc) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Acc,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Acc,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Acc,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Acc,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Acc,2), UBOUND(InData%Acc,2) + DO i1 = LBOUND(InData%Acc,1), UBOUND(InData%Acc,1) + ReKiBuf(Re_Xferred) = InData%Acc(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IntKiBuf(Int_Xferred) = InData%PrescribedInterpIdx + Int_Xferred = Int_Xferred + 1 + END SUBROUTINE StC_PackMisc + + SUBROUTINE StC_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(StC_MiscVarType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_UnPackMisc' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! F_stop not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%F_stop)) DEALLOCATE(OutData%F_stop) + ALLOCATE(OutData%F_stop(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%F_stop.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%F_stop,2), UBOUND(OutData%F_stop,2) + DO i1 = LBOUND(OutData%F_stop,1), UBOUND(OutData%F_stop,1) + OutData%F_stop(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! F_ext not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%F_ext)) DEALLOCATE(OutData%F_ext) + ALLOCATE(OutData%F_ext(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%F_ext.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%F_ext,2), UBOUND(OutData%F_ext,2) + DO i1 = LBOUND(OutData%F_ext,1), UBOUND(OutData%F_ext,1) + OutData%F_ext(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! F_fr not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%F_fr)) DEALLOCATE(OutData%F_fr) + ALLOCATE(OutData%F_fr(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%F_fr.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%F_fr,2), UBOUND(OutData%F_fr,2) + DO i1 = LBOUND(OutData%F_fr,1), UBOUND(OutData%F_fr,1) + OutData%F_fr(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! C_ctrl not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%C_ctrl)) DEALLOCATE(OutData%C_ctrl) + ALLOCATE(OutData%C_ctrl(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%C_ctrl.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%C_ctrl,2), UBOUND(OutData%C_ctrl,2) + DO i1 = LBOUND(OutData%C_ctrl,1), UBOUND(OutData%C_ctrl,1) + OutData%C_ctrl(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! C_Brake not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%C_Brake)) DEALLOCATE(OutData%C_Brake) + ALLOCATE(OutData%C_Brake(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%C_Brake.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%C_Brake,2), UBOUND(OutData%C_Brake,2) + DO i1 = LBOUND(OutData%C_Brake,1), UBOUND(OutData%C_Brake,1) + OutData%C_Brake(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! F_table not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%F_table)) DEALLOCATE(OutData%F_table) + ALLOCATE(OutData%F_table(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%F_table.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%F_table,2), UBOUND(OutData%F_table,2) + DO i1 = LBOUND(OutData%F_table,1), UBOUND(OutData%F_table,1) + OutData%F_table(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! F_k not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%F_k)) DEALLOCATE(OutData%F_k) + ALLOCATE(OutData%F_k(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%F_k.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%F_k,2), UBOUND(OutData%F_k,2) + DO i1 = LBOUND(OutData%F_k,1), UBOUND(OutData%F_k,1) + OutData%F_k(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! a_G not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%a_G)) DEALLOCATE(OutData%a_G) + ALLOCATE(OutData%a_G(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%a_G.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%a_G,2), UBOUND(OutData%a_G,2) + DO i1 = LBOUND(OutData%a_G,1), UBOUND(OutData%a_G,1) + OutData%a_G(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! rdisp_P not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%rdisp_P)) DEALLOCATE(OutData%rdisp_P) + ALLOCATE(OutData%rdisp_P(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%rdisp_P.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%rdisp_P,2), UBOUND(OutData%rdisp_P,2) + DO i1 = LBOUND(OutData%rdisp_P,1), UBOUND(OutData%rdisp_P,1) + OutData%rdisp_P(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! rdot_P not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%rdot_P)) DEALLOCATE(OutData%rdot_P) + ALLOCATE(OutData%rdot_P(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%rdot_P.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%rdot_P,2), UBOUND(OutData%rdot_P,2) + DO i1 = LBOUND(OutData%rdot_P,1), UBOUND(OutData%rdot_P,1) + OutData%rdot_P(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! rddot_P not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%rddot_P)) DEALLOCATE(OutData%rddot_P) + ALLOCATE(OutData%rddot_P(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%rddot_P.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%rddot_P,2), UBOUND(OutData%rddot_P,2) + DO i1 = LBOUND(OutData%rddot_P,1), UBOUND(OutData%rddot_P,1) + OutData%rddot_P(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! omega_P not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%omega_P)) DEALLOCATE(OutData%omega_P) + ALLOCATE(OutData%omega_P(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%omega_P.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%omega_P,2), UBOUND(OutData%omega_P,2) + DO i1 = LBOUND(OutData%omega_P,1), UBOUND(OutData%omega_P,1) + OutData%omega_P(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! alpha_P not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%alpha_P)) DEALLOCATE(OutData%alpha_P) + ALLOCATE(OutData%alpha_P(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%alpha_P.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%alpha_P,2), UBOUND(OutData%alpha_P,2) + DO i1 = LBOUND(OutData%alpha_P,1), UBOUND(OutData%alpha_P,1) + OutData%alpha_P(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! F_P not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%F_P)) DEALLOCATE(OutData%F_P) + ALLOCATE(OutData%F_P(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%F_P.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%F_P,2), UBOUND(OutData%F_P,2) + DO i1 = LBOUND(OutData%F_P,1), UBOUND(OutData%F_P,1) + OutData%F_P(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! M_P not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%M_P)) DEALLOCATE(OutData%M_P) + ALLOCATE(OutData%M_P(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%M_P.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%M_P,2), UBOUND(OutData%M_P,2) + DO i1 = LBOUND(OutData%M_P,1), UBOUND(OutData%M_P,1) + OutData%M_P(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Acc not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Acc)) DEALLOCATE(OutData%Acc) + ALLOCATE(OutData%Acc(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Acc.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Acc,2), UBOUND(OutData%Acc,2) + DO i1 = LBOUND(OutData%Acc,1), UBOUND(OutData%Acc,1) + OutData%Acc(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + OutData%PrescribedInterpIdx = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END SUBROUTINE StC_UnPackMisc + + SUBROUTINE StC_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) + TYPE(StC_ParameterType), INTENT(IN) :: SrcParamData + TYPE(StC_ParameterType), INTENT(INOUT) :: DstParamData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_CopyParam' +! + ErrStat = ErrID_None + ErrMsg = "" + DstParamData%DT = SrcParamData%DT + DstParamData%RootName = SrcParamData%RootName + DstParamData%StC_DOF_MODE = SrcParamData%StC_DOF_MODE + DstParamData%StC_X_DOF = SrcParamData%StC_X_DOF + DstParamData%StC_Y_DOF = SrcParamData%StC_Y_DOF + DstParamData%StC_Z_DOF = SrcParamData%StC_Z_DOF + DstParamData%M_X = SrcParamData%M_X + DstParamData%M_Y = SrcParamData%M_Y + DstParamData%M_Z = SrcParamData%M_Z + DstParamData%M_XY = SrcParamData%M_XY + DstParamData%K_X = SrcParamData%K_X + DstParamData%K_Y = SrcParamData%K_Y + DstParamData%K_Z = SrcParamData%K_Z + DstParamData%C_X = SrcParamData%C_X + DstParamData%C_Y = SrcParamData%C_Y + DstParamData%C_Z = SrcParamData%C_Z + DstParamData%K_S = SrcParamData%K_S + DstParamData%C_S = SrcParamData%C_S + DstParamData%P_SP = SrcParamData%P_SP + DstParamData%N_SP = SrcParamData%N_SP + DstParamData%Gravity = SrcParamData%Gravity + DstParamData%StC_CMODE = SrcParamData%StC_CMODE + DstParamData%StC_SA_MODE = SrcParamData%StC_SA_MODE + DstParamData%StC_X_C_HIGH = SrcParamData%StC_X_C_HIGH + DstParamData%StC_X_C_LOW = SrcParamData%StC_X_C_LOW + DstParamData%StC_Y_C_HIGH = SrcParamData%StC_Y_C_HIGH + DstParamData%StC_Y_C_LOW = SrcParamData%StC_Y_C_LOW + DstParamData%StC_Z_C_HIGH = SrcParamData%StC_Z_C_HIGH + DstParamData%StC_Z_C_LOW = SrcParamData%StC_Z_C_LOW + DstParamData%StC_X_C_BRAKE = SrcParamData%StC_X_C_BRAKE + DstParamData%StC_Y_C_BRAKE = SrcParamData%StC_Y_C_BRAKE + DstParamData%StC_Z_C_BRAKE = SrcParamData%StC_Z_C_BRAKE + DstParamData%L_X = SrcParamData%L_X + DstParamData%B_X = SrcParamData%B_X + DstParamData%area_X = SrcParamData%area_X + DstParamData%area_ratio_X = SrcParamData%area_ratio_X + DstParamData%headLossCoeff_X = SrcParamData%headLossCoeff_X + DstParamData%rho_X = SrcParamData%rho_X + DstParamData%L_Y = SrcParamData%L_Y + DstParamData%B_Y = SrcParamData%B_Y + DstParamData%area_Y = SrcParamData%area_Y + DstParamData%area_ratio_Y = SrcParamData%area_ratio_Y + DstParamData%headLossCoeff_Y = SrcParamData%headLossCoeff_Y + DstParamData%rho_Y = SrcParamData%rho_Y + DstParamData%Use_F_TBL = SrcParamData%Use_F_TBL +IF (ALLOCATED(SrcParamData%F_TBL)) THEN + i1_l = LBOUND(SrcParamData%F_TBL,1) + i1_u = UBOUND(SrcParamData%F_TBL,1) + i2_l = LBOUND(SrcParamData%F_TBL,2) + i2_u = UBOUND(SrcParamData%F_TBL,2) + IF (.NOT. ALLOCATED(DstParamData%F_TBL)) THEN + ALLOCATE(DstParamData%F_TBL(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%F_TBL.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%F_TBL = SrcParamData%F_TBL +ENDIF + DstParamData%NumMeshPts = SrcParamData%NumMeshPts + DstParamData%PrescribedForcesCoordSys = SrcParamData%PrescribedForcesCoordSys +IF (ALLOCATED(SrcParamData%StC_PrescribedForce)) THEN + i1_l = LBOUND(SrcParamData%StC_PrescribedForce,1) + i1_u = UBOUND(SrcParamData%StC_PrescribedForce,1) + i2_l = LBOUND(SrcParamData%StC_PrescribedForce,2) + i2_u = UBOUND(SrcParamData%StC_PrescribedForce,2) + IF (.NOT. ALLOCATED(DstParamData%StC_PrescribedForce)) THEN + ALLOCATE(DstParamData%StC_PrescribedForce(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%StC_PrescribedForce.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%StC_PrescribedForce = SrcParamData%StC_PrescribedForce +ENDIF + END SUBROUTINE StC_CopyParam + + SUBROUTINE StC_DestroyParam( ParamData, ErrStat, ErrMsg ) + TYPE(StC_ParameterType), INTENT(INOUT) :: ParamData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'StC_DestroyParam' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(ParamData%F_TBL)) THEN + DEALLOCATE(ParamData%F_TBL) +ENDIF +IF (ALLOCATED(ParamData%StC_PrescribedForce)) THEN + DEALLOCATE(ParamData%StC_PrescribedForce) +ENDIF + END SUBROUTINE StC_DestroyParam + + SUBROUTINE StC_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(StC_ParameterType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_PackParam' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Db_BufSz = Db_BufSz + 1 ! DT + Int_BufSz = Int_BufSz + 1*LEN(InData%RootName) ! RootName + Int_BufSz = Int_BufSz + 1 ! StC_DOF_MODE + Int_BufSz = Int_BufSz + 1 ! StC_X_DOF + Int_BufSz = Int_BufSz + 1 ! StC_Y_DOF + Int_BufSz = Int_BufSz + 1 ! StC_Z_DOF + Re_BufSz = Re_BufSz + 1 ! M_X + Re_BufSz = Re_BufSz + 1 ! M_Y + Re_BufSz = Re_BufSz + 1 ! M_Z + Re_BufSz = Re_BufSz + 1 ! M_XY + Re_BufSz = Re_BufSz + 1 ! K_X + Re_BufSz = Re_BufSz + 1 ! K_Y + Re_BufSz = Re_BufSz + 1 ! K_Z + Re_BufSz = Re_BufSz + 1 ! C_X + Re_BufSz = Re_BufSz + 1 ! C_Y + Re_BufSz = Re_BufSz + 1 ! C_Z + Re_BufSz = Re_BufSz + SIZE(InData%K_S) ! K_S + Re_BufSz = Re_BufSz + SIZE(InData%C_S) ! C_S + Re_BufSz = Re_BufSz + SIZE(InData%P_SP) ! P_SP + Re_BufSz = Re_BufSz + SIZE(InData%N_SP) ! N_SP + Re_BufSz = Re_BufSz + SIZE(InData%Gravity) ! Gravity + Int_BufSz = Int_BufSz + 1 ! StC_CMODE + Int_BufSz = Int_BufSz + 1 ! StC_SA_MODE + Re_BufSz = Re_BufSz + 1 ! StC_X_C_HIGH + Re_BufSz = Re_BufSz + 1 ! StC_X_C_LOW + Re_BufSz = Re_BufSz + 1 ! StC_Y_C_HIGH + Re_BufSz = Re_BufSz + 1 ! StC_Y_C_LOW + Re_BufSz = Re_BufSz + 1 ! StC_Z_C_HIGH + Re_BufSz = Re_BufSz + 1 ! StC_Z_C_LOW + Re_BufSz = Re_BufSz + 1 ! StC_X_C_BRAKE + Re_BufSz = Re_BufSz + 1 ! StC_Y_C_BRAKE + Re_BufSz = Re_BufSz + 1 ! StC_Z_C_BRAKE + Re_BufSz = Re_BufSz + 1 ! L_X + Re_BufSz = Re_BufSz + 1 ! B_X + Re_BufSz = Re_BufSz + 1 ! area_X + Re_BufSz = Re_BufSz + 1 ! area_ratio_X + Re_BufSz = Re_BufSz + 1 ! headLossCoeff_X + Re_BufSz = Re_BufSz + 1 ! rho_X + Re_BufSz = Re_BufSz + 1 ! L_Y + Re_BufSz = Re_BufSz + 1 ! B_Y + Re_BufSz = Re_BufSz + 1 ! area_Y + Re_BufSz = Re_BufSz + 1 ! area_ratio_Y + Re_BufSz = Re_BufSz + 1 ! headLossCoeff_Y + Re_BufSz = Re_BufSz + 1 ! rho_Y + Int_BufSz = Int_BufSz + 1 ! Use_F_TBL + Int_BufSz = Int_BufSz + 1 ! F_TBL allocated yes/no + IF ( ALLOCATED(InData%F_TBL) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! F_TBL upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%F_TBL) ! F_TBL + END IF + Int_BufSz = Int_BufSz + 1 ! NumMeshPts + Int_BufSz = Int_BufSz + 1 ! PrescribedForcesCoordSys + Int_BufSz = Int_BufSz + 1 ! StC_PrescribedForce allocated yes/no + IF ( ALLOCATED(InData%StC_PrescribedForce) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! StC_PrescribedForce upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%StC_PrescribedForce) ! StC_PrescribedForce + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + DbKiBuf(Db_Xferred) = InData%DT + Db_Xferred = Db_Xferred + 1 + DO I = 1, LEN(InData%RootName) + IntKiBuf(Int_Xferred) = ICHAR(InData%RootName(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + IntKiBuf(Int_Xferred) = InData%StC_DOF_MODE + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = TRANSFER(InData%StC_X_DOF, IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = TRANSFER(InData%StC_Y_DOF, IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = TRANSFER(InData%StC_Z_DOF, IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%M_X + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%M_Y + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%M_Z + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%M_XY + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%K_X + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%K_Y + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%K_Z + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%C_X + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%C_Y + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%C_Z + Re_Xferred = Re_Xferred + 1 + DO i1 = LBOUND(InData%K_S,1), UBOUND(InData%K_S,1) + ReKiBuf(Re_Xferred) = InData%K_S(i1) + Re_Xferred = Re_Xferred + 1 + END DO + DO i1 = LBOUND(InData%C_S,1), UBOUND(InData%C_S,1) + ReKiBuf(Re_Xferred) = InData%C_S(i1) + Re_Xferred = Re_Xferred + 1 + END DO + DO i1 = LBOUND(InData%P_SP,1), UBOUND(InData%P_SP,1) + ReKiBuf(Re_Xferred) = InData%P_SP(i1) + Re_Xferred = Re_Xferred + 1 + END DO + DO i1 = LBOUND(InData%N_SP,1), UBOUND(InData%N_SP,1) + ReKiBuf(Re_Xferred) = InData%N_SP(i1) + Re_Xferred = Re_Xferred + 1 + END DO + DO i1 = LBOUND(InData%Gravity,1), UBOUND(InData%Gravity,1) + ReKiBuf(Re_Xferred) = InData%Gravity(i1) + Re_Xferred = Re_Xferred + 1 + END DO + IntKiBuf(Int_Xferred) = InData%StC_CMODE + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%StC_SA_MODE + Int_Xferred = Int_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_X_C_HIGH + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_X_C_LOW + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Y_C_HIGH + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Y_C_LOW + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Z_C_HIGH + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Z_C_LOW + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_X_C_BRAKE + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Y_C_BRAKE + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%StC_Z_C_BRAKE + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%L_X + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%B_X + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%area_X + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%area_ratio_X + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%headLossCoeff_X + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%rho_X + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%L_Y + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%B_Y + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%area_Y + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%area_ratio_Y + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%headLossCoeff_Y + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%rho_Y + Re_Xferred = Re_Xferred + 1 + IntKiBuf(Int_Xferred) = TRANSFER(InData%Use_F_TBL, IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%F_TBL) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%F_TBL,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F_TBL,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%F_TBL,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%F_TBL,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%F_TBL,2), UBOUND(InData%F_TBL,2) + DO i1 = LBOUND(InData%F_TBL,1), UBOUND(InData%F_TBL,1) + ReKiBuf(Re_Xferred) = InData%F_TBL(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IntKiBuf(Int_Xferred) = InData%NumMeshPts + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%PrescribedForcesCoordSys + Int_Xferred = Int_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%StC_PrescribedForce) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%StC_PrescribedForce,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%StC_PrescribedForce,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%StC_PrescribedForce,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%StC_PrescribedForce,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%StC_PrescribedForce,2), UBOUND(InData%StC_PrescribedForce,2) + DO i1 = LBOUND(InData%StC_PrescribedForce,1), UBOUND(InData%StC_PrescribedForce,1) + ReKiBuf(Re_Xferred) = InData%StC_PrescribedForce(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + END SUBROUTINE StC_PackParam + + SUBROUTINE StC_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(StC_ParameterType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_UnPackParam' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%DT = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + DO I = 1, LEN(OutData%RootName) + OutData%RootName(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + OutData%StC_DOF_MODE = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%StC_X_DOF = TRANSFER(IntKiBuf(Int_Xferred), OutData%StC_X_DOF) + Int_Xferred = Int_Xferred + 1 + OutData%StC_Y_DOF = TRANSFER(IntKiBuf(Int_Xferred), OutData%StC_Y_DOF) + Int_Xferred = Int_Xferred + 1 + OutData%StC_Z_DOF = TRANSFER(IntKiBuf(Int_Xferred), OutData%StC_Z_DOF) + Int_Xferred = Int_Xferred + 1 + OutData%M_X = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%M_Y = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%M_Z = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%M_XY = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%K_X = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%K_Y = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%K_Z = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%C_X = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%C_Y = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%C_Z = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + i1_l = LBOUND(OutData%K_S,1) + i1_u = UBOUND(OutData%K_S,1) + DO i1 = LBOUND(OutData%K_S,1), UBOUND(OutData%K_S,1) + OutData%K_S(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + i1_l = LBOUND(OutData%C_S,1) + i1_u = UBOUND(OutData%C_S,1) + DO i1 = LBOUND(OutData%C_S,1), UBOUND(OutData%C_S,1) + OutData%C_S(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + i1_l = LBOUND(OutData%P_SP,1) + i1_u = UBOUND(OutData%P_SP,1) + DO i1 = LBOUND(OutData%P_SP,1), UBOUND(OutData%P_SP,1) + OutData%P_SP(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + i1_l = LBOUND(OutData%N_SP,1) + i1_u = UBOUND(OutData%N_SP,1) + DO i1 = LBOUND(OutData%N_SP,1), UBOUND(OutData%N_SP,1) + OutData%N_SP(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + i1_l = LBOUND(OutData%Gravity,1) + i1_u = UBOUND(OutData%Gravity,1) + DO i1 = LBOUND(OutData%Gravity,1), UBOUND(OutData%Gravity,1) + OutData%Gravity(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + OutData%StC_CMODE = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%StC_SA_MODE = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%StC_X_C_HIGH = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_X_C_LOW = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Y_C_HIGH = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Y_C_LOW = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Z_C_HIGH = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Z_C_LOW = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_X_C_BRAKE = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Y_C_BRAKE = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%StC_Z_C_BRAKE = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%L_X = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%B_X = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%area_X = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%area_ratio_X = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%headLossCoeff_X = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%rho_X = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%L_Y = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%B_Y = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%area_Y = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%area_ratio_Y = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%headLossCoeff_Y = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%rho_Y = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%Use_F_TBL = TRANSFER(IntKiBuf(Int_Xferred), OutData%Use_F_TBL) + Int_Xferred = Int_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! F_TBL not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%F_TBL)) DEALLOCATE(OutData%F_TBL) + ALLOCATE(OutData%F_TBL(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%F_TBL.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%F_TBL,2), UBOUND(OutData%F_TBL,2) + DO i1 = LBOUND(OutData%F_TBL,1), UBOUND(OutData%F_TBL,1) + OutData%F_TBL(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + OutData%NumMeshPts = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%PrescribedForcesCoordSys = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! StC_PrescribedForce not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%StC_PrescribedForce)) DEALLOCATE(OutData%StC_PrescribedForce) + ALLOCATE(OutData%StC_PrescribedForce(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%StC_PrescribedForce.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%StC_PrescribedForce,2), UBOUND(OutData%StC_PrescribedForce,2) + DO i1 = LBOUND(OutData%StC_PrescribedForce,1), UBOUND(OutData%StC_PrescribedForce,1) + OutData%StC_PrescribedForce(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + END SUBROUTINE StC_UnPackParam + + SUBROUTINE StC_CopyInput( SrcInputData, DstInputData, CtrlCode, ErrStat, ErrMsg ) + TYPE(StC_InputType), INTENT(INOUT) :: SrcInputData + TYPE(StC_InputType), INTENT(INOUT) :: DstInputData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_CopyInput' +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(SrcInputData%Mesh)) THEN + i1_l = LBOUND(SrcInputData%Mesh,1) + i1_u = UBOUND(SrcInputData%Mesh,1) + IF (.NOT. ALLOCATED(DstInputData%Mesh)) THEN + ALLOCATE(DstInputData%Mesh(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInputData%Mesh.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcInputData%Mesh,1), UBOUND(SrcInputData%Mesh,1) + CALL MeshCopy( SrcInputData%Mesh(i1), DstInputData%Mesh(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF + END SUBROUTINE StC_CopyInput + + SUBROUTINE StC_DestroyInput( InputData, ErrStat, ErrMsg ) + TYPE(StC_InputType), INTENT(INOUT) :: InputData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'StC_DestroyInput' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(InputData%Mesh)) THEN +DO i1 = LBOUND(InputData%Mesh,1), UBOUND(InputData%Mesh,1) + CALL MeshDestroy( InputData%Mesh(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(InputData%Mesh) +ENDIF + END SUBROUTINE StC_DestroyInput + + SUBROUTINE StC_PackInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(StC_InputType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_PackInput' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! Mesh allocated yes/no + IF ( ALLOCATED(InData%Mesh) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! Mesh upper/lower bounds for each dimension + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + DO i1 = LBOUND(InData%Mesh,1), UBOUND(InData%Mesh,1) + Int_BufSz = Int_BufSz + 3 ! Mesh: size of buffers for each call to pack subtype + CALL MeshPack( InData%Mesh(i1), Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, .TRUE. ) ! Mesh + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! Mesh + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! Mesh + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! Mesh + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IF ( .NOT. ALLOCATED(InData%Mesh) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Mesh,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Mesh,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%Mesh,1), UBOUND(InData%Mesh,1) + CALL MeshPack( InData%Mesh(i1), Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, OnlySize ) ! Mesh + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF + END SUBROUTINE StC_PackInput + + SUBROUTINE StC_UnPackInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(StC_InputType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_UnPackInput' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Mesh not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Mesh)) DEALLOCATE(OutData%Mesh) + ALLOCATE(OutData%Mesh(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Mesh.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%Mesh,1), UBOUND(OutData%Mesh,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MeshUnpack( OutData%Mesh(i1), Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2 ) ! Mesh + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + END SUBROUTINE StC_UnPackInput + + SUBROUTINE StC_CopyOutput( SrcOutputData, DstOutputData, CtrlCode, ErrStat, ErrMsg ) + TYPE(StC_OutputType), INTENT(INOUT) :: SrcOutputData + TYPE(StC_OutputType), INTENT(INOUT) :: DstOutputData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_CopyOutput' +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(SrcOutputData%Mesh)) THEN + i1_l = LBOUND(SrcOutputData%Mesh,1) + i1_u = UBOUND(SrcOutputData%Mesh,1) + IF (.NOT. ALLOCATED(DstOutputData%Mesh)) THEN + ALLOCATE(DstOutputData%Mesh(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstOutputData%Mesh.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcOutputData%Mesh,1), UBOUND(SrcOutputData%Mesh,1) + CALL MeshCopy( SrcOutputData%Mesh(i1), DstOutputData%Mesh(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF + END SUBROUTINE StC_CopyOutput + + SUBROUTINE StC_DestroyOutput( OutputData, ErrStat, ErrMsg ) + TYPE(StC_OutputType), INTENT(INOUT) :: OutputData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'StC_DestroyOutput' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" +IF (ALLOCATED(OutputData%Mesh)) THEN +DO i1 = LBOUND(OutputData%Mesh,1), UBOUND(OutputData%Mesh,1) + CALL MeshDestroy( OutputData%Mesh(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(OutputData%Mesh) +ENDIF + END SUBROUTINE StC_DestroyOutput + + SUBROUTINE StC_PackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(StC_OutputType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_PackOutput' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Int_BufSz = Int_BufSz + 1 ! Mesh allocated yes/no + IF ( ALLOCATED(InData%Mesh) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! Mesh upper/lower bounds for each dimension + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + DO i1 = LBOUND(InData%Mesh,1), UBOUND(InData%Mesh,1) + Int_BufSz = Int_BufSz + 3 ! Mesh: size of buffers for each call to pack subtype + CALL MeshPack( InData%Mesh(i1), Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, .TRUE. ) ! Mesh + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! Mesh + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! Mesh + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! Mesh + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + IF ( .NOT. ALLOCATED(InData%Mesh) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Mesh,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Mesh,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%Mesh,1), UBOUND(InData%Mesh,1) + CALL MeshPack( InData%Mesh(i1), Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, OnlySize ) ! Mesh + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF + END SUBROUTINE StC_PackOutput + + SUBROUTINE StC_UnPackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(StC_OutputType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'StC_UnPackOutput' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Mesh not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Mesh)) DEALLOCATE(OutData%Mesh) + ALLOCATE(OutData%Mesh(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Mesh.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%Mesh,1), UBOUND(OutData%Mesh,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MeshUnpack( OutData%Mesh(i1), Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2 ) ! Mesh + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + END SUBROUTINE StC_UnPackOutput + + + SUBROUTINE StC_Input_ExtrapInterp(u, t, u_out, t_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Input u_out at time t_out, from previous/future time +! values of u (which has values associated with times in t). Order of the interpolation is given by the size of u +! +! expressions below based on either +! +! f(t) = a +! f(t) = a + b * t, or +! f(t) = a + b * t + c * t**2 +! +! where a, b and c are determined as the solution to +! f(t1) = u1, f(t2) = u2, f(t3) = u3 (as appropriate) +! +!.................................................................................................................................. + + TYPE(StC_InputType), INTENT(INOUT) :: u(:) ! Input at t1 > t2 > t3 + REAL(DbKi), INTENT(IN ) :: t(:) ! Times associated with the Inputs + TYPE(StC_InputType), INTENT(INOUT) :: u_out ! Input at tin_out + REAL(DbKi), INTENT(IN ) :: t_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + INTEGER(IntKi) :: order ! order of polynomial fit (max 2) + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + CHARACTER(*), PARAMETER :: RoutineName = 'StC_Input_ExtrapInterp' + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + if ( size(t) .ne. size(u)) then + CALL SetErrStat(ErrID_Fatal,'size(t) must equal size(u)',ErrStat,ErrMsg,RoutineName) + RETURN + endif + order = SIZE(u) - 1 + IF ( order .eq. 0 ) THEN + CALL StC_CopyInput(u(1), u_out, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE IF ( order .eq. 1 ) THEN + CALL StC_Input_ExtrapInterp1(u(1), u(2), t, u_out, t_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE IF ( order .eq. 2 ) THEN + CALL StC_Input_ExtrapInterp2(u(1), u(2), u(3), t, u_out, t_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE + CALL SetErrStat(ErrID_Fatal,'size(u) must be less than 4 (order must be less than 3).',ErrStat,ErrMsg,RoutineName) + RETURN + ENDIF + END SUBROUTINE StC_Input_ExtrapInterp + + + SUBROUTINE StC_Input_ExtrapInterp1(u1, u2, tin, u_out, tin_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Input u_out at time t_out, from previous/future time +! values of u (which has values associated with times in t). Order of the interpolation is 1. +! +! f(t) = a + b * t, or +! +! where a and b are determined as the solution to +! f(t1) = u1, f(t2) = u2 +! +!.................................................................................................................................. + + TYPE(StC_InputType), INTENT(INOUT) :: u1 ! Input at t1 > t2 + TYPE(StC_InputType), INTENT(INOUT) :: u2 ! Input at t2 + REAL(DbKi), INTENT(IN ) :: tin(2) ! Times associated with the Inputs + TYPE(StC_InputType), INTENT(INOUT) :: u_out ! Input at tin_out + REAL(DbKi), INTENT(IN ) :: tin_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + REAL(DbKi) :: t(2) ! Times associated with the Inputs + REAL(DbKi) :: t_out ! Time to which to be extrap/interpd + CHARACTER(*), PARAMETER :: RoutineName = 'StC_Input_ExtrapInterp1' + REAL(DbKi) :: b ! temporary for extrapolation/interpolation + REAL(DbKi) :: ScaleFactor ! temporary for extrapolation/interpolation + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + INTEGER :: i01 ! dim1 level 0 counter variable for arrays of ddts + INTEGER :: i1 ! dim1 counter variable for arrays + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + ! we'll subtract a constant from the times to resolve some + ! numerical issues when t gets large (and to simplify the equations) + t = tin - tin(1) + t_out = tin_out - tin(1) + + IF ( EqualRealNos( t(1), t(2) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(2) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + + ScaleFactor = t_out / t(2) +IF (ALLOCATED(u_out%Mesh) .AND. ALLOCATED(u1%Mesh)) THEN + DO i1 = LBOUND(u_out%Mesh,1),UBOUND(u_out%Mesh,1) + CALL MeshExtrapInterp1(u1%Mesh(i1), u2%Mesh(i1), tin, u_out%Mesh(i1), tin_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ENDDO +END IF ! check if allocated + END SUBROUTINE StC_Input_ExtrapInterp1 + + + SUBROUTINE StC_Input_ExtrapInterp2(u1, u2, u3, tin, u_out, tin_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Input u_out at time t_out, from previous/future time +! values of u (which has values associated with times in t). Order of the interpolation is 2. +! +! expressions below based on either +! +! f(t) = a + b * t + c * t**2 +! +! where a, b and c are determined as the solution to +! f(t1) = u1, f(t2) = u2, f(t3) = u3 +! +!.................................................................................................................................. + + TYPE(StC_InputType), INTENT(INOUT) :: u1 ! Input at t1 > t2 > t3 + TYPE(StC_InputType), INTENT(INOUT) :: u2 ! Input at t2 > t3 + TYPE(StC_InputType), INTENT(INOUT) :: u3 ! Input at t3 + REAL(DbKi), INTENT(IN ) :: tin(3) ! Times associated with the Inputs + TYPE(StC_InputType), INTENT(INOUT) :: u_out ! Input at tin_out + REAL(DbKi), INTENT(IN ) :: tin_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + REAL(DbKi) :: t(3) ! Times associated with the Inputs + REAL(DbKi) :: t_out ! Time to which to be extrap/interpd + INTEGER(IntKi) :: order ! order of polynomial fit (max 2) + REAL(DbKi) :: b ! temporary for extrapolation/interpolation + REAL(DbKi) :: c ! temporary for extrapolation/interpolation + REAL(DbKi) :: ScaleFactor ! temporary for extrapolation/interpolation + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + CHARACTER(*), PARAMETER :: RoutineName = 'StC_Input_ExtrapInterp2' + INTEGER :: i01 ! dim1 level 0 counter variable for arrays of ddts + INTEGER :: i1 ! dim1 counter variable for arrays + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + ! we'll subtract a constant from the times to resolve some + ! numerical issues when t gets large (and to simplify the equations) + t = tin - tin(1) + t_out = tin_out - tin(1) + + IF ( EqualRealNos( t(1), t(2) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(2) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + ELSE IF ( EqualRealNos( t(2), t(3) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(2) must not equal t(3) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + ELSE IF ( EqualRealNos( t(1), t(3) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(3) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + + ScaleFactor = t_out / (t(2) * t(3) * (t(2) - t(3))) +IF (ALLOCATED(u_out%Mesh) .AND. ALLOCATED(u1%Mesh)) THEN + DO i1 = LBOUND(u_out%Mesh,1),UBOUND(u_out%Mesh,1) + CALL MeshExtrapInterp2(u1%Mesh(i1), u2%Mesh(i1), u3%Mesh(i1), tin, u_out%Mesh(i1), tin_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ENDDO +END IF ! check if allocated + END SUBROUTINE StC_Input_ExtrapInterp2 + + + SUBROUTINE StC_Output_ExtrapInterp(y, t, y_out, t_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Output y_out at time t_out, from previous/future time +! values of y (which has values associated with times in t). Order of the interpolation is given by the size of y +! +! expressions below based on either +! +! f(t) = a +! f(t) = a + b * t, or +! f(t) = a + b * t + c * t**2 +! +! where a, b and c are determined as the solution to +! f(t1) = y1, f(t2) = y2, f(t3) = y3 (as appropriate) +! +!.................................................................................................................................. + + TYPE(StC_OutputType), INTENT(INOUT) :: y(:) ! Output at t1 > t2 > t3 + REAL(DbKi), INTENT(IN ) :: t(:) ! Times associated with the Outputs + TYPE(StC_OutputType), INTENT(INOUT) :: y_out ! Output at tin_out + REAL(DbKi), INTENT(IN ) :: t_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + INTEGER(IntKi) :: order ! order of polynomial fit (max 2) + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + CHARACTER(*), PARAMETER :: RoutineName = 'StC_Output_ExtrapInterp' + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + if ( size(t) .ne. size(y)) then + CALL SetErrStat(ErrID_Fatal,'size(t) must equal size(y)',ErrStat,ErrMsg,RoutineName) + RETURN + endif + order = SIZE(y) - 1 + IF ( order .eq. 0 ) THEN + CALL StC_CopyOutput(y(1), y_out, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE IF ( order .eq. 1 ) THEN + CALL StC_Output_ExtrapInterp1(y(1), y(2), t, y_out, t_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE IF ( order .eq. 2 ) THEN + CALL StC_Output_ExtrapInterp2(y(1), y(2), y(3), t, y_out, t_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ELSE + CALL SetErrStat(ErrID_Fatal,'size(y) must be less than 4 (order must be less than 3).',ErrStat,ErrMsg,RoutineName) + RETURN + ENDIF + END SUBROUTINE StC_Output_ExtrapInterp + + + SUBROUTINE StC_Output_ExtrapInterp1(y1, y2, tin, y_out, tin_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Output y_out at time t_out, from previous/future time +! values of y (which has values associated with times in t). Order of the interpolation is 1. +! +! f(t) = a + b * t, or +! +! where a and b are determined as the solution to +! f(t1) = y1, f(t2) = y2 +! +!.................................................................................................................................. + + TYPE(StC_OutputType), INTENT(INOUT) :: y1 ! Output at t1 > t2 + TYPE(StC_OutputType), INTENT(INOUT) :: y2 ! Output at t2 + REAL(DbKi), INTENT(IN ) :: tin(2) ! Times associated with the Outputs + TYPE(StC_OutputType), INTENT(INOUT) :: y_out ! Output at tin_out + REAL(DbKi), INTENT(IN ) :: tin_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + REAL(DbKi) :: t(2) ! Times associated with the Outputs + REAL(DbKi) :: t_out ! Time to which to be extrap/interpd + CHARACTER(*), PARAMETER :: RoutineName = 'StC_Output_ExtrapInterp1' + REAL(DbKi) :: b ! temporary for extrapolation/interpolation + REAL(DbKi) :: ScaleFactor ! temporary for extrapolation/interpolation + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + INTEGER :: i01 ! dim1 level 0 counter variable for arrays of ddts + INTEGER :: i1 ! dim1 counter variable for arrays + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + ! we'll subtract a constant from the times to resolve some + ! numerical issues when t gets large (and to simplify the equations) + t = tin - tin(1) + t_out = tin_out - tin(1) + + IF ( EqualRealNos( t(1), t(2) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(2) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + + ScaleFactor = t_out / t(2) +IF (ALLOCATED(y_out%Mesh) .AND. ALLOCATED(y1%Mesh)) THEN + DO i1 = LBOUND(y_out%Mesh,1),UBOUND(y_out%Mesh,1) + CALL MeshExtrapInterp1(y1%Mesh(i1), y2%Mesh(i1), tin, y_out%Mesh(i1), tin_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ENDDO +END IF ! check if allocated + END SUBROUTINE StC_Output_ExtrapInterp1 + + + SUBROUTINE StC_Output_ExtrapInterp2(y1, y2, y3, tin, y_out, tin_out, ErrStat, ErrMsg ) +! +! This subroutine calculates a extrapolated (or interpolated) Output y_out at time t_out, from previous/future time +! values of y (which has values associated with times in t). Order of the interpolation is 2. +! +! expressions below based on either +! +! f(t) = a + b * t + c * t**2 +! +! where a, b and c are determined as the solution to +! f(t1) = y1, f(t2) = y2, f(t3) = y3 +! +!.................................................................................................................................. + + TYPE(StC_OutputType), INTENT(INOUT) :: y1 ! Output at t1 > t2 > t3 + TYPE(StC_OutputType), INTENT(INOUT) :: y2 ! Output at t2 > t3 + TYPE(StC_OutputType), INTENT(INOUT) :: y3 ! Output at t3 + REAL(DbKi), INTENT(IN ) :: tin(3) ! Times associated with the Outputs + TYPE(StC_OutputType), INTENT(INOUT) :: y_out ! Output at tin_out + REAL(DbKi), INTENT(IN ) :: tin_out ! time to be extrap/interp'd to + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + ! local variables + REAL(DbKi) :: t(3) ! Times associated with the Outputs + REAL(DbKi) :: t_out ! Time to which to be extrap/interpd + INTEGER(IntKi) :: order ! order of polynomial fit (max 2) + REAL(DbKi) :: b ! temporary for extrapolation/interpolation + REAL(DbKi) :: c ! temporary for extrapolation/interpolation + REAL(DbKi) :: ScaleFactor ! temporary for extrapolation/interpolation + INTEGER(IntKi) :: ErrStat2 ! local errors + CHARACTER(ErrMsgLen) :: ErrMsg2 ! local errors + CHARACTER(*), PARAMETER :: RoutineName = 'StC_Output_ExtrapInterp2' + INTEGER :: i01 ! dim1 level 0 counter variable for arrays of ddts + INTEGER :: i1 ! dim1 counter variable for arrays + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = "" + ! we'll subtract a constant from the times to resolve some + ! numerical issues when t gets large (and to simplify the equations) + t = tin - tin(1) + t_out = tin_out - tin(1) + + IF ( EqualRealNos( t(1), t(2) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(2) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + ELSE IF ( EqualRealNos( t(2), t(3) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(2) must not equal t(3) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + ELSE IF ( EqualRealNos( t(1), t(3) ) ) THEN + CALL SetErrStat(ErrID_Fatal, 't(1) must not equal t(3) to avoid a division-by-zero error.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + + ScaleFactor = t_out / (t(2) * t(3) * (t(2) - t(3))) +IF (ALLOCATED(y_out%Mesh) .AND. ALLOCATED(y1%Mesh)) THEN + DO i1 = LBOUND(y_out%Mesh,1),UBOUND(y_out%Mesh,1) + CALL MeshExtrapInterp2(y1%Mesh(i1), y2%Mesh(i1), y3%Mesh(i1), tin, y_out%Mesh(i1), tin_out, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + ENDDO +END IF ! check if allocated + END SUBROUTINE StC_Output_ExtrapInterp2 + +END MODULE StrucCtrl_Types +!ENDOFREGISTRYGENERATEDFILE From 2f77648e3112df48e7c7867462753573867c1279 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Wed, 7 Apr 2021 13:08:25 -0600 Subject: [PATCH 020/242] Replace non-standard tabs with spaces --- modules/moordyn/src/MoorDyn.f90 | 234 ++++++++++++++++---------------- 1 file changed, 117 insertions(+), 117 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index f02060c8e6..9a39072184 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -206,14 +206,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er - ! ----------------- go through file contents a first time, counting each entry ----------------------- - + ! ----------------- go through file contents a first time, counting each entry ----------------------- + i = 0 read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 !read a line do while ( ErrStat2 == 0 ) - + if (INDEX(Line, "---") > 0) then ! look for a header line if ( ( INDEX(Line, "LINE DICTIONARY") > 0) .or. ( INDEX(Line, "LINE TYPES") > 0) ) then ! if line dictionary header @@ -240,7 +240,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nRodTypes = p%nRodTypes + 1 read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - END DO + END DO else if ((INDEX(Line, "BODIES") > 0 ) .or. (INDEX(Line, "BODY LIST") > 0 ) .or. (INDEX(Line, "BODY PROPERTIES") > 0 )) then @@ -253,7 +253,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nBodies = p%nBodies + 1 read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - END DO + END DO else if ((INDEX(Line, "RODS") > 0 ) .or. (INDEX(Line, "ROD LIST") > 0) .or. (INDEX(Line, "ROD PROPERTIES") > 0)) then ! if rod properties header @@ -266,7 +266,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nRods = p%nRods + 1 read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - END DO + END DO else if ((INDEX(Line, "POINTS") > 0 ) .or. (INDEX(Line, "CONNECTION PROPERTIES") > 0) .or. (INDEX(Line, "NODE PROPERTIES") > 0) .or. (INDEX(Line, "POINT PROPERTIES") > 0) .or. (INDEX(Line, "POINT LIST") > 0) ) then ! if node properties header @@ -347,15 +347,15 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else ! otherwise ignore this line that isn't a recognized header line and read the next line read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 end if - + else ! otherwise ignore this line, which doesn't have the "---" or header line and read the next line read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 end if end do - + p%nConnectsExtra = p%nConnects + 2*p%nLines ! set maximum number of connections, accounting for possible detachment of each line end and a connection for that - + IF (wordy > 0) print *, " Identified ", p%nLineTypes , "LineTypes in input file." IF (wordy > 0) print *, " Identified ", p%nRodTypes , "RodTypes in input file." IF (wordy > 0) print *, " Identified ", p%nBodies , "Bodies in input file." @@ -416,7 +416,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! ---------------------- now go through again and process file contents -------------------- REWIND(UnIn) ! rewind to start of input file - + ! note: no longer worrying about "Echo" option Nx = 0 ! set state counter to zero @@ -467,7 +467,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er m%LineTypeList(l)%nEIpoints, & m%LineTypeList(l)%bstiffXs, & m%LineTypeList(l)%bstiffYs, ErrStat2, ErrMsg2) - + ! specify IdNum of line type for error checking m%LineTypeList(l)%IdNum = l @@ -551,12 +551,12 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !----------- process body type ----------------- - + call DecomposeString(tempString1, let1, num1, let2, num2, let3) - + READ(num1, *) m%BodyList(l)%IdNum ! convert to int, representing parent body index - if ((let2 == "COUPLED") .or. (let2 == "VESSEL") .or. (let1 == "CPLD") .or. (let1 == "VES")) then ! if a coupled body + if ((let2 == "COUPLED") .or. (let2 == "VESSEL") .or. (let1 == "CPLD") .or. (let1 == "VES")) then ! if a coupled body m%BodyList(l)%typeNum = -1 p%nCpldBodies=p%nCpldBodies+1 ! add this rod to coupled list @@ -570,7 +570,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er p%nFreeBodies=p%nFreeBodies+1 ! add this pinned rod to the free list because it is half free m%BodyStateIs1(p%nFreeBodies) = Nx+1 - m%BodyStateIsN(p%nFreeBodies) = Nx+12 + m%BodyStateIsN(p%nFreeBodies) = Nx+12 Nx = Nx + 12 ! add 12 state variables for free Body m%FreeBodyIs(p%nFreeBodies) = l @@ -631,23 +631,23 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !----------- process rod type ----------------- - + call DecomposeString(tempString1, let1, num1, let2, num2, let3) - - if ((let1 == "ANCHOR") .or. (let1 == "FIXED") .or. (let1 == "FIX")) then - m%RodList(l)%typeNum = 2 + + if ((let1 == "ANCHOR") .or. (let1 == "FIXED") .or. (let1 == "FIX")) then + m%RodList(l)%typeNum = 2 CALL Body_AddRod(m%GroundBody, l, tempArray) ! add rod l to Ground body - - else if ((let1 == "PINNED") .or. (let1 == "PIN")) then - m%RodList(l)%typeNum = 1 + + else if ((let1 == "PINNED") .or. (let1 == "PIN")) then + m%RodList(l)%typeNum = 1 CALL Body_AddRod(m%GroundBody, l, tempArray) ! add rod l to Ground body p%nFreeRods=p%nFreeRods+1 ! add this pinned rod to the free list because it is half free m%RodStateIs1(p%nFreeRods) = Nx+1 - m%RodStateIsN(p%nFreeRods) = Nx+6 - Nx = Nx + 6 ! add 6 state variables for each pinned rod + m%RodStateIsN(p%nFreeRods) = Nx+6 + Nx = Nx + 6 ! add 6 state variables for each pinned rod m%FreeRodIs(p%nFreeRods) = l @@ -667,8 +667,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er p%nFreeRods=p%nFreeRods+1 ! add this pinned rod to the free list because it is half free m%RodStateIs1(p%nFreeRods) = Nx+1 - m%RodStateIsN(p%nFreeRods) = Nx+6 - Nx = Nx + 6 ! add 6 state variables for each pinned rod + m%RodStateIsN(p%nFreeRods) = Nx+6 + Nx = Nx + 6 ! add 6 state variables for each pinned rod m%FreeRodIs(p%nFreeRods) = l @@ -689,7 +689,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if ((let1 == "VESSEL") .or. (let1 == "VES") .or. (let1 == "COUPLED") .or. (let1 == "CPLD")) then ! if a rigidly coupled rod, add to list and add m%RodList(l)%typeNum = -2 - p%nCpldRods=p%nCpldRods+1 ! add this rod to coupled list + p%nCpldRods=p%nCpldRods+1 ! add this rod to coupled list m%CpldRodIs(p%nCpldRods) = l @@ -700,8 +700,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er p%nFreeRods=p%nFreeRods+1 ! add this pinned rod to the free list because it is half free m%RodStateIs1(p%nFreeRods) = Nx+1 - m%RodStateIsN(p%nFreeRods) = Nx+6 - Nx = Nx + 6 ! add 6 state variables for each pinned rod + m%RodStateIsN(p%nFreeRods) = Nx+6 + Nx = Nx + 6 ! add 6 state variables for each pinned rod m%CpldRodIs(p%nCpldRods) = l m%FreeRodIs(p%nFreeRods) = l @@ -712,7 +712,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er p%nFreeRods=p%nFreeRods+1 ! add this pinned rod to the free list because it is half free m%RodStateIs1(p%nFreeRods) = Nx+1 - m%RodStateIsN(p%nFreeRods) = Nx+12 + m%RodStateIsN(p%nFreeRods) = Nx+12 Nx = Nx + 12 ! add 12 state variables for free Rod m%FreeRodIs(p%nFreeRods) = l @@ -826,17 +826,17 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !----------- process connection type ----------------- - + call DecomposeString(tempString1, let1, num1, let2, num2, let3) - - if ((let1 == "ANCHOR") .or. (let1 == "FIXED") .or. (let1 == "FIX")) then - m%ConnectList(l)%typeNum = 1 + + if ((let1 == "ANCHOR") .or. (let1 == "FIXED") .or. (let1 == "FIX")) then + m%ConnectList(l)%typeNum = 1 m%ConnectList(l)%r = tempArray(1:3) ! set initial node position CALL Body_AddConnect(m%GroundBody, l, tempArray(1:3)) ! add connection l to Ground body - - else if (let1 == "BODY") then ! attached to a body + + else if (let1 == "BODY") then ! attached to a body if (len_trim(num1) > 0) then READ(num1, *) J ! convert to int, representing parent body index @@ -878,7 +878,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er p%nFreeCons=p%nFreeCons+1 ! add this pinned rod to the free list because it is half free m%ConStateIs1(p%nFreeCons) = Nx+1 - m%ConStateIsN(p%nFreeCons) = Nx+6 + m%ConStateIsN(p%nFreeCons) = Nx+6 Nx = Nx + 6 ! add 12 state variables for free Connection m%FreeConIs(p%nFreeCons) = l @@ -996,7 +996,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er end if else CALL SetErrStat( ErrID_Severe, "Error: rod connection ID out of bounds for line "//trim(Num2LStr(l))//" end A attachment.", ErrStat, ErrMsg, RoutineName ) - return + return end if ! if J starts with a "C" or "Con" or goes straight ot the number then it's attached to a Connection @@ -1006,7 +1006,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL Connect_AddLine(m%ConnectList(J), l, 0) ! add line l (end A, denoted by 0) to connection J else CALL SetErrStat( ErrID_Severe, "Error: connection out of bounds for line "//trim(Num2LStr(l))//" end A attachment.", ErrStat, ErrMsg, RoutineName ) - return + return end if end if @@ -1037,7 +1037,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er end if else CALL SetErrStat( ErrID_Severe, "Error: rod connection ID out of bounds for line "//trim(Num2LStr(l))//" end B attachment.", ErrStat, ErrMsg, RoutineName ) - return + return end if ! if J starts with a "C" or "Con" or goes straight ot the number then it's attached to a Connection @@ -1047,7 +1047,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL Connect_AddLine(m%ConnectList(J), l, 1) ! add line l (end B, denoted by 1) to connection J else CALL SetErrStat( ErrID_Severe, "Error: connection out of bounds for line "//trim(Num2LStr(l))//" end B attachment.", ErrStat, ErrMsg, RoutineName ) - return + return end if end if @@ -1278,7 +1278,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else ! otherwise ignore this line that isn't a recognized header line and read the next line read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 end if - + !------------------------------------------------------------------------------------------- else ! otherwise ignore this line, which doesn't have the "---" or header line and read the next line @@ -1866,9 +1866,9 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !TODO: apply any initial adjustment of line length from active tensioning <<<<<<<<<<<< ! >>> maybe this should be skipped <<<< - + - ! Go through Bodys and write the coordinates to the state vector + ! Go through Bodys and write the coordinates to the state vector DO l = 1,p%nFreeBodies CALL Body_Initialize(m%BodyList(m%FreeBodyIs(l)), x%states(m%BodyStateIs1(l) : m%BodyStateIsN(l)), m) END DO @@ -2594,7 +2594,7 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er a6_in(4:6) = u%CoupledKinematics%RotationAcc(:,J) CALL Rod_SetKinematics(m%RodList(m%CpldRodIs(l)), r6_in, v6_in, a6_in, t, m) - + END DO ! any coupled points (type -1) @@ -4299,7 +4299,7 @@ SUBROUTINE Line_SetEndOrientation(Line, qin, topOfLine, rodEndB) REAL(DbKi), INTENT(IN ) :: qin(3) ! the rod's axis unit vector INTEGER(IntKi), INTENT(IN ) :: topOfLine ! 0 for end A (Node 0), 1 for end B (node N) INTEGER(IntKi), INTENT(IN ) :: rodEndB ! =0 means the line is attached to Rod end A, =1 means attached to Rod end B (implication for unit vector sign) - + if (topOfLine==1) then Line%endTypeB = 1 ! indicate attached to Rod (at every time step, just in case line get detached) @@ -4344,7 +4344,7 @@ SUBROUTINE Connect_Initialize(Connect, states, m) if (Connect%typeNum == 0) then ! error check - + ! pass kinematics to any attached lines so they have initial positions at this initialization stage DO l=1,Connect%nAttached IF (wordy > 1) print *, "Connect ", Connect%IdNum, " setting end kinematics of line ", Connect%attached(l), " to ", Connect%r @@ -4591,7 +4591,7 @@ SUBROUTINE Connect_GetCoupledForce(Connect, Fnet_out, m, p) IF (Connect%typeNum == -1) then ! calculate forces and masses of connect CALL Connect_DoRHS(Connect, m, p) - + ! add inertial loads as appropriate F_iner = -MATMUL(Connect%M, Connect%a) ! inertial loads Fnet_out = Connect%Fnet + F_iner ! add inertial loads @@ -4618,15 +4618,15 @@ SUBROUTINE Connect_GetNetForceAndMass(Connect, rRef, Fnet_out, M_out, m, p) CALL Connect_DoRHS(Connect, m, p) - + rRel = Connect%r - rRef ! vector from body reference point to node - + ! convert net force into 6dof force about body ref point CALL translateForce3to6DOF(rRel, Connect%Fnet, Fnet_out) ! convert mass matrix to 6by6 mass matrix about body ref point CALL translateMass3to6DOF(rRel, Connect%M, M_out) - + END SUBROUTINE Connect_GetNetForceAndMass @@ -4770,27 +4770,27 @@ SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) ! ------------------------- set some geometric properties and the starting kinematics ------------------------- - - CALL UnitVector(endCoords(1:3), endCoords(4:6), Rod%q, Rod%UnstrLen) ! get Rod axis direction vector and Rod length - - ! set Rod positions if applicable - if (Rod%typeNum==0) then ! for an independent rod, set the position right off the bat - + + CALL UnitVector(endCoords(1:3), endCoords(4:6), Rod%q, Rod%UnstrLen) ! get Rod axis direction vector and Rod length + + ! set Rod positions if applicable + if (Rod%typeNum==0) then ! for an independent rod, set the position right off the bat + Rod%r6(1:3) = endCoords(1:3) ! (end A coordinates) Rod%v6(1:3) = 0.0_DbKi ! (end A velocity, unrotated axes) Rod%r6(4:6) = Rod%q ! (Rod direction unit vector) Rod%v6(4:6) = 0.0_DbKi ! (rotational velocities about unrotated axes) - - - else if (abs(Rod%typeNum)==1) then ! for a pinned rod, just set the orientation (position will be set later by parent object) - + + + else if (abs(Rod%typeNum)==1) then ! for a pinned rod, just set the orientation (position will be set later by parent object) + Rod%r6(4:6) = Rod%q ! (Rod direction unit vector) Rod%v6(4:6) = 0.0_DbKi ! (rotational velocities about unrotated axes) - end if - ! otherwise (for a fixed rod) the positions will be set by the parent body or via coupling - + end if + ! otherwise (for a fixed rod) the positions will be set by the parent body or via coupling + ! save mass for future calculations >>>> should calculate I_l and I_r here in future <<<< @@ -4917,13 +4917,13 @@ SUBROUTINE Rod_SetKinematics(Rod, r6_in, v6_in, a6_in, t, m) else print *, "Error: Rod_SetKinematics called for a free Rod in MoorDyn." ! <<< end if - + ! update Rod direction unit vector (simply equal to last three entries of r6, presumably these were set elsewhere for pinned Rods) - Rod%q = Rod%r6(4:6) + Rod%q = Rod%r6(4:6) - + END SUBROUTINE Rod_SetKinematics !-------------------------------------------------------------- @@ -5097,7 +5097,7 @@ SUBROUTINE Rod_GetStateDeriv(Rod, Xd, m, p) ! TODO: add "controller" adjusting state derivatives of X(10:12) to artificially force X(10:12) to remain a unit vector <<<<<<<<<<< - ! fill in state derivatives + ! fill in state derivatives IF (Rod%typeNum == 0) THEN ! free Rod type, 12 states ! solve for accelerations in [M]{a}={f} using LU decomposition @@ -5105,7 +5105,7 @@ SUBROUTINE Rod_GetStateDeriv(Rod, Xd, m, p) Xd(7:9) = Rod%v6(1:3) !Xd[6 + I] = v6[ I]; ! dxdt = V (velocities) Xd(1:6) = acc !Xd[ I] = acc[ I]; ! dVdt = a (accelerations) - !Xd[3 + I] = acc[3+I]; ! rotational accelerations + !Xd[3 + I] = acc[3+I]; ! rotational accelerations ! rate of change of unit vector components!! CHECK! <<<<< Xd(10) = - Rod%v6(6)*Rod%r6(5) + Rod%v6(5)*Rod%r6(6) ! i.e. u_dot_x = -omega_z*u_y + omega_y*u_z @@ -5136,7 +5136,7 @@ SUBROUTINE Rod_GetStateDeriv(Rod, Xd, m, p) ! store angular accelerations in case they're useful as output Rod%a6(4:6) = acc(4:6) - END IF + END IF ! Note: accelerations that are dependent on parent objects) will not be known to this object ! (only those of free DOFs are coupled DOFs are known in this approach). @@ -5225,7 +5225,7 @@ SUBROUTINE Rod_GetNetForceAndMass(Rod, rRef, Fnet_out, M_out, m, p) rRel = Rod%r(:,0) - rRef ! vector from reference point to end A - CALL translateForce3to6DOF(rRel, Rod%F6net(1:3), Fnet_out) ! shift net forces + CALL translateForce3to6DOF(rRel, Rod%F6net(1:3), Fnet_out) ! shift net forces Fnet_out(4:6) = Fnet_out(4:6) + Rod%F6net(4:6) ! add in the existing moments CALL translateMass6to6DOF(rRel, Rod%M6net, M_out) ! shift mass matrix to be about ref point @@ -5234,7 +5234,7 @@ SUBROUTINE Rod_GetNetForceAndMass(Rod, rRef, Fnet_out, M_out, m, p) !if (abs(Rod%typeNum)==1) then ! Fnet_out(4:6) = 0.0_DbKi !end if - + END SUBROUTINE Rod_GetNetForceAndMass !-------------------------------------------------------------- @@ -5442,7 +5442,7 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ap = Rod%Ud(:,I) - aq ! normal component of fluid acceleration ! transverse Froude-Krylov force Rod%Ap(:,I) = VOF * p%rhoW*(1.0+Rod%Can)* v_i * ap ! - ! axial Froude-Krylov force + ! axial Froude-Krylov force Rod%Aq(:,I) = 0.0_DbKi ! p%rhoW*(1.0+Rod%Cat)* v_i * aq ! <<< just put a taper-based term here eventually? ! dynamic pressure @@ -5555,10 +5555,10 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) Rod%Fnet(:,I) = Rod%W(:,I) + Rod%Bo(:,I) + Rod%Dp(:,I) + Rod%Dq(:,I) & + Rod%Ap(:,I) + Rod%Aq(:,I) + Rod%Pd(:,I) + Rod%B(:,I) - + END DO ! I - done looping through nodes - + ! ----- add waterplane moment of inertia moment if applicable ----- IF ((Rod%r(3,0) < zeta) .and. (Rod%r(3,N) > zeta)) then ! check if it's crossing the water plane Mtemp = 1.0/16.0 *Pi*Rod%d**4 * p%rhoW*p%g * sinPhi * (1.0 + 0.5* tanPhi**2) @@ -5592,7 +5592,7 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) END DO ! ---------------- now lump everything in 6DOF about end A ----------------------------- - + ! question: do I really want to neglect the rotational inertia/drag/etc across the length of each segment? ! make sure 6DOF quantiaties are zeroed before adding them up @@ -5605,7 +5605,7 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) rRel = Rod%r(:,i) - Rod%r(:,0) ! vector from reference point to node ! convert segment net force into 6dof force about body ref point (if the Rod itself, end A) - CALL translateForce3to6DOF(rRel, Rod%Fnet(:,i), F6_i) + CALL translateForce3to6DOF(rRel, Rod%Fnet(:,i), F6_i) ! convert segment mass matrix to 6by6 mass matrix about body ref point (if the Rod itself, end A) CALL translateMass3to6DOF(rRel, Rod%M(:,:,i), M6_i) @@ -5905,7 +5905,7 @@ SUBROUTINE Body_Initialize(Body, states, m) ! assign initial body kinematics to state vector states(7:12) = Body%r6 states(1:6 ) = Body%v6 - + ! set positions of any dependent connections and rods now (before they are initialized) CALL Body_SetDependentKin(Body, 0.0_DbKi, m) @@ -6010,11 +6010,11 @@ SUBROUTINE Body_SetKinematics(Body, r_in, v_in, a_in, t, m) ! else ! print *, "Error: Body_SetKinematics called for a free Body." ! <<< ! end if - + END SUBROUTINE Body_SetKinematics !-------------------------------------------------------------- - + ! set the states (positions and velocities) of any connects or rods that are part of this body ! also computes the orientation matrix (never skip this sub!) !-------------------------------------------------------------- @@ -6057,7 +6057,7 @@ SUBROUTINE Body_SetDependentKin(Body, t, m) CALL TransformKinematicsA( Body%r6RodRel(1:3,l), Body%r6(1:3), Body%OrMat, Body%v6, Body%a6, rRod(1:3), vRod(1:3), aRod(1:3)) ! set first three entires (end A translation) of rRod and rdRod ! does the above function need to take in all 6 elements of r6RodRel?? - ! do rotational stuff + ! do rotational stuff rRod(4:6) = MATMUL(Body%OrMat, Body%r6RodRel(4:6,l)) !<<<<<< correct? <<<<< rotateVector3(r6RodRel[i]+3, OrMat, rRod+3); ! rotate rod relative unit vector by OrMat to get unit vec in reference coords vRod(4:6) = Body%v6(4:6) ! transformed rotational velocity. <<< is this okay as is? <<<< aRod(4:6) = Body%a6(4:6) @@ -6119,7 +6119,7 @@ SUBROUTINE Body_GetStateDeriv(Body, Xd, m, p) ! solve for accelerations in [M]{a}={f} using LU decomposition CALL LUsolve(6, Body%M, LU_temp, Body%F6net, y_temp, acc) - ! fill in state derivatives + ! fill in state derivatives Xd(7:12) = Body%v6 ! dxdt = V (velocities) Xd(1:6) = acc ! dVdt = a (accelerations) @@ -6163,32 +6163,32 @@ SUBROUTINE Body_DoRHS(Body, m, p) Real(DbKi) :: M6_i(6,6) ! mass and inertia from an attached object ! First, the body's own mass matrix must be adjusted based on its orientation so that - ! we have a mass matrix in the global orientation frame + ! we have a mass matrix in the global orientation frame Body%M = RotateM6(Body%M0, Body%OrMat) - + !gravity on core body Fgrav(1) = 0.0_DbKi Fgrav(2) = 0.0_DbKi Fgrav(3) = Body%bodyV * p%rhow * p%g - Body%bodyM * p%g ! weight+buoyancy vector - + body_rCGrotated = MATMUL(Body%OrMat, Body%rCG) ! rotateVector3(body_rCG, OrMat, body_rCGrotated); ! relative vector to body CG in inertial orientation CALL translateForce3to6DOF(body_rCGrotated, Fgrav, Body%F6net) ! gravity forces and moments about body ref point given CG location - - + + ! --------------------------------- apply wave kinematics ------------------------------------ !env->waves->getU(r6, t, U); ! call generic function to get water velocities <<<<<<<<< all needs updating - - ! for (int J=0; J<3; J++) - ! Ud[J] = 0.0; ! set water accelerations as zero for now + + ! for (int J=0; J<3; J++) + ! Ud[J] = 0.0; ! set water accelerations as zero for now ! ------------------------------------------------------------------------------------------ ! viscous drag calculation (on core body) vi(1:3) = U - Body%v6(1:3) ! relative flow velocity over body ref point vi(4:6) = - Body%v6(4:6) ! for rotation, this is just the negative of the body's rotation for now (not allowing flow rotation) - + Body%F6net = Body%F6net + 0.5*p%rhoW * vi * abs(vi) * Body%bodyCdA ! <<< NOTE, for body this should be fixed to account for orientation!! <<< what about drag in rotational DOFs??? <<<<<<<<<<<<<< - + ! Get contributions from any dependent connections @@ -6200,7 +6200,7 @@ SUBROUTINE Body_DoRHS(Body, m, p) ! sum quantitites Body%F6net = Body%F6net + F6_i Body%M = Body%M + M6_i - end do + end do ! Get contributions from any dependent Rods do l=1,Body%nAttachedR @@ -6212,7 +6212,7 @@ SUBROUTINE Body_DoRHS(Body, m, p) Body%F6net = Body%F6net + F6_i Body%M = Body%M + M6_i end do - + END SUBROUTINE Body_DoRHS !===================================================================== @@ -6346,14 +6346,14 @@ SUBROUTINE getInterpNums(xlist, xin, istart, i, fout) IF (xlist(istart) < xin) i1 = istart ! if istart is below the actual value, start with it instead of starting at 1 to save time DO i = i1, nx-1 - IF (xlist(i+1) > xin) THEN + IF (xlist(i+1) > xin) THEN fout = (xin - xlist(i) )/( xlist(i+1) - xlist(i) ) exit END IF - END DO + END DO END IF - END SUBROUTINE + END SUBROUTINE SUBROUTINE calculate4Dinterpolation(f, ix0, iy0, iz0, it0, fx, fy, fz, ft, c) @@ -6503,13 +6503,13 @@ SUBROUTINE ScaleVector( u_in, newlength, u_out ) REAL(DbKi) :: scaler INTEGER(IntKi) :: J - length_squared = 0.0; + length_squared = 0.0; DO J=1,3 - length_squared = length_squared + u_in(J)*u_in(J) + length_squared = length_squared + u_in(J)*u_in(J) END DO if (length_squared > 0) then - scaler = newlength/sqrt(length_squared) + scaler = newlength/sqrt(length_squared) else ! if original vector is zero, return zero scaler = 0_DbKi end if @@ -6674,14 +6674,14 @@ SUBROUTINE TransformKinematicsAtoB(rA, u, L, rd_in, r_out, rd_out) ! locations (unrotated reference frame) rRel = L*u ! relative location of point B from point A - r_out = rRel + rA ! absolute location of point B + r_out = rRel + rA ! absolute location of point B ! absolute velocities rd_out(1) = - rd_in(6)*rRel(2) + rd_in(5)*rRel(3) + rd_in(1) ! x rd_out(2) = rd_in(6)*rRel(1) - rd_in(4)*rRel(3) + rd_in(2) ! y rd_out(3) = -rd_in(5)*rRel(1) + rd_in(4)*rRel(2) + rd_in(3) ! z - + END SUBROUTINE TransformKinematicsAtoB !----------------------------------------------------------------------- @@ -6695,7 +6695,7 @@ SUBROUTINE TranslateForce3to6DOF(dx, F, Fout) Fout(1:3) = F Fout(4:6) = CROSS_PRODUCT(dx, F) - + END SUBROUTINE TranslateForce3to6DOF !----------------------------------------------------------------------- @@ -6717,17 +6717,17 @@ SUBROUTINE TranslateMass3to6DOF(dx, Min, Mout) ! | J^T I | H = getH(dx); - + ! mass matrix [m'] = [m] Mout(1:3,1:3) = Min - + ! product of inertia matrix [J'] = [m][H] + [J] Mout(1:3,4:6) = MATMUL(Min, H) Mout(4:6,1:3) = TRANSPOSE(Mout(1:3,4:6)) - + !moment of inertia matrix [I'] = [H][m][H]^T + [J]^T [H] + [H]^T [J] + [I] Mout(4:6,4:6) = MATMUL(MATMUL(H, Min), TRANSPOSE(H)) - + END SUBROUTINE TranslateMass3to6DOF !----------------------------------------------------------------------- @@ -6741,17 +6741,17 @@ SUBROUTINE TranslateMass6to6DOF(dx, Min, Mout) REAL(DbKi) :: H( 3,3) ! "anti-symmetric tensor components" from Sadeghi and Incecik H = getH(dx); - + ! mass matrix [m'] = [m] Mout(1:3,1:3) = Min(1:3,1:3) - + ! product of inertia matrix [J'] = [m][H] + [J] Mout(1:3,4:6) = MATMUL(Min(1:3,1:3), H) + Min(1:3,4:6) Mout(4:6,1:3) = TRANSPOSE(Mout(1:3,4:6)) - + !moment of inertia matrix [I'] = [H][m][H]^T + [J]^T [H] + [H]^T [J] + [I] Mout(4:6,4:6) = MATMUL(MATMUL(H, Min(1:3,1:3)), TRANSPOSE(H)) + MATMUL(Min(4:6,1:3),H) + MATMUL(TRANSPOSE(H),Min(1:3,4:6)) + Min(4:6,4:6) - + END SUBROUTINE TranslateMass6to6DOF !----------------------------------------------------------------------- @@ -6771,7 +6771,7 @@ FUNCTION GetH(r) GetH(1,1) = 0.0_DbKi GetH(2,2) = 0.0_DbKi GetH(3,3) = 0.0_DbKi - + END FUNCTION GetH !----------------------------------------------------------------------- @@ -6792,12 +6792,12 @@ FUNCTION RotateM6(Min, rotMat) result(outMat) ! 1. copy out the relevant 3x3 matrix section, ! 2. rotate it, and ! 3. paste it into the output 6x6 matrix - + ! mass matrix outMat(1:3,1:3) = rotateM3(Min(1:3,1:3), rotMat) - ! product of inertia matrix + ! product of inertia matrix outMat(1:3,4:6) = rotateM3(Min(1:3,4:6), rotMat) outMat(4:6,1:3) = TRANSPOSE(outMat(1:3,4:6)) @@ -6816,9 +6816,9 @@ FUNCTION RotateM3(Min, rotMat) result(outMat) Real(DbKi) :: outMat(3,3) ! rotated matrix ! overall operation is [m'] = [a]*[m]*[a]^T - + outMat = MATMUL( MATMUL(rotMat, Min), TRANSPOSE(rotMat) ) - + END FUNCTION RotateM3 @@ -6908,7 +6908,7 @@ SUBROUTINE LUsolve(n, A, LU, b, y, x) INTEGER(intKi) :: i,j,k,p Real(DbKi) :: sum - + DO k = 1,n DO i = k,n From b104356b2120a5d2c3bbac22b47693b4764a5d97 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Wed, 7 Apr 2021 13:09:30 -0600 Subject: [PATCH 021/242] Replace 1.0/0.0 with NaN -- gcc wouldn't compile --- modules/moordyn/src/MoorDyn.f90 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 9a39072184..f89eb15b2c 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -6539,7 +6539,7 @@ subroutine GetOrientationAngles(p1, p2, phi, sinPhi, cosPhi, tanPhi, beta, sinBe print *, "ERROR in GetOrientationAngles in MoorDyn" !call SeterrStat(ErrID_Fatal, 'An element of the Morison structure has co-located endpoints! This should never occur. Please review your model.', errStat, errMsg, 'Morison_CalcOutput' ) print *, p1 print *, p2 - k_hat = 1.0/0.0 + k_hat = NaN ! 1.0/0.0 else k_hat = vec / vecLen phi = atan2(vecLen2D, vec(3)) ! incline angle From 1f285c1224f79bda2d0f6d7933cf30dd446407d0 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Thu, 8 Apr 2021 09:57:59 -0600 Subject: [PATCH 022/242] MDv2: change InitInp to intent in only This is to match the framework. Moved the 4 variables into a simple derived type that gets removed at the end of the Init routine. --- modules/moordyn/src/MoorDyn.f90 | 46 +++--- modules/moordyn/src/MoorDyn_Registry.txt | 9 +- modules/moordyn/src/MoorDyn_Types.f90 | 189 +++++++++++++++++++---- 3 files changed, 189 insertions(+), 55 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index f89eb15b2c..8840de35aa 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -46,7 +46,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IMPLICIT NONE - TYPE(MD_InitInputType), INTENT(INOUT) :: InitInp ! INTENT(INOUT) : Input data for initialization routine + TYPE(MD_InitInputType), INTENT(IN ) :: InitInp ! INTENT(INOUT) : Input data for initialization routine TYPE(MD_InputType), INTENT( OUT) :: u ! INTENT( OUT) : An initial guess for the input; input mesh must be defined TYPE(MD_ParameterType), INTENT( OUT) :: p ! INTENT( OUT) : Parameters TYPE(MD_ContinuousStateType), INTENT( OUT) :: x ! INTENT( OUT) : Initial continuous states @@ -61,6 +61,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None ! local variables + TYPE(MD_InputFileType) :: InputFileDat ! Data read from input file for setup, but not stored after Init REAL(DbKi) :: t ! instantaneous time, to be used during IC generation INTEGER(IntKi) :: l ! index INTEGER(IntKi) :: I ! index @@ -1205,13 +1206,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if ( OptString == 'CBOT') then read (OptValue,*) p%cBot else if ( OptString == 'DTIC') then - read (OptValue,*) InitInp%dtIC + read (OptValue,*) InputFileDat%dtIC else if ( OptString == 'TMAXIC') then - read (OptValue,*) InitInp%TMaxIC + read (OptValue,*) InputFileDat%TMaxIC else if ( OptString == 'CDSCALEIC') then - read (OptValue,*) InitInp%CdScaleIC + read (OptValue,*) InputFileDat%CdScaleIC else if ( OptString == 'THRESHIC') then - read (OptValue,*) InitInp%threshIC + read (OptValue,*) InputFileDat%threshIC else if ( OptString == 'WATERKIN') then read (OptValue,*) p%WaterKin else @@ -1948,14 +1949,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! -------------------------------------------------------------------- ! only do this if TMaxIC > 0 - if (InitInp%TMaxIC > 0.0_DbKi) then + if (InputFileDat%TMaxIC > 0.0_DbKi) then CALL WrScr(" Finalizing initial conditions using dynamic relaxation."//NewLine) ! newline because next line writes over itself ! boost drag coefficient of each line type <<<<<<<< does this actually do anything or do lines hold these coefficients??? DO I = 1, p%nLineTypes - m%LineTypeList(I)%Cdn = m%LineTypeList(I)%Cdn * InitInp%CdScaleIC - m%LineTypeList(I)%Cdt = m%LineTypeList(I)%Cdt * InitInp%CdScaleIC ! <<<<< need to update this to apply to all objects' drag + m%LineTypeList(I)%Cdn = m%LineTypeList(I)%Cdn * InputFileDat%CdScaleIC + m%LineTypeList(I)%Cdt = m%LineTypeList(I)%Cdt * InputFileDat%CdScaleIC ! <<<<< need to update this to apply to all objects' drag END DO ! allocate array holding 10 latest fairlead tensions @@ -1974,8 +1975,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! round dt to integer number of time steps - NdtM = ceiling(InitInp%DTIC/p%dtM0) ! get number of mooring time steps to do based on desired time step size - dtM = InitInp%DTIC/real(NdtM, DbKi) ! adjust desired time step to satisfy dt with an integer number of time steps + NdtM = ceiling(InputFileDat%dtIC/p%dtM0) ! get number of mooring time steps to do based on desired time step size + dtM = InputFileDat%dtIC/real(NdtM, DbKi) ! adjust desired time step to satisfy dt with an integer number of time steps t = 0.0_DbKi ! start time at zero @@ -1984,7 +1985,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er call MD_CopyInput( u, u_interp, MESH_NEWCOPY, ErrStat2, ErrMsg2 ) ! also make an inputs object to interpExtrap to t_array(1) = t ! fill in the times "array" for u_array - DO I = 1, ceiling(InitInp%TMaxIC/InitInp%DTIC) ! loop through IC gen time steps, up to maximum + DO I = 1, ceiling(InputFileDat%TMaxIC/InputFileDat%dtIC) ! loop through IC gen time steps, up to maximum !loop through line integration time steps @@ -2013,7 +2014,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er END DO ! J time steps ! ! integrate the EOMs one DTIC s time step - ! CALL TimeStep ( t, InitInp%DTIC, u_array, t_array, p, x, xd, z, other, m, ErrStat, ErrMsg ) + ! CALL TimeStep ( t, InputFileDat%dtIC, u_array, t_array, p, x, xd, z, other, m, ErrStat, ErrMsg ) ! CALL CheckError( ErrStat2, ErrMsg2 ) ! IF (ErrStat >= AbortErrLev) RETURN @@ -2044,7 +2045,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er DO l = 1, p%nLines DO K = 1,9 - IF ( abs( FairTensIC(l,K)/FairTensIC(l,K+1) - 1.0 ) > InitInp%threshIC ) THEN + IF ( abs( FairTensIC(l,K)/FairTensIC(l,K+1) - 1.0 ) > InputFileDat%threshIC ) THEN Converged = 0 EXIT END IF @@ -2054,13 +2055,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er END DO IF (Converged == 1) THEN ! if we made it with all cases satisfying the threshold - CALL WrScr(' Fairlead tensions converged to '//trim(Num2LStr(100.0*InitInp%threshIC))//'% after '//trim(Num2LStr(t))//' seconds.') + CALL WrScr(' Fairlead tensions converged to '//trim(Num2LStr(100.0*InputFileDat%threshIC))//'% after '//trim(Num2LStr(t))//' seconds.') EXIT ! break out of the time stepping loop END IF END IF - IF (I == ceiling(InitInp%TMaxIC/InitInp%DTIC) ) THEN - CALL WrScr(' Fairlead tensions did not converge within TMaxIC='//trim(Num2LStr(InitInp%TMaxIC))//' seconds.') + IF (I == ceiling(InputFileDat%TMaxIC/InputFileDat%dtIC) ) THEN + CALL WrScr(' Fairlead tensions did not converge within TMaxIC='//trim(Num2LStr(InputFileDat%TMaxIC))//' seconds.') !ErrStat = ErrID_Warn !ErrMsg = ' MD_Init: ran dynamic convergence to TMaxIC without convergence' END IF @@ -2073,11 +2074,11 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! UNboost drag coefficient of each line type <<< DO I = 1, p%nLineTypes - m%LineTypeList(I)%Cdn = m%LineTypeList(I)%Cdn / InitInp%CdScaleIC - m%LineTypeList(I)%Cdt = m%LineTypeList(I)%Cdt / InitInp%CdScaleIC + m%LineTypeList(I)%Cdn = m%LineTypeList(I)%Cdn / InputFileDat%CdScaleIC + m%LineTypeList(I)%Cdt = m%LineTypeList(I)%Cdt / InputFileDat%CdScaleIC END DO - end if ! InitInp%TMaxIC > 0 + end if ! InputFileDat%TMaxIC > 0 p%dtCoupling = DTcoupling ! store coupling time step for use in updatestates @@ -2140,7 +2141,9 @@ SUBROUTINE CheckError(ErrID,Msg) IF (ALLOCATED(m%ConStateIs1 )) DEALLOCATE(m%ConStateIs1 ) IF (ALLOCATED(m%ConStateIsN )) DEALLOCATE(m%ConStateIsN ) IF (ALLOCATED(x%states )) DEALLOCATE(x%states ) - IF (ALLOCATED(FairTensIC )) DEALLOCATE(FairTensIC ) + IF (ALLOCATED(FairTensIC )) DEALLOCATE(FairTensIC ) + + call CleanUp() ! make sure to close files END IF END IF @@ -2148,7 +2151,8 @@ END SUBROUTINE CheckError SUBROUTINE CleanUp() ! ErrStat = ErrID_Fatal - CLOSE( UnIn ) + call MD_DestroyInputFileType( InputFileDat, ErrStat2, ErrMsg2 ) ! Ignore any error messages from this + if(UnIn > 0) CLOSE( UnIn ) ! Only if opened ! IF (InitInp%Echo) CLOSE( UnEc ) END SUBROUTINE diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index fa9f78e310..9ee0b39970 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -13,6 +13,11 @@ include Registry_NWTC_Library.txt +## ====== some data read from the input file, but not needed after init ====== +typedef MoorDyn/MD MD_InputFileType DbKi DTIC - - - "convergence check time step for IC generation" "[s]" +typedef ^ ^ DbKi TMaxIC - 120 - "maximum time to allow for getting converged ICs" "[s]" +typedef ^ ^ ReKi CdScaleIC - 1 - "factor to scale drag coefficients by during dynamic relaxation" "[]" +typedef ^ ^ ReKi threshIC - 0.01 - "convergence tolerance for ICs (0.01 means 1%)" "[]" ## ============================== Define input types here: ============================================================================================================================================ typedef MoorDyn/MD InitInputType ReKi g - -999.9 - "gravity constant" "[m/s^2]" @@ -23,10 +28,6 @@ typedef ^ ^ ReKi PtfmInit {:} - typedef ^ ^ CHARACTER(1024) FileName - "" - "MoorDyn input file" typedef ^ ^ CHARACTER(1024) RootName - - - "RootName for writing output files" - typedef ^ ^ LOGICAL Echo - "" - "echo parameter - do we want to echo the header line describing the input file?" -typedef ^ ^ DbKi DTIC - - - "convergence check time step for IC generation" "[s]" -typedef ^ ^ DbKi TMaxIC - 120 - "maximum time to allow for getting converged ICs" "[s]" -typedef ^ ^ ReKi CdScaleIC - 1 - "factor to scale drag coefficients by during dynamic relaxation" "[]" -typedef ^ ^ ReKi threshIC - 0.01 - "convergence tolerance for ICs (0.01 means 1%)" "[]" #typedef ^ ^ CHARACTER(ChanLen) OutList {:} "" - "string containing list of output channels requested in input file" #typedef ^ ^ DbKi UGrid {:}{:}{:} - - "water velocities time series at each grid point" - diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index 04bee87059..91d408cf30 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -33,6 +33,14 @@ MODULE MoorDyn_Types !--------------------------------------------------------------------------------------------------------------------------------- USE NWTC_Library IMPLICIT NONE +! ========= MD_InputFileType ======= + TYPE, PUBLIC :: MD_InputFileType + REAL(DbKi) :: DTIC !< convergence check time step for IC generation [[s]] + REAL(DbKi) :: TMaxIC = 120 !< maximum time to allow for getting converged ICs [[s]] + REAL(ReKi) :: CdScaleIC = 1 !< factor to scale drag coefficients by during dynamic relaxation [[]] + REAL(ReKi) :: threshIC = 0.01 !< convergence tolerance for ICs (0.01 means 1%) [[]] + END TYPE MD_InputFileType +! ======================= ! ========= MD_InitInputType ======= TYPE, PUBLIC :: MD_InitInputType REAL(ReKi) :: g = -999.9 !< gravity constant [[m/s^2]] @@ -42,10 +50,6 @@ MODULE MoorDyn_Types CHARACTER(1024) :: FileName !< MoorDyn input file [-] CHARACTER(1024) :: RootName !< RootName for writing output files [-] LOGICAL :: Echo !< echo parameter - do we want to echo the header line describing the input file? [-] - REAL(DbKi) :: DTIC !< convergence check time step for IC generation [[s]] - REAL(DbKi) :: TMaxIC = 120 !< maximum time to allow for getting converged ICs [[s]] - REAL(ReKi) :: CdScaleIC = 1 !< factor to scale drag coefficients by during dynamic relaxation [[]] - REAL(ReKi) :: threshIC = 0.01 !< convergence tolerance for ICs (0.01 means 1%) [[]] REAL(ReKi) , DIMENSION(:,:,:), ALLOCATABLE :: WaveVel !< [-] REAL(ReKi) , DIMENSION(:,:,:), ALLOCATABLE :: WaveAcc !< [-] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: WavePDyn !< [-] @@ -398,6 +402,157 @@ MODULE MoorDyn_Types END TYPE MD_OutputType ! ======================= CONTAINS + SUBROUTINE MD_CopyInputFileType( SrcInputFileTypeData, DstInputFileTypeData, CtrlCode, ErrStat, ErrMsg ) + TYPE(MD_InputFileType), INTENT(IN) :: SrcInputFileTypeData + TYPE(MD_InputFileType), INTENT(INOUT) :: DstInputFileTypeData + INTEGER(IntKi), INTENT(IN ) :: CtrlCode + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg +! Local + INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: i4, i4_l, i4_u ! bounds (upper/lower) for an array dimension 4 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyInputFileType' +! + ErrStat = ErrID_None + ErrMsg = "" + DstInputFileTypeData%DTIC = SrcInputFileTypeData%DTIC + DstInputFileTypeData%TMaxIC = SrcInputFileTypeData%TMaxIC + DstInputFileTypeData%CdScaleIC = SrcInputFileTypeData%CdScaleIC + DstInputFileTypeData%threshIC = SrcInputFileTypeData%threshIC + END SUBROUTINE MD_CopyInputFileType + + SUBROUTINE MD_DestroyInputFileType( InputFileTypeData, ErrStat, ErrMsg ) + TYPE(MD_InputFileType), INTENT(INOUT) :: InputFileTypeData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + CHARACTER(*), PARAMETER :: RoutineName = 'MD_DestroyInputFileType' + INTEGER(IntKi) :: i, i1, i2, i3, i4, i5 +! + ErrStat = ErrID_None + ErrMsg = "" + END SUBROUTINE MD_DestroyInputFileType + + SUBROUTINE MD_PackInputFileType( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) + REAL(ReKi), ALLOCATABLE, INTENT( OUT) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT( OUT) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT( OUT) :: IntKiBuf(:) + TYPE(MD_InputFileType), INTENT(IN) :: InData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + LOGICAL,OPTIONAL, INTENT(IN ) :: SizeOnly + ! Local variables + INTEGER(IntKi) :: Re_BufSz + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_BufSz + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_BufSz + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i,i1,i2,i3,i4,i5 + LOGICAL :: OnlySize ! if present and true, do not pack, just allocate buffers + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_PackInputFileType' + ! buffers to store subtypes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + + OnlySize = .FALSE. + IF ( PRESENT(SizeOnly) ) THEN + OnlySize = SizeOnly + ENDIF + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_BufSz = 0 + Db_BufSz = 0 + Int_BufSz = 0 + Db_BufSz = Db_BufSz + 1 ! DTIC + Db_BufSz = Db_BufSz + 1 ! TMaxIC + Re_BufSz = Re_BufSz + 1 ! CdScaleIC + Re_BufSz = Re_BufSz + 1 ! threshIC + IF ( Re_BufSz .GT. 0 ) THEN + ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating ReKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Db_BufSz .GT. 0 ) THEN + ALLOCATE( DbKiBuf( Db_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DbKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF ( Int_BufSz .GT. 0 ) THEN + ALLOCATE( IntKiBuf( Int_BufSz ), STAT=ErrStat2 ) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating IntKiBuf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + IF(OnlySize) RETURN ! return early if only trying to allocate buffers (not pack them) + + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + + DbKiBuf(Db_Xferred) = InData%DTIC + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%TMaxIC + Db_Xferred = Db_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%CdScaleIC + Re_Xferred = Re_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%threshIC + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE MD_PackInputFileType + + SUBROUTINE MD_UnPackInputFileType( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) + REAL(ReKi), ALLOCATABLE, INTENT(IN ) :: ReKiBuf(:) + REAL(DbKi), ALLOCATABLE, INTENT(IN ) :: DbKiBuf(:) + INTEGER(IntKi), ALLOCATABLE, INTENT(IN ) :: IntKiBuf(:) + TYPE(MD_InputFileType), INTENT(INOUT) :: OutData + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + ! Local variables + INTEGER(IntKi) :: Buf_size + INTEGER(IntKi) :: Re_Xferred + INTEGER(IntKi) :: Db_Xferred + INTEGER(IntKi) :: Int_Xferred + INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 + INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 + INTEGER(IntKi) :: i4, i4_l, i4_u ! bounds (upper/lower) for an array dimension 4 + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackInputFileType' + ! buffers to store meshes, if any + REAL(ReKi), ALLOCATABLE :: Re_Buf(:) + REAL(DbKi), ALLOCATABLE :: Db_Buf(:) + INTEGER(IntKi), ALLOCATABLE :: Int_Buf(:) + ! + ErrStat = ErrID_None + ErrMsg = "" + Re_Xferred = 1 + Db_Xferred = 1 + Int_Xferred = 1 + OutData%DTIC = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%TMaxIC = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%CdScaleIC = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + OutData%threshIC = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END SUBROUTINE MD_UnPackInputFileType + SUBROUTINE MD_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, ErrStat, ErrMsg ) TYPE(MD_InitInputType), INTENT(IN) :: SrcInitInputData TYPE(MD_InitInputType), INTENT(INOUT) :: DstInitInputData @@ -409,7 +564,6 @@ SUBROUTINE MD_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, ErrSt INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 - INTEGER(IntKi) :: i4, i4_l, i4_u ! bounds (upper/lower) for an array dimension 4 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyInitInput' @@ -434,10 +588,6 @@ SUBROUTINE MD_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, ErrSt DstInitInputData%FileName = SrcInitInputData%FileName DstInitInputData%RootName = SrcInitInputData%RootName DstInitInputData%Echo = SrcInitInputData%Echo - DstInitInputData%DTIC = SrcInitInputData%DTIC - DstInitInputData%TMaxIC = SrcInitInputData%TMaxIC - DstInitInputData%CdScaleIC = SrcInitInputData%CdScaleIC - DstInitInputData%threshIC = SrcInitInputData%threshIC IF (ALLOCATED(SrcInitInputData%WaveVel)) THEN i1_l = LBOUND(SrcInitInputData%WaveVel,1) i1_u = UBOUND(SrcInitInputData%WaveVel,1) @@ -587,10 +737,6 @@ SUBROUTINE MD_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg Int_BufSz = Int_BufSz + 1*LEN(InData%FileName) ! FileName Int_BufSz = Int_BufSz + 1*LEN(InData%RootName) ! RootName Int_BufSz = Int_BufSz + 1 ! Echo - Db_BufSz = Db_BufSz + 1 ! DTIC - Db_BufSz = Db_BufSz + 1 ! TMaxIC - Re_BufSz = Re_BufSz + 1 ! CdScaleIC - Re_BufSz = Re_BufSz + 1 ! threshIC Int_BufSz = Int_BufSz + 1 ! WaveVel allocated yes/no IF ( ALLOCATED(InData%WaveVel) ) THEN Int_BufSz = Int_BufSz + 2*3 ! WaveVel upper/lower bounds for each dimension @@ -674,14 +820,6 @@ SUBROUTINE MD_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg END DO ! I IntKiBuf(Int_Xferred) = TRANSFER(InData%Echo, IntKiBuf(1)) Int_Xferred = Int_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%DTIC - Db_Xferred = Db_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%TMaxIC - Db_Xferred = Db_Xferred + 1 - ReKiBuf(Re_Xferred) = InData%CdScaleIC - Re_Xferred = Re_Xferred + 1 - ReKiBuf(Re_Xferred) = InData%threshIC - Re_Xferred = Re_Xferred + 1 IF ( .NOT. ALLOCATED(InData%WaveVel) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -805,7 +943,6 @@ SUBROUTINE MD_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Err INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 INTEGER(IntKi) :: i3, i3_l, i3_u ! bounds (upper/lower) for an array dimension 3 - INTEGER(IntKi) :: i4, i4_l, i4_u ! bounds (upper/lower) for an array dimension 4 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackInitInput' @@ -853,14 +990,6 @@ SUBROUTINE MD_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Err END DO ! I OutData%Echo = TRANSFER(IntKiBuf(Int_Xferred), OutData%Echo) Int_Xferred = Int_Xferred + 1 - OutData%DTIC = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 - OutData%TMaxIC = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 - OutData%CdScaleIC = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 - OutData%threshIC = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WaveVel not allocated Int_Xferred = Int_Xferred + 1 ELSE From 556ad48db842957ec0981fbb02c8d5a87584ac9f Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Thu, 8 Apr 2021 12:15:42 -0600 Subject: [PATCH 023/242] MDv2: fix some array bounds issues in interpolation routines --- modules/moordyn/src/MoorDyn.f90 | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 8840de35aa..b0b6044543 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -6332,9 +6332,11 @@ SUBROUTINE getInterpNums(xlist, xin, istart, i, fout) Integer(IntKi),INTENT ( OUT) :: i ! lower index to interpolate from Real(DbKi), INTENT ( OUT) :: fout ! fraction to return such that y* = y[i] + fout*(y[i+1]-y[i]) - Integer(IntKi) :: i1 = 1 ! the index we'll start at + Integer(IntKi) :: i1 Integer(IntKi) :: nx + i1 = 1 ! Setting in declaration causes an implied save, which would never allow this routine to find anything at the start of the array. + nx = SIZE(xlist) if (xin <= xlist(1)) THEN ! below lowest data point @@ -6346,11 +6348,11 @@ SUBROUTINE getInterpNums(xlist, xin, istart, i, fout) fout = 0.0_DbKi else ! within the data range - - IF (xlist(istart) < xin) i1 = istart ! if istart is below the actual value, start with it instead of starting at 1 to save time + + IF (xlist(min(istart,nx)) < xin) i1 = istart ! if istart is below the actual value, start with it instead of starting at 1 to save time, but make sure it doesn't overstep the array DO i = i1, nx-1 - IF (xlist(i+1) > xin) THEN + IF (xlist(i+1) > xin) THEN fout = (xin - xlist(i) )/( xlist(i+1) - xlist(i) ) exit END IF @@ -6375,25 +6377,25 @@ SUBROUTINE calculate4Dinterpolation(f, ix0, iy0, iz0, it0, fx, fy, fz, ft, c) if (fx == 0) then ix1 = ix0 else - ix1 = ix0+1 + ix1 = min(ix0+1,size(f,4)) ! don't overstep bounds end if if (fy == 0) then iy1 = iy0 else - iy1 = iy0+1 + iy1 = min(iy0+1,size(f,3)) ! don't overstep bounds end if if (fz == 0) then iz1 = iz0 else - iz1 = iz0+1 + iz1 = min(iz0+1,size(f,2)) ! don't overstep bounds end if if (ft == 0) then it1 = it0 else - it1 = it0+1 + it1 = min(it0+1,size(f,1)) ! don't overstep bounds end if c000 = f(it0,iz0,iy0,ix0)*(1.0-ft) + f(it1,iz0,iy0,ix0)*ft @@ -6435,19 +6437,19 @@ SUBROUTINE calculate3Dinterpolation(f, ix0, iy0, iz0, fx, fy, fz, c) if (fx == 0) then ix1 = ix0 else - ix1 = ix0+1 + ix1 = min(ix0+1,size(f,3)) ! don't overstep bounds end if if (fy == 0) then iy1 = iy0 else - iy1 = iy0+1 + iy1 = min(iy0+1,size(f,2)) ! don't overstep bounds end if if (fz == 0) then iz1 = iz0 else - iz1 = iz0+1 + iz1 = min(iz0+1,size(f,1)) ! don't overstep bounds end if c000 = f(iz0,iy0,ix0) From b5c6150ffd050186cb8cdec6e9d8286b6dfa1e01 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Thu, 8 Apr 2021 17:55:20 -0600 Subject: [PATCH 024/242] MDv2: remove a few unnecessary REAL conversions The Is_NaN routine is an interface, so there are routines that handle DbKi, ReKi, SiKi. I was concerned that converting may erase a NaN that is being checked for, depending on the exact compiler used. The float will convert to the default-real type, not necessarily to a DbKi. --- modules/moordyn/src/MoorDyn.f90 | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index b0b6044543..53d00fc2c2 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -1995,7 +1995,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! check for NaNs - is this a good place/way to do it? DO K = 1, m%Nx - IF (Is_NaN(REAL(x%states(K),DbKi))) THEN + IF (Is_NaN(x%states(K))) THEN ErrStat = ErrID_Fatal ErrMsg = ' NaN state detected.' EXIT @@ -2231,7 +2231,7 @@ SUBROUTINE MD_UpdateStates( t, n, u, t_array, p, x, xd, z, other, m, ErrStat, Er ! round dt to integer number of time steps <<<< should this be calculated only once, up front? NdtM = ceiling(p%dtCoupling/p%dtM0) ! get number of mooring time steps to do based on desired time step size - dtM = p%dtCoupling/float(NdtM) ! adjust desired time step to satisfy dt with an integer number of time steps + dtM = p%dtCoupling/REAL(NdtM,DbKi) ! adjust desired time step to satisfy dt with an integer number of time steps !loop through line integration time steps @@ -2242,7 +2242,7 @@ SUBROUTINE MD_UpdateStates( t, n, u, t_array, p, x, xd, z, other, m, ErrStat, Er ! check for NaNs - is this a good place/way to do it? DO J = 1, m%Nx - IF (Is_NaN(REAL(x%states(J),DbKi))) THEN + IF (Is_NaN(x%states(J))) THEN ErrStat = ErrID_Fatal ErrMsg = ' NaN state detected.' EXIT @@ -2273,7 +2273,7 @@ SUBROUTINE MD_UpdateStates( t, n, u, t_array, p, x, xd, z, other, m, ErrStat, Er ! check for NaNs - is this a good place/way to do it? DO J = 1, m%Nx - IF (Is_NaN(REAL(x%states(J),DbKi))) THEN + IF (Is_NaN(x%states(J))) THEN ErrStat = ErrID_Fatal ErrMsg = ' NaN state detected.' EXIT @@ -2965,7 +2965,7 @@ SUBROUTINE TimeStep ( t, dtStep, u, t_array, p, x, xd, z, other, m, ErrStat, Err ! round dt to integer number of time steps NdtM = ceiling(dtStep/p%dtM0) ! get number of mooring time steps to do based on desired time step size - dtM = dtStep/float(NdtM) ! adjust desired time step to satisfy dt with an integer number of time steps + dtM = dtStep/REAL(NdtM,DbKi) ! adjust desired time step to satisfy dt with an integer number of time steps !loop through line integration time steps @@ -3025,7 +3025,7 @@ SUBROUTINE TimeStep ( t, dtStep, u, t_array, p, x, xd, z, other, m, ErrStat, Err ! check for NaNs - is this a good place/way to do it? DO J = 1, Nx - IF (Is_NaN(REAL(x%states(J),DbKi))) THEN + IF (Is_NaN(x%states(J))) THEN ErrStat = ErrID_Fatal ErrMsg = ' NaN state detected.' END IF @@ -4156,7 +4156,7 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, ! check for NaNs DO J = 1, 6*(N-1) - IF (Is_NaN(REAL(Xd(J),DbKi))) THEN + IF (Is_NaN(Xd(J))) THEN print *, "NaN detected at time ", Line%time, " in Line ", Line%IdNum, " in MoorDyn." IF (wordy > 1) THEN print *, "state derivatives:" @@ -4485,7 +4485,7 @@ SUBROUTINE Connect_GetStateDeriv(Connect, Xd, m, p) ! check for NaNs DO J = 1, 6 - IF (Is_NaN(REAL(Xd(J),DbKi))) THEN + IF (Is_NaN(Xd(J))) THEN print *, "NaN detected at time ", Connect%time, " in Point ",Connect%IdNum, " in MoorDyn." IF (wordy > 1) print *, "state derivatives:" IF (wordy > 1) print *, Xd @@ -5147,7 +5147,7 @@ SUBROUTINE Rod_GetStateDeriv(Rod, Xd, m, p) ! check for NaNs (should check all state derivatives, not just first 6) DO J = 1, 6 - IF (Is_NaN(REAL(Xd(J),DbKi))) THEN + IF (Is_NaN(Xd(J))) THEN print *, "NaN detected at time ", Rod%time, " in Rod ",Rod%IdNum IF (wordy > 1) THEN print *, " state derivatives:" @@ -6132,7 +6132,7 @@ SUBROUTINE Body_GetStateDeriv(Body, Xd, m, p) ! check for NaNs (should check all state derivatives, not just first 6) DO J = 1, 6 - IF (Is_NaN(REAL(Xd(J),DbKi))) THEN + IF (Is_NaN(Xd(J))) THEN print *, "NaN detected at time ", Body%time, " in Body ",Body%IdNum, "in MoorDyn," IF (wordy > 0) print *, "state derivatives:" IF (wordy > 0) print *, Xd From 6d1a1a6f3ed66b4b4377548bf65a81d204c06d7f Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Thu, 8 Apr 2021 17:56:34 -0600 Subject: [PATCH 025/242] MDv2: set some uninitialized variables --- modules/moordyn/src/MoorDyn.f90 | 27 +++++++++++++++++++++++- modules/moordyn/src/MoorDyn_Registry.txt | 2 +- modules/moordyn/src/MoorDyn_Types.f90 | 2 +- 3 files changed, 28 insertions(+), 3 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 53d00fc2c2..5bf3d546bb 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -207,6 +207,15 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er +!FIXME: Set some of the options -- the way parsing is written, they don't have to exist in the input file, but get used anyhow. +! Setting these to some value for the moment -- trying to figure out why I get NaN's with the -Wuninitialized -finit-real=inf -finit-integer=9999 flags set. +p%dtM0 = DTcoupling ! default to the coupling +!p%WtrDpth = InitInp%WtrDpth ! This will be passed in later. Right now use the default of -9999 in the registry +p%kBot = 0 +p%cBot = 0 +p%WaterKin = 0 + + ! ----------------- go through file contents a first time, counting each entry ----------------------- i = 0 @@ -1193,6 +1202,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL Conv2UC(OptString) ! check all possible options types and see if OptString is one of them, in which case set the variable. +!FIXME: if some of these are not found in the input file they won't get set if ( OptString == 'DTM') THEN read (OptValue,*) p%dtM0 ! InitInp%DTmooring else if ( OptString == 'G') then @@ -5012,7 +5022,10 @@ SUBROUTINE Rod_SetDependentKin(Rod, t, m) REAL(DbKi) :: dlEnd ! stretched length of attached line end segment REAL(DbKi) :: qMomentSum(3) ! summation of qEnd*EI/dl_stretched (with correct sign) for each attached line - + + ! Initialize variables + qMomentSum = 0.0_DbKi + ! in future pass accelerations here too? <<<< N = Rod%N @@ -5094,6 +5107,10 @@ SUBROUTINE Rod_GetStateDeriv(Rod, Xd, m, p) Real(DbKi) :: y_temp (6) ! temporary vector for LU decomposition Real(DbKi) :: LU_temp(6,6) ! temporary matrix for LU decomposition + ! Initialize some things to zero + y_temp = 0.0_DbKi +! FIXME: should LU_temp be set to M_out before calling LUsolve????? + LU_temp = 0.0_DbKi CALL Rod_GetNetForceAndMass(Rod, Rod%r(:,0), Fnet, M_out, m, p) @@ -6118,6 +6135,11 @@ SUBROUTINE Body_GetStateDeriv(Body, Xd, m, p) Real(DbKi) :: LU_temp(6,6) ! temporary matrix for LU decomposition + ! Initialize temp variables + y_temp = 0.0_DbKi +! FIXME: should LU_temp be set to M_out before calling LUsolve????? + LU_temp = 0.0_DbKi + CALL Body_DoRHS(Body, m, p) ! solve for accelerations in [M]{a}={f} using LU decomposition @@ -6166,6 +6188,9 @@ SUBROUTINE Body_DoRHS(Body, m, p) Real(DbKi) :: F6_i(6) ! net force and moments from an attached object Real(DbKi) :: M6_i(6,6) ! mass and inertia from an attached object + ! Initialize variables + U = 0.0_DbKi ! Set to zero for now + ! First, the body's own mass matrix must be adjusted based on its orientation so that ! we have a mass matrix in the global orientation frame Body%M = RotateM6(Body%M0, Body%OrMat) diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 9ee0b39970..db997aa10e 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -14,7 +14,7 @@ include Registry_NWTC_Library.txt ## ====== some data read from the input file, but not needed after init ====== -typedef MoorDyn/MD MD_InputFileType DbKi DTIC - - - "convergence check time step for IC generation" "[s]" +typedef MoorDyn/MD MD_InputFileType DbKi DTIC - 0.5 - "convergence check time step for IC generation" "[s]" typedef ^ ^ DbKi TMaxIC - 120 - "maximum time to allow for getting converged ICs" "[s]" typedef ^ ^ ReKi CdScaleIC - 1 - "factor to scale drag coefficients by during dynamic relaxation" "[]" typedef ^ ^ ReKi threshIC - 0.01 - "convergence tolerance for ICs (0.01 means 1%)" "[]" diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index 91d408cf30..ca64edf297 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -35,7 +35,7 @@ MODULE MoorDyn_Types IMPLICIT NONE ! ========= MD_InputFileType ======= TYPE, PUBLIC :: MD_InputFileType - REAL(DbKi) :: DTIC !< convergence check time step for IC generation [[s]] + REAL(DbKi) :: DTIC = 0.5 !< convergence check time step for IC generation [[s]] REAL(DbKi) :: TMaxIC = 120 !< maximum time to allow for getting converged ICs [[s]] REAL(ReKi) :: CdScaleIC = 1 !< factor to scale drag coefficients by during dynamic relaxation [[]] REAL(ReKi) :: threshIC = 0.01 !< convergence tolerance for ICs (0.01 means 1%) [[]] From 0e6b7dc4b2dcb36092e68f0f8f64bfab2899ee43 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Mon, 12 Apr 2021 11:27:36 -0600 Subject: [PATCH 026/242] MDv2: add passing of input FileInfo_Type, remove unused var from MDIO_OpenOutput --- modules/moordyn/src/MoorDyn.f90 | 18 +++-- modules/moordyn/src/MoorDyn_IO.f90 | 3 +- modules/moordyn/src/MoorDyn_Registry.txt | 4 +- modules/moordyn/src/MoorDyn_Types.f90 | 98 ++++++++++++++++++++++++ 4 files changed, 112 insertions(+), 11 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 5bf3d546bb..0f6762f50c 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -62,6 +62,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! local variables TYPE(MD_InputFileType) :: InputFileDat ! Data read from input file for setup, but not stored after Init + type(FileInfoType) :: FileInfo_In !< The derived type for holding the full input file for parsing -- we may pass this in the future REAL(DbKi) :: t ! instantaneous time, to be used during IC generation INTEGER(IntKi) :: l ! index INTEGER(IntKi) :: I ! index @@ -159,6 +160,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er p%RootName = TRIM(InitInp%RootName)//'.MD' ! all files written from this module will have this root name +!FIXME: Set some of the options -- the way parsing is written, they don't have to exist in the input file, but get used anyhow. +! Setting these to some value for the moment -- trying to figure out why I get NaN's with the -Wuninitialized -finit-real=inf -finit-integer=9999 flags set. +p%dtM0 = DTcoupling ! default to the coupling +!p%WtrDpth = InitInp%WtrDpth ! This will be passed in later. Right now use the default of -9999 in the registry +p%kBot = 0 +p%cBot = 0 +p%WaterKin = 0 + ! Check for farm-level inputs (indicating that this MoorDyn isntance is being run from FAST.Farm) !intead of below, check first dimension of PtfmInit !p%nTurbines = SIZE(InitInp%FarmCoupledKinematics) ! the number of turbines in the array (0 indicates a regular OpenFAST simulation with 1 turbine) @@ -207,13 +216,6 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er -!FIXME: Set some of the options -- the way parsing is written, they don't have to exist in the input file, but get used anyhow. -! Setting these to some value for the moment -- trying to figure out why I get NaN's with the -Wuninitialized -finit-real=inf -finit-integer=9999 flags set. -p%dtM0 = DTcoupling ! default to the coupling -!p%WtrDpth = InitInp%WtrDpth ! This will be passed in later. Right now use the default of -9999 in the registry -p%kBot = 0 -p%cBot = 0 -p%WaterKin = 0 ! ----------------- go through file contents a first time, counting each entry ----------------------- @@ -1932,7 +1934,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! -------------------------------------------------------------------- ! open output file(s) and write header lines - CALL MDIO_OpenOutput( InitInp%FileName, p, m, InitOut, ErrStat2, ErrMsg2 ) + CALL MDIO_OpenOutput( p, m, InitOut, ErrStat2, ErrMsg2 ) CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN ! -------------------------------------------------------------------- diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index f113898f36..d52caf4f22 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -629,10 +629,9 @@ END SUBROUTINE MDIO_ProcessOutList !----------------------------------------------------------------------------------------============ - SUBROUTINE MDIO_OpenOutput( OutRootName, p, m, InitOut, ErrStat, ErrMsg ) + SUBROUTINE MDIO_OpenOutput( p, m, InitOut, ErrStat, ErrMsg ) !---------------------------------------------------------------------------------------------------- - CHARACTER(*), INTENT( IN ) :: OutRootName ! Root name for the output file TYPE(MD_ParameterType), INTENT( INOUT ) :: p TYPE(MD_MiscVarType), INTENT( INOUT ) :: m TYPE(MD_InitOutPutType ), INTENT( IN ) :: InitOut ! diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index db997aa10e..6b3f3a616a 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -19,7 +19,7 @@ typedef ^ ^ DbKi TMaxIC - 12 typedef ^ ^ ReKi CdScaleIC - 1 - "factor to scale drag coefficients by during dynamic relaxation" "[]" typedef ^ ^ ReKi threshIC - 0.01 - "convergence tolerance for ICs (0.01 means 1%)" "[]" -## ============================== Define input types here: ============================================================================================================================================ +## ============================== Define initialization input types here: ============================================================================================================================= typedef MoorDyn/MD InitInputType ReKi g - -999.9 - "gravity constant" "[m/s^2]" typedef ^ ^ ReKi rhoW - -999.9 - "sea density" "[kg/m^3]" typedef ^ ^ ReKi WtrDepth - -999.9 - "depth of water" "[m]" @@ -27,6 +27,8 @@ typedef ^ ^ ReKi PtfmInit {:} - #typedef ^ ^ ReKi PtfmInit {:}{:} - - "initial position of platform(s) shape: 6, nTurbines" - typedef ^ ^ CHARACTER(1024) FileName - "" - "MoorDyn input file" typedef ^ ^ CHARACTER(1024) RootName - - - "RootName for writing output files" - +typedef ^ ^ LOGICAL UsePrimaryInputFile - .TRUE. - "Read input file instead of passed data" - +typedef ^ ^ FileInfoType PassedPrimaryInputData - - - "Primary input file as FileInfoType (set by driver/glue code) -- String array with metadata" - typedef ^ ^ LOGICAL Echo - "" - "echo parameter - do we want to echo the header line describing the input file?" #typedef ^ ^ CHARACTER(ChanLen) OutList {:} "" - "string containing list of output channels requested in input file" diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index ca64edf297..7f9da5c97f 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -49,6 +49,8 @@ MODULE MoorDyn_Types REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: PtfmInit !< initial position of platform(s) originally size 6 [-] CHARACTER(1024) :: FileName !< MoorDyn input file [-] CHARACTER(1024) :: RootName !< RootName for writing output files [-] + LOGICAL :: UsePrimaryInputFile = .TRUE. !< Read input file instead of passed data [-] + TYPE(FileInfoType) :: PassedPrimaryInputData !< Primary input file as FileInfoType (set by driver/glue code) -- String array with metadata [-] LOGICAL :: Echo !< echo parameter - do we want to echo the header line describing the input file? [-] REAL(ReKi) , DIMENSION(:,:,:), ALLOCATABLE :: WaveVel !< [-] REAL(ReKi) , DIMENSION(:,:,:), ALLOCATABLE :: WaveAcc !< [-] @@ -587,6 +589,10 @@ SUBROUTINE MD_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, ErrSt ENDIF DstInitInputData%FileName = SrcInitInputData%FileName DstInitInputData%RootName = SrcInitInputData%RootName + DstInitInputData%UsePrimaryInputFile = SrcInitInputData%UsePrimaryInputFile + CALL NWTC_Library_Copyfileinfotype( SrcInitInputData%PassedPrimaryInputData, DstInitInputData%PassedPrimaryInputData, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN DstInitInputData%Echo = SrcInitInputData%Echo IF (ALLOCATED(SrcInitInputData%WaveVel)) THEN i1_l = LBOUND(SrcInitInputData%WaveVel,1) @@ -674,6 +680,7 @@ SUBROUTINE MD_DestroyInitInput( InitInputData, ErrStat, ErrMsg ) IF (ALLOCATED(InitInputData%PtfmInit)) THEN DEALLOCATE(InitInputData%PtfmInit) ENDIF + CALL NWTC_Library_Destroyfileinfotype( InitInputData%PassedPrimaryInputData, ErrStat, ErrMsg ) IF (ALLOCATED(InitInputData%WaveVel)) THEN DEALLOCATE(InitInputData%WaveVel) ENDIF @@ -736,6 +743,25 @@ SUBROUTINE MD_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg END IF Int_BufSz = Int_BufSz + 1*LEN(InData%FileName) ! FileName Int_BufSz = Int_BufSz + 1*LEN(InData%RootName) ! RootName + Int_BufSz = Int_BufSz + 1 ! UsePrimaryInputFile + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + Int_BufSz = Int_BufSz + 3 ! PassedPrimaryInputData: size of buffers for each call to pack subtype + CALL NWTC_Library_Packfileinfotype( Re_Buf, Db_Buf, Int_Buf, InData%PassedPrimaryInputData, ErrStat2, ErrMsg2, .TRUE. ) ! PassedPrimaryInputData + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! PassedPrimaryInputData + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! PassedPrimaryInputData + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! PassedPrimaryInputData + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF Int_BufSz = Int_BufSz + 1 ! Echo Int_BufSz = Int_BufSz + 1 ! WaveVel allocated yes/no IF ( ALLOCATED(InData%WaveVel) ) THEN @@ -818,6 +844,36 @@ SUBROUTINE MD_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg IntKiBuf(Int_Xferred) = ICHAR(InData%RootName(I:I), IntKi) Int_Xferred = Int_Xferred + 1 END DO ! I + IntKiBuf(Int_Xferred) = TRANSFER(InData%UsePrimaryInputFile, IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + CALL NWTC_Library_Packfileinfotype( Re_Buf, Db_Buf, Int_Buf, InData%PassedPrimaryInputData, ErrStat2, ErrMsg2, OnlySize ) ! PassedPrimaryInputData + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF IntKiBuf(Int_Xferred) = TRANSFER(InData%Echo, IntKiBuf(1)) Int_Xferred = Int_Xferred + 1 IF ( .NOT. ALLOCATED(InData%WaveVel) ) THEN @@ -988,6 +1044,48 @@ SUBROUTINE MD_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Err OutData%RootName(I:I) = CHAR(IntKiBuf(Int_Xferred)) Int_Xferred = Int_Xferred + 1 END DO ! I + OutData%UsePrimaryInputFile = TRANSFER(IntKiBuf(Int_Xferred), OutData%UsePrimaryInputFile) + Int_Xferred = Int_Xferred + 1 + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL NWTC_Library_Unpackfileinfotype( Re_Buf, Db_Buf, Int_Buf, OutData%PassedPrimaryInputData, ErrStat2, ErrMsg2 ) ! PassedPrimaryInputData + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) OutData%Echo = TRANSFER(IntKiBuf(Int_Xferred), OutData%Echo) Int_Xferred = Int_Xferred + 1 IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WaveVel not allocated From 8e1a2e4e0986e0f5cf753e4387dd6266bac5c524 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Mon, 12 Apr 2021 11:29:21 -0600 Subject: [PATCH 027/242] MDv2: added FileInfo_Type parsing of input file - To avoid any possible over-run of the end of the FileInfo_In%Lines array, a small function is used to get the next line and increment line number. This avoids possible seg-faults with that array --- modules/moordyn/src/MoorDyn.f90 | 218 ++++++++++++++++++-------------- 1 file changed, 120 insertions(+), 98 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 0f6762f50c..704636f15a 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -91,7 +91,6 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CHARACTER(MaxWrScrLen) :: Message ! Local variables for reading file input (Previously in MDIO_ReadInput) - INTEGER(IntKi) :: UnIn ! Unit number for the input file INTEGER(IntKi) :: UnEc ! The local unit number for this module's echo file INTEGER(IntKi) :: UnOut ! for outputing wave kinematics data CHARACTER(200) :: Frmt ! a string to hold a format statement @@ -101,7 +100,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CHARACTER(20) :: LineOutString ! String to temporarially hold characters specifying line output options CHARACTER(20) :: OptString ! String to temporarially hold name of option variable CHARACTER(20) :: OptValue ! String to temporarially hold value of options variable input - INTEGER(IntKi) :: nOpts = 0 ! number of options lines in input file + INTEGER(IntKi) :: nOpts ! number of options lines in input file CHARACTER(40) :: TempString1 ! CHARACTER(40) :: TempString2 ! CHARACTER(40) :: TempString3 ! @@ -192,118 +191,130 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !--------------------------------------------------------------------------------------------- - UnEc = -1 - ! Initialize ErrStat ErrStat = ErrID_None ErrMsg = "" - !------------------------------------------------------------------------------------------------- - ! Open the file - !------------------------------------------------------------------------------------------------- - FileName = TRIM(InitInp%FileName) - CALL GetNewUnit( UnIn ) - CALL OpenFInpFile( UnIn, FileName, ErrStat2, ErrMsg2 ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF + CALL WrScr( ' Parsing MoorDyn input file: '//trim(InitInp%FileName) ) - CALL WrScr( ' Reading MoorDyn input file: '//FileName ) + ! ----------------------------------------------------------------- + ! Read the primary MoorDyn input file, or copy from passed input + if (InitInp%UsePrimaryInputFile) then + ! Read the entire input file, minus any comment lines, into the FileInfo_In + ! data structure in memory for further processing. + call ProcessComFile( InitInp%FileName, FileInfo_In, ErrStat2, ErrMsg2 ) + else + call NWTC_Library_CopyFileInfoType( InitInp%PassedPrimaryInputData, FileInfo_In, MESH_NEWCOPY, ErrStat2, ErrMsg2 ) + endif + if (Failed()) return; + ! For diagnostic purposes, the following can be used to display the contents + ! of the FileInfo_In data structure. + call Print_FileInfo_Struct( CU, FileInfo_In ) ! CU is the screen -- different number on different systems. + ! Parse the FileInfo_In structure of data from the inputfile into the InitInp%InputFile structure +! CALL ParsePrimaryFileInfo_BuildModel( PriPath, InitInp, FileInfo_In, InputFileDat, p, m, UnEc, ErrStat2, ErrMsg2 ) +! if (Failed()) return; + +!NOTE: This could be split into a separate routine for easier to read code + !------------------------------------------------------------------------------------------------- + ! Parsing of input file from the FileInfo_In data structure + ! - FileInfo_Type is essentially a string array with some metadata. + !------------------------------------------------------------------------------------------------- + + UnEc = -1 + nOpts = 0 ! Setting here rather than implied save + + ! ----------------- go through file contents a first time, counting each entry ----------------------- - i = 0 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 !read a line - - do while ( ErrStat2 == 0 ) + i = 1 ! set line number counter to first line + Line = NextLine(i); ! Get the line and increment counter. See description of routine. + do while ( i < FileInfo_In%NumLines ) if (INDEX(Line, "---") > 0) then ! look for a header line if ( ( INDEX(Line, "LINE DICTIONARY") > 0) .or. ( INDEX(Line, "LINE TYPES") > 0) ) then ! if line dictionary header ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) + Line = NextLine(i) ! find how many elements of this type there are - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nLineTypes = p%nLineTypes + 1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) END DO else if ( (INDEX(Line, "ROD DICTIONARY") > 0) .or. ( INDEX(Line, "ROD TYPES") > 0) ) then ! if rod dictionary header ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) + Line = NextLine(i) ! find how many elements of this type there are - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nRodTypes = p%nRodTypes + 1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) END DO else if ((INDEX(Line, "BODIES") > 0 ) .or. (INDEX(Line, "BODY LIST") > 0 ) .or. (INDEX(Line, "BODY PROPERTIES") > 0 )) then ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) + Line = NextLine(i) ! find how many elements of this type there are - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nBodies = p%nBodies + 1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) END DO else if ((INDEX(Line, "RODS") > 0 ) .or. (INDEX(Line, "ROD LIST") > 0) .or. (INDEX(Line, "ROD PROPERTIES") > 0)) then ! if rod properties header ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) + Line = NextLine(i) ! find how many elements of this type there are - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nRods = p%nRods + 1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) END DO else if ((INDEX(Line, "POINTS") > 0 ) .or. (INDEX(Line, "CONNECTION PROPERTIES") > 0) .or. (INDEX(Line, "NODE PROPERTIES") > 0) .or. (INDEX(Line, "POINT PROPERTIES") > 0) .or. (INDEX(Line, "POINT LIST") > 0) ) then ! if node properties header ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) + Line = NextLine(i) ! find how many elements of this type there are - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nConnects = p%nConnects + 1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) END DO else if ((INDEX(Line, "LINES") > 0 ) .or. (INDEX(Line, "LINE PROPERTIES") > 0) .or. (INDEX(Line, "LINE LIST") > 0) ) then ! if line properties header ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + i=i+2 ! find how many elements of this type there are - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nLines = p%nLines + 1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) END DO else if (INDEX(Line, "CONTROL") > 0) then ! if failure conditions header @@ -311,14 +322,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF (wordy > 1) print *, " Reading control channels: "; ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) + Line = NextLine(i) ! find how many elements of this type there are - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nCtrlChans = p%nCtrlChans + 1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) END DO else if (INDEX(Line, "FAILURE") > 0) then ! if failure conditions header @@ -326,14 +337,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF (wordy > 1) print *, " Reading failure conditions: "; ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) + Line = NextLine(i) ! find how many elements of this type there are - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line p%nFails = p%nFails + 1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) END DO @@ -342,10 +353,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! don't skip any lines (no column headers for the options section) ! find how many options have been specified - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line nOpts = nOpts + 1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) END DO @@ -353,15 +364,15 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! we don't need to count this section... - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) else ! otherwise ignore this line that isn't a recognized header line and read the next line - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) end if else ! otherwise ignore this line, which doesn't have the "---" or header line and read the next line - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) end if end do @@ -374,6 +385,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF (wordy > 0) print *, " Identified ", p%nRods , "Rods in input file." IF (wordy > 0) print *, " Identified ", p%nConnects , "Connections in input file." IF (wordy > 0) print *, " Identified ", p%nLines , "Lines in input file." + IF (wordy > 0) print *, " Identified ", nOpts , "Options in input file." @@ -427,16 +439,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! ---------------------- now go through again and process file contents -------------------- - REWIND(UnIn) ! rewind to start of input file ! note: no longer worrying about "Echo" option Nx = 0 ! set state counter to zero - i = 0 ! set line number counter to zero + i = 1 ! set line number counter to first line + Line = NextLine(i) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 !read a line - - do while ( ErrStat2 == 0 ) + do while ( i < FileInfo_In%NumLines ) if (INDEX(Line, "---") > 0) then ! look for a header line @@ -446,14 +456,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF (wordy > 0) print *, "Reading line types" ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) + Line = NextLine(i) ! process each line DO l = 1,p%nLineTypes !read into a line - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) ! parse out entries: Name Diam MassDenInAir EA cIntDamp >>EI(new)<< Can Cat Cdn Cdt READ(Line,*,IOSTAT=ErrStat2) m%LineTypeList(l)%name, m%LineTypeList(l)%d, & @@ -499,14 +509,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF (wordy > 0) print *, "Reading rod types" ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) + Line = NextLine(i) ! process each line DO l = 1,p%nRodTypes !read into a line - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) ! parse out entries: Name Diam MassDenInAir Can Cat Cdn Cdt IF (ErrStat2 == 0) THEN @@ -534,14 +544,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if ((INDEX(Line, "BODIES") > 0 ) .or. (INDEX(Line, "BODY LIST") > 0 ) .or. (INDEX(Line, "BODY PROPERTIES") > 0 )) then ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) + Line = NextLine(i) ! process each body DO l = 1,p%nBodies !read into a line - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) ! parse out entries: Name/ID X0 Y0 Z0 r0 p0 y0 Xcg Ycg Zcg M V IX IY IZ CdA-x,y,z Ca-x,y,z IF (ErrStat2 == 0) THEN @@ -625,14 +635,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF (wordy > 0) print *, "Reading Rods" ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) + Line = NextLine(i) ! process each rod DO l = 1,p%nRods !read into a line - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) ! parse out entries: RodID Type/BodyID RodType Xa Ya Za Xb Yb Zb NumSegs Flags/Outputs IF (ErrStat2 == 0) THEN @@ -804,14 +814,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF (wordy > 0) print *, "Reading Points" ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) + Line = NextLine(i) ! process each point DO l = 1,p%nConnects !read into a line - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) ! parse out entries: Node Type X Y Z M V FX FY FZ CdA Ca IF (ErrStat2 == 0) THEN @@ -948,14 +958,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF (wordy > 0) print *, "Reading Lines" ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) + Line = NextLine(i) ! process each line DO l = 1,p%nLines !read into a line - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) ! parse out entries: LineID LineType UnstrLen NumSegs NodeA NodeB Flags/Outputs IF (ErrStat2 == 0) THEN @@ -1122,14 +1132,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! TODO: add stuff <<<<<<<< ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) + Line = NextLine(i) ! process each line DO l = 1,p%nCtrlChans !read into a line - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) ! count commas to determine how many line IDs specified for this channel N = count(transfer(Line, 'a', len(Line)) == ",") + 1 ! number of line IDs given @@ -1162,14 +1172,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! TODO: add stuff <<<<<<<< ! skip following two lines (label line and unit line) - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) + Line = NextLine(i) ! process each line DO l = 1,p%nFails !read into a line - READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) READ(Line,*,IOSTAT=ErrStat2) m%LineList(l)%IdNum, tempString1, m%LineList(l)%UnstrLen, & @@ -1189,7 +1199,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er DO l = 1,nOpts !read into a line - READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) ! parse out entries: value, option keyword READ(Line,*,IOSTAT=ErrStat2) OptValue, OptString ! look at first two entries, ignore remaining words in line, which should be comments @@ -1244,10 +1254,6 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! allocate InitInp%Outliest (to a really big number for now...) CALL AllocAry( OutList, MaxAryLen, "MoorDyn Input File's Outlist", ErrStat2, ErrMsg2 ); if(Failed()) return - ! OutList - List of user-requested output channels (-): - !CALL ReadOutputList ( UnIn, FileName, InitInp%OutList, p%NumOuts, 'OutList', "List of user-requested output channels", ErrStat2, ErrMsg2, UnEc ); if(Failed()) return - - ! customm implementation to avoid need for "END" keyword line ! Initialize some values p%NumOuts = 0 ! start counter at zero @@ -1255,9 +1261,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! Read in all of the lines containing output parameters and store them in OutList(:) + ! customm implementation to avoid need for "END" keyword line DO ! read a line - READ(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) CALL Conv2UC(Line) ! convert to uppercase for easy string matching @@ -1289,21 +1296,23 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !------------------------------------------------------------------------------------------- else ! otherwise ignore this line that isn't a recognized header line and read the next line - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) end if !------------------------------------------------------------------------------------------- else ! otherwise ignore this line, which doesn't have the "---" or header line and read the next line - read(UnIn,'(A)',IOSTAT=ErrStat2) Line; i=i+1 + Line = NextLine(i) end if end do - ! this is the end of reading the input file, so close it now + ! this is the end of parsing the input file, so cleanup anything we don't need anymore CALL CleanUp() + ! End of input file parsing from the FileInfo_In data structure + !------------------------------------------------------------------------------------------------- @@ -2161,12 +2170,25 @@ SUBROUTINE CheckError(ErrID,Msg) END SUBROUTINE CheckError - SUBROUTINE CleanUp() + SUBROUTINE CleanUp() ! ErrStat = ErrID_Fatal call MD_DestroyInputFileType( InputFileDat, ErrStat2, ErrMsg2 ) ! Ignore any error messages from this - if(UnIn > 0) CLOSE( UnIn ) ! Only if opened ! IF (InitInp%Echo) CLOSE( UnEc ) - END SUBROUTINE + END SUBROUTINE + + !> If for some reason the file is truncated, it is possible to get into an infinite loop + !! in a while looking for the next section and accidentally overstep the end of the array + !! resulting in a segfault. This function will trap that issue and return a section break + CHARACTER(1024) function NextLine(i) + integer, intent(inout) :: i + if (i>FileInfo_In%NumLines) then + NextLine="---" ! Set as a separator so we can escape some of the while loops + i=FileInfo_In%NumLines + else + NextLine=trim(FileInfo_In%Lines(i)) + i=i+1 + endif + end function NextLine END SUBROUTINE MD_Init !----------------------------------------------------------------------------------------====== From 45aee88bfdacc71e1945a96ff7f93a1b189ee418 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Mon, 12 Apr 2021 14:32:34 -0600 Subject: [PATCH 028/242] MDv2: potential segfault on closing files that were never opened. Added note. Matt will need to review and fix this. --- modules/moordyn/src/MoorDyn_IO.f90 | 3 +++ 1 file changed, 3 insertions(+) diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index d52caf4f22..ecfadbe364 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -1032,6 +1032,9 @@ SUBROUTINE MDIO_CloseOutput ( p, m, ErrStat, ErrMsg ) ErrMsg = "" +!FIXME: make sure thes are actually open before trying to close them. Segfault will occur otherwise!!!! +! This bug can be triggered by an early failure of the parsing routines, before these files were ever opened +! which returns MD to OpenFAST as ErrID_Fatal, then OpenFAST calls MD_End, which calls this. ! close main MoorDyn output file CLOSE( p%MDUnOut, IOSTAT = ErrStat ) IF ( ErrStat /= 0 ) THEN From ef18dc22d8e68ad3f4cbdfdb6c051e76c54b3943 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Mon, 12 Apr 2021 14:45:29 -0600 Subject: [PATCH 029/242] MDv2: changed index for line number counting to current line instead of next line This better matches original --- modules/moordyn/src/MoorDyn.f90 | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 704636f15a..1dd29cf223 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -65,7 +65,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er type(FileInfoType) :: FileInfo_In !< The derived type for holding the full input file for parsing -- we may pass this in the future REAL(DbKi) :: t ! instantaneous time, to be used during IC generation INTEGER(IntKi) :: l ! index - INTEGER(IntKi) :: I ! index + INTEGER(IntKi) :: I ! Current line number of input file INTEGER(IntKi) :: J ! index INTEGER(IntKi) :: K ! index INTEGER(IntKi) :: Itemp ! index @@ -233,10 +233,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! ----------------- go through file contents a first time, counting each entry ----------------------- - i = 1 ! set line number counter to first line + i = 0 ! set line number counter to before first line Line = NextLine(i); ! Get the line and increment counter. See description of routine. - do while ( i < FileInfo_In%NumLines ) + do while ( i <= FileInfo_In%NumLines ) if (INDEX(Line, "---") > 0) then ! look for a header line @@ -443,10 +443,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! note: no longer worrying about "Echo" option Nx = 0 ! set state counter to zero - i = 1 ! set line number counter to first line + i = 0 ! set line number counter to before first line Line = NextLine(i) - do while ( i < FileInfo_In%NumLines ) + do while ( i <= FileInfo_In%NumLines ) if (INDEX(Line, "---") > 0) then ! look for a header line @@ -2180,13 +2180,12 @@ SUBROUTINE CleanUp() !! in a while looking for the next section and accidentally overstep the end of the array !! resulting in a segfault. This function will trap that issue and return a section break CHARACTER(1024) function NextLine(i) - integer, intent(inout) :: i + integer, intent(inout) :: i ! Current line number corresponding to contents of NextLine + i=i+1 ! Increment to line next line. if (i>FileInfo_In%NumLines) then NextLine="---" ! Set as a separator so we can escape some of the while loops - i=FileInfo_In%NumLines else NextLine=trim(FileInfo_In%Lines(i)) - i=i+1 endif end function NextLine From 15631e378b4c77b033ba027d1c4529616b4ea83a Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Mon, 12 Apr 2021 15:15:52 -0600 Subject: [PATCH 030/242] MDv2: turn off the verbose input file info --- modules/moordyn/src/MoorDyn.f90 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 1dd29cf223..5342e11af2 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -212,7 +212,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! For diagnostic purposes, the following can be used to display the contents ! of the FileInfo_In data structure. - call Print_FileInfo_Struct( CU, FileInfo_In ) ! CU is the screen -- different number on different systems. + !call Print_FileInfo_Struct( CU, FileInfo_In ) ! CU is the screen -- different number on different systems. ! Parse the FileInfo_In structure of data from the inputfile into the InitInp%InputFile structure ! CALL ParsePrimaryFileInfo_BuildModel( PriPath, InitInp, FileInfo_In, InputFileDat, p, m, UnEc, ErrStat2, ErrMsg2 ) From 137bb775a8ca12c9c5631f7b9f215fd244b8d45a Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Wed, 14 Apr 2021 18:58:09 -0600 Subject: [PATCH 031/242] copyright update --- modules/moordyn/src/MoorDyn.f90 | 3 ++- modules/moordyn/src/MoorDyn_Driver.f90 | 4 ++-- modules/moordyn/src/MoorDyn_IO.f90 | 3 ++- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index f02060c8e6..723f6e8db9 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -1,6 +1,7 @@ !********************************************************************************************************************************** ! LICENSING -! Copyright (C) 2015 Matthew Hall +! Copyright (C) 2020-2021 Alliance for Sustainable Energy, LLC +! Copyright (C) 2015-2019 Matthew Hall ! ! This file is part of MoorDyn. ! diff --git a/modules/moordyn/src/MoorDyn_Driver.f90 b/modules/moordyn/src/MoorDyn_Driver.f90 index a286b5f58a..aa0f380906 100644 --- a/modules/moordyn/src/MoorDyn_Driver.f90 +++ b/modules/moordyn/src/MoorDyn_Driver.f90 @@ -1,7 +1,7 @@ !********************************************************************************************************************************** ! LICENSING -! Copyright (C) 2020 National Renewable Energy Laboratory -! Copyright (C) 2020 Matthew Hall +! Copyright (C) 2020-2021 Alliance for Sustainable Energy, LLC +! Copyright (C) 2015-2019 Matthew Hall ! ! This file is part of MoorDyn. ! diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index f113898f36..d934d0ee91 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -1,6 +1,7 @@ !********************************************************************************************************************************** ! LICENSING -! Copyright (C) 2015 Matthew Hall +! Copyright (C) 2020-2021 Alliance for Sustainable Energy, LLC +! Copyright (C) 2015-2019 Matthew Hall ! ! This file is part of MoorDyn. ! From 8cd93eeb0fe90027fcca81f0a1bd0edb0695e268 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Mon, 10 May 2021 11:47:26 -0600 Subject: [PATCH 032/242] Adding linearization subroutines to MoorDyn v2: - The standard linearization subroutines in other modules (e.g. SD) are now also in MoorDyn. Gradients use finite differences in all cases. - Updated FAST_Lin to support mooring linearization with MoorDyn, alongside the existing MAP option. - Compiles, but needs debugging and then checking of MD linearization outputs. --- modules/moordyn/src/MoorDyn.f90 | 717 +++++++++++++++++- modules/moordyn/src/MoorDyn_Registry.txt | 17 + modules/moordyn/src/MoorDyn_Types.f90 | 646 ++++++++++++++++ modules/openfast-library/src/FAST_Lin.f90 | 288 ++++++- .../openfast-library/src/FAST_Registry.txt | 2 + modules/openfast-library/src/FAST_Types.f90 | 233 ++++++ 6 files changed, 1867 insertions(+), 36 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index e391b81015..ca8ce336dd 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -39,6 +39,11 @@ MODULE MoorDyn PUBLIC :: MD_CalcOutput PUBLIC :: MD_CalcContStateDeriv PUBLIC :: MD_End + PUBLIC :: MD_JacobianPContState + PUBLIC :: MD_JacobianPInput + PUBLIC :: MD_JacobianPDiscState + PUBLIC :: MD_JacobianPConstrState + PUBLIC :: MD_GetOP CONTAINS @@ -2109,6 +2114,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er xd%dummy = 0 z%dummy = 0 + if (InitInp%Linearize) then + call MD_Init_Jacobian(InitInp, p, u, y, m, InitOut, ErrStat2, ErrMsg2); if(Failed()) return + endif + CALL WrScr(' MoorDyn initialization completed.') ! TODO: add feature for automatic water depth increase based on max anchor depth! @@ -6326,6 +6335,406 @@ SUBROUTINE Body_AddRod(Body, rodID, coords) END SUBROUTINE Body_AddRod + +!++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +! ###### The following four routines are Jacobian routines for linearization capabilities ####### +! If the module does not implement them, set ErrStat = ErrID_Fatal in SD_Init() when InitInp%Linearize is .true. +!---------------------------------------------------------------------------------------------------------------------------------- +!> Routine to compute the Jacobians of the output (Y), continuous- (X), discrete- (Xd), and constraint-state (Z) functions +!! with respect to the inputs (u). The partial derivatives dY/du, dX/du, dXd/du, and DZ/du are returned. +SUBROUTINE MD_JacobianPInput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, dYdu, dXdu, dXddu, dZdu) + REAL(DbKi), INTENT(IN ) :: t !< Time in seconds at operating point + TYPE(MD_InputType), INTENT(INOUT) :: u !< Inputs at operating point (may change to inout if a mesh copy is required) + TYPE(MD_ParameterType), INTENT(IN ) :: p !< Parameters + TYPE(MD_ContinuousStateType), INTENT(IN ) :: x !< Continuous states at operating point + TYPE(MD_DiscreteStateType), INTENT(IN ) :: xd !< Discrete states at operating point + TYPE(MD_ConstraintStateType), INTENT(IN ) :: z !< Constraint states at operating point + TYPE(MD_OtherStateType), INTENT(IN ) :: OtherState !< Other states at operating point + TYPE(MD_OutputType), INTENT(INOUT) :: y !< Output (change to inout if a mesh copy is required); Output fields are not used by this routine, but type is available here so that mesh parameter information (i.e., connectivity) does not have to be recalculated for dYdu. + TYPE(MD_MiscVarType), INTENT(INOUT) :: m !< Misc/optimization variables + INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message if ErrStat /= ErrID_None + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dYdu(:,:) !< Partial derivatives of output functions (Y) wrt the inputs (u) [intent in to avoid deallocation] + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dXdu(:,:) !< Partial derivatives of continuous state functions (X) wrt the inputs (u) [intent in to avoid deallocation] + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dXddu(:,:) !< Partial derivatives of discrete state functions (Xd) wrt the inputs (u) [intent in to avoid deallocation] + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dZdu(:,:) !< Partial derivatives of constraint state functions (Z) wrt the inputs (u) [intent in to avoid deallocation] + + ! local variables + TYPE(MD_OutputType) :: y_m, y_p + TYPE(MD_ContinuousStateType) :: x_m, x_p + TYPE(MD_InputType) :: u_perturb + REAL(R8Ki) :: delta_p, delta_m ! delta change in input (plus, minus) + INTEGER(IntKi) :: i + integer(intKi) :: ErrStat2 + character(ErrMsgLen) :: ErrMsg2 + character(*), parameter :: RoutineName = 'MD_JacobianPInput' + + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = '' + + ! get OP values here: + call MD_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat2, ErrMsg2 ); if(Failed()) return + + ! make a copy of the inputs to perturb + call MD_CopyInput( u, u_perturb, MESH_NEWCOPY, ErrStat2, ErrMsg2); if(Failed()) return + + IF ( PRESENT( dYdu ) ) THEN + ! Calculate the partial derivative of the output functions (Y) with respect to the inputs (u) here: + if (.not. allocated(dYdu) ) then + call AllocAry(dYdu, p%Jac_ny, size(p%Jac_u_indx,1),'dYdu', ErrStat2, ErrMsg2); if(Failed()) return + end if + ! make a copy of outputs because we will need two for the central difference computations (with orientations) + call MD_CopyOutput( y, y_p, MESH_NEWCOPY, ErrStat2, ErrMsg2); if(Failed()) return + call MD_CopyOutput( y, y_m, MESH_NEWCOPY, ErrStat2, ErrMsg2); if(Failed()) return + do i=1,size(p%Jac_u_indx,1) + ! get u_op + delta_p u + call MD_CopyInput( u, u_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + call MD_Perturb_u( p, i, 1, u_perturb, delta_p ) + ! compute y at u_op + delta_p u + call MD_CalcOutput( t, u_perturb, p, x, xd, z, OtherState, y_p, m, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + ! get u_op - delta_m u + call MD_CopyInput( u, u_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + call MD_Perturb_u( p, i, -1, u_perturb, delta_m ) + ! compute y at u_op - delta_m u + call MD_CalcOutput( t, u_perturb, p, x, xd, z, OtherState, y_m, m, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + ! get central difference: + call MD_Compute_dY( p, y_p, y_m, delta_p, dYdu(:,i) ) + end do + if(Failed()) return + END IF + IF ( PRESENT( dXdu ) ) THEN + if (.not. allocated(dXdu)) then + call AllocAry(dXdu, p%Jac_nx, size(p%Jac_u_indx,1), 'dXdu', ErrStat2, ErrMsg2); if (Failed()) return + endif + do i=1,size(p%Jac_u_indx,1) + ! get u_op + delta u + call MD_CopyInput( u, u_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + call MD_Perturb_u( p, i, 1, u_perturb, delta_p ) + ! compute x at u_op + delta u + call MD_CalcContStateDeriv( t, u_perturb, p, x, xd, z, OtherState, m, x_p, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + ! get u_op - delta u + call MD_CopyInput( u, u_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + call MD_Perturb_u( p, i, -1, u_perturb, delta_m ) + ! compute x at u_op - delta u + call MD_CalcContStateDeriv( t, u_perturb, p, x, xd, z, OtherState, m, x_m, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + ! get central difference: + ! we may have had an error allocating memory, so we'll check + if(Failed()) return + ! get central difference: + call MD_Compute_dX( p, x_p, x_m, delta_p, dXdu(:,i) ) + end do + END IF ! dXdu + IF ( PRESENT( dXddu ) ) THEN + if (allocated(dXddu)) deallocate(dXddu) + END IF + IF ( PRESENT( dZdu ) ) THEN + if (allocated(dZdu)) deallocate(dZdu) + END IF + call CleanUp() +contains + + logical function Failed() + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + Failed = ErrStat >= AbortErrLev + if (Failed) call CleanUp() + end function Failed + + subroutine CleanUp() + call MD_DestroyContState( x_p, ErrStat2, ErrMsg2 ) ! we don't need this any more + call MD_DestroyContState( x_m, ErrStat2, ErrMsg2 ) ! we don't need this any more + call MD_DestroyOutput( y_p, ErrStat2, ErrMsg2 ) + call MD_DestroyOutput( y_m, ErrStat2, ErrMsg2 ) + call MD_DestroyInput(u_perturb, ErrStat2, ErrMsg2 ) + end subroutine cleanup + +END SUBROUTINE MD_JacobianPInput +!---------------------------------------------------------------------------------------------------------------------------------- +!> Routine to compute the Jacobians of the output (Y), continuous- (X), discrete- (Xd), and constraint-state (Z) functions +!! with respect to the continuous states (x). The partial derivatives dY/dx, dX/dx, dXd/dx, and dZ/dx are returned. +SUBROUTINE MD_JacobianPContState( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, dYdx, dXdx, dXddx, dZdx) + REAL(DbKi), INTENT(IN ) :: t !< Time in seconds at operating point + TYPE(MD_InputType), INTENT(INOUT) :: u !< Inputs at operating point (may change to inout if a mesh copy is required) + TYPE(MD_ParameterType), INTENT(IN ) :: p !< Parameters + TYPE(MD_ContinuousStateType), INTENT(IN ) :: x !< Continuous states at operating point + TYPE(MD_DiscreteStateType), INTENT(IN ) :: xd !< Discrete states at operating point + TYPE(MD_ConstraintStateType), INTENT(IN ) :: z !< Constraint states at operating point + TYPE(MD_OtherStateType), INTENT(IN ) :: OtherState !< Other states at operating point + TYPE(MD_OutputType), INTENT(INOUT) :: y !< Output (change to inout if a mesh copy is required); Output fields are not used by this routine, but type is available here so that mesh parameter information (i.e., connectivity) does not have to be recalculated for dYdx. + TYPE(MD_MiscVarType), INTENT(INOUT) :: m !< Misc/optimization variables + INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message if ErrStat /= ErrID_None + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dYdx(:,:) !< Partial derivatives of output functions wrt the continuous states (x) [intent in to avoid deallocation] + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dXdx(:,:) !< Partial derivatives of continuous state functions (X) wrt the continuous states (x) [intent in to avoid deallocation] + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dXddx(:,:) !< Partial derivatives of discrete state functions (Xd) wrt the continuous states (x) [intent in to avoid deallocation] + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dZdx(:,:) !< Partial derivatives of constraint state functions (Z) wrt the continuous states (x) [intent in to avoid deallocation] + ! local variables + TYPE(MD_OutputType) :: y_p, y_m + TYPE(MD_ContinuousStateType) :: x_p, x_m + TYPE(MD_ContinuousStateType) :: x_perturb + REAL(R8Ki) :: delta ! delta change in input or state + INTEGER(IntKi) :: i, k + INTEGER(IntKi) :: idx + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_JacobianPContState' + + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = '' + + ! make a copy of the continuous states to perturb NOTE: MESH_NEWCOPY + call MD_CopyContState( x, x_perturb, MESH_NEWCOPY, ErrStat2, ErrMsg2); if(Failed()) return + + IF ( PRESENT( dYdx ) ) THEN + ! Calculate the partial derivative of the output functions (Y) with respect to the continuous states (x) here: + if (.not. allocated(dYdx)) then + call AllocAry(dYdx, p%Jac_ny, p%Jac_nx, 'dYdx', ErrStat2, ErrMsg2); if(Failed()) return + end if + ! make a copy of outputs because we will need two for the central difference computations (with orientations) + call MD_CopyOutput( y, y_p, MESH_NEWCOPY, ErrStat2, ErrMsg2); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + call MD_CopyOutput( y, y_m, MESH_NEWCOPY, ErrStat2, ErrMsg2); if(Failed()) return + do i=1,p%Jac_nx + ! get x_op + delta x + call MD_CopyContState( x, x_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + call MD_perturb_x(p, i, 1, x_perturb, delta ) + ! compute y at x_op + delta x + call MD_CalcOutput( t, u, p, x_perturb, xd, z, OtherState, y_p, m, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + ! get x_op - delta x + call MD_CopyContState( x, x_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + call MD_perturb_x(p, i, -1, x_perturb, delta ) + ! compute y at x_op - delta x + call MD_CalcOutput( t, u, p, x_perturb, xd, z, OtherState, y_m, m, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + ! get central difference: + call MD_Compute_dY( p, y_p, y_m, delta, dYdx(:,idx) ) + end do + if(Failed()) return + END IF + + IF ( PRESENT( dXdx ) ) THEN + ! Calculate the partial derivative of the continuous state functions (X) with respect to the continuous states (x) here: + if (.not. allocated(dXdx)) then + call AllocAry(dXdx, p%Jac_nx, p%Jac_nx, 'dXdx', ErrStat2, ErrMsg2); if(Failed()) return + end if + do i=1,p%Jac_nx + ! get x_op + delta x + call MD_CopyContState( x, x_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + call MD_perturb_x(p, i, 1, x_perturb, delta ) + ! compute x at x_op + delta x + call MD_CalcContStateDeriv( t, u, p, x_perturb, xd, z, OtherState, m, x_p, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + ! get x_op - delta x + call MD_CopyContState( x, x_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + call MD_perturb_x(p, i, -1, x_perturb, delta ) + ! compute x at x_op - delta x + call MD_CalcContStateDeriv( t, u, p, x_perturb, xd, z, OtherState, m, x_m, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + if(Failed()) return + ! get central difference: + call MD_Compute_dX( p, x_p, x_m, delta, dXdx(:,idx) ) + end do + END IF + IF ( PRESENT( dXddx ) ) THEN + if (allocated(dXddx)) deallocate(dXddx) + END IF + IF ( PRESENT( dZdx ) ) THEN + if (allocated(dZdx)) deallocate(dZdx) + END IF + call CleanUp() + +contains + + logical function Failed() + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'MD_JacobianPContState') + Failed = ErrStat >= AbortErrLev + if (Failed) call CleanUp() + end function Failed + + subroutine CleanUp() + call MD_DestroyOutput( y_p, ErrStat2, ErrMsg2 ) + call MD_DestroyOutput( y_m, ErrStat2, ErrMsg2 ) + call MD_DestroyContState( x_p, ErrStat2, ErrMsg2 ) + call MD_DestroyContState( x_m, ErrStat2, ErrMsg2 ) + call MD_DestroyContState(x_perturb, ErrStat2, ErrMsg2 ) + end subroutine cleanup + +END SUBROUTINE MD_JacobianPContState + +!---------------------------------------------------------------------------------------------------------------------------------- +!> Routine to compute the Jacobians of the output (Y), continuous- (X), discrete- (Xd), and constraint-state (Z) functions +!! with respect to the discrete states (xd). The partial derivatives dY/dxd, dX/dxd, dXd/dxd, and DZ/dxd are returned. +SUBROUTINE MD_JacobianPDiscState( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, dYdxd, dXdxd, dXddxd, dZdxd ) + REAL(DbKi), INTENT(IN ) :: t !< Time in seconds at operating point + TYPE(MD_InputType), INTENT(INOUT) :: u !< Inputs at operating point (may change to inout if a mesh copy is required) + TYPE(MD_ParameterType), INTENT(IN ) :: p !< Parameters + TYPE(MD_ContinuousStateType), INTENT(IN ) :: x !< Continuous states at operating point + TYPE(MD_DiscreteStateType), INTENT(IN ) :: xd !< Discrete states at operating point + TYPE(MD_ConstraintStateType), INTENT(IN ) :: z !< Constraint states at operating point + TYPE(MD_OtherStateType), INTENT(IN ) :: OtherState !< Other states at operating point + TYPE(MD_OutputType), INTENT(INOUT) :: y !< Output (change to inout if a mesh copy is required); Output fields are not used by this routine, but type is available here so that mesh parameter information (i.e., connectivity) does not have to be recalculated for dYdx. + TYPE(MD_MiscVarType), INTENT(INOUT) :: m !< Misc/optimization variables + INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message if ErrStat /= ErrID_None + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dYdxd(:,:) !< Partial derivatives of output functions (Y) wrt the discrete states (xd) [intent in to avoid deallocation] + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dXdxd(:,:) !< Partial derivatives of continuous state functions (X) wrt the discrete states (xd) [intent in to avoid deallocation] + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dXddxd(:,:)!< Partial derivatives of discrete state functions (Xd) wrt the discrete states (xd) [intent in to avoid deallocation] + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dZdxd(:,:) !< Partial derivatives of constraint state functions (Z) wrt discrete states (xd) [intent in to avoid deallocation] + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = '' + IF ( PRESENT( dYdxd ) ) THEN + END IF + IF ( PRESENT( dXdxd ) ) THEN + END IF + IF ( PRESENT( dXddxd ) ) THEN + END IF + IF ( PRESENT( dZdxd ) ) THEN + END IF +END SUBROUTINE MD_JacobianPDiscState +!---------------------------------------------------------------------------------------------------------------------------------- +!> Routine to compute the Jacobians of the output (Y), continuous- (X), discrete- (Xd), and constraint-state (Z) functions +!! with respect to the constraint states (z). The partial derivatives dY/dz, dX/dz, dXd/dz, and DZ/dz are returned. +SUBROUTINE MD_JacobianPConstrState( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, dYdz, dXdz, dXddz, dZdz ) + REAL(DbKi), INTENT(IN ) :: t !< Time in seconds at operating point + TYPE(MD_InputType), INTENT(INOUT) :: u !< Inputs at operating point (may change to inout if a mesh copy is required) + TYPE(MD_ParameterType), INTENT(IN ) :: p !< Parameters + TYPE(MD_ContinuousStateType), INTENT(IN ) :: x !< Continuous states at operating point + TYPE(MD_DiscreteStateType), INTENT(IN ) :: xd !< Discrete states at operating point + TYPE(MD_ConstraintStateType), INTENT(IN ) :: z !< Constraint states at operating point + TYPE(MD_OtherStateType), INTENT(IN ) :: OtherState !< Other states at operating point + TYPE(MD_OutputType), INTENT(INOUT) :: y !< Output (change to inout if a mesh copy is required); Output fields are not used by this routine, but type is available here so that mesh parameter information (i.e., connectivity) does not have to be recalculated for dYdx. + TYPE(MD_MiscVarType), INTENT(INOUT) :: m !< Misc/optimization variables + INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message if ErrStat /= ErrID_None + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dYdz(:,:) !< Partial derivatives of output functions (Y) with respect to the constraint states (z) [intent in to avoid deallocation] + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dXdz(:,:) !< Partial derivatives of continuous state functions (X) with respect to the constraint states (z) [intent in to avoid deallocation] + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dXddz(:,:) !< Partial derivatives of discrete state functions (Xd) with respect to the constraint states (z) [intent in to avoid deallocation] + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dZdz(:,:) !< Partial derivatives of constraint state functions (Z) with respect to the constraint states (z) [intent in to avoid deallocation] + ! local variables + character(*), parameter :: RoutineName = 'MD_JacobianPConstrState' + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = '' + IF ( PRESENT( dYdz ) ) THEN + END IF + IF ( PRESENT( dXdz ) ) THEN + if (allocated(dXdz)) deallocate(dXdz) + END IF + IF ( PRESENT( dXddz ) ) THEN + if (allocated(dXddz)) deallocate(dXddz) + END IF + IF ( PRESENT(dZdz) ) THEN + END IF +END SUBROUTINE MD_JacobianPConstrState +!++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +!> Routine to pack the data structures representing the operating points into arrays for linearization. +SUBROUTINE MD_GetOP( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, u_op, y_op, x_op, dx_op, xd_op, z_op ) + REAL(DbKi), INTENT(IN ) :: t !< Time in seconds at operating point + TYPE(MD_InputType), INTENT(INOUT) :: u !< Inputs at operating point (may change to inout if a mesh copy is required) + TYPE(MD_ParameterType), INTENT(IN ) :: p !< Parameters + TYPE(MD_ContinuousStateType), INTENT(IN ) :: x !< Continuous states at operating point + TYPE(MD_DiscreteStateType), INTENT(IN ) :: xd !< Discrete states at operating point + TYPE(MD_ConstraintStateType), INTENT(IN ) :: z !< Constraint states at operating point + TYPE(MD_OtherStateType), INTENT(IN ) :: OtherState !< Other states at operating point + TYPE(MD_OutputType), INTENT(IN ) :: y !< Output at operating point + TYPE(MD_MiscVarType), INTENT(INOUT) :: m !< Misc/optimization variables + INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message if ErrStat /= ErrID_None + REAL(ReKi), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: u_op(:) !< values of linearized inputs + REAL(ReKi), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: y_op(:) !< values of linearized outputs + REAL(ReKi), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: x_op(:) !< values of linearized continuous states + REAL(ReKi), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dx_op(:) !< values of first time derivatives of linearized continuous states + REAL(ReKi), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: xd_op(:) !< values of linearized discrete states + REAL(ReKi), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: z_op(:) !< values of linearized constraint states + ! Local + INTEGER(IntKi) :: idx, i + INTEGER(IntKi) :: nu + INTEGER(IntKi) :: ny + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'MD_GetOP' + LOGICAL :: FieldMask(FIELDMASK_SIZE) + TYPE(MD_ContinuousStateType) :: dx ! derivative of continuous states at operating point + ErrStat = ErrID_None + ErrMsg = '' + ! inputs + IF ( PRESENT( u_op ) ) THEN + nu = size(p%Jac_u_indx,1) + u%CoupledKinematics%NNodes * 6 ! Jac_u_indx has 3 orientation angles, but the OP needs the full 9 elements of the DCM (thus 6 more per node) + if (.not. allocated(u_op)) then + call AllocAry(u_op, nu, 'u_op', ErrStat2, ErrMsg2); if(Failed()) return + end if + idx = 1 + FieldMask = .false. + FieldMask(MASKID_TranslationDisp) = .true. + FieldMask(MASKID_Orientation) = .true. + FieldMask(MASKID_TranslationVel) = .true. + FieldMask(MASKID_RotationVel) = .true. + FieldMask(MASKID_TranslationAcc) = .true. + FieldMask(MASKID_RotationAcc) = .true. + ! fill in the u_op values from the input mesh + call PackMotionMesh(u%CoupledKinematics, u_op, idx, FieldMask=FieldMask) + + ! now do the active tensioning commands if there are any + do i=1,p%nCtrlChans + u_op(idx) = u%DeltaL(i) + idx = idx + 1 + u_op(idx) = u%DeltaLdot(i) + idx = idx + 1 + end do + END IF + ! outputs + IF ( PRESENT( y_op ) ) THEN + ny = p%Jac_ny + y%CoupledLoads%NNodes * 6 ! Jac_ny has 3 orientation angles, but the OP needs the full 9 elements of the DCM (thus 6 more per node) + if (.not. allocated(y_op)) then + call AllocAry(y_op, ny, 'y_op', ErrStat2, ErrMsg2); if(Failed()) return + end if + idx = 1 + call PackLoadMesh(y%CoupledLoads, y_op, idx) + do i=1,p%NumOuts + y_op(idx) = y%WriteOutput(i) + idx = idx + 1 + end do + END IF + ! states + IF ( PRESENT( x_op ) ) THEN + if (.not. allocated(x_op)) then + call AllocAry(x_op, p%Jac_nx,'x_op',ErrStat2,ErrMsg2); if (Failed()) return + end if + do i=1, p%Jac_nx + x_op(i) = x%states(i) + end do + END IF + ! state derivatives? + IF ( PRESENT( dx_op ) ) THEN + if (.not. allocated(dx_op)) then + call AllocAry(dx_op, p%Jac_nx,'dx_op',ErrStat2,ErrMsg2); if(failed()) return + end if + call MD_CalcContStateDeriv( t, u, p, x, xd, z, OtherState, m, dx, ErrStat2, ErrMsg2 ) ; if(Failed()) return + do i=1, p%Jac_nx + dx_op(i) = dx%states(i) + end do + END IF + IF ( PRESENT( xd_op ) ) THEN + ! pass + END IF + IF ( PRESENT( z_op ) ) THEN + ! pass + END IF + call CleanUp() +contains + logical function Failed() + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Craig_Bampton') + Failed = ErrStat >= AbortErrLev + if (Failed) call CleanUp() + end function Failed + + subroutine CleanUp() + call MD_DestroyContState(dx, ErrStat2, ErrMsg2); + end subroutine +END SUBROUTINE MD_GetOP + + + + + ! :::::::::::::::::::::::::: below are some wave related functions ::::::::::::::::::::::::::::::: @@ -6840,15 +7249,11 @@ FUNCTION RotateM6(Min, rotMat) result(outMat) Real(DbKi), INTENT(IN) :: rotMat(3,3) ! rotation matrix (DCM) Real(DbKi) :: outMat(6,6) ! rotated matrix - Real(DbKi) :: tempM(3,3) - Real(DbKi) :: tempMrot(3,3) - ! the process for each of the following is to ! 1. copy out the relevant 3x3 matrix section, ! 2. rotate it, and ! 3. paste it into the output 6x6 matrix - ! mass matrix outMat(1:3,1:3) = rotateM3(Min(1:3,1:3), rotMat) @@ -7017,6 +7422,310 @@ SUBROUTINE LUsolve(n, A, LU, b, y, x) END SUBROUTINE LUsolve +!==================================================================================================== +!++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +!> This routine initializes the array that maps rows/columns of the Jacobian to specific mesh fields. +!! Do not change the order of this packing without changing subroutine ! +SUBROUTINE MD_Init_Jacobian(Init, p, u, y, m, InitOut, ErrStat, ErrMsg) + TYPE(MD_InitInputType) , INTENT(IN ) :: Init !< Init + TYPE(MD_ParameterType) , INTENT(INOUT) :: p !< parameters + TYPE(MD_InputType) , INTENT(IN ) :: u !< inputs + TYPE(MD_OutputType) , INTENT(IN ) :: y !< outputs + TYPE(MD_MiscVarType) , INTENT(INOUT) :: m !< misc variables <<<<<<<< + TYPE(MD_InitOutputType) , INTENT(INOUT) :: InitOut !< Initialization output data (for Jacobian row/column names) + INTEGER(IntKi) , INTENT( OUT) :: ErrStat !< Error status of the operation + CHARACTER(*) , INTENT( OUT) :: ErrMsg !< Error message if ErrStat /= ErrID_None + + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'SD_Init_Jacobian' + real(ReKi) :: dx, dy, dz, maxDim + + ! local variables: + ErrStat = ErrID_None + ErrMsg = "" + + !! --- System dimension + !dx = maxval(Init%Nodes(:,2))- minval(Init%Nodes(:,2)) + !dy = maxval(Init%Nodes(:,3))- minval(Init%Nodes(:,3)) + !dz = maxval(Init%Nodes(:,4))- minval(Init%Nodes(:,4)) + !maxDim = max(dx, dy, dz) + + ! --- System dimension + call Init_Jacobian_y(); if (Failed()) return + call Init_Jacobian_x(); if (Failed()) return + call Init_Jacobian_u(); if (Failed()) return + +contains + LOGICAL FUNCTION Failed() + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'SD_Init_Jacobian') + Failed = ErrStat >= AbortErrLev + END FUNCTION Failed + + !> This routine initializes the Jacobian parameters and initialization outputs for the linearized outputs. + SUBROUTINE Init_Jacobian_y() + INTEGER(IntKi) :: index_next, i + + ! Number of outputs + p%Jac_ny = y%CoupledLoads%nNodes * 6 & ! 3 forces + 3 moments at each node (moments may be zero) + + p%NumOuts ! WriteOutput values + ! Storage info for each output (names, rotframe) + call AllocAry(InitOut%LinNames_y, p%Jac_ny, 'LinNames_y',ErrStat2,ErrMsg2); if(ErrStat2/=ErrID_None) return + call AllocAry(InitOut%RotFrame_y, p%Jac_ny, 'RotFrame_y',ErrStat2,ErrMsg2); if(ErrStat2/=ErrID_None) return + ! Names + index_next = 1 + call PackLoadMesh_Names( y%CoupledLoads, 'LinNames_y', InitOut%LinNames_y, index_next) ! <<< should a specific name be provided here? + do i=1,p%NumOuts + InitOut%LinNames_y(i+index_next-1) = trim(InitOut%WriteOutputHdr(i))//', '//trim(InitOut%WriteOutputUnt(i)) + end do + + InitOut%RotFrame_y(:) = .false. + END SUBROUTINE Init_Jacobian_y + + !> This routine initializes the Jacobian parameters and initialization outputs for the linearized continuous states. + SUBROUTINE Init_Jacobian_x() + INTEGER(IntKi) :: i + INTEGER(IntKi) :: l + INTEGER(IntKi) :: N + + p%Jac_nx = m%Nx ! size of (continuous) state vector (includes the first derivatives) + + ! allocate space for the row/column names and for perturbation sizes + CALL AllocAry(InitOut%LinNames_x , p%Jac_nx, 'LinNames_x' , ErrStat2, ErrMsg2); if(ErrStat/=ErrID_None) return + CALL AllocAry(InitOut%RotFrame_x , p%Jac_nx, 'RotFrame_x' , ErrStat2, ErrMsg2); if(ErrStat/=ErrID_None) return + CALL AllocAry(InitOut%DerivOrder_x, p%Jac_nx, 'DerivOrder_x', ErrStat2, ErrMsg2); if(ErrStat/=ErrID_None) return + CALL AllocAry(p%dx, p%Jac_nx, 'p%dx' , ErrStat2, ErrMsg2); if(ErrStat/=ErrID_None) return + ! set linearization output names and default perturbations, p%dx: + + DO l = 1,p%nFreeBodies ! Body m%BodyList(m%FreeBodyIs(l)) + p%dx( m%BodyStateIs1(l) : m%BodyStateIs1(l)+2 ) = 0.1 ! body translational velocity [m/s] + p%dx( m%BodyStateIs1(l)+3 : m%BodyStateIs1(l)+5 ) = 0.1 ! body rotational velocity [rad/s] + p%dx( m%BodyStateIs1(l)+6 : m%BodyStateIs1(l)+8 ) = 0.2 ! body displacement [m] + p%dx( m%BodyStateIs1(l)+9 : m%BodyStateIs1(l)+11) = 0.02 ! body rotation [rad] + InitOut%LinNames_x(m%BodyStateIs1(l) ) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Vx, m/s' + InitOut%LinNames_x(m%BodyStateIs1(l)+1) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Vy, m/s' + InitOut%LinNames_x(m%BodyStateIs1(l)+2) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Vz, m/s' + InitOut%LinNames_x(m%BodyStateIs1(l)+3) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' omega_x, rad/s' + InitOut%LinNames_x(m%BodyStateIs1(l)+4) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' omega_y, rad/s' + InitOut%LinNames_x(m%BodyStateIs1(l)+5) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' omega_z, rad/s' + InitOut%LinNames_x(m%BodyStateIs1(l)+6) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Px, m' + InitOut%LinNames_x(m%BodyStateIs1(l)+7) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Py, m' + InitOut%LinNames_x(m%BodyStateIs1(l)+8) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Pz, m' + InitOut%LinNames_x(m%BodyStateIs1(l)+9) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' rot_x, rad' + InitOut%LinNames_x(m%BodyStateIs1(l)+10)= 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' rot_y, rad' + InitOut%LinNames_x(m%BodyStateIs1(l)+11)= 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' rot_z, rad' + END DO + DO l = 1,p%nFreeRods ! Rod m%RodList(m%FreeRodIs(l)) + if (m%RodList(m%FreeRodIs(l))%typeNum == 1) then ! pinned rod + p%dx( m%RodStateIs1(l) : m%RodStateIs1(l)+2 ) = 0.1 ! body rotational velocity [rad/s] + p%dx( m%RodStateIs1(l)+3 : m%RodStateIs1(l)+5 ) = 0.02 ! body rotation [rad] + InitOut%LinNames_x(m%RodStateIs1(l) ) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_x, rad/s' + InitOut%LinNames_x(m%RodStateIs1(l)+1) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_y, rad/s' + InitOut%LinNames_x(m%RodStateIs1(l)+2) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_z, rad/s' + InitOut%LinNames_x(m%RodStateIs1(l)+3) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_x, rad' + InitOut%LinNames_x(m%RodStateIs1(l)+4) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_y, rad' + InitOut%LinNames_x(m%RodStateIs1(l)+5) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_z, rad' + else ! free rod + p%dx( m%RodStateIs1(l) : m%RodStateIs1(l)+2 ) = 0.1 ! body translational velocity [m/s] + p%dx( m%RodStateIs1(l)+3 : m%RodStateIs1(l)+5 ) = 0.02 ! body rotational velocity [rad/s] + p%dx( m%RodStateIs1(l)+6 : m%RodStateIs1(l)+8 ) = 0.1 ! body displacement [m] + p%dx( m%RodStateIs1(l)+9 : m%RodStateIs1(l)+11) = 0.02 ! body rotation [rad] + InitOut%LinNames_x(m%RodStateIs1(l) ) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Vx, m/s' + InitOut%LinNames_x(m%RodStateIs1(l)+1) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Vy, m/s' + InitOut%LinNames_x(m%RodStateIs1(l)+2) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Vz, m/s' + InitOut%LinNames_x(m%RodStateIs1(l)+3) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_x, rad/s' + InitOut%LinNames_x(m%RodStateIs1(l)+4) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_y, rad/s' + InitOut%LinNames_x(m%RodStateIs1(l)+5) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_z, rad/s' + InitOut%LinNames_x(m%RodStateIs1(l)+6) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Px, m' + InitOut%LinNames_x(m%RodStateIs1(l)+7) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Py, m' + InitOut%LinNames_x(m%RodStateIs1(l)+8) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Pz, m' + InitOut%LinNames_x(m%RodStateIs1(l)+9) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_x, rad' + InitOut%LinNames_x(m%RodStateIs1(l)+10)= 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_y, rad' + InitOut%LinNames_x(m%RodStateIs1(l)+11)= 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_z, rad' + end if + END DO + DO l = 1,p%nFreeCons ! Point m%ConnectList(m%FreeConIs(l)) + p%dx( m%ConStateIs1(l) : m%ConStateIs1(l)+2 ) = 0.1 ! point translational velocity [m/s] + p%dx( m%ConStateIs1(l)+3 : m%ConStateIs1(l)+5 ) = 0.1 ! point displacement [m] + InitOut%LinNames_x(m%RodStateIs1(l) ) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Vx, m/s' + InitOut%LinNames_x(m%RodStateIs1(l)+1) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Vy, m/s' + InitOut%LinNames_x(m%RodStateIs1(l)+2) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Vz, m/s' + InitOut%LinNames_x(m%RodStateIs1(l)+3) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Px, m' + InitOut%LinNames_x(m%RodStateIs1(l)+4) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Py, m' + InitOut%LinNames_x(m%RodStateIs1(l)+5) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Pz, m' + END DO + DO l = 1,p%nLines ! Line m%LineList(l) + N = m%LineList(l)%N ! number of segments in the line + p%dx( m%LineStateIs1(l) : m%LineStateIs1(l)+3*N-4 ) = 0.1 ! line internal node translational velocity [m/s] + p%dx( m%LineStateIs1(l)+3*N-3 : m%LineStateIs1(l)+6*N-7 ) = 0.1 ! line internal node displacement [m] + DO i = 0,N-2 + InitOut%LinNames_x( m%LineStateIs1(l) +3*i ) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Vx, m/s' + InitOut%LinNames_x( m%LineStateIs1(l) +3*i+1) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Vy, m/s' + InitOut%LinNames_x( m%LineStateIs1(l) +3*i+2) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Vz, m/s' + InitOut%LinNames_x( m%LineStateIs1(l)+3*N +3*i-3) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Px, m' + InitOut%LinNames_x( m%LineStateIs1(l)+3*N +3*i-2) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Py, m' + InitOut%LinNames_x( m%LineStateIs1(l)+3*N +3*i-1) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Pz, m' + END DO + END DO + + InitOut%RotFrame_x = .false. + InitOut%DerivOrder_x = 2 + END SUBROUTINE Init_Jacobian_x + + SUBROUTINE Init_Jacobian_u() + REAL(R8Ki) :: perturb + INTEGER(IntKi) :: i, j, idx, nu, i_meshField + ! Number of inputs + nu = u%CoupledKinematics%nNodes * 18 & ! 3 Translation Displacements + 3 orientations + 6 velocities + 6 accelerations at each node <<<<<<< + + size(u%DeltaL)*2 ! a deltaL and rate of change for each active tension control channel + + ! --- Info of linearized inputs (Names, RotFrame, IsLoad) + call AllocAry(InitOut%LinNames_u, nu, 'LinNames_u', ErrStat2, ErrMsg2); if(ErrStat2/=ErrID_None) return + call AllocAry(InitOut%RotFrame_u, nu, 'RotFrame_u', ErrStat2, ErrMsg2); if(ErrStat2/=ErrID_None) return + call AllocAry(InitOut%IsLoad_u , nu, 'IsLoad_u' , ErrStat2, ErrMsg2); if(ErrStat2/=ErrID_None) return + + InitOut%IsLoad_u = .false. ! None of MoorDyn's inputs are loads + InitOut%RotFrame_u = .false. ! every input is on a mesh, which stores values in the global (not rotating) frame + + idx = 1 + call PackMotionMesh_Names(u%CoupledKinematics, 'CoupledKinematics', InitOut%LinNames_u, idx) ! all 6 motion fields + + ! --- Jac_u_indx: matrix to store index to help us figure out what the ith value of the u vector really means + ! (see perturb_u ... these MUST match ) + ! column 1 indicates module's mesh and field + ! column 2 indicates the first index (x-y-z component) of the field + ! column 3 is the node + call allocAry( p%Jac_u_indx, nu, 3, 'p%Jac_u_indx', ErrStat2, ErrMsg2); if(ErrStat2/=ErrID_None) return + idx = 1 + !Module/Mesh/Field: u%CoupledKinematics%TranslationDisp = 1; + !Module/Mesh/Field: u%CoupledKinematics%Orientation = 2; + !Module/Mesh/Field: u%CoupledKinematics%TranslationVel = 3; + !Module/Mesh/Field: u%CoupledKinematics%RotationVel = 4; + !Module/Mesh/Field: u%CoupledKinematics%TranslationAcc = 5; + !Module/Mesh/Field: u%CoupledKinematics%RotationAcc = 6; + do i_meshField = 1,6 + do i=1,u%CoupledKinematics%nNodes + do j=1,3 + p%Jac_u_indx(idx,1) = i_meshField ! mesh field type (indicated by 1-6) + p%Jac_u_indx(idx,2) = j ! x, y, or z + p%Jac_u_indx(idx,3) = i ! node + idx = idx + 1 + end do !j + end do !i + end do + ! now do the active tensioning commands if there are any + do i=1,p%nCtrlChans + p%Jac_u_indx(idx,1) = 10 ! 10-11 mean active tension changes (10: deltaL; 11: deltaLdot) + p%Jac_u_indx(idx,2) = 0 ! not used + p%Jac_u_indx(idx,3) = i ! indicates channel number + InitOut%LinNames_u(idx) = 'CtrlChan '//trim(num2lstr(i))//' DeltaL, m' + idx = idx + 1 + + p%Jac_u_indx(idx,1) = 11 + p%Jac_u_indx(idx,2) = 0 + p%Jac_u_indx(idx,3) = i + InitOut%LinNames_u(idx) = 'CtrlChan '//trim(num2lstr(i))//' DeltaLdot, m/s' + idx = idx + 1 + end do + + ! --- Default perturbations, p%du: + call allocAry( p%du, 11, 'p%du', ErrStat2, ErrMsg2); if(ErrStat2/=ErrID_None) return + perturb = 2.0_R8Ki*D2R_D + p%du( 1) = perturb ! u%CoupledKinematics%TranslationDisp = 1; + p%du( 2) = perturb ! u%CoupledKinematics%Orientation = 2; + p%du( 3) = perturb ! u%CoupledKinematics%TranslationVel = 3; + p%du( 4) = perturb ! u%CoupledKinematics%RotationVel = 4; + p%du( 5) = perturb ! u%CoupledKinematics%TranslationAcc = 5; + p%du( 6) = perturb ! u%CoupledKinematics%RotationAcc = 6; + p%du(10) = 0.2_ReKi ! deltaL [m] + p%du(11) = 0.2_ReKi ! deltaLdot [m/s] + END SUBROUTINE Init_Jacobian_u + +END SUBROUTINE MD_Init_Jacobian +!---------------------------------------------------------------------------------------------------------------------------------- +!> This routine perturbs the nth element of the u array (and mesh/field it corresponds to) +!! Do not change this without making sure subroutine MD_init_jacobian is consistant with this routine! +SUBROUTINE MD_Perturb_u( p, n, perturb_sign, u, du ) + TYPE(MD_ParameterType) , INTENT(IN ) :: p !< parameters + INTEGER( IntKi ) , INTENT(IN ) :: n !< number of array element to use + INTEGER( IntKi ) , INTENT(IN ) :: perturb_sign !< +1 or -1 (value to multiply perturbation by; positive or negative difference) + TYPE(MD_InputType) , INTENT(INOUT) :: u !< perturbed MD inputs + REAL( R8Ki ) , INTENT( OUT) :: du !< amount that specific input was perturbed + ! local variables + INTEGER :: fieldIndx + INTEGER :: node + fieldIndx = p%Jac_u_indx(n,2) + node = p%Jac_u_indx(n,3) + du = p%du( p%Jac_u_indx(n,1) ) + ! determine which mesh we're trying to perturb and perturb the input: + SELECT CASE( p%Jac_u_indx(n,1) ) + CASE ( 1) + u%CoupledKinematics%TranslationDisp( fieldIndx,node) = u%CoupledKinematics%TranslationDisp( fieldIndx,node) + du * perturb_sign + CASE ( 2) + CALL PerturbOrientationMatrix( u%CoupledKinematics%Orientation(:,:,node), du * perturb_sign, fieldIndx ) + CASE ( 3) + u%CoupledKinematics%TranslationVel( fieldIndx,node) = u%CoupledKinematics%TranslationVel( fieldIndx,node) + du * perturb_sign + CASE ( 4) + u%CoupledKinematics%RotationVel(fieldIndx,node) = u%CoupledKinematics%RotationVel(fieldIndx,node) + du * perturb_sign + CASE ( 5) + u%CoupledKinematics%TranslationAcc( fieldIndx,node) = u%CoupledKinematics%TranslationAcc( fieldIndx,node) + du * perturb_sign + CASE ( 6) + u%CoupledKinematics%RotationAcc(fieldIndx,node) = u%CoupledKinematics%RotationAcc(fieldIndx,node) + du * perturb_sign + CASE (10) + u%deltaL(node) = u%deltaL(node) + du * perturb_sign + CASE (11) + u%deltaLdot(node) = u%deltaLdot(node) + du * perturb_sign + END SELECT +END SUBROUTINE MD_Perturb_u +!---------------------------------------------------------------------------------------------------------------------------------- +!> This routine uses values of two output types to compute an array of differences. +!! Do not change this packing without making sure subroutine MD_init_jacobian is consistant with this routine! +SUBROUTINE MD_Compute_dY(p, y_p, y_m, delta, dY) + TYPE(MD_ParameterType) , INTENT(IN ) :: p !< parameters + TYPE(MD_OutputType) , INTENT(IN ) :: y_p !< MD outputs at \f$ u + \Delta_p u \f$ or \f$ z + \Delta_p z \f$ (p=plus) + TYPE(MD_OutputType) , INTENT(IN ) :: y_m !< MD outputs at \f$ u - \Delta_m u \f$ or \f$ z - \Delta_m z \f$ (m=minus) + REAL(R8Ki) , INTENT(IN ) :: delta !< difference in inputs or states \f$ delta_p = \Delta_p u \f$ or \f$ delta_p = \Delta_p x \f$ + REAL(R8Ki) , INTENT(INOUT) :: dY(:) !< column of dYdu or dYdx: \f$ \frac{\partial Y}{\partial u_i} = \frac{y_p - y_m}{2 \, \Delta u}\f$ or \f$ \frac{\partial Y}{\partial z_i} = \frac{y_p - y_m}{2 \, \Delta x}\f$ + ! local variables: + INTEGER(IntKi) :: i ! loop over outputs + INTEGER(IntKi) :: indx_first ! index indicating next value of dY to be filled + indx_first = 1 + call PackLoadMesh_dY( y_p%CoupledLoads, y_m%CoupledLoads, dY, indx_first) + !call PackMotionMesh_dY(y_p%Y2Mesh, y_m%Y2Mesh, dY, indx_first) ! all 6 motion fields + do i=1,p%NumOuts + dY(i+indx_first-1) = y_p%WriteOutput(i) - y_m%WriteOutput(i) + end do + dY = dY / (2.0_R8Ki*delta) +END SUBROUTINE MD_Compute_dY +!---------------------------------------------------------------------------------------------------------------------------------- +!> This routine perturbs the nth element of the x array (and mesh/field it corresponds to) +!! Do not change this without making sure subroutine MD_init_jacobian is consistant with this routine! +SUBROUTINE MD_Perturb_x( p, i, perturb_sign, x, dx ) + TYPE(MD_ParameterType) , INTENT(IN ) :: p !< parameters + INTEGER( IntKi ) , INTENT(IN ) :: i !< node number + INTEGER( IntKi ) , INTENT(IN ) :: perturb_sign !< +1 or -1 (value to multiply perturbation by; positive or negative difference) + TYPE(MD_ContinuousStateType), INTENT(INOUT) :: x !< perturbed SD states + REAL( R8Ki ) , INTENT( OUT) :: dx !< amount that specific state was perturbed + + dx=p%dx(i) + x%states(i) = x%states(i) + dx * perturb_sign +END SUBROUTINE MD_Perturb_x +!---------------------------------------------------------------------------------------------------------------------------------- +!> This routine uses values of two output types to compute an array of differences. +!! Do not change this packing without making sure subroutine MD_init_jacobian is consistant with this routine! +SUBROUTINE MD_Compute_dX(p, x_p, x_m, delta, dX) + TYPE(MD_ParameterType) , INTENT(IN ) :: p !< parameters + TYPE(MD_ContinuousStateType), INTENT(IN ) :: x_p !< SD continuous states at \f$ u + \Delta_p u \f$ or \f$ x + \Delta_p x \f$ (p=plus) + TYPE(MD_ContinuousStateType), INTENT(IN ) :: x_m !< SD continuous states at \f$ u - \Delta_m u \f$ or \f$ x - \Delta_m x \f$ (m=minus) + REAL(R8Ki) , INTENT(IN ) :: delta !< difference in inputs or states \f$ delta_p = \Delta_p u \f$ or \f$ delta_p = \Delta_p x \f$ + REAL(R8Ki) , INTENT(INOUT) :: dX(:) !< column of dXdu or dXdx: \f$ \frac{\partial X}{\partial u_i} = \frac{x_p - x_m}{2 \, \Delta u}\f$ or \f$ \frac{\partial X}{\partial x_i} = \frac{x_p - x_m}{2 \, \Delta x}\f$ + INTEGER(IntKi) :: i ! loop over modes + do i=1,p%Jac_nx + dX(i) = x_p%states(i) - x_m%states(i) + end do + dX = dX / (2.0_R8Ki*delta) +END SUBROUTINE MD_Compute_dX + END MODULE MoorDyn diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 6b3f3a616a..29afb011ad 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -31,6 +31,7 @@ typedef ^ ^ LOGICAL UsePrimaryInputFile typedef ^ ^ FileInfoType PassedPrimaryInputData - - - "Primary input file as FileInfoType (set by driver/glue code) -- String array with metadata" - typedef ^ ^ LOGICAL Echo - "" - "echo parameter - do we want to echo the header line describing the input file?" #typedef ^ ^ CHARACTER(ChanLen) OutList {:} "" - "string containing list of output channels requested in input file" +typedef ^ ^ Logical Linearize - .FALSE. - "Flag that tells this module if the glue code wants to linearize." - #typedef ^ ^ DbKi UGrid {:}{:}{:} - - "water velocities time series at each grid point" - #typedef ^ ^ DbKi UdGrid {:}{:}{:} - - "water accelerations time series at each grid point" - @@ -269,6 +270,15 @@ typedef ^ InitOutputType CHARACTER(ChanLen) writeOutputHdr {:} " typedef ^ ^ CHARACTER(ChanLen) writeOutputUnt {:} "" - "second line of output file contents: units" typedef ^ ^ ProgDesc Ver - "" - "this module's name, version, and date" typedef ^ ^ LOGICAL CableCChanRqst {:} .FALSE. - "flag indicating control channel for drive line active tensioning is requested" - +# --- InitOutputs for linearization --- +typedef ^ ^ CHARACTER(LinChanLen) LinNames_y {:} - - "Names of the outputs used in linearization" - +typedef ^ ^ CHARACTER(LinChanLen) LinNames_x {:} - - "Names of the continuous states used in linearization" - +typedef ^ ^ CHARACTER(LinChanLen) LinNames_u {:} - - "Names of the inputs used in linearization" - +typedef ^ ^ LOGICAL RotFrame_y {:} - - "Flag that tells FAST/MBC3 if the outputs used in linearization are in the rotating frame" - +typedef ^ ^ LOGICAL RotFrame_x {:} - - "Flag that tells FAST/MBC3 if the continuous states used in linearization are in the rotating frame (not used for glue)" - +typedef ^ ^ LOGICAL RotFrame_u {:} - - "Flag that tells FAST/MBC3 if the inputs used in linearization are in the rotating frame" - +typedef ^ ^ LOGICAL IsLoad_u {:} - - "Flag that tells FAST if the inputs used in linearization are loads (for preconditioning matrix)" - +typedef ^ ^ IntKi DerivOrder_x {:} - - "Integer that tells FAST/MBC3 the maximum derivative order of continuous states used in linearization" - ## ============================== Define Continuous states here: ===================================================================================================================================== @@ -363,6 +373,13 @@ typedef ^ ^ DbKi px {:} typedef ^ ^ DbKi py {:} - - "" - typedef ^ ^ DbKi pz {:} - - "" - typedef ^ ^ DbKi tWave {:} - - "wave data time step array" - +# --- Parameters for linearization --- +typedef ^ ^ Integer Nx0 - - - "copy of initial size of system state vector, for linearization routines" - +typedef ^ ^ Integer Jac_u_indx {:}{:} - - "matrix to help fill/pack the u vector in computing the jacobian" - +typedef ^ ^ R8Ki du {:} - - "vector that determines size of perturbation for u (inputs)" +typedef ^ ^ R8Ki dx {:} - - "vector that determines size of perturbation for x (continuous states)" +typedef ^ ^ Integer Jac_ny - - - "number of outputs in jacobian matrix" - +typedef ^ ^ Integer Jac_nx - - - "number of continuous states in jacobian matrix" - # ============================== Inputs ============================================================================================================================================ diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index 7f9da5c97f..26e1b6dd14 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -52,6 +52,7 @@ MODULE MoorDyn_Types LOGICAL :: UsePrimaryInputFile = .TRUE. !< Read input file instead of passed data [-] TYPE(FileInfoType) :: PassedPrimaryInputData !< Primary input file as FileInfoType (set by driver/glue code) -- String array with metadata [-] LOGICAL :: Echo !< echo parameter - do we want to echo the header line describing the input file? [-] + LOGICAL :: Linearize = .FALSE. !< Flag that tells this module if the glue code wants to linearize. [-] REAL(ReKi) , DIMENSION(:,:,:), ALLOCATABLE :: WaveVel !< [-] REAL(ReKi) , DIMENSION(:,:,:), ALLOCATABLE :: WaveAcc !< [-] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: WavePDyn !< [-] @@ -289,6 +290,14 @@ MODULE MoorDyn_Types CHARACTER(ChanLen) , DIMENSION(:), ALLOCATABLE :: writeOutputUnt !< second line of output file contents: units [-] TYPE(ProgDesc) :: Ver !< this module's name, version, and date [-] LOGICAL , DIMENSION(:), ALLOCATABLE :: CableCChanRqst !< flag indicating control channel for drive line active tensioning is requested [-] + CHARACTER(LinChanLen) , DIMENSION(:), ALLOCATABLE :: LinNames_y !< Names of the outputs used in linearization [-] + CHARACTER(LinChanLen) , DIMENSION(:), ALLOCATABLE :: LinNames_x !< Names of the continuous states used in linearization [-] + CHARACTER(LinChanLen) , DIMENSION(:), ALLOCATABLE :: LinNames_u !< Names of the inputs used in linearization [-] + LOGICAL , DIMENSION(:), ALLOCATABLE :: RotFrame_y !< Flag that tells FAST/MBC3 if the outputs used in linearization are in the rotating frame [-] + LOGICAL , DIMENSION(:), ALLOCATABLE :: RotFrame_x !< Flag that tells FAST/MBC3 if the continuous states used in linearization are in the rotating frame (not used for glue) [-] + LOGICAL , DIMENSION(:), ALLOCATABLE :: RotFrame_u !< Flag that tells FAST/MBC3 if the inputs used in linearization are in the rotating frame [-] + LOGICAL , DIMENSION(:), ALLOCATABLE :: IsLoad_u !< Flag that tells FAST if the inputs used in linearization are loads (for preconditioning matrix) [-] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: DerivOrder_x !< Integer that tells FAST/MBC3 the maximum derivative order of continuous states used in linearization [-] END TYPE MD_InitOutputType ! ======================= ! ========= MD_ContinuousStateType ======= @@ -388,6 +397,12 @@ MODULE MoorDyn_Types REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: py !< [-] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: pz !< [-] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: tWave !< wave data time step array [-] + INTEGER(IntKi) :: Nx0 !< copy of initial size of system state vector, for linearization routines [-] + INTEGER(IntKi) , DIMENSION(:,:), ALLOCATABLE :: Jac_u_indx !< matrix to help fill/pack the u vector in computing the jacobian [-] + REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: du !< vector that determines size of perturbation for u (inputs) [-] + REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: dx !< vector that determines size of perturbation for x (continuous states) [-] + INTEGER(IntKi) :: Jac_ny !< number of outputs in jacobian matrix [-] + INTEGER(IntKi) :: Jac_nx !< number of continuous states in jacobian matrix [-] END TYPE MD_ParameterType ! ======================= ! ========= MD_InputType ======= @@ -594,6 +609,7 @@ SUBROUTINE MD_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, ErrSt CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) IF (ErrStat>=AbortErrLev) RETURN DstInitInputData%Echo = SrcInitInputData%Echo + DstInitInputData%Linearize = SrcInitInputData%Linearize IF (ALLOCATED(SrcInitInputData%WaveVel)) THEN i1_l = LBOUND(SrcInitInputData%WaveVel,1) i1_u = UBOUND(SrcInitInputData%WaveVel,1) @@ -763,6 +779,7 @@ SUBROUTINE MD_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg DEALLOCATE(Int_Buf) END IF Int_BufSz = Int_BufSz + 1 ! Echo + Int_BufSz = Int_BufSz + 1 ! Linearize Int_BufSz = Int_BufSz + 1 ! WaveVel allocated yes/no IF ( ALLOCATED(InData%WaveVel) ) THEN Int_BufSz = Int_BufSz + 2*3 ! WaveVel upper/lower bounds for each dimension @@ -876,6 +893,8 @@ SUBROUTINE MD_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg ENDIF IntKiBuf(Int_Xferred) = TRANSFER(InData%Echo, IntKiBuf(1)) Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = TRANSFER(InData%Linearize, IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 IF ( .NOT. ALLOCATED(InData%WaveVel) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -1088,6 +1107,8 @@ SUBROUTINE MD_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Err IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) OutData%Echo = TRANSFER(IntKiBuf(Int_Xferred), OutData%Echo) Int_Xferred = Int_Xferred + 1 + OutData%Linearize = TRANSFER(IntKiBuf(Int_Xferred), OutData%Linearize) + Int_Xferred = Int_Xferred + 1 IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WaveVel not allocated Int_Xferred = Int_Xferred + 1 ELSE @@ -6225,6 +6246,102 @@ SUBROUTINE MD_CopyInitOutput( SrcInitOutputData, DstInitOutputData, CtrlCode, Er END IF END IF DstInitOutputData%CableCChanRqst = SrcInitOutputData%CableCChanRqst +ENDIF +IF (ALLOCATED(SrcInitOutputData%LinNames_y)) THEN + i1_l = LBOUND(SrcInitOutputData%LinNames_y,1) + i1_u = UBOUND(SrcInitOutputData%LinNames_y,1) + IF (.NOT. ALLOCATED(DstInitOutputData%LinNames_y)) THEN + ALLOCATE(DstInitOutputData%LinNames_y(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitOutputData%LinNames_y.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitOutputData%LinNames_y = SrcInitOutputData%LinNames_y +ENDIF +IF (ALLOCATED(SrcInitOutputData%LinNames_x)) THEN + i1_l = LBOUND(SrcInitOutputData%LinNames_x,1) + i1_u = UBOUND(SrcInitOutputData%LinNames_x,1) + IF (.NOT. ALLOCATED(DstInitOutputData%LinNames_x)) THEN + ALLOCATE(DstInitOutputData%LinNames_x(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitOutputData%LinNames_x.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitOutputData%LinNames_x = SrcInitOutputData%LinNames_x +ENDIF +IF (ALLOCATED(SrcInitOutputData%LinNames_u)) THEN + i1_l = LBOUND(SrcInitOutputData%LinNames_u,1) + i1_u = UBOUND(SrcInitOutputData%LinNames_u,1) + IF (.NOT. ALLOCATED(DstInitOutputData%LinNames_u)) THEN + ALLOCATE(DstInitOutputData%LinNames_u(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitOutputData%LinNames_u.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitOutputData%LinNames_u = SrcInitOutputData%LinNames_u +ENDIF +IF (ALLOCATED(SrcInitOutputData%RotFrame_y)) THEN + i1_l = LBOUND(SrcInitOutputData%RotFrame_y,1) + i1_u = UBOUND(SrcInitOutputData%RotFrame_y,1) + IF (.NOT. ALLOCATED(DstInitOutputData%RotFrame_y)) THEN + ALLOCATE(DstInitOutputData%RotFrame_y(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitOutputData%RotFrame_y.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitOutputData%RotFrame_y = SrcInitOutputData%RotFrame_y +ENDIF +IF (ALLOCATED(SrcInitOutputData%RotFrame_x)) THEN + i1_l = LBOUND(SrcInitOutputData%RotFrame_x,1) + i1_u = UBOUND(SrcInitOutputData%RotFrame_x,1) + IF (.NOT. ALLOCATED(DstInitOutputData%RotFrame_x)) THEN + ALLOCATE(DstInitOutputData%RotFrame_x(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitOutputData%RotFrame_x.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitOutputData%RotFrame_x = SrcInitOutputData%RotFrame_x +ENDIF +IF (ALLOCATED(SrcInitOutputData%RotFrame_u)) THEN + i1_l = LBOUND(SrcInitOutputData%RotFrame_u,1) + i1_u = UBOUND(SrcInitOutputData%RotFrame_u,1) + IF (.NOT. ALLOCATED(DstInitOutputData%RotFrame_u)) THEN + ALLOCATE(DstInitOutputData%RotFrame_u(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitOutputData%RotFrame_u.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitOutputData%RotFrame_u = SrcInitOutputData%RotFrame_u +ENDIF +IF (ALLOCATED(SrcInitOutputData%IsLoad_u)) THEN + i1_l = LBOUND(SrcInitOutputData%IsLoad_u,1) + i1_u = UBOUND(SrcInitOutputData%IsLoad_u,1) + IF (.NOT. ALLOCATED(DstInitOutputData%IsLoad_u)) THEN + ALLOCATE(DstInitOutputData%IsLoad_u(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitOutputData%IsLoad_u.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitOutputData%IsLoad_u = SrcInitOutputData%IsLoad_u +ENDIF +IF (ALLOCATED(SrcInitOutputData%DerivOrder_x)) THEN + i1_l = LBOUND(SrcInitOutputData%DerivOrder_x,1) + i1_u = UBOUND(SrcInitOutputData%DerivOrder_x,1) + IF (.NOT. ALLOCATED(DstInitOutputData%DerivOrder_x)) THEN + ALLOCATE(DstInitOutputData%DerivOrder_x(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitOutputData%DerivOrder_x.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitOutputData%DerivOrder_x = SrcInitOutputData%DerivOrder_x ENDIF END SUBROUTINE MD_CopyInitOutput @@ -6246,6 +6363,30 @@ SUBROUTINE MD_DestroyInitOutput( InitOutputData, ErrStat, ErrMsg ) CALL NWTC_Library_Destroyprogdesc( InitOutputData%Ver, ErrStat, ErrMsg ) IF (ALLOCATED(InitOutputData%CableCChanRqst)) THEN DEALLOCATE(InitOutputData%CableCChanRqst) +ENDIF +IF (ALLOCATED(InitOutputData%LinNames_y)) THEN + DEALLOCATE(InitOutputData%LinNames_y) +ENDIF +IF (ALLOCATED(InitOutputData%LinNames_x)) THEN + DEALLOCATE(InitOutputData%LinNames_x) +ENDIF +IF (ALLOCATED(InitOutputData%LinNames_u)) THEN + DEALLOCATE(InitOutputData%LinNames_u) +ENDIF +IF (ALLOCATED(InitOutputData%RotFrame_y)) THEN + DEALLOCATE(InitOutputData%RotFrame_y) +ENDIF +IF (ALLOCATED(InitOutputData%RotFrame_x)) THEN + DEALLOCATE(InitOutputData%RotFrame_x) +ENDIF +IF (ALLOCATED(InitOutputData%RotFrame_u)) THEN + DEALLOCATE(InitOutputData%RotFrame_u) +ENDIF +IF (ALLOCATED(InitOutputData%IsLoad_u)) THEN + DEALLOCATE(InitOutputData%IsLoad_u) +ENDIF +IF (ALLOCATED(InitOutputData%DerivOrder_x)) THEN + DEALLOCATE(InitOutputData%DerivOrder_x) ENDIF END SUBROUTINE MD_DestroyInitOutput @@ -6317,6 +6458,46 @@ SUBROUTINE MD_PackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMs Int_BufSz = Int_BufSz + 2*1 ! CableCChanRqst upper/lower bounds for each dimension Int_BufSz = Int_BufSz + SIZE(InData%CableCChanRqst) ! CableCChanRqst END IF + Int_BufSz = Int_BufSz + 1 ! LinNames_y allocated yes/no + IF ( ALLOCATED(InData%LinNames_y) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! LinNames_y upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%LinNames_y)*LEN(InData%LinNames_y) ! LinNames_y + END IF + Int_BufSz = Int_BufSz + 1 ! LinNames_x allocated yes/no + IF ( ALLOCATED(InData%LinNames_x) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! LinNames_x upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%LinNames_x)*LEN(InData%LinNames_x) ! LinNames_x + END IF + Int_BufSz = Int_BufSz + 1 ! LinNames_u allocated yes/no + IF ( ALLOCATED(InData%LinNames_u) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! LinNames_u upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%LinNames_u)*LEN(InData%LinNames_u) ! LinNames_u + END IF + Int_BufSz = Int_BufSz + 1 ! RotFrame_y allocated yes/no + IF ( ALLOCATED(InData%RotFrame_y) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! RotFrame_y upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%RotFrame_y) ! RotFrame_y + END IF + Int_BufSz = Int_BufSz + 1 ! RotFrame_x allocated yes/no + IF ( ALLOCATED(InData%RotFrame_x) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! RotFrame_x upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%RotFrame_x) ! RotFrame_x + END IF + Int_BufSz = Int_BufSz + 1 ! RotFrame_u allocated yes/no + IF ( ALLOCATED(InData%RotFrame_u) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! RotFrame_u upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%RotFrame_u) ! RotFrame_u + END IF + Int_BufSz = Int_BufSz + 1 ! IsLoad_u allocated yes/no + IF ( ALLOCATED(InData%IsLoad_u) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! IsLoad_u upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%IsLoad_u) ! IsLoad_u + END IF + Int_BufSz = Int_BufSz + 1 ! DerivOrder_x allocated yes/no + IF ( ALLOCATED(InData%DerivOrder_x) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! DerivOrder_x upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%DerivOrder_x) ! DerivOrder_x + END IF IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) IF (ErrStat2 /= 0) THEN @@ -6421,6 +6602,132 @@ SUBROUTINE MD_PackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMs Int_Xferred = Int_Xferred + 1 END DO END IF + IF ( .NOT. ALLOCATED(InData%LinNames_y) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%LinNames_y,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%LinNames_y,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%LinNames_y,1), UBOUND(InData%LinNames_y,1) + DO I = 1, LEN(InData%LinNames_y) + IntKiBuf(Int_Xferred) = ICHAR(InData%LinNames_y(i1)(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + END DO + END IF + IF ( .NOT. ALLOCATED(InData%LinNames_x) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%LinNames_x,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%LinNames_x,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%LinNames_x,1), UBOUND(InData%LinNames_x,1) + DO I = 1, LEN(InData%LinNames_x) + IntKiBuf(Int_Xferred) = ICHAR(InData%LinNames_x(i1)(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + END DO + END IF + IF ( .NOT. ALLOCATED(InData%LinNames_u) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%LinNames_u,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%LinNames_u,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%LinNames_u,1), UBOUND(InData%LinNames_u,1) + DO I = 1, LEN(InData%LinNames_u) + IntKiBuf(Int_Xferred) = ICHAR(InData%LinNames_u(i1)(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I + END DO + END IF + IF ( .NOT. ALLOCATED(InData%RotFrame_y) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%RotFrame_y,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%RotFrame_y,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%RotFrame_y,1), UBOUND(InData%RotFrame_y,1) + IntKiBuf(Int_Xferred) = TRANSFER(InData%RotFrame_y(i1), IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%RotFrame_x) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%RotFrame_x,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%RotFrame_x,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%RotFrame_x,1), UBOUND(InData%RotFrame_x,1) + IntKiBuf(Int_Xferred) = TRANSFER(InData%RotFrame_x(i1), IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%RotFrame_u) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%RotFrame_u,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%RotFrame_u,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%RotFrame_u,1), UBOUND(InData%RotFrame_u,1) + IntKiBuf(Int_Xferred) = TRANSFER(InData%RotFrame_u(i1), IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%IsLoad_u) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%IsLoad_u,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%IsLoad_u,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%IsLoad_u,1), UBOUND(InData%IsLoad_u,1) + IntKiBuf(Int_Xferred) = TRANSFER(InData%IsLoad_u(i1), IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%DerivOrder_x) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%DerivOrder_x,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%DerivOrder_x,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%DerivOrder_x,1), UBOUND(InData%DerivOrder_x,1) + IntKiBuf(Int_Xferred) = InData%DerivOrder_x(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF END SUBROUTINE MD_PackInitOutput SUBROUTINE MD_UnPackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) @@ -6548,6 +6855,156 @@ SUBROUTINE MD_UnPackInitOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Er Int_Xferred = Int_Xferred + 1 END DO END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! LinNames_y not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%LinNames_y)) DEALLOCATE(OutData%LinNames_y) + ALLOCATE(OutData%LinNames_y(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%LinNames_y.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%LinNames_y,1), UBOUND(OutData%LinNames_y,1) + DO I = 1, LEN(OutData%LinNames_y) + OutData%LinNames_y(i1)(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! LinNames_x not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%LinNames_x)) DEALLOCATE(OutData%LinNames_x) + ALLOCATE(OutData%LinNames_x(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%LinNames_x.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%LinNames_x,1), UBOUND(OutData%LinNames_x,1) + DO I = 1, LEN(OutData%LinNames_x) + OutData%LinNames_x(i1)(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! LinNames_u not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%LinNames_u)) DEALLOCATE(OutData%LinNames_u) + ALLOCATE(OutData%LinNames_u(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%LinNames_u.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%LinNames_u,1), UBOUND(OutData%LinNames_u,1) + DO I = 1, LEN(OutData%LinNames_u) + OutData%LinNames_u(i1)(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! RotFrame_y not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%RotFrame_y)) DEALLOCATE(OutData%RotFrame_y) + ALLOCATE(OutData%RotFrame_y(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%RotFrame_y.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%RotFrame_y,1), UBOUND(OutData%RotFrame_y,1) + OutData%RotFrame_y(i1) = TRANSFER(IntKiBuf(Int_Xferred), OutData%RotFrame_y(i1)) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! RotFrame_x not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%RotFrame_x)) DEALLOCATE(OutData%RotFrame_x) + ALLOCATE(OutData%RotFrame_x(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%RotFrame_x.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%RotFrame_x,1), UBOUND(OutData%RotFrame_x,1) + OutData%RotFrame_x(i1) = TRANSFER(IntKiBuf(Int_Xferred), OutData%RotFrame_x(i1)) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! RotFrame_u not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%RotFrame_u)) DEALLOCATE(OutData%RotFrame_u) + ALLOCATE(OutData%RotFrame_u(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%RotFrame_u.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%RotFrame_u,1), UBOUND(OutData%RotFrame_u,1) + OutData%RotFrame_u(i1) = TRANSFER(IntKiBuf(Int_Xferred), OutData%RotFrame_u(i1)) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! IsLoad_u not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%IsLoad_u)) DEALLOCATE(OutData%IsLoad_u) + ALLOCATE(OutData%IsLoad_u(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%IsLoad_u.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%IsLoad_u,1), UBOUND(OutData%IsLoad_u,1) + OutData%IsLoad_u(i1) = TRANSFER(IntKiBuf(Int_Xferred), OutData%IsLoad_u(i1)) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! DerivOrder_x not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%DerivOrder_x)) DEALLOCATE(OutData%DerivOrder_x) + ALLOCATE(OutData%DerivOrder_x(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%DerivOrder_x.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%DerivOrder_x,1), UBOUND(OutData%DerivOrder_x,1) + OutData%DerivOrder_x(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF END SUBROUTINE MD_UnPackInitOutput SUBROUTINE MD_CopyContState( SrcContStateData, DstContStateData, CtrlCode, ErrStat, ErrMsg ) @@ -9555,6 +10012,47 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) END IF DstParamData%tWave = SrcParamData%tWave ENDIF + DstParamData%Nx0 = SrcParamData%Nx0 +IF (ALLOCATED(SrcParamData%Jac_u_indx)) THEN + i1_l = LBOUND(SrcParamData%Jac_u_indx,1) + i1_u = UBOUND(SrcParamData%Jac_u_indx,1) + i2_l = LBOUND(SrcParamData%Jac_u_indx,2) + i2_u = UBOUND(SrcParamData%Jac_u_indx,2) + IF (.NOT. ALLOCATED(DstParamData%Jac_u_indx)) THEN + ALLOCATE(DstParamData%Jac_u_indx(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%Jac_u_indx.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%Jac_u_indx = SrcParamData%Jac_u_indx +ENDIF +IF (ALLOCATED(SrcParamData%du)) THEN + i1_l = LBOUND(SrcParamData%du,1) + i1_u = UBOUND(SrcParamData%du,1) + IF (.NOT. ALLOCATED(DstParamData%du)) THEN + ALLOCATE(DstParamData%du(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%du.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%du = SrcParamData%du +ENDIF +IF (ALLOCATED(SrcParamData%dx)) THEN + i1_l = LBOUND(SrcParamData%dx,1) + i1_u = UBOUND(SrcParamData%dx,1) + IF (.NOT. ALLOCATED(DstParamData%dx)) THEN + ALLOCATE(DstParamData%dx(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%dx.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%dx = SrcParamData%dx +ENDIF + DstParamData%Jac_ny = SrcParamData%Jac_ny + DstParamData%Jac_nx = SrcParamData%Jac_nx END SUBROUTINE MD_CopyParam SUBROUTINE MD_DestroyParam( ParamData, ErrStat, ErrMsg ) @@ -9607,6 +10105,15 @@ SUBROUTINE MD_DestroyParam( ParamData, ErrStat, ErrMsg ) ENDIF IF (ALLOCATED(ParamData%tWave)) THEN DEALLOCATE(ParamData%tWave) +ENDIF +IF (ALLOCATED(ParamData%Jac_u_indx)) THEN + DEALLOCATE(ParamData%Jac_u_indx) +ENDIF +IF (ALLOCATED(ParamData%du)) THEN + DEALLOCATE(ParamData%du) +ENDIF +IF (ALLOCATED(ParamData%dx)) THEN + DEALLOCATE(ParamData%dx) ENDIF END SUBROUTINE MD_DestroyParam @@ -9759,6 +10266,24 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Int_BufSz = Int_BufSz + 2*1 ! tWave upper/lower bounds for each dimension Db_BufSz = Db_BufSz + SIZE(InData%tWave) ! tWave END IF + Int_BufSz = Int_BufSz + 1 ! Nx0 + Int_BufSz = Int_BufSz + 1 ! Jac_u_indx allocated yes/no + IF ( ALLOCATED(InData%Jac_u_indx) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Jac_u_indx upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%Jac_u_indx) ! Jac_u_indx + END IF + Int_BufSz = Int_BufSz + 1 ! du allocated yes/no + IF ( ALLOCATED(InData%du) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! du upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%du) ! du + END IF + Int_BufSz = Int_BufSz + 1 ! dx allocated yes/no + IF ( ALLOCATED(InData%dx) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! dx upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%dx) ! dx + END IF + Int_BufSz = Int_BufSz + 1 ! Jac_ny + Int_BufSz = Int_BufSz + 1 ! Jac_nx IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) IF (ErrStat2 /= 0) THEN @@ -10186,6 +10711,62 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Db_Xferred = Db_Xferred + 1 END DO END IF + IntKiBuf(Int_Xferred) = InData%Nx0 + Int_Xferred = Int_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%Jac_u_indx) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Jac_u_indx,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Jac_u_indx,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Jac_u_indx,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Jac_u_indx,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Jac_u_indx,2), UBOUND(InData%Jac_u_indx,2) + DO i1 = LBOUND(InData%Jac_u_indx,1), UBOUND(InData%Jac_u_indx,1) + IntKiBuf(Int_Xferred) = InData%Jac_u_indx(i1,i2) + Int_Xferred = Int_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%du) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%du,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%du,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%du,1), UBOUND(InData%du,1) + DbKiBuf(Db_Xferred) = InData%du(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%dx) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%dx,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%dx,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%dx,1), UBOUND(InData%dx,1) + DbKiBuf(Db_Xferred) = InData%dx(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IntKiBuf(Int_Xferred) = InData%Jac_ny + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%Jac_nx + Int_Xferred = Int_Xferred + 1 END SUBROUTINE MD_PackParam SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) @@ -10669,6 +11250,71 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg Db_Xferred = Db_Xferred + 1 END DO END IF + OutData%Nx0 = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Jac_u_indx not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Jac_u_indx)) DEALLOCATE(OutData%Jac_u_indx) + ALLOCATE(OutData%Jac_u_indx(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Jac_u_indx.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Jac_u_indx,2), UBOUND(OutData%Jac_u_indx,2) + DO i1 = LBOUND(OutData%Jac_u_indx,1), UBOUND(OutData%Jac_u_indx,1) + OutData%Jac_u_indx(i1,i2) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! du not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%du)) DEALLOCATE(OutData%du) + ALLOCATE(OutData%du(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%du.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%du,1), UBOUND(OutData%du,1) + OutData%du(i1) = REAL(DbKiBuf(Db_Xferred), R8Ki) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! dx not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%dx)) DEALLOCATE(OutData%dx) + ALLOCATE(OutData%dx(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%dx.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%dx,1), UBOUND(OutData%dx,1) + OutData%dx(i1) = REAL(DbKiBuf(Db_Xferred), R8Ki) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + OutData%Jac_ny = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%Jac_nx = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 END SUBROUTINE MD_UnPackParam SUBROUTINE MD_CopyInput( SrcInputData, DstInputData, CtrlCode, ErrStat, ErrMsg ) diff --git a/modules/openfast-library/src/FAST_Lin.f90 b/modules/openfast-library/src/FAST_Lin.f90 index 50b0cc42b6..e6e92668f6 100644 --- a/modules/openfast-library/src/FAST_Lin.f90 +++ b/modules/openfast-library/src/FAST_Lin.f90 @@ -118,6 +118,9 @@ SUBROUTINE Init_Lin(p_FAST, y_FAST, m_FAST, AD, ED, NumBl, NumBlNodes, ErrStat, if ( p_FAST%CompMooring == Module_MAP ) then p_FAST%Lin_NumMods = p_FAST%Lin_NumMods + 1 p_FAST%Lin_ModOrder( p_FAST%Lin_NumMods ) = Module_MAP + else if ( p_FAST%CompMooring == Module_MD ) then + p_FAST%Lin_NumMods = p_FAST%Lin_NumMods + 1 + p_FAST%Lin_ModOrder( p_FAST%Lin_NumMods ) = Module_MD end if @@ -1092,6 +1095,51 @@ SUBROUTINE FAST_Linearize_OP(t_global, p_FAST, y_FAST, m_FAST, ED, BD, SrvD, AD, end if ! if ( p_FAST%LinOutMod ) end if ! if ( p_FAST%CompMooring == Module_MAP ) + + !..................... + ! MoorDyn + !..................... + if ( p_FAST%CompMooring == Module_MD ) then + + call MD_JacobianPInput( t_global, MD%Input(1), MD%p, MD%x(STATE_CURR), MD%xd(STATE_CURR), MD%z(STATE_CURR), & + MD%OtherSt(STATE_CURR), MD%y, MD%m, ErrStat2, ErrMsg2, y_FAST%Lin%Modules(Module_MD)%Instance(1)%D ) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + + ! get the operating point + call MD_GetOP( t_global, MD%Input(1), MD%p, MD%x(STATE_CURR), MD%xd(STATE_CURR), MD%z(STATE_CURR), & + MD%OtherSt(STATE_CURR), MD%y, MD%m, ErrStat2, ErrMsg2, & + y_FAST%Lin%Modules(Module_MD)%Instance(1)%op_u, y_FAST%Lin%Modules(Module_MD)%Instance(1)%op_y ) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + if (ErrStat >=AbortErrLev) then + call cleanup() + return + end if + + ! write the module matrices: + if (p_FAST%LinOutMod) then + + OutFileName = trim(LinRootName)//'.'//TRIM(y_FAST%Module_Abrev(Module_MD)) + call WrLinFile_txt_Head(t_global, p_FAST, y_FAST, y_FAST%Lin%Modules(Module_MD)%Instance(1), OutFileName, Un, ErrStat2, ErrMsg2 ) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + if (ErrStat >=AbortErrLev) then + call cleanup() + return + end if + + if (p_FAST%LinOutJac) then + ! Jacobians + !dYdu: + call WrPartialMatrix( y_FAST%Lin%Modules(Module_MD)%Instance(1)%D, Un, p_FAST%OutFmt, 'dYdu', & + UseRow=y_FAST%Lin%Modules(Module_MD)%Instance(1)%use_y, & + UseCol=y_FAST%Lin%Modules(Module_MD)%Instance(1)%use_u ) + end if + + ! finish writing the file + call WrLinFile_txt_End(Un, p_FAST, y_FAST%Lin%Modules(Module_MD)%Instance(1) ) + + end if ! if ( p_FAST%LinOutMod ) + end if ! if ( p_FAST%CompMooring == Module_MD ) + !..................... ! Linearization of glue code Input/Output solve: !..................... @@ -1643,7 +1691,7 @@ SUBROUTINE Glue_Jacobians( p_FAST, y_FAST, m_FAST, ED, BD, SrvD, AD, IfW, OpFM, !............ ! we need to do this for CompElast=ED and CompElast=BD - call Linear_ED_InputSolve_du( p_FAST, y_FAST, ED%Input(1), ED%y, AD%y, AD%Input(1), BD, HD, SD, MAPp, MeshMapData, dUdu, ErrStat2, ErrMsg2 ) + call Linear_ED_InputSolve_du( p_FAST, y_FAST, ED%Input(1), ED%y, AD%y, AD%Input(1), BD, HD, SD, MAPp, MD, MeshMapData, dUdu, ErrStat2, ErrMsg2 ) call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) !............ @@ -1679,7 +1727,7 @@ SUBROUTINE Glue_Jacobians( p_FAST, y_FAST, m_FAST, ED, BD, SrvD, AD, IfW, OpFM, ! \f$ \frac{\partial U_\Lambda^{SD}}{\partial u^{MAP}} \end{bmatrix} = \f$ (dUdu block row 7=SD) !............ IF (p_FAST%CompSub == MODULE_SD) THEN - call Linear_SD_InputSolve_du( p_FAST, y_FAST, SD%Input(1), SD%y, ED%y, HD, MAPp, MeshMapData, dUdu, ErrStat2, ErrMsg2 ) + call Linear_SD_InputSolve_du( p_FAST, y_FAST, SD%Input(1), SD%y, ED%y, HD, MAPp, MD, MeshMapData, dUdu, ErrStat2, ErrMsg2 ) call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) ELSE IF (p_FAST%CompSub == Module_ExtPtfm) THEN CALL WrScr('>>> FAST_LIN: Linear_ExtPtfm_InputSolve_du, TODO') @@ -1733,7 +1781,7 @@ SUBROUTINE Glue_Jacobians( p_FAST, y_FAST, m_FAST, ED, BD, SrvD, AD, IfW, OpFM, ! \f$ \frac{\partial U_\Lambda^{ED}}{\partial y^{MAP}} \end{bmatrix} = \f$ (dUdy block row 3=ED) !............ - call Linear_ED_InputSolve_dy( p_FAST, y_FAST, ED%Input(1), ED%y, AD%y, AD%Input(1), BD, HD, SD, MAPp, MeshMapData, dUdy, ErrStat2, ErrMsg2 ) + call Linear_ED_InputSolve_dy( p_FAST, y_FAST, ED%Input(1), ED%y, AD%y, AD%Input(1), BD, HD, SD, MAPp, MD, MeshMapData, dUdy, ErrStat2, ErrMsg2 ) call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) !............ @@ -1785,7 +1833,7 @@ SUBROUTINE Glue_Jacobians( p_FAST, y_FAST, m_FAST, ED, BD, SrvD, AD, IfW, OpFM, !LIN-TODO: Add doc strings and look at above doc string IF (p_FAST%CompSub == Module_SD) THEN - call Linear_SD_InputSolve_dy( p_FAST, y_FAST, SD%Input(1), SD%y, ED%y, HD, MAPp, MeshMapData, dUdy, ErrStat2, ErrMsg2 ) + call Linear_SD_InputSolve_dy( p_FAST, y_FAST, SD%Input(1), SD%y, ED%y, HD, MAPp, MD, MeshMapData, dUdy, ErrStat2, ErrMsg2 ) call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) ELSE IF (p_FAST%CompSub == Module_ExtPtfm) THEN write(*,*)'>>> FAST_LIN: Linear_ExtPtfm_InputSolve_dy, TODO' @@ -1799,6 +1847,14 @@ SUBROUTINE Glue_Jacobians( p_FAST, y_FAST, m_FAST, ED, BD, SrvD, AD, IfW, OpFM, call Linear_MAP_InputSolve_dy( p_FAST, y_FAST, MAPp%Input(1), ED%y, SD%y, MeshMapData, dUdy, ErrStat2, ErrMsg2 ) call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) end if + !............ + ! \f$ \frac{\partial U_\Lambda^{MD}}{\partial y^{ED}} \end{bmatrix} = \f$ + ! \f$ \frac{\partial U_\Lambda^{MD}}{\partial y^{SD}} \end{bmatrix} = \f$ (dUdy block row 9=MD) <<<< + !............ + if (p_FAST%CompMooring == MODULE_MD) then + call Linear_MD_InputSolve_dy( p_FAST, y_FAST, MD%Input(1), ED%y, SD%y, MeshMapData, dUdy, ErrStat2, ErrMsg2 ) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + end if @@ -1865,7 +1921,7 @@ SUBROUTINE Linear_IfW_InputSolve_du_AD( p_FAST, y_FAST, u_AD, dUdu ) END SUBROUTINE Linear_IfW_InputSolve_du_AD !---------------------------------------------------------------------------------------------------------------------------------- !> This routine forms the dU^{ED}/du^{BD} and dU^{ED}/du^{AD} blocks (ED row) of dUdu. (i.e., how do changes in the AD and BD inputs affect the ED inputs?) -SUBROUTINE Linear_ED_InputSolve_du( p_FAST, y_FAST, u_ED, y_ED, y_AD, u_AD, BD, HD, SD, MAPp, MeshMapData, dUdu, ErrStat, ErrMsg ) +SUBROUTINE Linear_ED_InputSolve_du( p_FAST, y_FAST, u_ED, y_ED, y_AD, u_AD, BD, HD, SD, MAPp, MD, MeshMapData, dUdu, ErrStat, ErrMsg ) TYPE(FAST_ParameterType), INTENT(IN ) :: p_FAST !< Glue-code simulation parameters TYPE(FAST_OutputFileType), INTENT(IN ) :: y_FAST !< Glue-code output parameters (for linearization) @@ -1877,6 +1933,7 @@ SUBROUTINE Linear_ED_InputSolve_du( p_FAST, y_FAST, u_ED, y_ED, y_AD, u_AD, BD, TYPE(HydroDyn_Data), INTENT(INOUT) :: HD !< HD data at t TYPE(SubDyn_Data), INTENT(INOUT) :: SD !< SD data at t TYPE(MAP_Data), INTENT(INOUT) :: MAPp !< MAP data at t + TYPE(MoorDyn_Data), INTENT(INOUT) :: MD !< MD data at t TYPE(FAST_ModuleMapType), INTENT(INOUT) :: MeshMapData !< Data for mapping between modules REAL(R8Ki), INTENT(INOUT) :: dUdu(:,:) !< Jacobian matrix of which we are computing the dU^(ED)/du^(AD) block INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status @@ -1890,6 +1947,7 @@ SUBROUTINE Linear_ED_InputSolve_du( p_FAST, y_FAST, u_ED, y_ED, y_AD, u_AD, BD, INTEGER(IntKi) :: HD_Start ! starting index of dUdu (column) where HD motion inputs are located INTEGER(IntKi) :: SD_Start ! starting index of dUdu (column) where SD TP motion inputs are located INTEGER(IntKi) :: MAP_Start ! starting index of dUdu (column) where MAP fairlead motion inputs are located + INTEGER(IntKi) :: MD_Start ! starting index of dUdu (column) where MD fairlead motion inputs are located INTEGER(IntKi) :: ErrStat2 ! temporary Error status of the operation CHARACTER(ErrMsgLen) :: ErrMsg2 ! temporary Error message if ErrStat /= ErrID_None @@ -2057,6 +2115,29 @@ SUBROUTINE Linear_ED_InputSolve_du( p_FAST, y_FAST, u_ED, y_ED, y_AD, u_AD, BD, call SetBlockMatrix( dUdu, MeshMapData%Mooring_P_2_ED_P%dM%m_us, ED_Start_mt, MAP_Start ) end if + !.......... + ! dU^{ED}/du^{MD} + !.......... + else if ( p_FAST%CompMooring == Module_MD ) then + + ED_Start_mt = Indx_u_ED_Platform_Start(u_ED, y_FAST) & + + u_ED%PlatformPtMesh%NNodes * 3 ! 3 forces at each node (we're going to start at the moments) + + ! Transfer MD loads to ED PlatformPtmesh input: + ! we're mapping loads, so we also need the sibling meshes' displacements: + + MD_Start = y_FAST%Lin%Modules(Module_MD)%Instance(1)%LinStartIndx(LIN_INPUT_COL) + + ! NOTE: Assumes at least one coupled MD object + + CALL Linearize_Point_to_Point( MD%y%CoupledLoads, u_ED%PlatformPtMesh, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2, MD%Input(1)%CoupledKinematics, y_ED%PlatformPtMesh) + CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) + + ! HD is source in the mapping, so we want M_{uSm} + if (allocated(MeshMapData%Mooring_P_2_ED_P%dM%m_us )) then + call SetBlockMatrix( dUdu, MeshMapData%Mooring_P_2_ED_P%dM%m_us, ED_Start_mt, MD_Start ) + end if + end if end if @@ -2064,7 +2145,7 @@ END SUBROUTINE Linear_ED_InputSolve_du !---------------------------------------------------------------------------------------------------------------------------------- !> This routine forms the dU^{SD}/du^{HD} and dU^{SD}/du^{SD} and dU^{SD}/du^{MAP} blocks (SD row) of dUdu. (i.e., how do changes in the HD and SD inputs affect the SD inputs?) -SUBROUTINE Linear_SD_InputSolve_du( p_FAST, y_FAST, u_SD, y_SD, y_ED, HD, MAPp, MeshMapData, dUdu, ErrStat, ErrMsg ) +SUBROUTINE Linear_SD_InputSolve_du( p_FAST, y_FAST, u_SD, y_SD, y_ED, HD, MAPp, MD, MeshMapData, dUdu, ErrStat, ErrMsg ) TYPE(FAST_ParameterType), INTENT(IN ) :: p_FAST !< Glue-code simulation parameters TYPE(FAST_OutputFileType), INTENT(IN ) :: y_FAST !< Glue-code output parameters (for linearization) @@ -2073,6 +2154,7 @@ SUBROUTINE Linear_SD_InputSolve_du( p_FAST, y_FAST, u_SD, y_SD, y_ED, HD, MAPp, TYPE(ED_OutputType), INTENT(IN ) :: y_ED !< ElastoDyn outputs TYPE(HydroDyn_Data), INTENT(INOUT) :: HD !< HD data at t TYPE(MAP_Data), INTENT(INOUT) :: MAPp !< MAP data at t + TYPE(MoorDyn_Data), INTENT(INOUT) :: MD !< MD data at t TYPE(FAST_ModuleMapType), INTENT(INOUT) :: MeshMapData !< Data for mapping between modules REAL(R8Ki), INTENT(INOUT) :: dUdu(:,:) !< Jacobian matrix of which we are computing the dU^(SD)/du^(AD) block INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status @@ -2080,7 +2162,7 @@ SUBROUTINE Linear_SD_InputSolve_du( p_FAST, y_FAST, u_SD, y_SD, y_ED, HD, MAPp, ! local variables INTEGER(IntKi) :: HD_Start - INTEGER(IntKi) :: MAP_Start + INTEGER(IntKi) :: MAP_Start, MD_Start INTEGER(IntKi) :: SD_Start, SD_Start_td, SD_Start_tr INTEGER(IntKi) :: ErrStat2 ! temporary Error status of the operation CHARACTER(ErrMsgLen) :: ErrMsg2 ! temporary Error message if ErrStat /= ErrID_None @@ -2173,30 +2255,51 @@ SUBROUTINE Linear_SD_InputSolve_du( p_FAST, y_FAST, u_SD, y_SD, y_ED, HD, MAPp, ! dU^{SD}/du^{MAP} !.......... - if ( p_FAST%CompMooring == Module_MAP ) then + if ( p_FAST%CompMooring == Module_MAP ) then - ! Transfer MAP loads to ED PlatformPtmesh input: - ! we're mapping loads, so we also need the sibling meshes' displacements: + ! Transfer MAP loads to ED PlatformPtmesh input: + ! we're mapping loads, so we also need the sibling meshes' displacements: + + MAP_Start = y_FAST%Lin%Modules(MODULE_MAP)%Instance(1)%LinStartIndx(LIN_INPUT_COL) - MAP_Start = y_FAST%Lin%Modules(MODULE_MAP)%Instance(1)%LinStartIndx(LIN_INPUT_COL) - - ! NOTE: Assumes at least one MAP Fairlead point - - CALL Linearize_Point_to_Point( MAPp%y%ptFairleadLoad, u_SD%LMesh, MeshMapData%Mooring_P_2_SD_P, ErrStat2, ErrMsg2, MAPp%Input(1)%PtFairDisplacement, y_SD%Y2Mesh) !MAPp%Input(1)%ptFairleadLoad and y_SD%Y2Mesh contain the displaced positions for load calculations - CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) - - ! SD is source in the mapping, so we want M_{uSm} - if (allocated(MeshMapData%Mooring_P_2_SD_P%dM%m_us )) then - call SetBlockMatrix( dUdu, MeshMapData%Mooring_P_2_SD_P%dM%m_us, SD_Start, MAP_Start ) - end if + ! NOTE: Assumes at least one MAP Fairlead point + + CALL Linearize_Point_to_Point( MAPp%y%ptFairleadLoad, u_SD%LMesh, MeshMapData%Mooring_P_2_SD_P, ErrStat2, ErrMsg2, MAPp%Input(1)%PtFairDisplacement, y_SD%Y2Mesh) !MAPp%Input(1)%ptFairleadLoad and y_SD%Y2Mesh contain the displaced positions for load calculations + CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) + + ! SD is source in the mapping, so we want M_{uSm} + if (allocated(MeshMapData%Mooring_P_2_SD_P%dM%m_us )) then + call SetBlockMatrix( dUdu, MeshMapData%Mooring_P_2_SD_P%dM%m_us, SD_Start, MAP_Start ) + end if + + !.......... + ! dU^{SD}/du^{MD} + !.......... + else if ( p_FAST%CompMooring == Module_MD ) then + + ! Transfer MD loads to ED PlatformPtmesh input: + ! we're mapping loads, so we also need the sibling meshes' displacements: + + MD_Start = y_FAST%Lin%Modules(Module_MD)%Instance(1)%LinStartIndx(LIN_INPUT_COL) + ! NOTE: Assumes at least one coupled MD object + + CALL Linearize_Point_to_Point( MD%y%CoupledLoads, u_SD%LMesh, MeshMapData%Mooring_P_2_SD_P, ErrStat2, ErrMsg2, MD%Input(1)%CoupledKinematics, y_SD%Y2Mesh) + CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) + + ! SD is source in the mapping, so we want M_{uSm} + if (allocated(MeshMapData%Mooring_P_2_SD_P%dM%m_us )) then + call SetBlockMatrix( dUdu, MeshMapData%Mooring_P_2_SD_P%dM%m_us, SD_Start, MD_Start ) end if + + end if + END IF END SUBROUTINE Linear_SD_InputSolve_du !---------------------------------------------------------------------------------------------------------------------------------- !> This routine forms the dU^{SD}/du^{HD} and dU^{SD}/du^{SD} blocks (SD row) of dUdu. (i.e., how do changes in the HD and SD inputs affect the SD inputs?) -SUBROUTINE Linear_SD_InputSolve_dy( p_FAST, y_FAST, u_SD, y_SD, y_ED, HD, MAPp, MeshMapData, dUdy, ErrStat, ErrMsg ) +SUBROUTINE Linear_SD_InputSolve_dy( p_FAST, y_FAST, u_SD, y_SD, y_ED, HD, MAPp, MD, MeshMapData, dUdy, ErrStat, ErrMsg ) TYPE(FAST_ParameterType), INTENT(IN ) :: p_FAST !< Glue-code simulation parameters TYPE(FAST_OutputFileType), INTENT(IN ) :: y_FAST !< Glue-code output parameters (for linearization) @@ -2205,14 +2308,15 @@ SUBROUTINE Linear_SD_InputSolve_dy( p_FAST, y_FAST, u_SD, y_SD, y_ED, HD, MAPp, TYPE(ED_OutputType), INTENT(IN ) :: y_ED !< ElastoDyn outputs TYPE(HydroDyn_Data), INTENT(INOUT) :: HD !< HD data at t TYPE(MAP_Data), INTENT(INOUT) :: MAPp !< MAP data at t + TYPE(MoorDyn_Data), INTENT(INOUT) :: MD !< MD data at t TYPE(FAST_ModuleMapType), INTENT(INOUT) :: MeshMapData !< Data for mapping between modules REAL(R8Ki), INTENT(INOUT) :: dUdy(:,:) !< Jacobian matrix of which we are computing the dU^(SD)/dy^(SD) block INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message ! local variables - INTEGER(IntKi) :: SD_Start, SD_Out_Start, HD_Start, HD_Out_Start, ED_Out_Start, MAP_Out_Start - INTEGER(IntKi) :: MAP_Start + INTEGER(IntKi) :: SD_Start, SD_Out_Start, HD_Start, HD_Out_Start, ED_Out_Start, MAP_Out_Start, MD_Out_Start + INTEGER(IntKi) :: MAP_Start, MD_Start INTEGER(IntKi) :: ErrStat2 ! temporary Error status of the operation CHARACTER(ErrMsgLen) :: ErrMsg2 ! temporary Error message if ErrStat /= ErrID_None @@ -2285,6 +2389,23 @@ SUBROUTINE Linear_SD_InputSolve_dy( p_FAST, y_FAST, u_SD, y_SD, y_ED, HD, MAPp, SD_Start = Indx_u_SD_LMesh_Start(u_SD, y_FAST) ! start of u_SD%LMesh%TranslationDisp field call Assemble_dUdy_Loads(MAPp%y%ptFairLeadLoad, u_SD%LMesh, MeshMapData%Mooring_P_2_SD_P, SD_Start, MAP_Out_Start, dUdy) + ! SD translation displacement-to-SD moment transfer (dU^{SD}/dy^{SD}): + SD_Start = Indx_u_SD_LMesh_Start(u_SD, y_FAST) + u_SD%LMesh%NNodes*3 ! start of u_ED%LMesh%Moment field (skip the SD forces) + SD_Out_Start = Indx_y_SD_Y2Mesh_Start(y_SD, y_FAST) ! start of y_SD%Y2Mesh%TranslationDisp field + call SumBlockMatrix( dUdy, MeshMapData%Mooring_P_2_SD_P%dM%m_uD, SD_Start, SD_Out_Start ) + end if + + !.......... + ! dU^{SD}/dy^{MD} + !.......... + else if ( p_FAST%CompMooring == Module_MD ) then + if ( MD%y%CoupledLoads%Committed ) then ! meshes for floating + !!! ! This linearization was done in forming dUdu (see Linear_SD_InputSolve_du()), so we don't need to re-calculate these matrices + !!! ! while forming dUdy, too. + MD_Out_Start = y_FAST%Lin%Modules(Module_MD)%Instance(1)%LinStartIndx(LIN_OUTPUT_COL) + SD_Start = Indx_u_SD_LMesh_Start(u_SD, y_FAST) ! start of u_SD%LMesh%TranslationDisp field + call Assemble_dUdy_Loads(MD%y%CoupledLoads, u_SD%LMesh, MeshMapData%Mooring_P_2_SD_P, SD_Start, MD_Out_Start, dUdy) + ! SD translation displacement-to-SD moment transfer (dU^{SD}/dy^{SD}): SD_Start = Indx_u_SD_LMesh_Start(u_SD, y_FAST) + u_SD%LMesh%NNodes*3 ! start of u_ED%LMesh%Moment field (skip the SD forces) SD_Out_Start = Indx_y_SD_Y2Mesh_Start(y_SD, y_FAST) ! start of y_SD%Y2Mesh%TranslationDisp field @@ -2545,7 +2666,7 @@ END SUBROUTINE Linear_SrvD_InputSolve_dy !---------------------------------------------------------------------------------------------------------------------------------- !> This routine forms the dU^{ED}/dy^{SrvD}, dU^{ED}/dy^{ED}, dU^{ED}/dy^{BD}, dU^{ED}/dy^{AD}, dU^{ED}/dy^{HD}, and dU^{ED}/dy^{MAP} !! blocks of dUdy. (i.e., how do changes in the SrvD, ED, BD, AD, HD, and MAP outputs effect the ED inputs?) -SUBROUTINE Linear_ED_InputSolve_dy( p_FAST, y_FAST, u_ED, y_ED, y_AD, u_AD, BD, HD, SD, MAPp, MeshMapData, dUdy, ErrStat, ErrMsg ) +SUBROUTINE Linear_ED_InputSolve_dy( p_FAST, y_FAST, u_ED, y_ED, y_AD, u_AD, BD, HD, SD, MAPp, MD, MeshMapData, dUdy, ErrStat, ErrMsg ) TYPE(FAST_ParameterType), INTENT(IN ) :: p_FAST !< Glue-code simulation parameters TYPE(FAST_OutputFileType), INTENT(IN ) :: y_FAST !< FAST output file data (for linearization) @@ -2557,6 +2678,7 @@ SUBROUTINE Linear_ED_InputSolve_dy( p_FAST, y_FAST, u_ED, y_ED, y_AD, u_AD, BD, TYPE(HydroDyn_Data), INTENT(INOUT) :: HD !< HD data at t TYPE(SubDyn_Data), INTENT(INOUT) :: SD !< SD data at t TYPE(MAP_Data), INTENT(INOUT) :: MAPp !< MAP data at t + TYPE(MoorDyn_Data), INTENT(INOUT) :: MD !< MD data at t TYPE(FAST_ModuleMapType), INTENT(INOUT) :: MeshMapData !< Data for mapping between modules REAL(R8Ki), INTENT(INOUT) :: dUdy(:,:) !< Jacobian matrix of which we are computing the dU^(ED)/du^(AD) block @@ -2573,6 +2695,7 @@ SUBROUTINE Linear_ED_InputSolve_dy( p_FAST, y_FAST, u_ED, y_ED, y_AD, u_AD, BD, INTEGER(IntKi) :: HD_Out_Start ! starting index of dUdy (column) where HD output fields are located INTEGER(IntKi) :: SD_Out_Start ! starting index of dUdy (column) where SD output fields are located INTEGER(IntKi) :: MAP_Out_Start ! starting index of dUdy (column) where MAP output fields are located + INTEGER(IntKi) :: MD_Out_Start ! starting index of dUdy (column) where MoorDyn output fields are located CHARACTER(*), PARAMETER :: RoutineName = 'Linear_ED_InputSolve_dy' @@ -2725,7 +2848,21 @@ SUBROUTINE Linear_ED_InputSolve_dy( p_FAST, y_FAST, u_ED, y_ED, y_AD, u_AD, BD, ED_Out_Start = Indx_y_ED_Platform_Start(y_ED, y_FAST) ! start of y_ED%PlatformPtMesh%TranslationDisp field call SumBlockMatrix( dUdy, MeshMapData%Mooring_P_2_ED_P%dM%m_uD, ED_Start, ED_Out_Start ) end if - + ! MoorDyn + ! parts of dU^{ED}/dy^{MD} and dU^{ED}/dy^{ED}: + else if ( p_FAST%CompMooring == Module_MD ) then + if ( MD%y%CoupledLoads%Committed ) then ! meshes for floating + !!! ! This linearization was done in forming dUdu (see Linear_ED_InputSolve_du()), so we don't need to re-calculate these matrices + !!! ! while forming dUdy, too. + MD_Out_Start = y_FAST%Lin%Modules(Module_MD)%Instance(1)%LinStartIndx(LIN_OUTPUT_COL) + ED_Start = Indx_u_ED_Platform_Start(u_ED, y_FAST) ! start of u_ED%PlatformPtMesh%TranslationDisp field + call Assemble_dUdy_Loads(MD%y%CoupledLoads, u_ED%PlatformPtMesh, MeshMapData%Mooring_P_2_ED_P, ED_Start, MD_Out_Start, dUdy) + + ! ED translation displacement-to-ED moment transfer (dU^{ED}/dy^{ED}): + ED_Start = Indx_u_ED_Platform_Start(u_ED, y_FAST) + u_ED%PlatformPtMesh%NNodes*3 ! start of u_ED%PlatformPtMesh%Moment field (skip the ED forces) + ED_Out_Start = Indx_y_ED_Platform_Start(y_ED, y_FAST) ! start of y_ED%PlatformPtMesh%TranslationDisp field + call SumBlockMatrix( dUdy, MeshMapData%Mooring_P_2_ED_P%dM%m_uD, ED_Start, ED_Out_Start ) + end if end if else if ( p_FAST%CompSub == Module_SD ) then ! SubDyn @@ -3358,6 +3495,59 @@ SUBROUTINE Linear_MAP_InputSolve_dy( p_FAST, y_FAST, u_MAP, y_ED, y_SD, MeshMapD END IF END SUBROUTINE Linear_MAP_InputSolve_dy +!---------------------------------------------------------------------------------------------------------------------------------- +!> This routine forms the dU^{MD}/dy^{ED} block of dUdy. (i.e., how do changes in the ED outputs affect +!! the MD inputs?) +SUBROUTINE Linear_MD_InputSolve_dy( p_FAST, y_FAST, u_MD, y_ED, y_SD, MeshMapData, dUdy, ErrStat, ErrMsg ) + + ! Passed variables + TYPE(FAST_ParameterType), INTENT(IN ) :: p_FAST !< FAST parameter data + TYPE(FAST_OutputFileType), INTENT(IN ) :: y_FAST !< FAST output file data (for linearization) + TYPE(MD_InputType), INTENT(INOUT) :: u_MD !< The inputs to MoorDyn + TYPE(ED_OutputType), INTENT(IN) :: y_ED !< The outputs from the ElastoDyn structural dynamics module + TYPE(SD_OutputType), INTENT(IN) :: y_SD !< The outputs from the SubDyn structural dynamics module + TYPE(FAST_ModuleMapType), INTENT(INOUT) :: MeshMapData !< Data for mapping between modules + REAL(R8Ki), INTENT(INOUT) :: dUdy(:,:) !< Jacobian matrix of which we are computing the dU^{MD}/dy^{ED} block + + INTEGER(IntKi) :: ErrStat !< Error status of the operation + CHARACTER(*) :: ErrMsg !< Error message if ErrStat /= ErrID_None + + ! Local variables: + + INTEGER(IntKi) :: MD_Start ! starting index of dUdy (column) where particular MD fields are located + INTEGER(IntKi) :: ED_Out_Start! starting index of dUdy (row) where particular ED fields are located + INTEGER(IntKi) :: SD_Out_Start! starting index of dUdy (row) where particular SD fields are located + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'Linear_MD_InputSolve_dy' + + + ErrStat = ErrID_None + ErrMsg = "" + IF (u_MD%CoupledKinematics%Committed) THEN + !................................... + ! FairLead Mesh + !................................... + + MD_Start = y_FAST%Lin%Modules(MODULE_MD)%Instance(1)%LinStartIndx(LIN_INPUT_COL) + + if ( p_FAST%CompSub == Module_SD ) THEN + ! dU^{MD}/dy^{SD} + SD_Out_Start = Indx_y_SD_Y2Mesh_Start(y_SD, y_FAST) ! start of y_SD%Y2Mesh%TranslationDisp field + call Linearize_Point_to_Point( y_SD%Y2Mesh, u_MD%CoupledKinematics, MeshMapData%SD_P_2_Mooring_P, ErrStat2, ErrMsg2 ) + call Assemble_dUdy_Motions( y_SD%Y2Mesh, u_MD%CoupledKinematics, MeshMapData%SD_P_2_Mooring_P, MD_Start, SD_Out_Start, dUdy, OnlyTranslationDisp=.true.) + + else if ( p_FAST%CompSub == Module_None ) THEN + ! dU^{MD}/dy^{ED} + ED_Out_Start = Indx_y_ED_Platform_Start(y_ED, y_FAST) ! start of y_ED%PlatformPtMesh%TranslationDisp field + call Linearize_Point_to_Point( y_ED%PlatformPtMesh, u_MD%CoupledKinematics, MeshMapData%ED_P_2_Mooring_P, ErrStat2, ErrMsg2 ) + call Assemble_dUdy_Motions(y_ED%PlatformPtMesh, u_MD%CoupledKinematics, MeshMapData%ED_P_2_Mooring_P, MD_Start, ED_Out_Start, dUdy, OnlyTranslationDisp=.true.) + + end if + + END IF +END SUBROUTINE Linear_MD_InputSolve_dy + !---------------------------------------------------------------------------------------------------------------------------------- !> This routine allocates the state matrices for the glue code and concatenates the module-level state matrices into @@ -5550,8 +5740,25 @@ SUBROUTINE FAST_InitSteadyOutputs( psi, p_FAST, m_FAST, ED, BD, SrvD, AD, IfW, H call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName ) end if - !! MoorDyn - !ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN + ! MoorDyn + ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN + + allocate( MD%Output( p_FAST%LinInterpOrder+1 ), STAT = ErrStat2 ) + if (ErrStat2 /= 0) then + call SetErrStat(ErrID_Fatal, "Error allocating MD%Output.", ErrStat, ErrMsg, RoutineName ) + else + do j = 1, p_FAST%LinInterpOrder + 1 + call MD_CopyOutput(MD%y, MD%Output(j), MESH_NEWCOPY, ErrStat2, ErrMsg2) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName ) + end do + + call MD_CopyOutput(MD%y, MD%y_interp, MESH_NEWCOPY, ErrStat2, ErrMsg2) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName ) + end if + + + + !! FEAM !ELSEIF ( p_FAST%CompMooring == Module_FEAM ) THEN !! OrcaFlex @@ -5727,8 +5934,17 @@ SUBROUTINE FAST_SaveOutputs( psi, p_FAST, m_FAST, ED, BD, SrvD, AD, IfW, HD, SD, CALL MAP_CopyOutput (MAPp%y, MAPp%Output(1), MESH_UPDATECOPY, Errstat2, ErrMsg2) CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName ) - !! MoorDyn - !ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN + ! MoorDyn + ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN + + DO j = p_FAST%LinInterpOrder, 1, -1 + CALL MD_CopyOutput (MD%Output(j), MD%Output(j+1), MESH_UPDATECOPY, Errstat2, ErrMsg2) + CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName ) + END DO + + CALL MD_CopyOutput (MD%y, MD%Output(1), MESH_UPDATECOPY, Errstat2, ErrMsg2) + CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName ) + !! FEAM !ELSEIF ( p_FAST%CompMooring == Module_FEAM ) THEN !! OrcaFlex @@ -5889,8 +6105,16 @@ SUBROUTINE FAST_DiffInterpOutputs( psi_target, p_FAST, y_FAST, m_FAST, ED, BD, S call MAP_GetOP( t_global, MAPp%Input(1), MAPp%p, MAPp%x(STATE_CURR), MAPp%xd(STATE_CURR), MAPp%z(STATE_CURR), MAPp%OtherSt, & MAPp%y_interp, ErrStat2, ErrMsg2, y_op=y_FAST%Lin%Modules(Module_MAP)%Instance(1)%op_y) call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) - !! MoorDyn - !ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN + ! MoorDyn + ELSEIF ( p_FAST%CompMooring == Module_MD ) THEN + + CALL MD_Output_ExtrapInterp (MD%Output, m_FAST%Lin%Psi, MD%y_interp, psi_target, ErrStat2, ErrMsg2) + CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName ) + + call MD_GetOP( t_global, MD%Input(1), MD%p, MD%x(STATE_CURR), MD%xd(STATE_CURR), MD%z(STATE_CURR), MD%OtherSt(STATE_CURR), & + MD%y_interp, MD%m, ErrStat2, ErrMsg2, y_op=y_FAST%Lin%Modules(Module_MD)%Instance(1)%op_y) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + !! FEAM !ELSEIF ( p_FAST%CompMooring == Module_FEAM ) THEN !! OrcaFlex diff --git a/modules/openfast-library/src/FAST_Registry.txt b/modules/openfast-library/src/FAST_Registry.txt index 2c70911cf1..6b8e1ff5fc 100644 --- a/modules/openfast-library/src/FAST_Registry.txt +++ b/modules/openfast-library/src/FAST_Registry.txt @@ -545,6 +545,8 @@ typedef ^ ^ MD_ParameterType p - - - "Parameters" typedef ^ ^ MD_InputType u - - - "System inputs" typedef ^ ^ MD_OutputType y - - - "System outputs" typedef ^ ^ MD_MiscVarType m - - - "Misc/optimization variables" +typedef ^ ^ MD_OutputType Output {:} - - "Array of outputs associated with CalcSteady Azimuths" +typedef ^ ^ MD_OutputType y_interp - - - "interpolated system outputs for CalcSteady" typedef ^ ^ MD_InputType Input {:} - - "Array of inputs associated with InputTimes" typedef ^ ^ DbKi InputTimes {:} - - "Array of times associated with Input Array" diff --git a/modules/openfast-library/src/FAST_Types.f90 b/modules/openfast-library/src/FAST_Types.f90 index 968d71edfd..2fdec1b8e3 100644 --- a/modules/openfast-library/src/FAST_Types.f90 +++ b/modules/openfast-library/src/FAST_Types.f90 @@ -597,6 +597,8 @@ MODULE FAST_Types TYPE(MD_InputType) :: u !< System inputs [-] TYPE(MD_OutputType) :: y !< System outputs [-] TYPE(MD_MiscVarType) :: m !< Misc/optimization variables [-] + TYPE(MD_OutputType) , DIMENSION(:), ALLOCATABLE :: Output !< Array of outputs associated with CalcSteady Azimuths [-] + TYPE(MD_OutputType) :: y_interp !< interpolated system outputs for CalcSteady [-] TYPE(MD_InputType) , DIMENSION(:), ALLOCATABLE :: Input !< Array of inputs associated with InputTimes [-] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: InputTimes !< Array of times associated with Input Array [-] END TYPE MoorDyn_Data @@ -33700,6 +33702,25 @@ SUBROUTINE FAST_CopyMoorDyn_Data( SrcMoorDyn_DataData, DstMoorDyn_DataData, Ctrl CALL MD_CopyMisc( SrcMoorDyn_DataData%m, DstMoorDyn_DataData%m, CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) IF (ErrStat>=AbortErrLev) RETURN +IF (ALLOCATED(SrcMoorDyn_DataData%Output)) THEN + i1_l = LBOUND(SrcMoorDyn_DataData%Output,1) + i1_u = UBOUND(SrcMoorDyn_DataData%Output,1) + IF (.NOT. ALLOCATED(DstMoorDyn_DataData%Output)) THEN + ALLOCATE(DstMoorDyn_DataData%Output(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMoorDyn_DataData%Output.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcMoorDyn_DataData%Output,1), UBOUND(SrcMoorDyn_DataData%Output,1) + CALL MD_CopyOutput( SrcMoorDyn_DataData%Output(i1), DstMoorDyn_DataData%Output(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF + CALL MD_CopyOutput( SrcMoorDyn_DataData%y_interp, DstMoorDyn_DataData%y_interp, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN IF (ALLOCATED(SrcMoorDyn_DataData%Input)) THEN i1_l = LBOUND(SrcMoorDyn_DataData%Input,1) i1_u = UBOUND(SrcMoorDyn_DataData%Input,1) @@ -33755,6 +33776,13 @@ SUBROUTINE FAST_DestroyMoorDyn_Data( MoorDyn_DataData, ErrStat, ErrMsg ) CALL MD_DestroyInput( MoorDyn_DataData%u, ErrStat, ErrMsg ) CALL MD_DestroyOutput( MoorDyn_DataData%y, ErrStat, ErrMsg ) CALL MD_DestroyMisc( MoorDyn_DataData%m, ErrStat, ErrMsg ) +IF (ALLOCATED(MoorDyn_DataData%Output)) THEN +DO i1 = LBOUND(MoorDyn_DataData%Output,1), UBOUND(MoorDyn_DataData%Output,1) + CALL MD_DestroyOutput( MoorDyn_DataData%Output(i1), ErrStat, ErrMsg ) +ENDDO + DEALLOCATE(MoorDyn_DataData%Output) +ENDIF + CALL MD_DestroyOutput( MoorDyn_DataData%y_interp, ErrStat, ErrMsg ) IF (ALLOCATED(MoorDyn_DataData%Input)) THEN DO i1 = LBOUND(MoorDyn_DataData%Input,1), UBOUND(MoorDyn_DataData%Input,1) CALL MD_DestroyInput( MoorDyn_DataData%Input(i1), ErrStat, ErrMsg ) @@ -33946,6 +33974,46 @@ SUBROUTINE FAST_PackMoorDyn_Data( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, E Int_BufSz = Int_BufSz + SIZE( Int_Buf ) DEALLOCATE(Int_Buf) END IF + Int_BufSz = Int_BufSz + 1 ! Output allocated yes/no + IF ( ALLOCATED(InData%Output) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! Output upper/lower bounds for each dimension + DO i1 = LBOUND(InData%Output,1), UBOUND(InData%Output,1) + Int_BufSz = Int_BufSz + 3 ! Output: size of buffers for each call to pack subtype + CALL MD_PackOutput( Re_Buf, Db_Buf, Int_Buf, InData%Output(i1), ErrStat2, ErrMsg2, .TRUE. ) ! Output + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! Output + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! Output + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! Output + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 3 ! y_interp: size of buffers for each call to pack subtype + CALL MD_PackOutput( Re_Buf, Db_Buf, Int_Buf, InData%y_interp, ErrStat2, ErrMsg2, .TRUE. ) ! y_interp + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! y_interp + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! y_interp + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! y_interp + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF Int_BufSz = Int_BufSz + 1 ! Input allocated yes/no IF ( ALLOCATED(InData%Input) ) THEN Int_BufSz = Int_BufSz + 2*1 ! Input upper/lower bounds for each dimension @@ -34209,6 +34277,75 @@ SUBROUTINE FAST_PackMoorDyn_Data( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, E CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF ( .NOT. ALLOCATED(InData%Output) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Output,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Output,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%Output,1), UBOUND(InData%Output,1) + CALL MD_PackOutput( Re_Buf, Db_Buf, Int_Buf, InData%Output(i1), ErrStat2, ErrMsg2, OnlySize ) ! Output + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF + CALL MD_PackOutput( Re_Buf, Db_Buf, Int_Buf, InData%y_interp, ErrStat2, ErrMsg2, OnlySize ) ! y_interp + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + IF(ALLOCATED(Re_Buf)) THEN IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf @@ -34651,6 +34788,102 @@ SUBROUTINE FAST_UnPackMoorDyn_Data( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Output not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Output)) DEALLOCATE(OutData%Output) + ALLOCATE(OutData%Output(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Output.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%Output,1), UBOUND(OutData%Output,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_UnpackOutput( Re_Buf, Db_Buf, Int_Buf, OutData%Output(i1), ErrStat2, ErrMsg2 ) ! Output + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MD_UnpackOutput( Re_Buf, Db_Buf, Int_Buf, OutData%y_interp, ErrStat2, ErrMsg2 ) ! y_interp + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) From ddfdff6c5dc262007f34d5a396dd8f4fc43ad7fd Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Mon, 10 May 2021 19:52:10 -0600 Subject: [PATCH 033/242] Removing 'MD can't linearize' error message --- modules/openfast-library/src/FAST_Subs.f90 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/openfast-library/src/FAST_Subs.f90 b/modules/openfast-library/src/FAST_Subs.f90 index 45dd5c6345..86e4254146 100644 --- a/modules/openfast-library/src/FAST_Subs.f90 +++ b/modules/openfast-library/src/FAST_Subs.f90 @@ -1889,7 +1889,7 @@ SUBROUTINE ValidateInputData(p, m_FAST, ErrStat, ErrMsg) if (p%CompAero == MODULE_AD14) call SetErrStat(ErrID_Fatal,'Linearization is not implemented for the AeroDyn v14 module.',ErrStat, ErrMsg, RoutineName) !if (p%CompSub == MODULE_SD) call SetErrStat(ErrID_Fatal,'Linearization is not implemented for the SubDyn module.',ErrStat, ErrMsg, RoutineName) if (p%CompSub /= MODULE_None .and. p%CompSub /= MODULE_SD ) call SetErrStat(ErrID_Fatal,'Linearization is not implemented for the ExtPtfm_MCKF substructure module.',ErrStat, ErrMsg, RoutineName) - if (p%CompMooring /= MODULE_None .and. p%CompMooring /= MODULE_MAP) call SetErrStat(ErrID_Fatal,'Linearization is not implemented for the FEAMooring or MoorDyn mooring modules.',ErrStat, ErrMsg, RoutineName) + if (p%CompMooring /= MODULE_None .and. p%CompMooring == MODULE_FEAM) call SetErrStat(ErrID_Fatal,'Linearization is not implemented for the FEAMooring mooring module.',ErrStat, ErrMsg, RoutineName) if (p%CompIce /= MODULE_None) call SetErrStat(ErrID_Fatal,'Linearization is not implemented for any of the ice loading modules.',ErrStat, ErrMsg, RoutineName) end if From ca9380ce95831f07ee27b275ee4e52e9ccf0b17e Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Tue, 11 May 2021 09:03:11 -0600 Subject: [PATCH 034/242] Removing hard-coded wave grid for general use pending future wave capabilities --- modules/hydrodyn/src/HydroDyn.f90 | 23 +- modules/hydrodyn/src/HydroDyn_Input.f90 | 22 +- modules/hydrodyn/src/Waves.f90 | 58 ++-- modules/moordyn/src/MoorDyn.f90 | 428 ++++++++++++------------ 4 files changed, 265 insertions(+), 266 deletions(-) diff --git a/modules/hydrodyn/src/HydroDyn.f90 b/modules/hydrodyn/src/HydroDyn.f90 index f209e7d652..563387d281 100644 --- a/modules/hydrodyn/src/HydroDyn.f90 +++ b/modules/hydrodyn/src/HydroDyn.f90 @@ -1351,18 +1351,17 @@ SUBROUTINE HydroDyn_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, I !::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: !@mhall: Making sure wave info from the grid points are saved for output here... - ALLOCATE ( InitOut%WaveVel (InitLocal%Morison%NStepWave, WaveGrid_n, 3) ) - ALLOCATE ( InitOut%WaveAcc (InitLocal%Morison%NStepWave, WaveGrid_n, 3) ) - ALLOCATE ( InitOut%WaveDynP (InitLocal%Morison%NStepWave, WaveGrid_n) ) - ALLOCATE ( InitOut%WaveElev (InitLocal%Morison%NStepWave, WaveGrid_nx*WaveGrid_ny) ) ! unlike the above, this array is just 5x5, for the surface. - ALLOCATE ( InitOut%WaveTime (InitLocal%Morison%NStepWave) ) - - - InitOut%WaveVel = InitLocal%Morison%WaveVel( :,InitLocal%Morison%NNodes+1:,:) - InitOut%WaveAcc = InitLocal%Morison%WaveAcc( :,InitLocal%Morison%NNodes+1:,:) - InitOut%WaveDynP = InitLocal%Morison%WaveDynP(:,InitLocal%Morison%NNodes+1:) - InitOut%WaveElev = Waves_InitOut%WaveElevMD ! unlike the above, this array is just 5x5, for the surface. - InitOut%WaveTime = InitLocal%Morison%WaveTime(:) + !ALLOCATE ( InitOut%WaveVel (InitLocal%Morison%NStepWave, WaveGrid_n, 3) ) + !ALLOCATE ( InitOut%WaveAcc (InitLocal%Morison%NStepWave, WaveGrid_n, 3) ) + !ALLOCATE ( InitOut%WaveDynP (InitLocal%Morison%NStepWave, WaveGrid_n) ) + !ALLOCATE ( InitOut%WaveElev (InitLocal%Morison%NStepWave, WaveGrid_nx*WaveGrid_ny) ) ! unlike the above, this array is just 5x5, for the surface. + !ALLOCATE ( InitOut%WaveTime (InitLocal%Morison%NStepWave) ) + ! + !InitOut%WaveVel = InitLocal%Morison%WaveVel( :,InitLocal%Morison%NNodes+1:,:) + !InitOut%WaveAcc = InitLocal%Morison%WaveAcc( :,InitLocal%Morison%NNodes+1:,:) + !InitOut%WaveDynP = InitLocal%Morison%WaveDynP(:,InitLocal%Morison%NNodes+1:) + !InitOut%WaveElev = Waves_InitOut%WaveElevMD ! unlike the above, this array is just 5x5, for the surface. + !InitOut%WaveTime = InitLocal%Morison%WaveTime(:) !::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: diff --git a/modules/hydrodyn/src/HydroDyn_Input.f90 b/modules/hydrodyn/src/HydroDyn_Input.f90 index c4329ce67d..33f09c6a0b 100644 --- a/modules/hydrodyn/src/HydroDyn_Input.f90 +++ b/modules/hydrodyn/src/HydroDyn_Input.f90 @@ -4301,17 +4301,17 @@ SUBROUTINE HydroDynInput_ProcessInitData( InitInp, ErrStat, ErrMsg ) InitInp%Current%MorisonNodezi(I) = InitInp%Waves%WaveKinzi(I) END DO !@mhall: hard-coding the coordinates of those additional nodes for the grid (remember, must be in increasing order) <<< move these to module global parameters<<<< - DO I=1,WaveGrid_nz !z - DO J = 1,WaveGrid_ny !y - DO K = 1,WaveGrid_nx !x - Itemp = InitInp%Morison%NNodes + (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node - InitInp%Waves%WaveKinzi(Itemp) = 1.0 - 2.0**(WaveGrid_nz-I) ! -127, -63, -31, -15, -7, -3, -1, 0 - InitInp%Waves%WaveKinyi(Itemp) = WaveGrid_y0 + WaveGrid_dy*(J-1) - InitInp%Waves%WaveKinxi(Itemp) = WaveGrid_x0 + WaveGrid_dx*(K-1) - InitInp%Current%MorisonNodezi(Itemp)= InitInp%Waves%WaveKinzi(I) - END DO - END DO - END DO + !DO I=1,WaveGrid_nz !z + ! DO J = 1,WaveGrid_ny !y + ! DO K = 1,WaveGrid_nx !x + ! Itemp = InitInp%Morison%NNodes + (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node + ! InitInp%Waves%WaveKinzi(Itemp) = 1.0 - 2.0**(WaveGrid_nz-I) ! -127, -63, -31, -15, -7, -3, -1, 0 + ! InitInp%Waves%WaveKinyi(Itemp) = WaveGrid_y0 + WaveGrid_dy*(J-1) + ! InitInp%Waves%WaveKinxi(Itemp) = WaveGrid_x0 + WaveGrid_dx*(K-1) + ! InitInp%Current%MorisonNodezi(Itemp)= InitInp%Waves%WaveKinzi(I) + ! END DO + ! END DO + !END DO ! If we are using the Waves module, the node information must be copied over. InitInp%Waves2%NWaveKin = InitInp%Waves%NWaveKin ! Number of points where the incident wave kinematics will be computed (-) diff --git a/modules/hydrodyn/src/Waves.f90 b/modules/hydrodyn/src/Waves.f90 index 9cc8b600da..8dcf3e3768 100644 --- a/modules/hydrodyn/src/Waves.f90 +++ b/modules/hydrodyn/src/Waves.f90 @@ -37,17 +37,17 @@ MODULE Waves ! ..... @mhall: Public variables for hard-coded wave kinematics grid (temporary solution) ........................... - INTEGER, PUBLIC :: WaveGrid_n = 150 ! Number of wave kinematics grid points = nx*ny*nz - - REAL(SiKi), PUBLIC :: WaveGrid_x0 = -35.0 ! first grid point in x direction - REAL(SiKi), PUBLIC :: WaveGrid_dx = 10.0 ! step size in x direction - INTEGER, PUBLIC :: WaveGrid_nx = 10 ! Number of wave kinematics grid points in x - - REAL(SiKi), PUBLIC :: WaveGrid_y0 = -35.0 ! same for y - REAL(SiKi), PUBLIC :: WaveGrid_dy = 35.0 - INTEGER, PUBLIC :: WaveGrid_ny = 3 - - INTEGER, PUBLIC :: WaveGrid_nz = 5 ! Number of wave kinematics grid points in z (locations decided by 1.0 - 2.0**(WaveGrid_nz-I)) + INTEGER, PUBLIC :: WaveGrid_n = 0 !150 Number of wave kinematics grid points = nx*ny*nz + ! + !REAL(SiKi), PUBLIC :: WaveGrid_x0 = -35.0 ! first grid point in x direction + !REAL(SiKi), PUBLIC :: WaveGrid_dx = 10.0 ! step size in x direction + !INTEGER, PUBLIC :: WaveGrid_nx = 10 ! Number of wave kinematics grid points in x + ! + !REAL(SiKi), PUBLIC :: WaveGrid_y0 = -35.0 ! same for y + !REAL(SiKi), PUBLIC :: WaveGrid_dy = 35.0 + !INTEGER, PUBLIC :: WaveGrid_ny = 3 + ! + !INTEGER, PUBLIC :: WaveGrid_nz = 5 ! Number of wave kinematics grid points in z (locations decided by 1.0 - 2.0**(WaveGrid_nz-I)) ! ..... Public Subroutines ................................................................................................... @@ -1799,22 +1799,22 @@ SUBROUTINE VariousWaves_Init ( InitInp, InitOut, ErrStat, ErrMsg ) ! :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: !@mhall: hard-coding some additional wave elevation time series output for now - ALLOCATE ( InitOut%WaveElevMD (0:InitOut%NStepWave, WaveGrid_nx*WaveGrid_ny), STAT=ErrStatTmp ) - IF (ErrStatTmp /= 0) CALL SetErrStat(ErrID_Fatal,'Cannot allocate array InitOut%WaveElevMD.', ErrStat,ErrMsg,'VariousWaves_Init') - - DO J = 1,WaveGrid_ny !y = -60.0 + 20.0*J - DO K = 1,WaveGrid_nx !x = -60.0 + 20.0*K - - I = (J-1)*WaveGrid_nx + K ! index of actual node - - CALL WaveElevTimeSeriesAtXY( WaveGrid_x0 + WaveGrid_dx*(K-1), WaveGrid_y0 + WaveGrid_dy*(J-1), InitOut%WaveElevMD(:,I), ErrStatTmp, ErrMsgTmp ) - CALL SetErrStat(ErrStatTmp,'Error occured while applying the FFT to InitOut%WaveElevMD.',ErrStat,ErrMsg,'VariousWaves_Init') - IF ( ErrStat >= AbortErrLev ) THEN - CALL CleanUp() - RETURN - END IF - END DO - END DO + !ALLOCATE ( InitOut%WaveElevMD (0:InitOut%NStepWave, WaveGrid_nx*WaveGrid_ny), STAT=ErrStatTmp ) + !IF (ErrStatTmp /= 0) CALL SetErrStat(ErrID_Fatal,'Cannot allocate array InitOut%WaveElevMD.', ErrStat,ErrMsg,'VariousWaves_Init') + ! + !DO J = 1,WaveGrid_ny !y = -60.0 + 20.0*J + ! DO K = 1,WaveGrid_nx !x = -60.0 + 20.0*K + ! + ! I = (J-1)*WaveGrid_nx + K ! index of actual node + ! + ! CALL WaveElevTimeSeriesAtXY( WaveGrid_x0 + WaveGrid_dx*(K-1), WaveGrid_y0 + WaveGrid_dy*(J-1), InitOut%WaveElevMD(:,I), ErrStatTmp, ErrMsgTmp ) + ! CALL SetErrStat(ErrStatTmp,'Error occured while applying the FFT to InitOut%WaveElevMD.',ErrStat,ErrMsg,'VariousWaves_Init') + ! IF ( ErrStat >= AbortErrLev ) THEN + ! CALL CleanUp() + ! RETURN + ! END IF + ! END DO + !END DO ! :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: @@ -2221,8 +2221,8 @@ SUBROUTINE Waves_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, Init CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,'Waves_Init') !@mhall: :::: ensure all arrays needed for the wave grid to MoorDyn are allocated in the WaveMod=0 case too :::: - ALLOCATE ( InitOut%WaveElevMD (0:InitOut%NStepWave, WaveGrid_nx*WaveGrid_ny), STAT=ErrStatTmp ) - InitOut%WaveElevMD = 0.0_DbKi ! zero it + !ALLOCATE ( InitOut%WaveElevMD (0:InitOut%NStepWave, WaveGrid_nx*WaveGrid_ny), STAT=ErrStatTmp ) + !InitOut%WaveElevMD = 0.0_DbKi ! zero it ! ::::: end ::::: IF ( ErrStat >= AbortErrLev ) RETURN diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index ca8ce336dd..1e6800cecc 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -24,7 +24,7 @@ MODULE MoorDyn USE MoorDyn_IO USE NWTC_Library - USE WAVES, only: WaveGrid_n, WaveGrid_x0, WaveGrid_dx, WaveGrid_nx, WaveGrid_y0, WaveGrid_dy, WaveGrid_ny, WaveGrid_nz ! seeing if I can get waves data here directly... + !USE WAVES, only: WaveGrid_n, WaveGrid_x0, WaveGrid_dx, WaveGrid_nx, WaveGrid_y0, WaveGrid_dy, WaveGrid_ny, WaveGrid_nz ! seeing if I can get waves data here directly... IMPLICIT NONE @@ -1663,207 +1663,207 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! END DO ! :::::::::::::::: the above might be used eventually. For now, let's store wave info grids within this module ::::::::::::::::: - ! allocate arrays - ntWave = SIZE(InitInp%WaveTime) - ALLOCATE ( p%ux (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ALLOCATE ( p%uy (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ALLOCATE ( p%uz (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ALLOCATE ( p%ax (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ALLOCATE ( p%ay (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ALLOCATE ( p%az (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ALLOCATE ( p%PDyn(ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ALLOCATE ( p%zeta(ntWave,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) ! 2D grid over x and y only - ALLOCATE ( p%px(WaveGrid_nx), STAT = ErrStat2 ) - ALLOCATE ( p%py(WaveGrid_ny), STAT = ErrStat2 ) - ALLOCATE ( p%pz(WaveGrid_nz), STAT = ErrStat2 ) - ALLOCATE ( p%tWave(ntWave), STAT = ErrStat2 ) - - ! get grid and time info (currenltly this is hard-coded to match what's in HydroDyn_Input - DO I=1,WaveGrid_nz - p%pz(I) = 1.0 - 2.0**(WaveGrid_nz-I) ! -127, -63, -31, -15, -7, -3, -1, 0 - END DO - DO J = 1,WaveGrid_ny - p%py(J) = WaveGrid_y0 + WaveGrid_dy*(J-1) - END DO - DO K = 1,WaveGrid_nx - p%px(K) = WaveGrid_x0 + WaveGrid_dx*(K-1) - END DO - - p%tWave = InitInp%WaveTime - - ! fill in the grid data (the for loops match those in HydroDyn_Input) - DO I=1,WaveGrid_nz - DO J = 1,WaveGrid_ny - DO K = 1,WaveGrid_nx - Itemp = (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node on 3D grid - - p%ux (:,I,J,K) = InitInp%WaveVel( :,Itemp,1) ! note: indices are t, z, y, x - p%uy (:,I,J,K) = InitInp%WaveVel( :,Itemp,2) - p%uz (:,I,J,K) = InitInp%WaveVel( :,Itemp,3) - p%ax (:,I,J,K) = InitInp%WaveAcc( :,Itemp,1) - p%ay (:,I,J,K) = InitInp%WaveAcc( :,Itemp,2) - p%az (:,I,J,K) = InitInp%WaveAcc( :,Itemp,3) - p%PDyn(:,I,J,K) = InitInp%WavePDyn(:,Itemp) - END DO - END DO - END DO - - ! fill in the grid data (the for loops match those in HydroDyn_Input) - DO J = 1,WaveGrid_ny - DO K = 1,WaveGrid_nx - Itemp = (J-1)*WaveGrid_nx + K ! index of actual node on surface 2D grid - p%zeta(:,J,K) = InitInp%WaveElev(:,Itemp) - END DO - END DO - - - ! write the date to an output file for testing purposes - - CALL GetNewUnit( UnOut) - - CALL OpenFOutFile ( UnOut, "waves.txt", ErrStat, ErrMsg ) - IF ( ErrStat > ErrID_None ) THEN - ErrMsg = ' Error opening wave grid file: '//TRIM(ErrMsg) - ErrStat = ErrID_Fatal - RETURN - END IF - - WRITE(UnOut, *, IOSTAT=ErrStat2) TRIM( 'MoorDyn v2 wave/current kinematics grid file' ) - WRITE(UnOut, *, IOSTAT=ErrStat2) TRIM( '---------------------------------------------' ) - WRITE(UnOut, *, IOSTAT=ErrStat2) TRIM( 'The following 6 lines (4-9) specify the input type then the inputs for x, then, y, then z coordinates.' ) - - WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - X input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) - Frmt = '('//TRIM(Int2LStr(5))//'(A1,e10.4))' - WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%px(I))), I=1,WaveGrid_nx ) - - WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - Y input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) - Frmt = '('//TRIM(Int2LStr(5))//'(A1,e10.4))' - WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%py(I))), I=1,WaveGrid_ny ) - - WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - Z input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) - Frmt = '('//TRIM(Int2LStr(8))//'(A1,e10.4))' - WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%pz(I))), I=1,WaveGrid_nz ) - - CLOSE(UnOut, IOSTAT = ErrStat ) - IF ( ErrStat /= 0 ) THEN - ErrMsg = 'Error closing wave grid file' - END IF - - - CALL GetNewUnit( UnOut) - - CALL OpenFOutFile ( UnOut, "wave data.txt", ErrStat, ErrMsg ) - IF ( ErrStat > ErrID_None ) THEN - ErrMsg = ' Error opening wave grid file: '//TRIM(ErrMsg) - ErrStat = ErrID_Fatal - RETURN - END IF - - ! write channel labels - - - ! time - WRITE(UnOut,"(A10)", IOSTAT=ErrStat2, advance="no") "Time" - - DO J = 1,WaveGrid_ny !y - DO K = 1,WaveGrid_nx !x - WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ze0", Num2Lstr(J+10*K) - END DO - END DO - DO I=1,WaveGrid_nz !z - DO J = 1,WaveGrid_ny !y - DO K = 1,WaveGrid_nx !x - WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ux", Num2Lstr(I+10*J+100*K) - WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " uy", Num2Lstr(I+10*J+100*K) - WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " uz", Num2Lstr(I+10*J+100*K) - END DO - END DO - END DO - DO I=1,WaveGrid_nz !z - DO J = 1,WaveGrid_ny !y - DO K = 1,WaveGrid_nx !x - WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ax", Num2Lstr(I+10*J+100*K) - WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ay", Num2Lstr(I+10*J+100*K) - WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " az", Num2Lstr(I+10*J+100*K) - END DO - END DO - END DO - DO I=1,WaveGrid_nz !z - DO J = 1,WaveGrid_ny !y - DO K = 1,WaveGrid_nx !x - WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " pd", Num2Lstr(I+10*J+100*K) - END DO - END DO - END DO - - ! end the line - WRITE(UnOut, "(A1)", IOSTAT=ErrStat2) " " - - - - DO l=1, SIZE(InitInp%WaveTime) ! loop through all time steps - - ! time - WRITE(UnOut,"(F10.4)", IOSTAT=ErrStat2, advance="no") p%tWave(l) - !WRITE(UnOut,"(F10.4)", IOSTAT=ErrStat2, advance="no") InitInp%WaveTime(l) - - ! wave elevation (all slices for now, to check) - DO J = 1,WaveGrid_ny !y - DO K = 1,WaveGrid_nx !x - Itemp = (J-1)*WaveGrid_nx + K ! index of actual node - - WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%zeta(l,J,K) - END DO - END DO - - ! wave velocities - DO I=1,WaveGrid_nz !z - DO J = 1,WaveGrid_ny !y - DO K = 1,WaveGrid_nx !x - Itemp = (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node - - WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%ux(l,I,J,K) - WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%uy(l,I,J,K) - WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%uz(l,I,J,K) - END DO - END DO - END DO - - ! wave accelerations - DO I=1,WaveGrid_nz !z - DO J = 1,WaveGrid_ny !y - DO K = 1,WaveGrid_nx !x - Itemp = (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node - - WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%ax(l,I,J,K) - WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%ay(l,I,J,K) - WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%az(l,I,J,K) - END DO - END DO - END DO - - ! dynamic pressure - DO I=1,WaveGrid_nz !z - DO J = 1,WaveGrid_ny !y - DO K = 1,WaveGrid_nx !x - Itemp = (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node - - WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%PDyn(l,I,J,K) - END DO - END DO - END DO - - ! end the line - WRITE(UnOut, "(A1)", IOSTAT=ErrStat2) " " - - - END DO - - - CLOSE(UnOut, IOSTAT = ErrStat ) - IF ( ErrStat /= 0 ) THEN - ErrMsg = 'Error closing wave grid file' - END IF + ! ! allocate arrays + ! ntWave = SIZE(InitInp%WaveTime) + ! ALLOCATE ( p%ux (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ! ALLOCATE ( p%uy (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ! ALLOCATE ( p%uz (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ! ALLOCATE ( p%ax (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ! ALLOCATE ( p%ay (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ! ALLOCATE ( p%az (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ! ALLOCATE ( p%PDyn(ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) + ! ALLOCATE ( p%zeta(ntWave,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) ! 2D grid over x and y only + ! ALLOCATE ( p%px(WaveGrid_nx), STAT = ErrStat2 ) + ! ALLOCATE ( p%py(WaveGrid_ny), STAT = ErrStat2 ) + ! ALLOCATE ( p%pz(WaveGrid_nz), STAT = ErrStat2 ) + ! ALLOCATE ( p%tWave(ntWave), STAT = ErrStat2 ) + ! + ! ! get grid and time info (currenltly this is hard-coded to match what's in HydroDyn_Input + ! DO I=1,WaveGrid_nz + ! p%pz(I) = 1.0 - 2.0**(WaveGrid_nz-I) ! -127, -63, -31, -15, -7, -3, -1, 0 + ! END DO + ! DO J = 1,WaveGrid_ny + ! p%py(J) = WaveGrid_y0 + WaveGrid_dy*(J-1) + ! END DO + ! DO K = 1,WaveGrid_nx + ! p%px(K) = WaveGrid_x0 + WaveGrid_dx*(K-1) + ! END DO + ! + ! p%tWave = InitInp%WaveTime + ! + ! ! fill in the grid data (the for loops match those in HydroDyn_Input) + ! DO I=1,WaveGrid_nz + ! DO J = 1,WaveGrid_ny + ! DO K = 1,WaveGrid_nx + ! Itemp = (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node on 3D grid + ! + ! p%ux (:,I,J,K) = InitInp%WaveVel( :,Itemp,1) ! note: indices are t, z, y, x + ! p%uy (:,I,J,K) = InitInp%WaveVel( :,Itemp,2) + ! p%uz (:,I,J,K) = InitInp%WaveVel( :,Itemp,3) + ! p%ax (:,I,J,K) = InitInp%WaveAcc( :,Itemp,1) + ! p%ay (:,I,J,K) = InitInp%WaveAcc( :,Itemp,2) + ! p%az (:,I,J,K) = InitInp%WaveAcc( :,Itemp,3) + ! p%PDyn(:,I,J,K) = InitInp%WavePDyn(:,Itemp) + ! END DO + ! END DO + ! END DO + ! + ! ! fill in the grid data (the for loops match those in HydroDyn_Input) + ! DO J = 1,WaveGrid_ny + ! DO K = 1,WaveGrid_nx + ! Itemp = (J-1)*WaveGrid_nx + K ! index of actual node on surface 2D grid + ! p%zeta(:,J,K) = InitInp%WaveElev(:,Itemp) + ! END DO + ! END DO + ! + ! + ! ! write the date to an output file for testing purposes + ! + ! CALL GetNewUnit( UnOut) + ! + ! CALL OpenFOutFile ( UnOut, "waves.txt", ErrStat, ErrMsg ) + ! IF ( ErrStat > ErrID_None ) THEN + ! ErrMsg = ' Error opening wave grid file: '//TRIM(ErrMsg) + ! ErrStat = ErrID_Fatal + ! RETURN + ! END IF + ! + ! WRITE(UnOut, *, IOSTAT=ErrStat2) TRIM( 'MoorDyn v2 wave/current kinematics grid file' ) + ! WRITE(UnOut, *, IOSTAT=ErrStat2) TRIM( '---------------------------------------------' ) + ! WRITE(UnOut, *, IOSTAT=ErrStat2) TRIM( 'The following 6 lines (4-9) specify the input type then the inputs for x, then, y, then z coordinates.' ) + ! + ! WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - X input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) + ! Frmt = '('//TRIM(Int2LStr(5))//'(A1,e10.4))' + ! WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%px(I))), I=1,WaveGrid_nx ) + ! + ! WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - Y input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) + ! Frmt = '('//TRIM(Int2LStr(5))//'(A1,e10.4))' + ! WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%py(I))), I=1,WaveGrid_ny ) + ! + ! WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - Z input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) + ! Frmt = '('//TRIM(Int2LStr(8))//'(A1,e10.4))' + ! WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%pz(I))), I=1,WaveGrid_nz ) + ! + ! CLOSE(UnOut, IOSTAT = ErrStat ) + ! IF ( ErrStat /= 0 ) THEN + ! ErrMsg = 'Error closing wave grid file' + ! END IF + ! + ! + ! CALL GetNewUnit( UnOut) + ! + ! CALL OpenFOutFile ( UnOut, "wave data.txt", ErrStat, ErrMsg ) + ! IF ( ErrStat > ErrID_None ) THEN + ! ErrMsg = ' Error opening wave grid file: '//TRIM(ErrMsg) + ! ErrStat = ErrID_Fatal + ! RETURN + ! END IF + ! + ! ! write channel labels + ! + ! + ! ! time + ! WRITE(UnOut,"(A10)", IOSTAT=ErrStat2, advance="no") "Time" + ! + ! DO J = 1,WaveGrid_ny !y + ! DO K = 1,WaveGrid_nx !x + ! WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ze0", Num2Lstr(J+10*K) + ! END DO + ! END DO + ! DO I=1,WaveGrid_nz !z + ! DO J = 1,WaveGrid_ny !y + ! DO K = 1,WaveGrid_nx !x + ! WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ux", Num2Lstr(I+10*J+100*K) + ! WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " uy", Num2Lstr(I+10*J+100*K) + ! WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " uz", Num2Lstr(I+10*J+100*K) + ! END DO + ! END DO + ! END DO + ! DO I=1,WaveGrid_nz !z + ! DO J = 1,WaveGrid_ny !y + ! DO K = 1,WaveGrid_nx !x + ! WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ax", Num2Lstr(I+10*J+100*K) + ! WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ay", Num2Lstr(I+10*J+100*K) + ! WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " az", Num2Lstr(I+10*J+100*K) + ! END DO + ! END DO + ! END DO + ! DO I=1,WaveGrid_nz !z + ! DO J = 1,WaveGrid_ny !y + ! DO K = 1,WaveGrid_nx !x + ! WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " pd", Num2Lstr(I+10*J+100*K) + ! END DO + ! END DO + ! END DO + ! + ! ! end the line + ! WRITE(UnOut, "(A1)", IOSTAT=ErrStat2) " " + ! + ! + ! + ! DO l=1, SIZE(InitInp%WaveTime) ! loop through all time steps + ! + ! ! time + ! WRITE(UnOut,"(F10.4)", IOSTAT=ErrStat2, advance="no") p%tWave(l) + ! !WRITE(UnOut,"(F10.4)", IOSTAT=ErrStat2, advance="no") InitInp%WaveTime(l) + ! + ! ! wave elevation (all slices for now, to check) + ! DO J = 1,WaveGrid_ny !y + ! DO K = 1,WaveGrid_nx !x + ! Itemp = (J-1)*WaveGrid_nx + K ! index of actual node + ! + ! WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%zeta(l,J,K) + ! END DO + ! END DO + ! + ! ! wave velocities + ! DO I=1,WaveGrid_nz !z + ! DO J = 1,WaveGrid_ny !y + ! DO K = 1,WaveGrid_nx !x + ! Itemp = (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node + ! + ! WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%ux(l,I,J,K) + ! WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%uy(l,I,J,K) + ! WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%uz(l,I,J,K) + ! END DO + ! END DO + ! END DO + ! + ! ! wave accelerations + ! DO I=1,WaveGrid_nz !z + ! DO J = 1,WaveGrid_ny !y + ! DO K = 1,WaveGrid_nx !x + ! Itemp = (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node + ! + ! WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%ax(l,I,J,K) + ! WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%ay(l,I,J,K) + ! WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%az(l,I,J,K) + ! END DO + ! END DO + ! END DO + ! + ! ! dynamic pressure + ! DO I=1,WaveGrid_nz !z + ! DO J = 1,WaveGrid_ny !y + ! DO K = 1,WaveGrid_nx !x + ! Itemp = (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node + ! + ! WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%PDyn(l,I,J,K) + ! END DO + ! END DO + ! END DO + ! + ! ! end the line + ! WRITE(UnOut, "(A1)", IOSTAT=ErrStat2) " " + ! + ! + ! END DO + ! + ! + ! CLOSE(UnOut, IOSTAT = ErrStat ) + ! IF ( ErrStat /= 0 ) THEN + ! ErrMsg = 'Error closing wave grid file' + ! END IF @@ -4041,11 +4041,11 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, ! --------------------------------- apply wave kinematics ------------------------------------ - IF (p%WaterKin > 0) THEN ! wave kinematics interpolated from global grid in Waves object - DO i=0,N - CALL getWaveKin(p, Line%r(1,i), Line%r(2,i), Line%r(3,i), Line%time, m%WaveTi, Line%U(:,i), Line%Ud(:,i), Line%zeta(i), Line%PDyn(i)) - END DO - END IF + ! IF (p%WaterKin > 0) THEN ! wave kinematics interpolated from global grid in Waves object + ! DO i=0,N + ! CALL getWaveKin(p, Line%r(1,i), Line%r(2,i), Line%r(3,i), Line%time, m%WaveTi, Line%U(:,i), Line%Ud(:,i), Line%zeta(i), Line%PDyn(i)) + ! END DO + ! END IF ! --------------- calculate mass (including added mass) matrix for each node ----------------- @@ -5376,13 +5376,13 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ! --------------------------------- apply wave kinematics ------------------------------------ - IF (p%WaterKin == 1) THEN ! wave kinematics interpolated from global grid in Waves object - DO i=0,N - CALL getWaveKin(p, Rod%r(1,i), Rod%r(2,i), Rod%r(3,i), Rod%time, m%WaveTi, Rod%U(:,i), Rod%Ud(:,i), Rod%zeta(i), Rod%PDyn(i)) - !F(i) = 1.0 ! set VOF value to one for now (everything submerged - eventually this should be element-based!!!) <<<< - ! <<<< currently F is not being used and instead a VOF variable is used within the node loop - END DO - END IF + ! IF (p%WaterKin == 1) THEN ! wave kinematics interpolated from global grid in Waves object + ! DO i=0,N + ! CALL getWaveKin(p, Rod%r(1,i), Rod%r(2,i), Rod%r(3,i), Rod%time, m%WaveTi, Rod%U(:,i), Rod%Ud(:,i), Rod%zeta(i), Rod%PDyn(i)) + ! !F(i) = 1.0 ! set VOF value to one for now (everything submerged - eventually this should be element-based!!!) <<<< + ! ! <<<< currently F is not being used and instead a VOF variable is used within the node loop + ! END DO + ! END IF ! ! wave kinematics not implemented yet <<< From 2ed8c232ce785e3c9fc17a02350659d72d2ef4a1 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Thu, 10 Jun 2021 17:53:24 -0600 Subject: [PATCH 035/242] SrvD array bugfix from Andy, and proper MD groundBody initialization --- cmake/OpenfastFortranOptions.cmake | 2 +- modules/moordyn/src/MoorDyn.f90 | 5 ++++- modules/servodyn/src/ServoDyn.f90 | 30 +++++++++++++++++++++--------- 3 files changed, 26 insertions(+), 11 deletions(-) diff --git a/cmake/OpenfastFortranOptions.cmake b/cmake/OpenfastFortranOptions.cmake index 70126e17ef..591c03f72e 100644 --- a/cmake/OpenfastFortranOptions.cmake +++ b/cmake/OpenfastFortranOptions.cmake @@ -120,7 +120,7 @@ macro(set_fast_gfortran) # debug flags if(CMAKE_BUILD_TYPE MATCHES Debug) - set( CMAKE_Fortran_FLAGS_DEBUG "${CMAKE_Fortran_FLAGS_DEBUG} -fcheck=all,no-array-temps -pedantic -fbacktrace -finit-real=inf -finit-integer=9999." ) + set( CMAKE_Fortran_FLAGS_DEBUG "${CMAKE_Fortran_FLAGS_DEBUG} -fcheck=all,no-array-temps -pedantic -fbacktrace -finit-real=zero -finit-integer=9999." ) endif() if(CYGWIN) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 1e6800cecc..43f75427f9 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -445,6 +445,9 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! ---------------------- now go through again and process file contents -------------------- + call Body_Setup( m%GroundBody, m%zeros6, p%rhoW, ErrStat2, ErrMsg2) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN ! note: no longer worrying about "Echo" option @@ -1453,8 +1456,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! ================================ initialize system ================================ - ! call ground body to update all the fixed things... + m%GroundBody%r6(4:6) = 0.0_DbKi CALL Body_SetDependentKin(m%GroundBody, 0.0_DbKi, m) ! m%GroundBody%OrMat = EulerConstruct( m%GroundBody%r6(4:6) ) ! make sure it's OrMat is set up <<< need to check this approach diff --git a/modules/servodyn/src/ServoDyn.f90 b/modules/servodyn/src/ServoDyn.f90 index 9c4ee0e410..2cc39feb87 100644 --- a/modules/servodyn/src/ServoDyn.f90 +++ b/modules/servodyn/src/ServoDyn.f90 @@ -3890,16 +3890,28 @@ SUBROUTINE CableControl_CalcOutput( t, u, p, x, xd, z, OtherState, CableDeltaL, endif ! User-defined cable control from Bladed-style DLL CASE ( ControlMode_DLL ) - if (p%DLL_Ramp) then - factor = (t - m%LastTimeCalled) / m%dll_data%DLL_DT - CableDeltaL(1:p%NumCableControl) = m%dll_data%PrevCableDeltaL( 1:p%NumCableControl) + & - factor * ( m%dll_data%CableDeltaL( 1:p%NumCableControl) - m%dll_data%PrevCableDeltaL( 1:p%NumCableControl) ) - CableDeltaLdot(1:p%NumCableControl) = m%dll_data%PrevCableDeltaLdot(1:p%NumCableControl) + & - factor * ( m%dll_data%CableDeltaLdot(1:p%NumCableControl) - m%dll_data%PrevCableDeltaLdot(1:p%NumCableControl) ) + if (allocated(m%dll_data%PrevCableDeltaL)) then + if (p%DLL_Ramp) then + factor = (t - m%LastTimeCalled) / m%dll_data%DLL_DT + CableDeltaL(1:p%NumCableControl) = m%dll_data%PrevCableDeltaL( 1:p%NumCableControl) + & + factor * ( m%dll_data%CableDeltaL( 1:p%NumCableControl) - m%dll_data%PrevCableDeltaL( 1:p%NumCableControl) ) + else + CableDeltaL( 1:p%NumCableControl) = m%dll_data%CableDeltaL( 1:p%NumCableControl) + end if else - CableDeltaL( 1:p%NumCableControl) = m%dll_data%CableDeltaL( 1:p%NumCableControl) - CableDeltaLdot(1:p%NumCableControl) = m%dll_data%CableDeltaLdot(1:p%NumCableControl) - end if + CableDeltaL = 0.0_ReKi + endif + if (allocated(m%dll_data%PrevCableDeltaLdot)) then + if (p%DLL_Ramp) then + factor = (t - m%LastTimeCalled) / m%dll_data%DLL_DT + CableDeltaLdot(1:p%NumCableControl) = m%dll_data%PrevCableDeltaLdot(1:p%NumCableControl) + & + factor * ( m%dll_data%CableDeltaLdot(1:p%NumCableControl) - m%dll_data%PrevCableDeltaLdot(1:p%NumCableControl) ) + else + CableDeltaLdot(1:p%NumCableControl) = m%dll_data%CableDeltaLdot(1:p%NumCableControl) + endif + else + CableDeltaLdot = 0.0_ReKi + endif END SELECT END SUBROUTINE CableControl_CalcOutput From ff925dd9065970725c16dda30ef857c4550b6cee Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Mon, 14 Jun 2021 11:58:06 -0600 Subject: [PATCH 036/242] StC ctrl: controller input nominally 0 --- docs/source/user/subdyn/theory.rst | 75 +++++++++++++++++++----------- modules/subdyn/src/SD_FEM.f90 | 3 +- modules/subdyn/src/SubDyn.f90 | 6 ++- 3 files changed, 56 insertions(+), 28 deletions(-) diff --git a/docs/source/user/subdyn/theory.rst b/docs/source/user/subdyn/theory.rst index 9ce6b69c18..5d213ecffc 100644 --- a/docs/source/user/subdyn/theory.rst +++ b/docs/source/user/subdyn/theory.rst @@ -625,7 +625,7 @@ of the element writes :math:`\boldsymbol{f}_e=\boldsymbol{K}_e\boldsymbol{u}+\boldsymbol{f}_{e,0}`, with: -.. math:: +.. math:: :label: StiffnessMatrixCable \begin{aligned} \begin{bmatrix} @@ -664,7 +664,7 @@ with: 0\\ 1\\ \end{bmatrix} - \label{eq:StiffnessMatrixCable}\end{aligned} + \end{aligned} The relation above is expressed in the element coordinate system. The stiffness matrix and force vector are transferred to the global system @@ -706,26 +706,50 @@ with :math:`L_e` the *undisplaced* length of the element (not Controlled pretension cable ^^^^^^^^^^^^^^^^^^^^^^^^^^^ -The controller updates the value of :math:`\Delta L` at each time step, -which effectively changes the pretension properties of the cable. The -quantity :math:`\Delta L` is the change in restlength if the cable had -no pretension. Since cable extension beyond the element length -(:math:`L_e`) is not allowed in SubDyn, :math:`\Delta L` is limited to -negative values. - -At a given time, the restlength of the cable is :math:`L_r(t)` (instead +The controller changes the rest length of the cable at each time step, effectively changing the pretension properties of the cable. +At a given time, the restlength of the cable is :math:`L_r(t)=L_e + \Delta L` (instead of :math:`L_0`), and the pretension force is :math:`T(t)` (instead of -:math:`T_0`). The pretension force is then given as: +:math:`T_0`). The pretension force is given as: -.. math:: +.. math:: :label: tensionUnsteady + + \begin{aligned} + T(t)= E A \frac{-\Delta L(t)}{L_r(t)} = E A \frac{-\Delta L(t)}{L_e + \Delta L(t)} + \end{aligned} + +At :math:`t=0`, when no controller action is present, the pretension force and length are: + +.. math:: :label: tensionZero \begin{aligned} - T(t)= E A \frac{-\Delta L_r(t)}{L_r(t)} = E A \frac{-\Delta L_r(t)}{L_e + \Delta L(t)} - ,\quad T(0) =T_0= E A \frac{-\Delta L_0}{L_e + \Delta L_0} ,\quad - \Delta L(0) = \Delta L_0\end{aligned} + \Delta L(0) = \Delta L_0 = \frac{-L_e T_0}{EA+T_0} + \end{aligned} + + +The quantity :math:`\Delta L` is the change in restlength, and it is given as: + +.. math:: :label: DeltaLTot + \begin{aligned} + \Delta L(t) = \Delta L_0 + \Delta L_c(t) + \end{aligned} + +where :math:`\Delta L_c` is the change of length prescribed by the controller, and :math:`\Delta L_0` +is the change of length attributed to the initial pretension. This choice is such that the controller input is nominally 0. Cable extension beyond the element length +(:math:`L_e`) is not allowed in SubDyn, therefore :math:`\Delta L` is limited to +negative values (:math:`L_r=L_e+\Delta L <= L_e`). +The tension force at a given time is given by inserting :eq:`DeltaLTot` into :eq:`tensionUnsteady`: + +.. math:: + + \begin{aligned} + T(t)=- E A \frac{\Delta L_0 + \Delta L_c }{L_e + \Delta L_0 + \Delta L_c} + \end{aligned} + + +In the following we provide details on the implementation and the approximation introduced. The “equations of motions” for a cable element are written: .. math:: @@ -733,28 +757,27 @@ The “equations of motions” for a cable element are written: \begin{aligned} \boldsymbol{M}_e\boldsymbol{\ddot{u}}_e&= \boldsymbol{f}_e\end{aligned} -If the pretension force is constant, equal to :math:`T_0` then the -element force is: +If the pretension force is constant (equal to :math:`T_0`), and additional external loads are neglected, then the element force is: -.. math:: +.. math:: :label: CstCableA \begin{aligned} \boldsymbol{f}_e=\boldsymbol{f}_e (t,T_0) &=-\boldsymbol{K}_c(T_0) \boldsymbol{u}_e + \boldsymbol{f}_c(T_0)+ \boldsymbol{f}_g - \label{eq:CableEqMotionT0}\end{aligned} + \end{aligned} where :math:`\boldsymbol{f}_c(T_0)` and :math:`\boldsymbol{K}_c(T_0)` -are given in . If the pretension force is varying with time +are given in :eq:`StiffnessMatrixCable`. If the pretension force is varying with time (:math:`T=T(t)`), then the force is: -.. math:: +.. math:: :label: VaryingCableA \begin{aligned} - \boldsymbol{f}_e (t) =-\boldsymbol{K}_c(T) \boldsymbol{u}_e + \boldsymbol{f}_c(T)+ \boldsymbol{f}_g - \label{eq:VaryingCableA}\end{aligned} + \boldsymbol{f}_e (t) =-\boldsymbol{K}_c(T) \boldsymbol{u}_e + \boldsymbol{f}_c(T)+ \boldsymbol{f}_g + \end{aligned} -where is evaluated with :math:`\epsilon=\frac{T}{EA}` and -:math:`L=\frac{L_e}{1+\epsilon}`. We seek to express , as a correction -term added to the equation of a constant pretension cable (i.e. , with +where :eq:`VaryingCableA` is evaluated with :math:`\epsilon=\frac{T}{EA}` and +:math:`L=\frac{L_e}{1+\epsilon}`. We seek to express :eq:`VaryingCableA`, as a correction +term added to the equation of a constant pretension cable (i.e. :eq:`CstCableA`, with :math:`T(0)=T_0`). We add :math:`\pm\boldsymbol{f}_e(t,T_0)` to , leading to: diff --git a/modules/subdyn/src/SD_FEM.f90 b/modules/subdyn/src/SD_FEM.f90 index ca2cd5b5d9..03af422913 100644 --- a/modules/subdyn/src/SD_FEM.f90 +++ b/modules/subdyn/src/SD_FEM.f90 @@ -1183,7 +1183,8 @@ subroutine ControlCableMapping(Init, uInit, p, InitOut, ErrStat, ErrMsg) do i = 1, nCC iElem = p%CtrlElem2Channel(i,1) ! DeltaL 0 = - Le T0 / (EA + T0) = - Le eps0 / (1+eps0) - uInit%CableDeltaL(i) = - p%ElemProps(iElem)%Length * p%ElemProps(iElem)%T0 / (p%ElemProps(iElem)%YoungE*p%ElemProps(iElem)%Area + p%ElemProps(iElem)%T0) + !uInit%CableDeltaL(i) = - p%ElemProps(iElem)%Length * p%ElemProps(iElem)%T0 / (p%ElemProps(iElem)%YoungE*p%ElemProps(iElem)%Area + p%ElemProps(iElem)%T0) + uInit%CableDeltaL(i) = 0.0_ReKi enddo contains diff --git a/modules/subdyn/src/SubDyn.f90 b/modules/subdyn/src/SubDyn.f90 index 37428bbc19..7d177ad77b 100644 --- a/modules/subdyn/src/SubDyn.f90 +++ b/modules/subdyn/src/SubDyn.f90 @@ -3001,6 +3001,7 @@ SUBROUTINE GetExtForceOnInternalDOF(u, p, x, m, F_L, ErrStat, ErrMsg, GuyanLoadC integer :: iCC, iElem, iChannel !< Index on control cables, element, Channel integer(IntKi), dimension(12) :: IDOF ! 12 DOF indices in global unconstrained system real(ReKi) :: CableTension ! Controllable Cable force + real(ReKi) :: DeltaL ! Change of length real(ReKi) :: rotations(3) real(ReKi) :: du(3), Moment(3), Force(3) real(ReKi) :: u_TP(6) @@ -3050,8 +3051,11 @@ SUBROUTINE GetExtForceOnInternalDOF(u, p, x, m, F_L, ErrStat, ErrMsg, GuyanLoadC iElem = p%CtrlElem2Channel(iCC,1) iChannel = p%CtrlElem2Channel(iCC,2) IDOF = p%ElemsDOF(1:12, iElem) + ! DeltaL = DeltaL0 + DeltaL_control = - Le T0/(EA+T0) + DeltaL_control + DeltaL = - p%ElemProps(iElem)%Length * p%ElemProps(iElem)%T0 / (p%ElemProps(iElem)%YoungE*p%ElemProps(iElem)%Area + p%ElemProps(iElem)%T0) + DeltaL = DeltaL + u%CableDeltaL(iChannel) ! T(t) = - EA * DeltaL(t) /(Le + Delta L(t)) ! NOTE DeltaL<0 - CableTension = -p%ElemProps(iElem)%YoungE*p%ElemProps(iElem)%Area * u%CableDeltaL(iChannel) / (p%ElemProps(iElem)%Length + u%CableDeltaL(iChannel)) + CableTension = -p%ElemProps(iElem)%YoungE*p%ElemProps(iElem)%Area * DeltaL / (p%ElemProps(iElem)%Length + DeltaL) if (RotateLoads) then ! in body coordinate ! We only rotate the loads, moments are rotated below m%Fext(IDOF(1:3)) = m%Fext(IDOF(1:3)) + matmul(Rg2b,m%FC_unit( IDOF(1:3) ) * (CableTension - p%ElemProps(iElem)%T0)) From 8a4489e0cf3f250237c24d330bce1a2f8875f0d2 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Wed, 16 Jun 2021 13:12:44 -0600 Subject: [PATCH 037/242] Revert commit 2ed8c232 changes to cmake setup --- cmake/OpenfastFortranOptions.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/OpenfastFortranOptions.cmake b/cmake/OpenfastFortranOptions.cmake index 591c03f72e..70126e17ef 100644 --- a/cmake/OpenfastFortranOptions.cmake +++ b/cmake/OpenfastFortranOptions.cmake @@ -120,7 +120,7 @@ macro(set_fast_gfortran) # debug flags if(CMAKE_BUILD_TYPE MATCHES Debug) - set( CMAKE_Fortran_FLAGS_DEBUG "${CMAKE_Fortran_FLAGS_DEBUG} -fcheck=all,no-array-temps -pedantic -fbacktrace -finit-real=zero -finit-integer=9999." ) + set( CMAKE_Fortran_FLAGS_DEBUG "${CMAKE_Fortran_FLAGS_DEBUG} -fcheck=all,no-array-temps -pedantic -fbacktrace -finit-real=inf -finit-integer=9999." ) endif() if(CYGWIN) From dc295314e9e42a52370a8d77bbe01adf5213db76 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Wed, 16 Jun 2021 13:19:44 -0600 Subject: [PATCH 038/242] MDv2: missed wave kin stuff in FAST_Subs for commit ca9380ce --- modules/openfast-library/src/FAST_Subs.f90 | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/modules/openfast-library/src/FAST_Subs.f90 b/modules/openfast-library/src/FAST_Subs.f90 index 86e4254146..873f3adc4f 100644 --- a/modules/openfast-library/src/FAST_Subs.f90 +++ b/modules/openfast-library/src/FAST_Subs.f90 @@ -929,17 +929,17 @@ SUBROUTINE FAST_InitializeAll( t_initial, p_FAST, y_FAST, m_FAST, ED, BD, SrvD, !::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: !@mhall: for now, passing some hardcoded wave kinematics grid info from HD to MD - ALLOCATE ( Init%InData_MD%WaveVel (HD%p%NStepWave, WaveGrid_n, 3) ) - ALLOCATE ( Init%InData_MD%WaveAcc (HD%p%NStepWave, WaveGrid_n, 3) ) - ALLOCATE ( Init%InData_MD%WavePDyn (HD%p%NStepWave, WaveGrid_n) ) - ALLOCATE ( Init%InData_MD%WaveElev (HD%p%NStepWave, WaveGrid_n) ) - ALLOCATE ( Init%InData_MD%WaveTime (HD%p%NStepWave) ) - - Init%InData_MD%WaveVel = Init%OutData_HD%WaveVel - Init%InData_MD%WaveAcc = Init%OutData_HD%WaveAcc - Init%InData_MD%WavePDyn = Init%OutData_HD%WaveDynP - Init%InData_MD%WaveElev = Init%OutData_HD%WaveElev - Init%InData_MD%WaveTime = Init%OutData_HD%WaveTime +! ALLOCATE ( Init%InData_MD%WaveVel (HD%p%NStepWave, WaveGrid_n, 3) ) +! ALLOCATE ( Init%InData_MD%WaveAcc (HD%p%NStepWave, WaveGrid_n, 3) ) +! ALLOCATE ( Init%InData_MD%WavePDyn (HD%p%NStepWave, WaveGrid_n) ) +! ALLOCATE ( Init%InData_MD%WaveElev (HD%p%NStepWave, WaveGrid_n) ) +! ALLOCATE ( Init%InData_MD%WaveTime (HD%p%NStepWave) ) + +! Init%InData_MD%WaveVel = Init%OutData_HD%WaveVel +! Init%InData_MD%WaveAcc = Init%OutData_HD%WaveAcc +! Init%InData_MD%WavePDyn = Init%OutData_HD%WaveDynP +! Init%InData_MD%WaveElev = Init%OutData_HD%WaveElev +! Init%InData_MD%WaveTime = Init%OutData_HD%WaveTime !CALL MOVE_ALLOC( Init%OutData_HD%WaveVel , Init%InData_MD%WaveVel ) !CALL MOVE_ALLOC( Init%OutData_HD%WaveAcc , Init%InData_MD%WaveAcc ) From 0a37d12063fe2662c01e0a54824f0aa8d54c7a90 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Thu, 17 Jun 2021 11:33:51 -0600 Subject: [PATCH 039/242] MDv2 lin: missing initialization and unsafe size() of potentially unallocated arrays --- modules/moordyn/src/MoorDyn.f90 | 65 ++++++++++++++-------- modules/openfast-library/src/FAST_Subs.f90 | 19 ++++++- 2 files changed, 61 insertions(+), 23 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 43f75427f9..43ff891518 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -2679,7 +2679,7 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er IF ( u%DeltaL(m%LineList(L)%CtrlChan) > m%LineList(L)%UnstrLen / m%LineList(L)%N ) then ErrStat = ErrID_Fatal ErrMsg = ' Active tension command will make a segment longer than the limit of twice its original length.' - print *, u%DeltaL(m%LineList(L)%CtrlChan), " is an increase of more than ", (m%LineList(L)%UnstrLen / m%LineList(L)%N) + call WrScr(trim(Num2LStr(u%DeltaL(m%LineList(L)%CtrlChan)))//" is an increase of more than "//trim(Num2LStr(m%LineList(L)%UnstrLen / m%LineList(L)%N))) IF (wordy > 0) print *, u%DeltaL IF (wordy > 0) print*, m%LineList(L)%CtrlChan RETURN @@ -2687,7 +2687,7 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er IF ( u%DeltaL(m%LineList(L)%CtrlChan) < -0.5 * m%LineList(L)%UnstrLen / m%LineList(L)%N ) then ErrStat = ErrID_Fatal ErrMsg = ' Active tension command will make a segment shorter than the limit of half its original length.' - print *, u%DeltaL(m%LineList(L)%CtrlChan), " is a reduction of more than half of ", (m%LineList(L)%UnstrLen / m%LineList(L)%N) + call WrScr(trim(Num2LStr(u%DeltaL(m%LineList(L)%CtrlChan)))//" is a reduction of more than half of "//trim(Num2LStr(m%LineList(L)%UnstrLen / m%LineList(L)%N))) IF (wordy > 0) print *, u%DeltaL IF (wordy > 0) print*, m%LineList(L)%CtrlChan RETURN @@ -6676,12 +6676,14 @@ SUBROUTINE MD_GetOP( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, u_op, call PackMotionMesh(u%CoupledKinematics, u_op, idx, FieldMask=FieldMask) ! now do the active tensioning commands if there are any - do i=1,p%nCtrlChans - u_op(idx) = u%DeltaL(i) - idx = idx + 1 - u_op(idx) = u%DeltaLdot(i) - idx = idx + 1 - end do + if (allocated(u%DeltaL)) then + do i=1,size(u%DeltaL) + u_op(idx) = u%DeltaL(i) + idx = idx + 1 + u_op(idx) = u%DeltaLdot(i) + idx = idx + 1 + end do + endif END IF ! outputs IF ( PRESENT( y_op ) ) THEN @@ -7578,9 +7580,13 @@ END SUBROUTINE Init_Jacobian_x SUBROUTINE Init_Jacobian_u() REAL(R8Ki) :: perturb INTEGER(IntKi) :: i, j, idx, nu, i_meshField + character(10) :: LinStr ! for noting which line a DeltaL control is attached to + logical :: LinCtrl ! Is the current DeltaL channel associated with a line? ! Number of inputs + i = 0 + if (allocated(u%DeltaL)) i=size(u%DeltaL) nu = u%CoupledKinematics%nNodes * 18 & ! 3 Translation Displacements + 3 orientations + 6 velocities + 6 accelerations at each node <<<<<<< - + size(u%DeltaL)*2 ! a deltaL and rate of change for each active tension control channel + + i*2 ! a deltaL and rate of change for each active tension control channel ! --- Info of linearized inputs (Names, RotFrame, IsLoad) call AllocAry(InitOut%LinNames_u, nu, 'LinNames_u', ErrStat2, ErrMsg2); if(ErrStat2/=ErrID_None) return @@ -7599,6 +7605,7 @@ SUBROUTINE Init_Jacobian_u() ! column 2 indicates the first index (x-y-z component) of the field ! column 3 is the node call allocAry( p%Jac_u_indx, nu, 3, 'p%Jac_u_indx', ErrStat2, ErrMsg2); if(ErrStat2/=ErrID_None) return + p%Jac_u_indx = 0 ! initialize to zero idx = 1 !Module/Mesh/Field: u%CoupledKinematics%TranslationDisp = 1; !Module/Mesh/Field: u%CoupledKinematics%Orientation = 2; @@ -7617,19 +7624,33 @@ SUBROUTINE Init_Jacobian_u() end do !i end do ! now do the active tensioning commands if there are any - do i=1,p%nCtrlChans - p%Jac_u_indx(idx,1) = 10 ! 10-11 mean active tension changes (10: deltaL; 11: deltaLdot) - p%Jac_u_indx(idx,2) = 0 ! not used - p%Jac_u_indx(idx,3) = i ! indicates channel number - InitOut%LinNames_u(idx) = 'CtrlChan '//trim(num2lstr(i))//' DeltaL, m' - idx = idx + 1 - - p%Jac_u_indx(idx,1) = 11 - p%Jac_u_indx(idx,2) = 0 - p%Jac_u_indx(idx,3) = i - InitOut%LinNames_u(idx) = 'CtrlChan '//trim(num2lstr(i))//' DeltaLdot, m/s' - idx = idx + 1 - end do + if (allocated(u%DeltaL)) then + do i=1,size(u%DeltaL) ! Signals may be passed in without being requested for control + ! Figure out if this DeltaL control channel is associated with a line or multiple or none and label + LinCtrl = .FALSE. + LinStr = '(lines: ' + do J=1,p%NLines + if (m%LineList(J)%CtrlChan == i) then + LinCtrl = .TRUE. + LinStr = LinStr//trim(num2lstr(i))//' ' + endif + enddo + if ( LinCtrl) LinStr = LinStr//' )' + if (.not. LinCtrl) LinStr = '(lines: none)' + + p%Jac_u_indx(idx,1) = 10 ! 10-11 mean active tension changes (10: deltaL; 11: deltaLdot) + p%Jac_u_indx(idx,2) = 0 ! not used + p%Jac_u_indx(idx,3) = i ! indicates DeltaL entry number + InitOut%LinNames_u(idx) = 'CtrlChan DeltaL '//trim(num2lstr(i))//', m '//trim(LinStr) + idx = idx + 1 + + p%Jac_u_indx(idx,1) = 11 + p%Jac_u_indx(idx,2) = 0 + p%Jac_u_indx(idx,3) = i + InitOut%LinNames_u(idx) = 'CtrlChan DeltaLdot '//trim(num2lstr(i))//', m/s'//trim(LinStr) + idx = idx + 1 + end do + endif ! --- Default perturbations, p%du: call allocAry( p%du, 11, 'p%du', ErrStat2, ErrMsg2); if(ErrStat2/=ErrID_None) return diff --git a/modules/openfast-library/src/FAST_Subs.f90 b/modules/openfast-library/src/FAST_Subs.f90 index 873f3adc4f..322f2a941b 100644 --- a/modules/openfast-library/src/FAST_Subs.f90 +++ b/modules/openfast-library/src/FAST_Subs.f90 @@ -925,7 +925,9 @@ SUBROUTINE FAST_InitializeAll( t_initial, p_FAST, y_FAST, m_FAST, ED, BD, SrvD, Init%InData_MD%g = Init%OutData_ED%Gravity ! This need to be according to g used in ElastoDyn Init%InData_MD%rhoW = Init%OutData_HD%WtrDens ! This needs to be set according to seawater density in HydroDyn Init%InData_MD%WtrDepth = Init%OutData_HD%WtrDpth ! This need to be set according to the water depth in HydroDyn - + + Init%InData_MD%Linearize = p_FAST%Linearize + !::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: !@mhall: for now, passing some hardcoded wave kinematics grid info from HD to MD @@ -957,6 +959,21 @@ SUBROUTINE FAST_InitializeAll( t_initial, p_FAST, y_FAST, m_FAST, ED, BD, SrvD, CALL SetModuleSubstepTime(Module_MD, p_FAST, y_FAST, ErrStat2, ErrMsg2) CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + allocate( y_FAST%Lin%Modules(MODULE_MD)%Instance(1), stat=ErrStat2) + if (ErrStat2 /= 0 ) then + call SetErrStat(ErrID_Fatal, "Error allocating Lin%Modules(MD).", ErrStat, ErrMsg, RoutineName ) + else + if (allocated(Init%OutData_MD%LinNames_y)) call move_alloc(Init%OutData_MD%LinNames_y,y_FAST%Lin%Modules(MODULE_MD)%Instance(1)%Names_y) + if (allocated(Init%OutData_MD%LinNames_x)) call move_alloc(Init%OutData_MD%LinNames_x,y_FAST%Lin%Modules(MODULE_MD)%Instance(1)%Names_x) + if (allocated(Init%OutData_MD%LinNames_u)) call move_alloc(Init%OutData_MD%LinNames_u,y_FAST%Lin%Modules(MODULE_MD)%Instance(1)%Names_u) + if (allocated(Init%OutData_MD%RotFrame_y)) call move_alloc(Init%OutData_MD%RotFrame_y,y_FAST%Lin%Modules(MODULE_MD)%Instance(1)%RotFrame_y) + if (allocated(Init%OutData_MD%RotFrame_x)) call move_alloc(Init%OutData_MD%RotFrame_x,y_FAST%Lin%Modules(MODULE_MD)%Instance(1)%RotFrame_x) + if (allocated(Init%OutData_MD%RotFrame_u)) call move_alloc(Init%OutData_MD%RotFrame_u,y_FAST%Lin%Modules(MODULE_MD)%Instance(1)%RotFrame_u) + if (allocated(Init%OutData_MD%IsLoad_u )) call move_alloc(Init%OutData_MD%IsLoad_u ,y_FAST%Lin%Modules(MODULE_MD)%Instance(1)%IsLoad_u ) + if (allocated(Init%OutData_MD%WriteOutputHdr)) y_FAST%Lin%Modules(MODULE_MD)%Instance(1)%NumOutputs = size(Init%OutData_MD%WriteOutputHdr) + if (allocated(Init%OutData_MD%DerivOrder_x)) call move_alloc(Init%OutData_MD%DerivOrder_x,y_FAST%Lin%Modules(MODULE_MD)%Instance(1)%DerivOrder_x) + end if + IF (ErrStat >= AbortErrLev) THEN CALL Cleanup() RETURN From c5280321852753d06a2489fd49f199f1a6593315 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Fri, 25 Jun 2021 15:49:07 -0600 Subject: [PATCH 040/242] MoorDyn linearization fixes and default parameters: - Added missing items for states in my FAST_Lin calling to MD. - Added missing items for states in MD linearization subroutines. - Added allocate dxdt when needed in MD_CalcContStateDeriv. - Adjusted MD settings/parameters so that all have default values specified up-front, to avoid any potential for uninitialized ones. --- modules/moordyn/src/MoorDyn.f90 | 59 +++++++++++++---------- modules/openfast-library/src/FAST_Lin.f90 | 22 +++++++-- 2 files changed, 51 insertions(+), 30 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 43ff891518..32de00d774 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -155,23 +155,27 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! Get all the inputs taken care of !--------------------------------------------------------------------------------------------- + p%RootName = TRIM(InitInp%RootName)//'.MD' ! all files written from this module will have this root name + + ! set default values for the simulation settings + ! these defaults are based on the glue code + p%dtM0 = DTcoupling ! default to the coupling interval (but will likely need to be smaller) + p%g = InitInp%g + p%rhoW = InitInp%rhoW + p%WtrDpth = InitInp%WtrDepth + ! set the following to some defaults + p%kBot = 3.0E6 + p%cBot = 3.0E5 + InputFileDat%dtIC = 2.0_DbKi + InputFileDat%TMaxIC = 60.0_DbKi + InputFileDat%CdScaleIC = 4.0_ReKi + InputFileDat%threshIC = 0.01_ReKi + p%WaterKin = 0_IntKi + !p%dtOut = 0.0_DbKi - ! set environmental parameters from input data and error check - ! (should remove these values as options from MoorDyn input file for consistency?) - p%g = InitInp%g - p%WtrDpth = InitInp%WtrDepth - p%rhoW = InitInp%rhoW - p%RootName = TRIM(InitInp%RootName)//'.MD' ! all files written from this module will have this root name -!FIXME: Set some of the options -- the way parsing is written, they don't have to exist in the input file, but get used anyhow. -! Setting these to some value for the moment -- trying to figure out why I get NaN's with the -Wuninitialized -finit-real=inf -finit-integer=9999 flags set. -p%dtM0 = DTcoupling ! default to the coupling -!p%WtrDpth = InitInp%WtrDpth ! This will be passed in later. Right now use the default of -9999 in the registry -p%kBot = 0 -p%cBot = 0 -p%WaterKin = 0 ! Check for farm-level inputs (indicating that this MoorDyn isntance is being run from FAST.Farm) !intead of below, check first dimension of PtfmInit @@ -1223,9 +1227,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL Conv2UC(OptString) ! check all possible options types and see if OptString is one of them, in which case set the variable. -!FIXME: if some of these are not found in the input file they won't get set if ( OptString == 'DTM') THEN - read (OptValue,*) p%dtM0 ! InitInp%DTmooring + read (OptValue,*) p%dtM0 else if ( OptString == 'G') then read (OptValue,*) p%g else if ( OptString == 'RHOW') then @@ -1246,6 +1249,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er read (OptValue,*) InputFileDat%threshIC else if ( OptString == 'WATERKIN') then read (OptValue,*) p%WaterKin + !else if ( OptString == 'DTOUT') then + ! read (OptValue,*) p%dtOut else CALL SetErrStat( ErrID_Warn, 'unable to interpret input '//trim(OptString), ErrStat, ErrMsg, RoutineName ) end if @@ -2199,6 +2204,7 @@ CHARACTER(1024) function NextLine(i) NextLine="---" ! Set as a separator so we can escape some of the while loops else NextLine=trim(FileInfo_In%Lines(i)) + !TODO: add comment character recognition here? (discard any characters past a #) endif end function NextLine @@ -2584,16 +2590,20 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er REAL(DbKi) :: rd_in(3) ! temporary for passing kinematics REAL(DbKi) :: a_in(3) ! temporary for passing kinematics + INTEGER(IntKi) :: ErrStat2 ! Error status of the operation + CHARACTER(ErrMsgLen) :: ErrMsg2 ! Error message if ErrStat2 /= ErrID_None + character(*), parameter :: RoutineName = 'MD_CalcContStateDeriv' + ! Initialize ErrStat ErrStat = ErrID_None ErrMsg = "" -! ! allocations of dxdt (as in SubDyn. "INTENT(OUT) automatically deallocates the arrays on entry, we have to allocate them here" is this right/efficient?) -! ALLOCATE ( dxdt%states(size(x%states)), STAT = ErrStat ) -! IF ( ErrStat /= ErrID_None ) THEN -! ErrMsg = ' Error allocating dxdt%states array.' -! RETURN -! END IF + ! allocate dxdt if not already allocated (e.g. if called for linearization) + IF (.NOT. ALLOCATED(dxdt%states) ) THEN + CALL AllocAry( dxdt%states, SIZE(x%states), 'dxdt%states', ErrStat2, ErrMsg2 ) + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF ( ErrStat >= AbortErrLev ) RETURN + END IF ! clear connection force and mass values = AbortErrLev if (Failed) call CleanUp() end function Failed diff --git a/modules/openfast-library/src/FAST_Lin.f90 b/modules/openfast-library/src/FAST_Lin.f90 index e6e92668f6..cfc099b6ba 100644 --- a/modules/openfast-library/src/FAST_Lin.f90 +++ b/modules/openfast-library/src/FAST_Lin.f90 @@ -1105,10 +1105,18 @@ SUBROUTINE FAST_Linearize_OP(t_global, p_FAST, y_FAST, m_FAST, ED, BD, SrvD, AD, MD%OtherSt(STATE_CURR), MD%y, MD%m, ErrStat2, ErrMsg2, y_FAST%Lin%Modules(Module_MD)%Instance(1)%D ) call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + call MD_JacobianPContState( t_global, MD%Input(1), MD%p, MD%x(STATE_CURR), MD%xd(STATE_CURR), MD%z(STATE_CURR), MD%OtherSt(STATE_CURR), & + MD%y, MD%m, ErrStat2, ErrMsg2, dYdx=y_FAST%Lin%Modules(Module_MD)%Instance(1)%C, & + dXdx=y_FAST%Lin%Modules(Module_MD)%Instance(1)%A ) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + ! get the operating point call MD_GetOP( t_global, MD%Input(1), MD%p, MD%x(STATE_CURR), MD%xd(STATE_CURR), MD%z(STATE_CURR), & MD%OtherSt(STATE_CURR), MD%y, MD%m, ErrStat2, ErrMsg2, & - y_FAST%Lin%Modules(Module_MD)%Instance(1)%op_u, y_FAST%Lin%Modules(Module_MD)%Instance(1)%op_y ) + u_op=y_FAST%Lin%Modules(Module_MD)%Instance(1)%op_u, & + y_op=y_FAST%Lin%Modules(Module_MD)%Instance(1)%op_y, & + x_op=y_FAST%Lin%Modules(Module_MD)%Instance(1)%op_x, & + dx_op=y_FAST%Lin%Modules(Module_MD)%Instance(1)%op_dx ) call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) if (ErrStat >=AbortErrLev) then call cleanup() @@ -1128,10 +1136,12 @@ SUBROUTINE FAST_Linearize_OP(t_global, p_FAST, y_FAST, m_FAST, ED, BD, SrvD, AD, if (p_FAST%LinOutJac) then ! Jacobians - !dYdu: - call WrPartialMatrix( y_FAST%Lin%Modules(Module_MD)%Instance(1)%D, Un, p_FAST%OutFmt, 'dYdu', & - UseRow=y_FAST%Lin%Modules(Module_MD)%Instance(1)%use_y, & - UseCol=y_FAST%Lin%Modules(Module_MD)%Instance(1)%use_u ) + ! dXdx, dXdu, dYdx, dYdu: + call WrPartialMatrix( y_FAST%Lin%Modules(Module_MD)%Instance(1)%A, Un, p_FAST%OutFmt, 'dXdx' ) + call WrPartialMatrix( y_FAST%Lin%Modules(Module_MD)%Instance(1)%B, Un, p_FAST%OutFmt, 'dXdu', UseCol=y_FAST%Lin%Modules(Module_MD)%Instance(1)%use_u ) + call WrPartialMatrix( y_FAST%Lin%Modules(Module_MD)%Instance(1)%C, Un, p_FAST%OutFmt, 'dYdx', UseRow=y_FAST%Lin%Modules(Module_MD)%Instance(1)%use_y ) + call WrPartialMatrix( y_FAST%Lin%Modules(Module_MD)%Instance(1)%D, Un, p_FAST%OutFmt, 'dYdu', UseRow=y_FAST%Lin%Modules(Module_MD)%Instance(1)%use_y, & + UseCol=y_FAST%Lin%Modules(Module_MD)%Instance(1)%use_u ) end if ! finish writing the file @@ -4904,6 +4914,7 @@ SUBROUTINE SaveOP(i, p_FAST, y_FAST, ED, BD, SrvD, AD, IfW, OpFM, HD, SD, ExtPtf CALL MAP_CopyInput (MAPp%Input(1), y_FAST%op%u_MAP(i), CtrlCode, Errstat2, ErrMsg2) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + ELSEIF (p_FAST%CompMooring == Module_MD) THEN CALL MD_CopyContState (MD%x( STATE_CURR), y_FAST%op%x_MD(i), CtrlCode, Errstat2, ErrMsg2) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) @@ -4916,6 +4927,7 @@ SUBROUTINE SaveOP(i, p_FAST, y_FAST, ED, BD, SrvD, AD, IfW, OpFM, HD, SD, ExtPtf CALL MD_CopyInput (MD%Input(1), y_FAST%op%u_MD(i), CtrlCode, Errstat2, ErrMsg2) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + ELSEIF (p_FAST%CompMooring == Module_FEAM) THEN CALL FEAM_CopyContState (FEAM%x( STATE_CURR), y_FAST%op%x_FEAM(i), CtrlCode, Errstat2, ErrMsg2) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) From 044cc8dbc88420359fb025a0fe5939b9b35abe8a Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Fri, 2 Jul 2021 10:21:04 -0600 Subject: [PATCH 041/242] Fixed/improved error handling edits in FARM_UpdateStates: - Fixed bug with unallocated AWAE errstat check. - Now using ...AWAE rather than ...F error variables for AWAE for clarity. - Better order of error checks. --- glue-codes/fast-farm/src/FAST_Farm_Subs.f90 | 40 +++++++++++---------- modules/moordyn/src/MoorDyn.f90 | 4 ++- 2 files changed, 25 insertions(+), 19 deletions(-) diff --git a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 index 33dff9a311..b21e476c93 100644 --- a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 +++ b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 @@ -1868,10 +1868,12 @@ subroutine FARM_MD_Increment(t, n, farm, ErrStat, ErrMsg) CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'FARM_MD_Increment' - + ErrStat = ErrID_None + ErrMsg = "" + ! ----- extrapolate MD inputs ----- t_next = t + farm%p%DT_mooring - + ! Do a linear extrapolation to estimate MoorDyn inputs at time n_ss+1 CALL MD_Input_ExtrapInterp(farm%MD%Input, farm%MD%InputTimes, farm%MD%u, t_next, ErrStat2, ErrMsg2) CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName ) @@ -1880,13 +1882,13 @@ subroutine FARM_MD_Increment(t, n, farm, ErrStat, ErrMsg) CALL MD_CopyInput (farm%MD%Input(1), farm%MD%Input(2), MESH_UPDATECOPY, Errstat2, ErrMsg2) CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName ) farm%MD%InputTimes(2) = farm%MD%InputTimes(1) - + ! update index 1 entries with the new extrapolated values CALL MD_CopyInput (farm%MD%u, farm%MD%Input(1), MESH_UPDATECOPY, Errstat2, ErrMsg2) CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName ) farm%MD%InputTimes(1) = t_next - - + + ! ----- map substructure kinematics to MoorDyn inputs ----- (from mapping called at start of CalcOutputs Solve INputs) do nt = 1,farm%p%NumTurbines @@ -1901,7 +1903,7 @@ subroutine FARM_MD_Increment(t, n, farm, ErrStat, ErrMsg) !CALL Transfer_Point_to_Point( farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh, farm%MD%Input(1)%PtFairleadDisplacement(nt), farm%m%FWrap_2_MD(nt), ErrStat, ErrMsg ) !end if end do - + ! ----- update states and calculate outputs ----- @@ -1944,6 +1946,8 @@ subroutine FARM_MD_Increment(t, n, farm, ErrStat, ErrMsg) !farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Moment = farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Moment + farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2%Moment end if end do + + end subroutine Farm_MD_Increment !---------------------------------------------------------------------------------------------------------------------------------- !> This routine performs the initial call to calculate outputs (at t=0). @@ -2134,9 +2138,9 @@ subroutine FARM_UpdateStates(t, n, farm, ErrStat, ErrMsg) ErrStat = ErrID_None ErrMsg = "" - allocate ( ErrStatF ( farm%p%NumTurbines+1 ), STAT=errStat2 ) + allocate ( ErrStatF ( farm%p%NumTurbines ), STAT=errStat2 ) if (errStat2 /= 0) call SetErrStat ( ErrID_Fatal, 'Could not allocate memory for ErrStatF.', errStat, errMsg, RoutineName ) - allocate ( ErrMsgF ( farm%p%NumTurbines+1 ), STAT=errStat2 ) + allocate ( ErrMsgF ( farm%p%NumTurbines ), STAT=errStat2 ) if (errStat2 /= 0) call SetErrStat ( ErrID_Fatal, 'Could not allocate memory for ErrMsgF.', errStat, errMsg, RoutineName ) if (ErrStat >= AbortErrLev) return @@ -2202,7 +2206,7 @@ subroutine FARM_UpdateStates(t, n, farm, ErrStat, ErrMsg) ! tm3 = omp_get_wtime() !#endif call AWAE_UpdateStates( t, n, farm%AWAE%u, farm%AWAE%p, farm%AWAE%x, farm%AWAE%xd, farm%AWAE%z, & - farm%AWAE%OtherSt, farm%AWAE%m, errStatF(nt), errMsgF(nt) ) + farm%AWAE%OtherSt, farm%AWAE%m, ErrStatAWAE, ErrMsgAWAE ) !#ifdef _OPENMP ! tm2 = omp_get_wtime() @@ -2244,14 +2248,13 @@ subroutine FARM_UpdateStates(t, n, farm, ErrStat, ErrMsg) ! call farm-level MoorDyn time step here (can't multithread this with FAST since it needs inputs from all FAST instances) call Farm_MD_Increment( t2, n_FMD, farm, ErrStatMD, ErrMsgMD) - call SetErrStat(ErrStatMD, ErrMsgMD, ErrStat, ErrMsg, 'FARM_UpdateStates') ! MD error status - + call SetErrStat(ErrStatMD, ErrMsgMD, ErrStat, ErrMsg, 'FARM_UpdateStates') ! MD error status <<<<< end do ! n_ss substepping ! The second section, for updating AWAE states on a separate thread in parallel with the FAST/MoorDyn time stepping !$OMP SECTION call AWAE_UpdateStates( t, n, farm%AWAE%u, farm%AWAE%p, farm%AWAE%x, farm%AWAE%xd, farm%AWAE%z, & - farm%AWAE%OtherSt, farm%AWAE%m, errStatF(nt), errMsgF(nt) ) + farm%AWAE%OtherSt, farm%AWAE%m, ErrStatAWAE, ErrMsgAWAE ) !$OMP END PARALLEL SECTIONS @@ -2259,6 +2262,12 @@ subroutine FARM_UpdateStates(t, n, farm, ErrStat, ErrMsg) CALL SetErrStat( ErrID_Fatal, 'MooringMod must be 0 or 3.', ErrStat, ErrMsg, RoutineName ) end if + ! update error messages from FAST's and AWAE's time steps + DO nt = 1,farm%p%NumTurbines + call SetErrStat(ErrStatF(nt), ErrMsgF(nt), ErrStat, ErrMsg, 'T'//trim(num2lstr(nt))//':FARM_UpdateStates') ! FAST error status + END DO + + call SetErrStat(ErrStatAWAE, ErrMsgAWAE, ErrStat, ErrMsg, 'FARM_UpdateStates') ! AWAE error status ! calculate outputs from FAST as needed by FAST.Farm do nt = 1,farm%p%NumTurbines @@ -2266,12 +2275,7 @@ subroutine FARM_UpdateStates(t, n, farm, ErrStat, ErrMsg) call setErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) end do - ! update error messages for FAST and AWAE - DO nt = 1,farm%p%NumTurbines - call SetErrStat(ErrStatF(nt), ErrMsgF(nt), ErrStat, ErrMsg, 'T'//trim(num2lstr(nt))//':FARM_UpdateStates') ! FAST error status - END DO - call SetErrStat(ErrStatAWAE, ErrMsgAWAE, ErrStat, ErrMsg, 'FARM_UpdateStates') ! AWAE error status - + if (ErrStat >= AbortErrLev) return diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 394c886dee..7f595ec5da 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -620,7 +620,9 @@ SUBROUTINE MD_UpdateStates( t, n, u, utimes, p, x, xd, z, other, m, ErrStat, Err REAL(DbKi) :: t2 ! copy of time passed to TimeStep - + ErrStat = ErrID_None + ErrMsg = "" + nTime = size(u) ! the number of times of input data provided? t2 = t From 43e06efdf363849f81400095bf9df6d2f2b06cb4 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Fri, 9 Jul 2021 22:35:01 -0600 Subject: [PATCH 042/242] Remaining MD I/O mesh fixes in FAST_Solver and _Lin --- modules/openfast-library/src/FAST_Lin.f90 | 22 ++++++++++---------- modules/openfast-library/src/FAST_Solver.f90 | 4 ++-- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/modules/openfast-library/src/FAST_Lin.f90 b/modules/openfast-library/src/FAST_Lin.f90 index cfc099b6ba..3e4f4bacdd 100644 --- a/modules/openfast-library/src/FAST_Lin.f90 +++ b/modules/openfast-library/src/FAST_Lin.f90 @@ -2140,7 +2140,7 @@ SUBROUTINE Linear_ED_InputSolve_du( p_FAST, y_FAST, u_ED, y_ED, y_AD, u_AD, BD, ! NOTE: Assumes at least one coupled MD object - CALL Linearize_Point_to_Point( MD%y%CoupledLoads, u_ED%PlatformPtMesh, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2, MD%Input(1)%CoupledKinematics, y_ED%PlatformPtMesh) + CALL Linearize_Point_to_Point( MD%y%CoupledLoads(1), u_ED%PlatformPtMesh, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2, MD%Input(1)%CoupledKinematics(1), y_ED%PlatformPtMesh) CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) ! HD is source in the mapping, so we want M_{uSm} @@ -2294,7 +2294,7 @@ SUBROUTINE Linear_SD_InputSolve_du( p_FAST, y_FAST, u_SD, y_SD, y_ED, HD, MAPp, ! NOTE: Assumes at least one coupled MD object - CALL Linearize_Point_to_Point( MD%y%CoupledLoads, u_SD%LMesh, MeshMapData%Mooring_P_2_SD_P, ErrStat2, ErrMsg2, MD%Input(1)%CoupledKinematics, y_SD%Y2Mesh) + CALL Linearize_Point_to_Point( MD%y%CoupledLoads(1), u_SD%LMesh, MeshMapData%Mooring_P_2_SD_P, ErrStat2, ErrMsg2, MD%Input(1)%CoupledKinematics(1), y_SD%Y2Mesh) CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) ! SD is source in the mapping, so we want M_{uSm} @@ -2409,12 +2409,12 @@ SUBROUTINE Linear_SD_InputSolve_dy( p_FAST, y_FAST, u_SD, y_SD, y_ED, HD, MAPp, ! dU^{SD}/dy^{MD} !.......... else if ( p_FAST%CompMooring == Module_MD ) then - if ( MD%y%CoupledLoads%Committed ) then ! meshes for floating + if ( MD%y%CoupledLoads(1)%Committed ) then ! meshes for floating !!! ! This linearization was done in forming dUdu (see Linear_SD_InputSolve_du()), so we don't need to re-calculate these matrices !!! ! while forming dUdy, too. MD_Out_Start = y_FAST%Lin%Modules(Module_MD)%Instance(1)%LinStartIndx(LIN_OUTPUT_COL) SD_Start = Indx_u_SD_LMesh_Start(u_SD, y_FAST) ! start of u_SD%LMesh%TranslationDisp field - call Assemble_dUdy_Loads(MD%y%CoupledLoads, u_SD%LMesh, MeshMapData%Mooring_P_2_SD_P, SD_Start, MD_Out_Start, dUdy) + call Assemble_dUdy_Loads(MD%y%CoupledLoads(1), u_SD%LMesh, MeshMapData%Mooring_P_2_SD_P, SD_Start, MD_Out_Start, dUdy) ! SD translation displacement-to-SD moment transfer (dU^{SD}/dy^{SD}): SD_Start = Indx_u_SD_LMesh_Start(u_SD, y_FAST) + u_SD%LMesh%NNodes*3 ! start of u_ED%LMesh%Moment field (skip the SD forces) @@ -2861,12 +2861,12 @@ SUBROUTINE Linear_ED_InputSolve_dy( p_FAST, y_FAST, u_ED, y_ED, y_AD, u_AD, BD, ! MoorDyn ! parts of dU^{ED}/dy^{MD} and dU^{ED}/dy^{ED}: else if ( p_FAST%CompMooring == Module_MD ) then - if ( MD%y%CoupledLoads%Committed ) then ! meshes for floating + if ( MD%y%CoupledLoads(1)%Committed ) then ! meshes for floating !!! ! This linearization was done in forming dUdu (see Linear_ED_InputSolve_du()), so we don't need to re-calculate these matrices !!! ! while forming dUdy, too. MD_Out_Start = y_FAST%Lin%Modules(Module_MD)%Instance(1)%LinStartIndx(LIN_OUTPUT_COL) ED_Start = Indx_u_ED_Platform_Start(u_ED, y_FAST) ! start of u_ED%PlatformPtMesh%TranslationDisp field - call Assemble_dUdy_Loads(MD%y%CoupledLoads, u_ED%PlatformPtMesh, MeshMapData%Mooring_P_2_ED_P, ED_Start, MD_Out_Start, dUdy) + call Assemble_dUdy_Loads(MD%y%CoupledLoads(1), u_ED%PlatformPtMesh, MeshMapData%Mooring_P_2_ED_P, ED_Start, MD_Out_Start, dUdy) ! ED translation displacement-to-ED moment transfer (dU^{ED}/dy^{ED}): ED_Start = Indx_u_ED_Platform_Start(u_ED, y_FAST) + u_ED%PlatformPtMesh%NNodes*3 ! start of u_ED%PlatformPtMesh%Moment field (skip the ED forces) @@ -3534,7 +3534,7 @@ SUBROUTINE Linear_MD_InputSolve_dy( p_FAST, y_FAST, u_MD, y_ED, y_SD, MeshMapDat ErrStat = ErrID_None ErrMsg = "" - IF (u_MD%CoupledKinematics%Committed) THEN + IF (u_MD%CoupledKinematics(1)%Committed) THEN !................................... ! FairLead Mesh !................................... @@ -3544,14 +3544,14 @@ SUBROUTINE Linear_MD_InputSolve_dy( p_FAST, y_FAST, u_MD, y_ED, y_SD, MeshMapDat if ( p_FAST%CompSub == Module_SD ) THEN ! dU^{MD}/dy^{SD} SD_Out_Start = Indx_y_SD_Y2Mesh_Start(y_SD, y_FAST) ! start of y_SD%Y2Mesh%TranslationDisp field - call Linearize_Point_to_Point( y_SD%Y2Mesh, u_MD%CoupledKinematics, MeshMapData%SD_P_2_Mooring_P, ErrStat2, ErrMsg2 ) - call Assemble_dUdy_Motions( y_SD%Y2Mesh, u_MD%CoupledKinematics, MeshMapData%SD_P_2_Mooring_P, MD_Start, SD_Out_Start, dUdy, OnlyTranslationDisp=.true.) + call Linearize_Point_to_Point( y_SD%Y2Mesh, u_MD%CoupledKinematics(1), MeshMapData%SD_P_2_Mooring_P, ErrStat2, ErrMsg2 ) + call Assemble_dUdy_Motions( y_SD%Y2Mesh, u_MD%CoupledKinematics(1), MeshMapData%SD_P_2_Mooring_P, MD_Start, SD_Out_Start, dUdy, OnlyTranslationDisp=.true.) else if ( p_FAST%CompSub == Module_None ) THEN ! dU^{MD}/dy^{ED} ED_Out_Start = Indx_y_ED_Platform_Start(y_ED, y_FAST) ! start of y_ED%PlatformPtMesh%TranslationDisp field - call Linearize_Point_to_Point( y_ED%PlatformPtMesh, u_MD%CoupledKinematics, MeshMapData%ED_P_2_Mooring_P, ErrStat2, ErrMsg2 ) - call Assemble_dUdy_Motions(y_ED%PlatformPtMesh, u_MD%CoupledKinematics, MeshMapData%ED_P_2_Mooring_P, MD_Start, ED_Out_Start, dUdy, OnlyTranslationDisp=.true.) + call Linearize_Point_to_Point( y_ED%PlatformPtMesh, u_MD%CoupledKinematics(1), MeshMapData%ED_P_2_Mooring_P, ErrStat2, ErrMsg2 ) + call Assemble_dUdy_Motions( y_ED%PlatformPtMesh, u_MD%CoupledKinematics(1), MeshMapData%ED_P_2_Mooring_P, MD_Start, ED_Out_Start, dUdy, OnlyTranslationDisp=.true.) end if diff --git a/modules/openfast-library/src/FAST_Solver.f90 b/modules/openfast-library/src/FAST_Solver.f90 index 1171ab3cf8..690a431d47 100644 --- a/modules/openfast-library/src/FAST_Solver.f90 +++ b/modules/openfast-library/src/FAST_Solver.f90 @@ -2040,7 +2040,7 @@ SUBROUTINE U_ED_HD_Residual( y_ED2, y_HD2, u_IN, U_Resid) CALL Transfer_Point_to_Point( y_ED2%PlatformPtMesh, u_MD%CoupledKinematics(1), MeshMapData%ED_P_2_Mooring_P, ErrStat, ErrMsg ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - CALL Transfer_Point_to_Point( y_MD%CoupledLoads(1), MeshMapData%u_ED_PlatformPtMesh, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2, u_MD%CoupledKinematics, PlatformMotions ) !u_MD and y_ED contain the displacements needed for moment calculations + CALL Transfer_Point_to_Point( y_MD%CoupledLoads(1), MeshMapData%u_ED_PlatformPtMesh, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2, u_MD%CoupledKinematics(1), PlatformMotions ) !u_MD and y_ED contain the displacements needed for moment calculations CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) ELSEIF ( p_FAST%CompMooring == Module_FEAM ) THEN @@ -4764,7 +4764,7 @@ SUBROUTINE InitModuleMappings(p_FAST, ED, BD, AD14, AD, HD, SD, ExtPtfm, SrvD, M ! ElastoDyn <-> MoorDyn !------------------------- ! MoorDyn point mesh to/from ElastoDyn point mesh - CALL MeshMapCreate( MD%y%CoupledLoads,(1) PlatformLoads, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2 ) + CALL MeshMapCreate( MD%y%CoupledLoads(1), PlatformLoads, MeshMapData%Mooring_P_2_ED_P, ErrStat2, ErrMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':Mooring_P_2_Ptfm' ) CALL MeshMapCreate( PlatformMotion, MD%Input(1)%CoupledKinematics(1), MeshMapData%ED_P_2_Mooring_P, ErrStat2, ErrMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':Ptfm_2_Mooring_P' ) From 12728ccf6e4073b21be53241cccecadbf04483af Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Sat, 10 Jul 2021 01:41:33 -0600 Subject: [PATCH 043/242] First batch of changes to make shared mooring ability work with MDv2: - Making all variables for keeping track of coupled objects have an additional dimension to correspond to the turbine number in the array. - Integrated old fast-farm mooring system accounting (based on fairleads) with MDv2 objects -- so far done with Coupled Points, but things are part way in place to also do with Rods and Bodies. - TO-DO: fix indenting, try compiling, eventually add Rods and Bodies. --- modules/moordyn/src/MoorDyn.f90 | 366 +++++++++-------------- modules/moordyn/src/MoorDyn_Registry.txt | 6 +- 2 files changed, 150 insertions(+), 222 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 188d2f52c2..16241e663b 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -177,13 +177,6 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! Check if this MoorDyn instance is being run from FAST.Farm (indicated by FarmSize > 0) - p%nTurbines = InitInp%FarmSize ! 0 indicates regular FAST module mode - - if (p%nTurbines == 0) p%nTurbines = 1 ! if a regular FAST module mode, we treat it like a nTurbine=1 farm case - - allocate( p%NFairs( p%nTurbines)) ! allocate the array that will hold the number of fairleads for each turbine - allocate( p%TurbineRefPos( 3, p%nTurbines)) - if (InitInp%FarmSize > 0) then CALL WrScr(' >>> MoorDyn is running in array mode <<< ') @@ -191,41 +184,37 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !InitInp%FarmNCpldCons + p%nTurbines = InitInp%FarmSize + ! copy over turbine reference positions for later use p%TurbineRefPos = InitInp%TurbineRefPos - else ! normal, FAST module mode + else ! FarmSize==0 indicates normal, FAST module mode + + p%nTurbines = 1 ! if a regular FAST module mode, we treat it like a nTurbine=1 farm case m%PtfmInit = InitInp%PtfmInit(:,1) ! save a copy of PtfmInit so it's available at the farm level p%TurbineRefPos = 0.0_DbKi ! for now assuming this is zero for FAST use END IF + ! allocate some parameter arrays that are for each turbine (size 1 if regular OpenFAST use) + allocate( p%nCpldBodies( p%nTurbines)) + allocate( p%nCpldRods ( p%nTurbines)) + allocate( p%nCpldCons ( p%nTurbines)) + allocate( p%TurbineRefPos(3, p%nTurbines)) + + ! initialize the arrays (to zero, except for passed in farm turbine reference positions) + p%nCpldBodies = 0 + p%nCpldRods = 0 + p%nCpldCons = 0 + + if (InitInp%FarmSize > 0) then + p%TurbineRefPos = InitInp%TurbineRefPos ! copy over turbine reference positions for later use + else + p%TurbineRefPos = 0.0_DbKi ! for now assuming this is zero for FAST use + end if - !FIXME: the following probably needs changing >>> - do iTurb = 1, p%nTurbines - p%NFairs(iTurb) = 0 ! this is the number of fairleads (for each turbine if in farm mode) - end do - ! <<<< - - ! Check for farm-level inputs (indicating that this MoorDyn isntance is being run from FAST.Farm) - !intead of below, check first dimension of PtfmInit - !p%nTurbines = SIZE(InitInp%FarmCoupledKinematics) ! the number of turbines in the array (0 indicates a regular OpenFAST simulation with 1 turbine) - ! - !IF (p%nTurbines > 0) then - ! WrScr(' >>> MoorDyn is running in array mode <<< ') - ! - ! ! copy over the meshes to be this instance's array of mesh inputs (a duplicate) - ! ! >>> initialize array u%FarmCoupledKinematics - ! ! >>> copy each mesh from InitInp%FarmCoupledKinematics into it - ! ! CALL MeshCopy ( SrcMesh = InitInp%FarmCoupledKinematics, DestMesh = u%FarmCoupledKinematics, CtrlCode = MESH_SIBLING, IOS=COMPONENT_OUTPUT, Force=.TRUE., Moment=.TRUE., ErrStat = ErrStat2, ErrMess=ErrMsg2 ) - ! - ! ! OR instead of copying the meshes, could just make empty meshes, then populate with exactly the elements needed at the farm level - ! !InitInp%FarmNCpldBodies - ! !InitInp%FarmNCpldRods - ! !InitInp%FarmNCpldCons - ! - !END IF !--------------------------------------------------------------------------------------------- ! read input file and create cross-referenced mooring system objects @@ -473,9 +462,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ALLOCATE(m%ConStateIs1(p%nConnects), m%ConStateIsN(p%nConnects), STAT=ErrStat2); if(AllocateFailed("ConStateIs1/N" )) return ALLOCATE(m%LineStateIs1(p%nLines) , m%LineStateIsN(p%nLines) , STAT=ErrStat2); if(AllocateFailed("LineStateIs1/N")) return - ALLOCATE(m%FreeBodyIs( p%nBodies ), m%CpldBodyIs(p%nBodies ), STAT=ErrStat2); if(AllocateFailed("BodyIs")) return - ALLOCATE(m%FreeRodIs( p%nRods ), m%CpldRodIs( p%nRods ), STAT=ErrStat2); if(AllocateFailed("RodIs")) return - ALLOCATE(m%FreeConIs( p%nConnects), m%CpldConIs(p%nConnects),STAT=ErrStat2); if(AllocateFailed("ConnectIs")) return + ALLOCATE(m%FreeBodyIs( p%nBodies ), STAT=ErrStat2); if(AllocateFailed("FreeBodyIs")) return + ALLOCATE(m%FreeRodIs( p%nRods ), STAT=ErrStat2); if(AllocateFailed("FreeRodIs")) return + ALLOCATE(m%FreeConIs( p%nConnects), STAT=ErrStat2); if(AllocateFailed("FreeConnectIs")) return + + ALLOCATE(m%CpldBodyIs(p%nBodies , p%nTurbines), STAT=ErrStat2); if(AllocateFailed("CpldBodyIs")) return + ALLOCATE(m%CpldRodIs( p%nRods , p%nTurbines), STAT=ErrStat2); if(AllocateFailed("CpldRodIs")) return + ALLOCATE(m%CpldConIs(p%nConnects, p%nTurbines), STAT=ErrStat2); if(AllocateFailed("CpldConnectIs")) return ! ---------------------- now go through again and process file contents -------------------- @@ -625,11 +618,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er if ((let2 == "COUPLED") .or. (let2 == "VESSEL") .or. (let1 == "CPLD") .or. (let1 == "VES")) then ! if a coupled body m%BodyList(l)%typeNum = -1 - p%nCpldBodies=p%nCpldBodies+1 ! add this rod to coupled list - m%CpldBodyIs(p%nCpldBodies) = l + p%nCpldBodies(1)=p%nCpldBodies(1)+1 ! add this rod to coupled list + m%CpldBodyIs(p%nCpldBodies(1),1) = l ! body initial position due to coupling will be adjusted later + ! TODO: add option for body coupling to different turbines in FAST.Farm <<< + else if ((let2 == "FREE") .or. (LEN_TRIM(let2)== 0)) then ! if a free body m%BodyList(l)%typeNum = 0 @@ -755,23 +750,25 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if ((let1 == "VESSEL") .or. (let1 == "VES") .or. (let1 == "COUPLED") .or. (let1 == "CPLD")) then ! if a rigidly coupled rod, add to list and add m%RodList(l)%typeNum = -2 - p%nCpldRods=p%nCpldRods+1 ! add this rod to coupled list + p%nCpldRods(1)=p%nCpldRods(1)+1 ! add this rod to coupled list - m%CpldRodIs(p%nCpldRods) = l + m%CpldRodIs(p%nCpldRods(1),1) = l else if ((let1 == "VESSELPINNED") .or. (let1 == "VESPIN") .or. (let1 == "COUPLEDPINNED") .or. (let1 == "CPLDPIN")) then ! if a pinned coupled rod, add to list and add m%RodList(l)%typeNum = -1 - p%nCpldRods=p%nCpldRods+1 ! add - p%nFreeRods=p%nFreeRods+1 ! add this pinned rod to the free list because it is half free + p%nCpldRods(1)=p%nCpldRods(1)+1 ! add + p%nFreeRods =p%nFreeRods+1 ! add this pinned rod to the free list because it is half free m%RodStateIs1(p%nFreeRods) = Nx+1 m%RodStateIsN(p%nFreeRods) = Nx+6 Nx = Nx + 6 ! add 6 state variables for each pinned rod - m%CpldRodIs(p%nCpldRods) = l + m%CpldRodIs(p%nCpldRods(1),1) = l m%FreeRodIs(p%nFreeRods) = l - + + ! TODO: add option for body coupling to different turbines in FAST.Farm <<< + else if ((let1 == "CONNECT") .or. (let1 == "CON") .or. (let1 == "FREE")) then m%RodList(l)%typeNum = 0 @@ -889,7 +886,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er RETURN END IF - + m%ConnectList(l)%r = tempArray(1:3) ! set initial, or reference, node position (for coupled or child objects, this will be the local reference location about the parent) !----------- process connection type ----------------- @@ -898,7 +895,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er if ((let1 == "ANCHOR") .or. (let1 == "FIXED") .or. (let1 == "FIX")) then m%ConnectList(l)%typeNum = 1 - m%ConnectList(l)%r = tempArray(1:3) ! set initial node position + !m%ConnectList(l)%r = tempArray(1:3) ! set initial node position CALL Body_AddConnect(m%GroundBody, l, tempArray(1:3)) ! add connection l to Ground body @@ -922,23 +919,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if ((let1 == "VESSEL") .or. (let1 == "VES") .or. (let1 == "COUPLED") .or. (let1 == "CPLD")) then ! if a fairlead, add to list and add m%ConnectList(l)%typeNum = -1 - p%nCpldCons=p%nCpldCons+1 ! add this rod to coupled list - m%CpldConIs(p%nCpldCons) = l - - p%NFairs(1) = p%NFairs(1) + 1 !FIXME: <<<<< ! if a vessel connection, increment fairlead counter - - ! this is temporary for backwards compatibility >>>>> will need to update for more versatile coupling >>>> <<<<<<< this looks pretty good. Make sure it's done only once - either here or near end of init. Same for Rods and bodies. - ! NOTE: second index would be used for multi-turbine couplings in FAST.Farm - CALL SmllRotTrans('PtfmInit', InitInp%PtfmInit(4),InitInp%PtfmInit(5),InitInp%PtfmInit(6), OrMat, '', ErrStat2, ErrMsg2) - !CALL SmllRotTrans('PtfmInit', InitInp%PtfmInit(4,1),InitInp%PtfmInit(5,1),InitInp%PtfmInit(6,1), OrMat, '', ErrStat2, ErrMsg2) - - ! set initial node position, including adjustments due to initial platform rotations and translations <<< could convert to array math - m%ConnectList(l)%r(1) = InitInp%PtfmInit(1) + OrMat(1,1)*tempArray(1) + OrMat(2,1)*tempArray(2) + OrMat(3,1)*tempArray(3) - m%ConnectList(l)%r(2) = InitInp%PtfmInit(2) + OrMat(1,2)*tempArray(1) + OrMat(2,2)*tempArray(2) + OrMat(3,2)*tempArray(3) - m%ConnectList(l)%r(3) = InitInp%PtfmInit(3) + OrMat(1,3)*tempArray(1) + OrMat(2,3)*tempArray(2) + OrMat(3,3)*tempArray(3) - !m%ConnectList(l)%r(1) = InitInp%PtfmInit(1,1) + OrMat(1,1)*tempArray(1) + OrMat(2,1)*tempArray(2) + OrMat(3,1)*tempArray(3) - !m%ConnectList(l)%r(2) = InitInp%PtfmInit(2,1) + OrMat(1,2)*tempArray(1) + OrMat(2,2)*tempArray(2) + OrMat(3,2)*tempArray(3) - !m%ConnectList(l)%r(3) = InitInp%PtfmInit(3,1) + OrMat(1,3)*tempArray(1) + OrMat(2,3)*tempArray(2) + OrMat(3,3)*tempArray(3) + p%nCpldCons(1)=p%nCpldCons(1)+1 + m%CpldConIs(p%nCpldCons(1),1) = l else if ((let1 == "CONNECT") .or. (let1 == "CON") .or. (let1 == "FREE")) then m%ConnectList(l)%typeNum = 0 @@ -951,30 +933,22 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er m%FreeConIs(p%nFreeCons) = l - m%ConnectList(l)%r = tempArray(1:3) ! set initial node position + !m%ConnectList(l)%r = tempArray(1:3) ! set initial node position else if ((let1 == "TURBINE") .or. (let1 == "T")) then ! turbine-coupled in FAST.Farm case - !FIXME: >>>>> - if (p%nTurbines > 0) then - K = scan(TempString , '1234567890' ) ! find index of first number in the string - READ(TempString(K:), *) iTurb ! READ(TRIM(TempString(K:)), *) iTurb ! convert to int, representing turbine index - m%ConnectList(I)%TypeNum = -iTurb ! typeNum < 0 indicates -turbine number - p%NFairs(iTurb) = p%NFairs(iTurb) + 1 ! increment fairlead counter - print *, ' added connection ', I, ' as fairlead for turbine ', iTurb - - !FIXME: <<<< need to set positions here including offset from turbine ref positions in x and y, p%TurbineRefPos(1-2,iTurb) - - else - call CheckError( ErrID_Fatal, 'Error: Turbine[n] Connect types can only be used with FAST.Farm.' ) + K = scan(TempString , '1234567890' ) ! find index of first number in the string + READ(TempString(K:), *) iTurb ! convert to int, representing turbine index + if (iTurb > p%nTurbines) then + call CheckError( ErrID_Fatal, 'Error: Turbine[n] Connect types can only be used with FAST.Farm and must not exceed the number of turbines.' ) return end if - ! <<<<<< - ! iTurbine = num1 - ! >>> nvm: this is where we could identify the element index in the corresponding mesh in u%FarmCoupledKinematics(iTurbine) for this coupled point - ! nvm: using info from InitInp%FarmNCpldBodies, InitInp%FarmNCpldRods, !InitInp%FarmNCpldCons - ! >>> nvm: Then store iTurbine and the element index IN THIS POINT OBJECT, for easy use with input and output meshes! <<< - ! >>> Do all the initialization stuff as is done with normal coupled points. + + m%ConnectList(I)%TypeNum = -iTurb ! typeNum < 0 indicates -turbine number <<<< NOT USED, RIGHT?? + p%nCpldCons(iTurb) = p%nCpldCons(iTurb) + 1 ! increment counter for the appropriate turbine + m%CpldConIs(p%nCpldCons(iTurb),iTurb) = l + print *, ' added connection ', I, ' as fairlead for turbine ', iTurb + else CALL SetErrStat( ErrID_Severe, "Unidentified Type/BodyID for Connection "//trim(Num2LStr(l))//": "//trim(tempString2), ErrStat, ErrMsg, RoutineName ) return @@ -1507,7 +1481,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! ================================ initialize system ================================ - + ! This will also set the initial positions of any dependent (child) objects ! call ground body to update all the fixed things... m%GroundBody%r6(4:6) = 0.0_DbKi @@ -1524,25 +1498,6 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! end if ! END DO - !FIXMI: >>>> - ! allocate fairleads list (size to the maximum number of fairleads on any given turbine - some entries may not be used) - ALLOCATE ( m%FairIdList(MAXVAL(p%NFairs),p%nTurbines), STAT = ErrStat ) - - ! now go back through and record the fairlead Id numbers (this is all the "connecting" that's required) - p%NFairs = 0 - p%NConns = 0 ! re-using this as a counter for connect number (should end back at same value) - DO I = 1,p%NConnects - IF (m%ConnectList(I)%TypeNum < 0) THEN - iTurb = -m%ConnectList(I)%TypeNum - p%NFairs(iTurb) = p%NFairs(iTurb) + 1 - m%FairIdList(p%NFairs(iTurb), iTurb) = I ! if a vessel connection, add ID to list of the appropriate turbine - ELSE IF (m%ConnectList(I)%TypeNum == 2) THEN - p%NConns = p%NConns + 1 - m%ConnIdList(p%NConns) = I ! if a connect connection, add ID to list - END IF - END DO - ! <<<< - ! Initialize coupled objects based on passed kinematics ! (set up initial condition of each coupled object based on values specified by glue code) @@ -1572,13 +1527,12 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er RETURN END IF - ! Go through each turbine and set up its mesh and initial fairlead positions + ! Go through each turbine and set up its mesh and initial positions of coupled objects DO iTurb = 1,p%nTurbines - ! Always have at least one node (it will be a dummy node if no fairleads are attached) - - K = p%NFairs(iTurb) !p%nCpldBodies + p%nCpldRods + p%nCpldCons, & !FIXME: the number for each turbine needs fixing - if (K == 0) K = 1 + ! count number of coupling nodes needed for the mesh of this turbine + K = p%nCpldBodies(iTurb) + p%nCpldRods(iTurb) + p%nCpldCons(iTurb) + if (K == 0) K = 1 ! Always have at least one node (it will be a dummy node if no fairleads are attached) ! create input mesh for fairlead kinematics CALL MeshCreate(BlankMesh=u%CoupledKinematics(iTurb) , & @@ -1592,21 +1546,19 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN - - ! note: in MoorDyn-F v2, the points in the mesh correspond in order to all the coupled bodies, then rods, then connections ! >>> make sure all coupled objects have been offset correctly by the PtfmInit values, including if it's a farm situation -- below or where the objects are first created <<<< - J = 0 ! this is the counter through the mesh points + J = 0 ! this is the counter through the mesh points for each turbine - DO l = 1,p%nCpldBodies + DO l = 1,p%nCpldBodies(iTurb) J = J + 1 - rRef = m%BodyList(m%CpldBodyIs(l))%r6 ! for now set reference position as per input file <<< + rRef = m%BodyList(m%CpldBodyIs(l,iTurb))%r6 ! for now set reference position as per input file <<< - CALL MeshPositionNode(u%CoupledKinematics, J, rRef(1:3), ErrStat2, ErrMsg2) ! defaults to identity orientation matrix - u%CoupledKinematics%TranslationDisp(:,J) = 0.0_ReKi ! no displacement from reference position + CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef(1:3), ErrStat2, ErrMsg2) ! defaults to identity orientation matrix + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) = 0.0_ReKi ! no displacement from reference position CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN @@ -1617,86 +1569,73 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! u%CoupledKinematics%TranslationDisp(2, i) = InitInp%PtfmInit(2) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2) ! u%CoupledKinematics%TranslationDisp(3, i) = InitInp%PtfmInit(3) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) - CALL MeshConstructElement(u%CoupledKinematics, ELEMENT_POINT, ErrStat2, ErrMsg2, J) ! set node as point element - CALL Body_InitializeUnfree( m%BodyList(m%CpldBodyIs(l)), m ) + CALL MeshConstructElement(u%CoupledKinematics(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, J) ! set node as point element + CALL Body_InitializeUnfree( m%BodyList(m%CpldBodyIs(l,iTurb)), m ) END DO - DO l = 1,p%nCpldRods ! keeping this one simple for now, positioning at whatever is specified in input file <<<<< should change to glue code! + DO l = 1,p%nCpldRods(iTurb) ! keeping this one simple for now, positioning at whatever is specified in input file <<<<< should change to glue code! J = J + 1 - rRef = m%RodList(m%CpldRodIs(l))%r6 ! for now set reference position as per input file <<< + rRef = m%RodList(m%CpldRodIs(l,iTurb))%r6 ! for now set reference position as per input file <<< - CALL MeshPositionNode(u%CoupledKinematics, J, rRef, ErrStat2, ErrMsg2) ! defaults to identity orientation matrix - u%CoupledKinematics%TranslationDisp(:,J) = 0.0_ReKi ! no displacement from reference position - CALL MeshConstructElement(u%CoupledKinematics, ELEMENT_POINT, ErrStat2, ErrMsg2, J) + CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef, ErrStat2, ErrMsg2) ! defaults to identity orientation matrix + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) = 0.0_ReKi ! no displacement from reference position + CALL MeshConstructElement(u%CoupledKinematics(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, J) - CALL Rod_SetKinematics(m%RodList(m%CpldRodIs(l)), DBLE(rRef), m%zeros6, m%zeros6, 0.0_DbKi, m) + CALL Rod_SetKinematics(m%RodList(m%CpldRodIs(l,iTurb)), DBLE(rRef), m%zeros6, m%zeros6, 0.0_DbKi, m) END DO - !DO l = 1,p%nCpldCons ! keeping this one simple for now, positioning at whatever is specified by glue code <<< - DO l = 1,p%NFairs(iTurb) !FIXME: <<<< and check all indices including J + DO l = 1,p%nCpldCons(iTurb) ! keeping this one simple for now, positioning at whatever is specified by glue code <<< J = J + 1 - rRef(1:3) = m%ConnectList(m%CpldConIs(l))%r ! for now set reference position as per input file <<< - - !FIXME: >>>> these lines should be used? - rPos(1) = m%ConnectList(m%FairIdList(i, iTurb))%conX ! reference position of each fairlead relative to each turbine's local reference position - rPos(2) = m%ConnectList(m%FairIdList(i, iTurb))%conY - rPos(3) = m%ConnectList(m%FairIdList(i, iTurb))%conZ - ! <<<< - - CALL MeshPositionNode(u%CoupledKinematics, J, rRef, ErrStat2, ErrMsg2) ! defaults to identity orientation matrix - u%CoupledKinematics%TranslationDisp(:,J) = 0.0_ReKi ! no displacement from reference position + ! set reference position as per input file + rRef(1:3) = m%ConnectList(m%CpldConIs(l,iTurb))%r + CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef, ErrStat2, ErrMsg2) + + ! calculate initial point relative position, adjusted due to initial platform rotations and translations <<< could convert to array math + CALL SmllRotTrans('PtfmInit', InitInp%PtfmInit(4,iTurb),InitInp%PtfmInit(5,iTurb),InitInp%PtfmInit(6,iTurb), OrMat, '', ErrStat2, ErrMsg2) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN + + u%CoupledKinematics(iTurb)%TranslationDisp(1,i) = InitInp%PtfmInit(1,iTurb) + OrMat(1,1)*rRef(1) + OrMat(2,1)*rRef(2) + OrMat(3,1)*rRef(3) - rRef(1) + u%CoupledKinematics(iTurb)%TranslationDisp(2,i) = InitInp%PtfmInit(2,iTurb) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2) + u%CoupledKinematics(iTurb)%TranslationDisp(3,i) = InitInp%PtfmInit(3,iTurb) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) + + ! set absolute initial positions in MoorDyn + m%ConnectList(m%CpldConIs(l,iTurb))%r = u%CoupledKinematics%Position(:,iTurb) + u%CoupledKinematics%TranslationDisp(:,iTurb) + p%TurbineRefPos(:,iTurb) - !FIXME: >>>> - ! set offset position of each node to according to initial platform position and rotation - CALL SmllRotTrans('PtfmInit', InitInp%PtfmInit(4,iTurb),InitInp%PtfmInit(5,iTurb),InitInp%PtfmInit(6,iTurb), TransMat, '', ErrStat2, ErrMsg2) - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN - - ! Apply initial platform rotations and translations (fixed Jun 19, 2015) - u%PtFairleadDisplacement(iTurb)%TranslationDisp(1,i) = InitInp%PtfmInit(1,iTurb) + Transmat(1,1)*rPos(1) + Transmat(2,1)*rPos(2) + TransMat(3,1)*rPos(3) - rPos(1) - u%PtFairleadDisplacement(iTurb)%TranslationDisp(2,i) = InitInp%PtfmInit(2,iTurb) + Transmat(1,2)*rPos(1) + Transmat(2,2)*rPos(2) + TransMat(3,2)*rPos(3) - rPos(2) - u%PtFairleadDisplacement(iTurb)%TranslationDisp(3,i) = InitInp%PtfmInit(3,iTurb) + Transmat(1,3)*rPos(1) + Transmat(2,3)*rPos(2) + TransMat(3,3)*rPos(3) - rPos(3) - - ! set velocity of each node to zero - u%PtFairleadDisplacement(iTurb)%TranslationVel(1,i) = 0.0_DbKi - u%PtFairleadDisplacement(iTurb)%TranslationVel(2,i) = 0.0_DbKi - u%PtFairleadDisplacement(iTurb)%TranslationVel(3,i) = 0.0_DbKi !print *, 'Fairlead ', i, ' z TranslationDisp at start is ', u%PtFairleadDisplacement(iTurb)%TranslationDisp(3,i) !print *, 'Fairlead ', i, ' z Position at start is ', u%PtFairleadDisplacement(iTurb)%Position(3,i) ! <<<< - CALL MeshConstructElement(u%CoupledKinematics, ELEMENT_POINT, ErrStat2, ErrMsg2, J) + CALL MeshConstructElement(u%CoupledKinematics(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, J) ! lastly, do this to set the attached line endpoint positions: rRefDub = rRef(1:3) - CALL Connect_SetKinematics(m%ConnectList(m%CpldConIs(l)), rRefDub, m%zeros6(1:3), m%zeros6(1:3), 0.0_DbKi, m) + CALL Connect_SetKinematics(m%ConnectList(m%CpldConIs(l,iTurb)), rRefDub, m%zeros6(1:3), m%zeros6(1:3), 0.0_DbKi, m) END DO CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN - !FIXME: >>>> - ! add a single dummy element for turbines that aren't coupled with, to keep I/O interp/extrap routines happy - if (p%NFairs(iTurb) == 0) then - rPos = 0.0_DbKi ! position at PRP - CALL MeshPositionNode(u%PtFairleadDisplacement(iTurb), 1, rPos, ErrStat2, ErrMsg2) - CALL MeshConstructElement(u%PtFairleadDisplacement(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, 1) + ! if no coupled objects exist for this turbine, add a single dummy element to keep I/O interp/extrap routines happy + if (J == 0) then + rRef = 0.0_DbKi ! position at PRP + CALL MeshPositionNode(u%CoupledKinematics(iTurb), 1, rRef, ErrStat2, ErrMsg2) + CALL MeshConstructElement(u%CoupledKinematics(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, 1) CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN end if - !<<<< ! set velocities/accelerations of all mesh nodes to zero - u%CoupledKinematics%TranslationVel = 0.0_ReKi - u%CoupledKinematics%TranslationAcc = 0.0_ReKi - u%CoupledKinematics%RotationVel = 0.0_ReKi - u%CoupledKinematics%RotationAcc = 0.0_ReKi + u%CoupledKinematics(iTurb)%TranslationVel = 0.0_ReKi + u%CoupledKinematics(iTurb)%TranslationAcc = 0.0_ReKi + u%CoupledKinematics(iTurb)%RotationVel = 0.0_ReKi + u%CoupledKinematics(iTurb)%RotationAcc = 0.0_ReKi CALL MeshCommit ( u%CoupledKinematics(iTurb), ErrStat, ErrMsg ) CALL CheckError( ErrStat2, ErrMsg2 ) @@ -1704,8 +1643,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! copy the input fairlead kinematics mesh to make the output mesh for fairlead loads, PtFairleadLoad CALL MeshCopy ( SrcMesh = u%CoupledKinematics(iTurb), DestMesh = y%CoupledLoads(iTurb), & - CtrlCode = MESH_SIBLING, IOS = COMPONENT_OUTPUT, & - Force = .TRUE., ErrStat = ErrStat2, ErrMess=ErrMsg2 ) + CtrlCode = MESH_SIBLING, IOS = COMPONENT_OUTPUT, & + Force = .TRUE., Moment = .TRUE., ErrStat = ErrStat2, ErrMess=ErrMsg2 ) CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN @@ -2604,36 +2543,32 @@ SUBROUTINE MD_CalcOutput( t, u, p, x, xd, z, other, y, m, ErrStat, ErrMsg ) ! END DO ! now that forces have been updated, write them to the output mesh - J = 0 - DO l = 1,p%nCpldBodies + + do iTurb = 1,p%nTurbines + + J = 0 ! mesh index + DO l = 1,p%nCpldBodies(iTurb) J = J + 1 - CALL Body_GetCoupledForce(m%BodyList(m%CpldBodyIs(l)), F6net, m, p) - y%CoupledLoads%Force( :,J) = F6net(1:3) - y%CoupledLoads%Moment(:,J) = F6net(4:6) + CALL Body_GetCoupledForce(m%BodyList(m%CpldBodyIs(l,iTurb)), F6net, m, p) + y%CoupledLoads(iTurb)%Force( :,J) = F6net(1:3) + y%CoupledLoads(iTurb)%Moment(:,J) = F6net(4:6) END DO - DO l = 1,p%nCpldRods + DO l = 1,p%nCpldRods(iTurb) J = J + 1 - CALL Rod_GetCoupledForce(m%RodList(m%CpldRodIs(l)), F6net, m, p) - y%CoupledLoads%Force( :,J) = F6net(1:3) - y%CoupledLoads%Moment(:,J) = F6net(4:6) + CALL Rod_GetCoupledForce(m%RodList(m%CpldRodIs(l,iTurb)), F6net, m, p) + y%CoupledLoads(iTurb)%Force( :,J) = F6net(1:3) + y%CoupledLoads(iTurb)%Moment(:,J) = F6net(4:6) END DO - DO l = 1,p%nCpldCons + DO l = 1,p%nCpldCons(iTurb) J = J + 1 - CALL Connect_GetCoupledForce(m%ConnectList(m%CpldConIs(l)), F6net(1:3), m, p) - y%CoupledLoads%Force(:,J) = F6net(1:3) + CALL Connect_GetCoupledForce(m%ConnectList(m%CpldConIs(l,iTurb)), F6net(1:3), m, p) + y%CoupledLoads(iTurb)%Force(:,J) = F6net(1:3) END DO - !FIXME: >>>> - do iTurb = 1,p%nTurbines - DO i = 1, p%NFairs(iTurb) - DO J=1,3 - y%PtFairleadLoad(iTurb)%Force(J,I) = m%ConnectList(m%FairIdList(I,iTurb))%Ftot(J) - END DO - END DO - ! <<<< - + end do + ! ! write all node positions to the node positons output array ! ! go through the nodes and fill in the data (this should maybe be turned into a global function) ! J=0 @@ -2775,50 +2710,55 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er ! ---------------------------------- coupled things --------------------------------- ! Apply displacement and velocity terms here. Accelerations will be considered to calculate inertial loads at the end. + ! Note: TurbineRefPos is to offset into farm's true global reference based on turbine X and Y reference positions (these should be 0 for regular FAST use) + + + DO iTurb = 1, p%nTurbines J = 0 ! J is the index of the coupling points in the input mesh CoupledKinematics ! any coupled bodies (type -1) - DO l = 1,p%nCpldBodies + DO l = 1,p%nCpldBodies(iTurb) J = J + 1 - r6_in(1:3) = u%CoupledKinematics%Position(:,J) + u%CoupledKinematics%TranslationDisp(:,J) - !r6_in(4:6) = EulerExtract( TRANSPOSE( u%CoupledKinematics%Orientation(:,:,J) ) ) - r6_in(4:6) = EulerExtract( u%CoupledKinematics%Orientation(:,:,J) ) ! <<< changing back - v6_in(1:3) = u%CoupledKinematics%TranslationVel(:,J) - v6_in(4:6) = u%CoupledKinematics%RotationVel(:,J) - a6_in(1:3) = u%CoupledKinematics%TranslationAcc(:,J) - a6_in(4:6) = u%CoupledKinematics%RotationAcc(:,J) - - CALL Body_SetKinematics(m%BodyList(m%CpldBodyIs(l)), r6_in, v6_in, a6_in, t, m) + r6_in(1:3) = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + !r6_in(4:6) = EulerExtract( TRANSPOSE( u%CoupledKinematics(iTurb)%Orientation(:,:,J) ) ) + r6_in(4:6) = EulerExtract( u%CoupledKinematics(iTurb)%Orientation(:,:,J) ) ! <<< changing back + v6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationVel(:,J) + v6_in(4:6) = u%CoupledKinematics(iTurb)%RotationVel(:,J) + a6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationAcc(:,J) + a6_in(4:6) = u%CoupledKinematics(iTurb)%RotationAcc(:,J) + + CALL Body_SetKinematics(m%BodyList(m%CpldBodyIs(l,iTurb)), r6_in, v6_in, a6_in, t, m) END DO ! any coupled rods (type -1 or -2) note, rotations ignored if it's a pinned rod - DO l = 1,p%nCpldRods + DO l = 1,p%nCpldRods(iTurb) J = J + 1 - r6_in(1:3) = u%CoupledKinematics%Position(:,J) + u%CoupledKinematics%TranslationDisp(:,J) - r6_in(4:6) = MATMUL( u%CoupledKinematics%Orientation(:,:,J) , (/0.0, 0.0, 1.0/) ) ! <<<< CHECK ! adjustment because rod's rotational entries are a unit vector, q - v6_in(1:3) = u%CoupledKinematics%TranslationVel(:,J) - v6_in(4:6) = u%CoupledKinematics%RotationVel(:,J) - a6_in(1:3) = u%CoupledKinematics%TranslationAcc(:,J) - a6_in(4:6) = u%CoupledKinematics%RotationAcc(:,J) + r6_in(1:3) = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + r6_in(4:6) = MATMUL( u%CoupledKinematics(iTurb)%Orientation(:,:,J) , (/0.0, 0.0, 1.0/) ) ! <<<< CHECK ! adjustment because rod's rotational entries are a unit vector, q + v6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationVel(:,J) + v6_in(4:6) = u%CoupledKinematics(iTurb)%RotationVel(:,J) + a6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationAcc(:,J) + a6_in(4:6) = u%CoupledKinematics(iTurb)%RotationAcc(:,J) - CALL Rod_SetKinematics(m%RodList(m%CpldRodIs(l)), r6_in, v6_in, a6_in, t, m) + CALL Rod_SetKinematics(m%RodList(m%CpldRodIs(l,iTurb)), r6_in, v6_in, a6_in, t, m) END DO ! any coupled points (type -1) - DO l = 1, p%nCpldCons + DO l = 1, p%nCpldCons(iTurb) J = J + 1 - r_in = u%CoupledKinematics%Position(:,J) + u%CoupledKinematics%TranslationDisp(:,J) - rd_in = u%CoupledKinematics%TranslationVel(:,J) - a_in(1:3) = u%CoupledKinematics%TranslationAcc(:,J) - CALL Connect_SetKinematics(m%ConnectList(m%CpldConIs(l)), r_in, rd_in, a_in, t, m) + r_in = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + p%TurbineRefPos(:,iTurb) + rd_in = u%CoupledKinematics(iTurb)%TranslationVel(:,J) + a_in(1:3) = u%CoupledKinematics(iTurb)%TranslationAcc(:,J) + CALL Connect_SetKinematics(m%ConnectList(m%CpldConIs(l,iTurb)), r_in, rd_in, a_in, t, m) !print *, u%PtFairleadDisplacement%Position(:,l) + u%PtFairleadDisplacement%TranslationDisp(:,l) !print *, u%PtFairleadDisplacement%TranslationVel(:,l) END DO + end do ! iTurb ! >>>>> in theory I would repeat the above but for each turbine in the case of array use here <<<<< @@ -2829,18 +2769,6 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er ! nvm: using knowledge of p%meshIndex or something ! in theory might also support individual line tensioning control commands from turbines this way too, or maybe it's supercontroller level (not a short term problem though) - !FIXME: below is pasted in and needs to be removed or integrated! >>>>> - do iTurb = 1,p%nTurbines - DO I = 1,p%NFairs(iTurb) - DO J = 1,3 - m%ConnectList(m%FairIdList(I,iTurb))%r(J) = u%PtFairleadDisplacement(iTurb)%Position(J,I) + u%PtFairleadDisplacement(iTurb)%TranslationDisp(J,I) - m%ConnectList(m%FairIdList(I,iTurb))%rd(J) = u%PtFairleadDisplacement(iTurb)%TranslationVel(J,I) - END DO - ! offset into farm's true global reference based on turbine X and Y reference positions (these should be 0 for regular FAST use) - m%ConnectList(m%FairIdList(I,iTurb))%r(1) = m%ConnectList(m%FairIdList(I,iTurb))%r(1) + p%TurbineRefPos(1,iTurb) - m%ConnectList(m%FairIdList(I,iTurb))%r(2) = m%ConnectList(m%FairIdList(I,iTurb))%r(2) + p%TurbineRefPos(2,iTurb) - end do - ! <<<<< ! apply line length changes from active tensioning if applicable DO L = 1, p%NLines diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index f39868a82f..72abe8b3a6 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -340,9 +340,9 @@ typedef ^ ^ IntKi nFails - typedef ^ ^ IntKi nFreeBodies - 0 - "" "" typedef ^ ^ IntKi nFreeRods - 0 - "" "" typedef ^ ^ IntKi nFreeCons - 0 - "" "" -typedef ^ ^ IntKi nCpldBodies - 0 - "" "" -typedef ^ ^ IntKi nCpldRods - 0 - "" "" -typedef ^ ^ IntKi nCpldCons - 0 - "number of Fairlead Connections" "" +typedef ^ ^ IntKi nCpldBodies {:} - - "number of coupled bodies (for FAST.Farm, size>1 with an entry for each turbine)" "" +typedef ^ ^ IntKi nCpldRods {:} - - "number of coupled rods (for FAST.Farm, size>1 with an entry for each turbine)" "" +typedef ^ ^ IntKi nCpldCons {:} - - "number of coupled points (for FAST.Farm, size>1 with an entry for each turbine)" "" typedef ^ ^ IntKi NConns - 0 - "number of Connect type Connections - not to be confused with NConnects" "" typedef ^ ^ IntKi NAnchs - 0 - "number of Anchor type Connections" "" typedef ^ ^ DbKi g - 9.81 - "gravitational constant (positive)" "[m/s^2]" From 0f1b8707967634b8fefe61e00f2f6b1643cc40ac Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Sat, 10 Jul 2021 01:46:11 -0600 Subject: [PATCH 044/242] Adjusted some indenting in MoorDyn --- modules/moordyn/src/MoorDyn.f90 | 262 ++++++++++++++++---------------- 1 file changed, 131 insertions(+), 131 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 16241e663b..27fca4c88d 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -1527,8 +1527,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er RETURN END IF - ! Go through each turbine and set up its mesh and initial positions of coupled objects - DO iTurb = 1,p%nTurbines + ! Go through each turbine and set up its mesh and initial positions of coupled objects + DO iTurb = 1,p%nTurbines ! count number of coupling nodes needed for the mesh of this turbine K = p%nCpldBodies(iTurb) + p%nCpldRods(iTurb) + p%nCpldCons(iTurb) @@ -1546,77 +1546,77 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN - ! note: in MoorDyn-F v2, the points in the mesh correspond in order to all the coupled bodies, then rods, then connections - ! >>> make sure all coupled objects have been offset correctly by the PtfmInit values, including if it's a farm situation -- below or where the objects are first created <<<< - - J = 0 ! this is the counter through the mesh points for each turbine - - DO l = 1,p%nCpldBodies(iTurb) - - J = J + 1 - - rRef = m%BodyList(m%CpldBodyIs(l,iTurb))%r6 ! for now set reference position as per input file <<< + ! note: in MoorDyn-F v2, the points in the mesh correspond in order to all the coupled bodies, then rods, then connections + ! >>> make sure all coupled objects have been offset correctly by the PtfmInit values, including if it's a farm situation -- below or where the objects are first created <<<< - CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef(1:3), ErrStat2, ErrMsg2) ! defaults to identity orientation matrix - u%CoupledKinematics(iTurb)%TranslationDisp(:,J) = 0.0_ReKi ! no displacement from reference position - - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN - - ! ! Apply offsets due to initial platform rotations and translations (fixed Jun 19, 2015) - ! CALL SmllRotTrans('body rotation matrix', InitInp%PtfmInit(4),InitInp%PtfmInit(5),InitInp%PtfmInit(6), OrMat, '', ErrStat2, ErrMsg2) - ! u%CoupledKinematics%TranslationDisp(1, i) = InitInp%PtfmInit(1) + OrMat(1,1)*rRef(1) + OrMat(2,1)*rRef(2) + OrMat(3,1)*rRef(3) - rRef(1) - ! u%CoupledKinematics%TranslationDisp(2, i) = InitInp%PtfmInit(2) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2) - ! u%CoupledKinematics%TranslationDisp(3, i) = InitInp%PtfmInit(3) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) + J = 0 ! this is the counter through the mesh points for each turbine - CALL MeshConstructElement(u%CoupledKinematics(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, J) ! set node as point element - CALL Body_InitializeUnfree( m%BodyList(m%CpldBodyIs(l,iTurb)), m ) - - END DO - - DO l = 1,p%nCpldRods(iTurb) ! keeping this one simple for now, positioning at whatever is specified in input file <<<<< should change to glue code! - - J = J + 1 + DO l = 1,p%nCpldBodies(iTurb) - rRef = m%RodList(m%CpldRodIs(l,iTurb))%r6 ! for now set reference position as per input file <<< + J = J + 1 - CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef, ErrStat2, ErrMsg2) ! defaults to identity orientation matrix - u%CoupledKinematics(iTurb)%TranslationDisp(:,J) = 0.0_ReKi ! no displacement from reference position - CALL MeshConstructElement(u%CoupledKinematics(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, J) - - CALL Rod_SetKinematics(m%RodList(m%CpldRodIs(l,iTurb)), DBLE(rRef), m%zeros6, m%zeros6, 0.0_DbKi, m) - END DO + rRef = m%BodyList(m%CpldBodyIs(l,iTurb))%r6 ! for now set reference position as per input file <<< + + CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef(1:3), ErrStat2, ErrMsg2) ! defaults to identity orientation matrix + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) = 0.0_ReKi ! no displacement from reference position - DO l = 1,p%nCpldCons(iTurb) ! keeping this one simple for now, positioning at whatever is specified by glue code <<< - J = J + 1 - - ! set reference position as per input file - rRef(1:3) = m%ConnectList(m%CpldConIs(l,iTurb))%r - CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef, ErrStat2, ErrMsg2) - - ! calculate initial point relative position, adjusted due to initial platform rotations and translations <<< could convert to array math - CALL SmllRotTrans('PtfmInit', InitInp%PtfmInit(4,iTurb),InitInp%PtfmInit(5,iTurb),InitInp%PtfmInit(6,iTurb), OrMat, '', ErrStat2, ErrMsg2) - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN - u%CoupledKinematics(iTurb)%TranslationDisp(1,i) = InitInp%PtfmInit(1,iTurb) + OrMat(1,1)*rRef(1) + OrMat(2,1)*rRef(2) + OrMat(3,1)*rRef(3) - rRef(1) - u%CoupledKinematics(iTurb)%TranslationDisp(2,i) = InitInp%PtfmInit(2,iTurb) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2) - u%CoupledKinematics(iTurb)%TranslationDisp(3,i) = InitInp%PtfmInit(3,iTurb) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) - - ! set absolute initial positions in MoorDyn - m%ConnectList(m%CpldConIs(l,iTurb))%r = u%CoupledKinematics%Position(:,iTurb) + u%CoupledKinematics%TranslationDisp(:,iTurb) + p%TurbineRefPos(:,iTurb) + ! ! Apply offsets due to initial platform rotations and translations (fixed Jun 19, 2015) + ! CALL SmllRotTrans('body rotation matrix', InitInp%PtfmInit(4),InitInp%PtfmInit(5),InitInp%PtfmInit(6), OrMat, '', ErrStat2, ErrMsg2) + ! u%CoupledKinematics%TranslationDisp(1, i) = InitInp%PtfmInit(1) + OrMat(1,1)*rRef(1) + OrMat(2,1)*rRef(2) + OrMat(3,1)*rRef(3) - rRef(1) + ! u%CoupledKinematics%TranslationDisp(2, i) = InitInp%PtfmInit(2) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2) + ! u%CoupledKinematics%TranslationDisp(3, i) = InitInp%PtfmInit(3) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) + + CALL MeshConstructElement(u%CoupledKinematics(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, J) ! set node as point element + CALL Body_InitializeUnfree( m%BodyList(m%CpldBodyIs(l,iTurb)), m ) + + END DO + DO l = 1,p%nCpldRods(iTurb) ! keeping this one simple for now, positioning at whatever is specified in input file <<<<< should change to glue code! + + J = J + 1 + + rRef = m%RodList(m%CpldRodIs(l,iTurb))%r6 ! for now set reference position as per input file <<< + + CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef, ErrStat2, ErrMsg2) ! defaults to identity orientation matrix + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) = 0.0_ReKi ! no displacement from reference position + CALL MeshConstructElement(u%CoupledKinematics(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, J) + + CALL Rod_SetKinematics(m%RodList(m%CpldRodIs(l,iTurb)), DBLE(rRef), m%zeros6, m%zeros6, 0.0_DbKi, m) + END DO + + DO l = 1,p%nCpldCons(iTurb) ! keeping this one simple for now, positioning at whatever is specified by glue code <<< + J = J + 1 + + ! set reference position as per input file + rRef(1:3) = m%ConnectList(m%CpldConIs(l,iTurb))%r + CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef, ErrStat2, ErrMsg2) + + ! calculate initial point relative position, adjusted due to initial platform rotations and translations <<< could convert to array math + CALL SmllRotTrans('PtfmInit', InitInp%PtfmInit(4,iTurb),InitInp%PtfmInit(5,iTurb),InitInp%PtfmInit(6,iTurb), OrMat, '', ErrStat2, ErrMsg2) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN + + u%CoupledKinematics(iTurb)%TranslationDisp(1,i) = InitInp%PtfmInit(1,iTurb) + OrMat(1,1)*rRef(1) + OrMat(2,1)*rRef(2) + OrMat(3,1)*rRef(3) - rRef(1) + u%CoupledKinematics(iTurb)%TranslationDisp(2,i) = InitInp%PtfmInit(2,iTurb) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2) + u%CoupledKinematics(iTurb)%TranslationDisp(3,i) = InitInp%PtfmInit(3,iTurb) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) + + ! set absolute initial positions in MoorDyn + m%ConnectList(m%CpldConIs(l,iTurb))%r = u%CoupledKinematics%Position(:,iTurb) + u%CoupledKinematics%TranslationDisp(:,iTurb) + p%TurbineRefPos(:,iTurb) - !print *, 'Fairlead ', i, ' z TranslationDisp at start is ', u%PtFairleadDisplacement(iTurb)%TranslationDisp(3,i) - !print *, 'Fairlead ', i, ' z Position at start is ', u%PtFairleadDisplacement(iTurb)%Position(3,i) - ! <<<< + + !print *, 'Fairlead ', i, ' z TranslationDisp at start is ', u%PtFairleadDisplacement(iTurb)%TranslationDisp(3,i) + !print *, 'Fairlead ', i, ' z Position at start is ', u%PtFairleadDisplacement(iTurb)%Position(3,i) + ! <<<< - CALL MeshConstructElement(u%CoupledKinematics(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, J) + CALL MeshConstructElement(u%CoupledKinematics(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, J) - ! lastly, do this to set the attached line endpoint positions: - rRefDub = rRef(1:3) - CALL Connect_SetKinematics(m%ConnectList(m%CpldConIs(l,iTurb)), rRefDub, m%zeros6(1:3), m%zeros6(1:3), 0.0_DbKi, m) - END DO + ! lastly, do this to set the attached line endpoint positions: + rRefDub = rRef(1:3) + CALL Connect_SetKinematics(m%ConnectList(m%CpldConIs(l,iTurb)), rRefDub, m%zeros6(1:3), m%zeros6(1:3), 0.0_DbKi, m) + END DO CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN @@ -1629,13 +1629,12 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN end if - - - ! set velocities/accelerations of all mesh nodes to zero - u%CoupledKinematics(iTurb)%TranslationVel = 0.0_ReKi - u%CoupledKinematics(iTurb)%TranslationAcc = 0.0_ReKi - u%CoupledKinematics(iTurb)%RotationVel = 0.0_ReKi - u%CoupledKinematics(iTurb)%RotationAcc = 0.0_ReKi + + ! set velocities/accelerations of all mesh nodes to zero + u%CoupledKinematics(iTurb)%TranslationVel = 0.0_ReKi + u%CoupledKinematics(iTurb)%TranslationAcc = 0.0_ReKi + u%CoupledKinematics(iTurb)%RotationVel = 0.0_ReKi + u%CoupledKinematics(iTurb)%RotationAcc = 0.0_ReKi CALL MeshCommit ( u%CoupledKinematics(iTurb), ErrStat, ErrMsg ) CALL CheckError( ErrStat2, ErrMsg2 ) @@ -2546,27 +2545,27 @@ SUBROUTINE MD_CalcOutput( t, u, p, x, xd, z, other, y, m, ErrStat, ErrMsg ) do iTurb = 1,p%nTurbines - J = 0 ! mesh index - DO l = 1,p%nCpldBodies(iTurb) - J = J + 1 - CALL Body_GetCoupledForce(m%BodyList(m%CpldBodyIs(l,iTurb)), F6net, m, p) - y%CoupledLoads(iTurb)%Force( :,J) = F6net(1:3) - y%CoupledLoads(iTurb)%Moment(:,J) = F6net(4:6) - END DO - - DO l = 1,p%nCpldRods(iTurb) - J = J + 1 - CALL Rod_GetCoupledForce(m%RodList(m%CpldRodIs(l,iTurb)), F6net, m, p) - y%CoupledLoads(iTurb)%Force( :,J) = F6net(1:3) - y%CoupledLoads(iTurb)%Moment(:,J) = F6net(4:6) - END DO - - DO l = 1,p%nCpldCons(iTurb) - J = J + 1 - CALL Connect_GetCoupledForce(m%ConnectList(m%CpldConIs(l,iTurb)), F6net(1:3), m, p) - y%CoupledLoads(iTurb)%Force(:,J) = F6net(1:3) - END DO - + J = 0 ! mesh index + DO l = 1,p%nCpldBodies(iTurb) + J = J + 1 + CALL Body_GetCoupledForce(m%BodyList(m%CpldBodyIs(l,iTurb)), F6net, m, p) + y%CoupledLoads(iTurb)%Force( :,J) = F6net(1:3) + y%CoupledLoads(iTurb)%Moment(:,J) = F6net(4:6) + END DO + + DO l = 1,p%nCpldRods(iTurb) + J = J + 1 + CALL Rod_GetCoupledForce(m%RodList(m%CpldRodIs(l,iTurb)), F6net, m, p) + y%CoupledLoads(iTurb)%Force( :,J) = F6net(1:3) + y%CoupledLoads(iTurb)%Moment(:,J) = F6net(4:6) + END DO + + DO l = 1,p%nCpldCons(iTurb) + J = J + 1 + CALL Connect_GetCoupledForce(m%ConnectList(m%CpldConIs(l,iTurb)), F6net(1:3), m, p) + y%CoupledLoads(iTurb)%Force(:,J) = F6net(1:3) + END DO + end do ! ! write all node positions to the node positons output array @@ -2714,50 +2713,51 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er DO iTurb = 1, p%nTurbines - - J = 0 ! J is the index of the coupling points in the input mesh CoupledKinematics - ! any coupled bodies (type -1) - DO l = 1,p%nCpldBodies(iTurb) - J = J + 1 - r6_in(1:3) = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) - !r6_in(4:6) = EulerExtract( TRANSPOSE( u%CoupledKinematics(iTurb)%Orientation(:,:,J) ) ) - r6_in(4:6) = EulerExtract( u%CoupledKinematics(iTurb)%Orientation(:,:,J) ) ! <<< changing back - v6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationVel(:,J) - v6_in(4:6) = u%CoupledKinematics(iTurb)%RotationVel(:,J) - a6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationAcc(:,J) - a6_in(4:6) = u%CoupledKinematics(iTurb)%RotationAcc(:,J) - - CALL Body_SetKinematics(m%BodyList(m%CpldBodyIs(l,iTurb)), r6_in, v6_in, a6_in, t, m) - END DO - - ! any coupled rods (type -1 or -2) note, rotations ignored if it's a pinned rod - DO l = 1,p%nCpldRods(iTurb) - J = J + 1 + + J = 0 ! J is the index of the coupling points in the input mesh CoupledKinematics + ! any coupled bodies (type -1) + DO l = 1,p%nCpldBodies(iTurb) + J = J + 1 + r6_in(1:3) = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + !r6_in(4:6) = EulerExtract( TRANSPOSE( u%CoupledKinematics(iTurb)%Orientation(:,:,J) ) ) + r6_in(4:6) = EulerExtract( u%CoupledKinematics(iTurb)%Orientation(:,:,J) ) ! <<< changing back + v6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationVel(:,J) + v6_in(4:6) = u%CoupledKinematics(iTurb)%RotationVel(:,J) + a6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationAcc(:,J) + a6_in(4:6) = u%CoupledKinematics(iTurb)%RotationAcc(:,J) + + CALL Body_SetKinematics(m%BodyList(m%CpldBodyIs(l,iTurb)), r6_in, v6_in, a6_in, t, m) + END DO + + ! any coupled rods (type -1 or -2) note, rotations ignored if it's a pinned rod + DO l = 1,p%nCpldRods(iTurb) + J = J + 1 - r6_in(1:3) = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) - r6_in(4:6) = MATMUL( u%CoupledKinematics(iTurb)%Orientation(:,:,J) , (/0.0, 0.0, 1.0/) ) ! <<<< CHECK ! adjustment because rod's rotational entries are a unit vector, q - v6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationVel(:,J) - v6_in(4:6) = u%CoupledKinematics(iTurb)%RotationVel(:,J) - a6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationAcc(:,J) - a6_in(4:6) = u%CoupledKinematics(iTurb)%RotationAcc(:,J) - - CALL Rod_SetKinematics(m%RodList(m%CpldRodIs(l,iTurb)), r6_in, v6_in, a6_in, t, m) - - END DO - - ! any coupled points (type -1) - DO l = 1, p%nCpldCons(iTurb) - J = J + 1 + r6_in(1:3) = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + r6_in(4:6) = MATMUL( u%CoupledKinematics(iTurb)%Orientation(:,:,J) , (/0.0, 0.0, 1.0/) ) ! <<<< CHECK ! adjustment because rod's rotational entries are a unit vector, q + v6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationVel(:,J) + v6_in(4:6) = u%CoupledKinematics(iTurb)%RotationVel(:,J) + a6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationAcc(:,J) + a6_in(4:6) = u%CoupledKinematics(iTurb)%RotationAcc(:,J) - r_in = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + p%TurbineRefPos(:,iTurb) - rd_in = u%CoupledKinematics(iTurb)%TranslationVel(:,J) - a_in(1:3) = u%CoupledKinematics(iTurb)%TranslationAcc(:,J) - CALL Connect_SetKinematics(m%ConnectList(m%CpldConIs(l,iTurb)), r_in, rd_in, a_in, t, m) + CALL Rod_SetKinematics(m%RodList(m%CpldRodIs(l,iTurb)), r6_in, v6_in, a6_in, t, m) + + END DO - !print *, u%PtFairleadDisplacement%Position(:,l) + u%PtFairleadDisplacement%TranslationDisp(:,l) - !print *, u%PtFairleadDisplacement%TranslationVel(:,l) + ! any coupled points (type -1) + DO l = 1, p%nCpldCons(iTurb) + J = J + 1 + + r_in = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + p%TurbineRefPos(:,iTurb) + rd_in = u%CoupledKinematics(iTurb)%TranslationVel(:,J) + a_in(1:3) = u%CoupledKinematics(iTurb)%TranslationAcc(:,J) + CALL Connect_SetKinematics(m%ConnectList(m%CpldConIs(l,iTurb)), r_in, rd_in, a_in, t, m) + + !print *, u%PtFairleadDisplacement%Position(:,l) + u%PtFairleadDisplacement%TranslationDisp(:,l) + !print *, u%PtFairleadDisplacement%TranslationVel(:,l) + + END DO - END DO end do ! iTurb From 73d47e03b48c7e8e5f4d5d4768a5506991f7ebf3 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Sun, 11 Jul 2021 01:26:23 -0600 Subject: [PATCH 045/242] MoorDyn further edits to merge MDv2 and shared-mooring farm capablities. Now compiles. --- modules/moordyn/src/MoorDyn.f90 | 126 ++++----- modules/moordyn/src/MoorDyn_Registry.txt | 7 +- modules/moordyn/src/MoorDyn_Types.f90 | 329 ++++++++++++++++++++--- 3 files changed, 341 insertions(+), 121 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 27fca4c88d..9989640d11 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -179,23 +179,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! Check if this MoorDyn instance is being run from FAST.Farm (indicated by FarmSize > 0) if (InitInp%FarmSize > 0) then CALL WrScr(' >>> MoorDyn is running in array mode <<< ') - ! could make sure the size of this is right: SIZE(InitInp%FarmCoupledKinematics) - - !InitInp%FarmNCpldCons - p%nTurbines = InitInp%FarmSize - - ! copy over turbine reference positions for later use - p%TurbineRefPos = InitInp%TurbineRefPos - else ! FarmSize==0 indicates normal, FAST module mode - p%nTurbines = 1 ! if a regular FAST module mode, we treat it like a nTurbine=1 farm case - - m%PtfmInit = InitInp%PtfmInit(:,1) ! save a copy of PtfmInit so it's available at the farm level - p%TurbineRefPos = 0.0_DbKi ! for now assuming this is zero for FAST use - END IF ! allocate some parameter arrays that are for each turbine (size 1 if regular OpenFAST use) @@ -1501,22 +1488,9 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! Initialize coupled objects based on passed kinematics ! (set up initial condition of each coupled object based on values specified by glue code) - ! Also create i/o meshes + ! Also create i/o meshes - ! <<<<<<<< look here when changing to shared mooring compatibility - - ! create input mesh for all coupled objects (as mesh node points) - CALL MeshCreate(BlankMesh=u%CoupledKinematics, IOS= COMPONENT_INPUT, & - Nnodes = p%nCpldBodies + p%nCpldRods + p%nCpldCons, & - TranslationDisp=.TRUE., TranslationVel=.TRUE., & - Orientation=.TRUE., RotationVel=.TRUE., & - TranslationAcc=.TRUE., RotationAcc= .TRUE., & - ErrStat=ErrStat2, ErrMess=ErrMsg2) - - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN - - ALLOCATE ( u%CoupledKinematics(p%nTurbines), STAT = ErrStat ) + ALLOCATE ( u%CoupledKinematics(p%nTurbines), STAT = ErrStat ) IF ( ErrStat /= ErrID_None ) THEN CALL CheckError(ErrID_Fatal, ' Error allocating CoupledKinematics input array.') RETURN @@ -1536,8 +1510,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! create input mesh for fairlead kinematics CALL MeshCreate(BlankMesh=u%CoupledKinematics(iTurb) , & - IOS= COMPONENT_INPUT, & - Nnodes = K + IOS= COMPONENT_INPUT, Nnodes = K, & TranslationDisp=.TRUE., TranslationVel=.TRUE., & Orientation=.TRUE., RotationVel=.TRUE., & TranslationAcc=.TRUE., RotationAcc= .TRUE., & @@ -1604,7 +1577,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er u%CoupledKinematics(iTurb)%TranslationDisp(3,i) = InitInp%PtfmInit(3,iTurb) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) ! set absolute initial positions in MoorDyn - m%ConnectList(m%CpldConIs(l,iTurb))%r = u%CoupledKinematics%Position(:,iTurb) + u%CoupledKinematics%TranslationDisp(:,iTurb) + p%TurbineRefPos(:,iTurb) + m%ConnectList(m%CpldConIs(l,iTurb))%r = u%CoupledKinematics(iTurb)%Position(:,iTurb) + u%CoupledKinematics(iTurb)%TranslationDisp(:,iTurb) + p%TurbineRefPos(:,iTurb) !print *, 'Fairlead ', i, ' z TranslationDisp at start is ', u%PtFairleadDisplacement(iTurb)%TranslationDisp(3,i) @@ -2892,23 +2865,24 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er ! get dynamics/forces (doRHS()) of coupled objects, which weren't addressed in above calls (this includes inertial loads) ! note: can do this in any order since there are no dependencies among coupled objects - - DO l = 1,p%nCpldCons - - ! >>>>>>>> here we should pass along accelerations and include inertial loads in the calculation!!! <<>>>>>>> here we should pass along accelerations and include inertial loads in the calculation!!! <<1 with an entry for each turbine) [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: nCpldRods !< number of coupled rods (for FAST.Farm, size>1 with an entry for each turbine) [] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: nCpldCons !< number of coupled points (for FAST.Farm, size>1 with an entry for each turbine) [] INTEGER(IntKi) :: NConns = 0 !< number of Connect type Connections - not to be confused with NConnects [] INTEGER(IntKi) :: NAnchs = 0 !< number of Anchor type Connections [] REAL(DbKi) :: g = 9.81 !< gravitational constant (positive) [[m/s^2]] @@ -388,6 +388,7 @@ MODULE MoorDyn_Types INTEGER(IntKi) :: MDUnOut !< Unit number of main output file [-] INTEGER(IntKi) :: WaterKin !< Flag for whether or how to consider water kinematics [-] INTEGER(IntKi) :: nTurbines !< Number of turbines if MoorDyn is performing an array-level simulation with FAST.Farm, otherwise 0 [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: TurbineRefPos !< reference position of turbines in farm, shape: 3, nTurbines [-] REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: ux !< water velocities time series at each grid point [-] REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: uy !< water velocities time series at each grid point [-] REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: uz !< water velocities time series at each grid point [-] @@ -7708,6 +7709,7 @@ SUBROUTINE MD_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) ! Local INTEGER(IntKi) :: i,j,k INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_CopyMisc' @@ -7844,8 +7846,10 @@ SUBROUTINE MD_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) IF (ALLOCATED(SrcMiscData%CpldConIs)) THEN i1_l = LBOUND(SrcMiscData%CpldConIs,1) i1_u = UBOUND(SrcMiscData%CpldConIs,1) + i2_l = LBOUND(SrcMiscData%CpldConIs,2) + i2_u = UBOUND(SrcMiscData%CpldConIs,2) IF (.NOT. ALLOCATED(DstMiscData%CpldConIs)) THEN - ALLOCATE(DstMiscData%CpldConIs(i1_l:i1_u),STAT=ErrStat2) + ALLOCATE(DstMiscData%CpldConIs(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%CpldConIs.', ErrStat, ErrMsg,RoutineName) RETURN @@ -7868,8 +7872,10 @@ SUBROUTINE MD_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) IF (ALLOCATED(SrcMiscData%CpldRodIs)) THEN i1_l = LBOUND(SrcMiscData%CpldRodIs,1) i1_u = UBOUND(SrcMiscData%CpldRodIs,1) + i2_l = LBOUND(SrcMiscData%CpldRodIs,2) + i2_u = UBOUND(SrcMiscData%CpldRodIs,2) IF (.NOT. ALLOCATED(DstMiscData%CpldRodIs)) THEN - ALLOCATE(DstMiscData%CpldRodIs(i1_l:i1_u),STAT=ErrStat2) + ALLOCATE(DstMiscData%CpldRodIs(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%CpldRodIs.', ErrStat, ErrMsg,RoutineName) RETURN @@ -7892,8 +7898,10 @@ SUBROUTINE MD_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) IF (ALLOCATED(SrcMiscData%CpldBodyIs)) THEN i1_l = LBOUND(SrcMiscData%CpldBodyIs,1) i1_u = UBOUND(SrcMiscData%CpldBodyIs,1) + i2_l = LBOUND(SrcMiscData%CpldBodyIs,2) + i2_u = UBOUND(SrcMiscData%CpldBodyIs,2) IF (.NOT. ALLOCATED(DstMiscData%CpldBodyIs)) THEN - ALLOCATE(DstMiscData%CpldBodyIs(i1_l:i1_u),STAT=ErrStat2) + ALLOCATE(DstMiscData%CpldBodyIs(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%CpldBodyIs.', ErrStat, ErrMsg,RoutineName) RETURN @@ -8342,7 +8350,7 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz END IF Int_BufSz = Int_BufSz + 1 ! CpldConIs allocated yes/no IF ( ALLOCATED(InData%CpldConIs) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! CpldConIs upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + 2*2 ! CpldConIs upper/lower bounds for each dimension Int_BufSz = Int_BufSz + SIZE(InData%CpldConIs) ! CpldConIs END IF Int_BufSz = Int_BufSz + 1 ! FreeRodIs allocated yes/no @@ -8352,7 +8360,7 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz END IF Int_BufSz = Int_BufSz + 1 ! CpldRodIs allocated yes/no IF ( ALLOCATED(InData%CpldRodIs) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! CpldRodIs upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + 2*2 ! CpldRodIs upper/lower bounds for each dimension Int_BufSz = Int_BufSz + SIZE(InData%CpldRodIs) ! CpldRodIs END IF Int_BufSz = Int_BufSz + 1 ! FreeBodyIs allocated yes/no @@ -8362,7 +8370,7 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz END IF Int_BufSz = Int_BufSz + 1 ! CpldBodyIs allocated yes/no IF ( ALLOCATED(InData%CpldBodyIs) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! CpldBodyIs upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + 2*2 ! CpldBodyIs upper/lower bounds for each dimension Int_BufSz = Int_BufSz + SIZE(InData%CpldBodyIs) ! CpldBodyIs END IF Int_BufSz = Int_BufSz + 1 ! LineStateIs1 allocated yes/no @@ -8812,11 +8820,16 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_Xferred = Int_Xferred + 1 IntKiBuf( Int_Xferred ) = LBOUND(InData%CpldConIs,1) IntKiBuf( Int_Xferred + 1) = UBOUND(InData%CpldConIs,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%CpldConIs,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%CpldConIs,2) Int_Xferred = Int_Xferred + 2 - DO i1 = LBOUND(InData%CpldConIs,1), UBOUND(InData%CpldConIs,1) - IntKiBuf(Int_Xferred) = InData%CpldConIs(i1) - Int_Xferred = Int_Xferred + 1 + DO i2 = LBOUND(InData%CpldConIs,2), UBOUND(InData%CpldConIs,2) + DO i1 = LBOUND(InData%CpldConIs,1), UBOUND(InData%CpldConIs,1) + IntKiBuf(Int_Xferred) = InData%CpldConIs(i1,i2) + Int_Xferred = Int_Xferred + 1 + END DO END DO END IF IF ( .NOT. ALLOCATED(InData%FreeRodIs) ) THEN @@ -8842,11 +8855,16 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_Xferred = Int_Xferred + 1 IntKiBuf( Int_Xferred ) = LBOUND(InData%CpldRodIs,1) IntKiBuf( Int_Xferred + 1) = UBOUND(InData%CpldRodIs,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%CpldRodIs,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%CpldRodIs,2) Int_Xferred = Int_Xferred + 2 - DO i1 = LBOUND(InData%CpldRodIs,1), UBOUND(InData%CpldRodIs,1) - IntKiBuf(Int_Xferred) = InData%CpldRodIs(i1) - Int_Xferred = Int_Xferred + 1 + DO i2 = LBOUND(InData%CpldRodIs,2), UBOUND(InData%CpldRodIs,2) + DO i1 = LBOUND(InData%CpldRodIs,1), UBOUND(InData%CpldRodIs,1) + IntKiBuf(Int_Xferred) = InData%CpldRodIs(i1,i2) + Int_Xferred = Int_Xferred + 1 + END DO END DO END IF IF ( .NOT. ALLOCATED(InData%FreeBodyIs) ) THEN @@ -8872,11 +8890,16 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_Xferred = Int_Xferred + 1 IntKiBuf( Int_Xferred ) = LBOUND(InData%CpldBodyIs,1) IntKiBuf( Int_Xferred + 1) = UBOUND(InData%CpldBodyIs,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%CpldBodyIs,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%CpldBodyIs,2) Int_Xferred = Int_Xferred + 2 - DO i1 = LBOUND(InData%CpldBodyIs,1), UBOUND(InData%CpldBodyIs,1) - IntKiBuf(Int_Xferred) = InData%CpldBodyIs(i1) - Int_Xferred = Int_Xferred + 1 + DO i2 = LBOUND(InData%CpldBodyIs,2), UBOUND(InData%CpldBodyIs,2) + DO i1 = LBOUND(InData%CpldBodyIs,1), UBOUND(InData%CpldBodyIs,1) + IntKiBuf(Int_Xferred) = InData%CpldBodyIs(i1,i2) + Int_Xferred = Int_Xferred + 1 + END DO END DO END IF IF ( .NOT. ALLOCATED(InData%LineStateIs1) ) THEN @@ -9094,6 +9117,7 @@ SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) INTEGER(IntKi) :: Int_Xferred INTEGER(IntKi) :: i INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 + INTEGER(IntKi) :: i2, i2_l, i2_u ! bounds (upper/lower) for an array dimension 2 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'MD_UnPackMisc' @@ -9564,15 +9588,20 @@ SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) i1_l = IntKiBuf( Int_Xferred ) i1_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 IF (ALLOCATED(OutData%CpldConIs)) DEALLOCATE(OutData%CpldConIs) - ALLOCATE(OutData%CpldConIs(i1_l:i1_u),STAT=ErrStat2) + ALLOCATE(OutData%CpldConIs(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%CpldConIs.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i1 = LBOUND(OutData%CpldConIs,1), UBOUND(OutData%CpldConIs,1) - OutData%CpldConIs(i1) = IntKiBuf(Int_Xferred) - Int_Xferred = Int_Xferred + 1 + DO i2 = LBOUND(OutData%CpldConIs,2), UBOUND(OutData%CpldConIs,2) + DO i1 = LBOUND(OutData%CpldConIs,1), UBOUND(OutData%CpldConIs,1) + OutData%CpldConIs(i1,i2) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO END DO END IF IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! FreeRodIs not allocated @@ -9600,15 +9629,20 @@ SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) i1_l = IntKiBuf( Int_Xferred ) i1_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 IF (ALLOCATED(OutData%CpldRodIs)) DEALLOCATE(OutData%CpldRodIs) - ALLOCATE(OutData%CpldRodIs(i1_l:i1_u),STAT=ErrStat2) + ALLOCATE(OutData%CpldRodIs(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%CpldRodIs.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i1 = LBOUND(OutData%CpldRodIs,1), UBOUND(OutData%CpldRodIs,1) - OutData%CpldRodIs(i1) = IntKiBuf(Int_Xferred) - Int_Xferred = Int_Xferred + 1 + DO i2 = LBOUND(OutData%CpldRodIs,2), UBOUND(OutData%CpldRodIs,2) + DO i1 = LBOUND(OutData%CpldRodIs,1), UBOUND(OutData%CpldRodIs,1) + OutData%CpldRodIs(i1,i2) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO END DO END IF IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! FreeBodyIs not allocated @@ -9636,15 +9670,20 @@ SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) i1_l = IntKiBuf( Int_Xferred ) i1_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 IF (ALLOCATED(OutData%CpldBodyIs)) DEALLOCATE(OutData%CpldBodyIs) - ALLOCATE(OutData%CpldBodyIs(i1_l:i1_u),STAT=ErrStat2) + ALLOCATE(OutData%CpldBodyIs(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%CpldBodyIs.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i1 = LBOUND(OutData%CpldBodyIs,1), UBOUND(OutData%CpldBodyIs,1) - OutData%CpldBodyIs(i1) = IntKiBuf(Int_Xferred) - Int_Xferred = Int_Xferred + 1 + DO i2 = LBOUND(OutData%CpldBodyIs,2), UBOUND(OutData%CpldBodyIs,2) + DO i1 = LBOUND(OutData%CpldBodyIs,1), UBOUND(OutData%CpldBodyIs,1) + OutData%CpldBodyIs(i1,i2) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO END DO END IF IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! LineStateIs1 not allocated @@ -9931,9 +9970,42 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) DstParamData%nFreeBodies = SrcParamData%nFreeBodies DstParamData%nFreeRods = SrcParamData%nFreeRods DstParamData%nFreeCons = SrcParamData%nFreeCons +IF (ALLOCATED(SrcParamData%nCpldBodies)) THEN + i1_l = LBOUND(SrcParamData%nCpldBodies,1) + i1_u = UBOUND(SrcParamData%nCpldBodies,1) + IF (.NOT. ALLOCATED(DstParamData%nCpldBodies)) THEN + ALLOCATE(DstParamData%nCpldBodies(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%nCpldBodies.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF DstParamData%nCpldBodies = SrcParamData%nCpldBodies +ENDIF +IF (ALLOCATED(SrcParamData%nCpldRods)) THEN + i1_l = LBOUND(SrcParamData%nCpldRods,1) + i1_u = UBOUND(SrcParamData%nCpldRods,1) + IF (.NOT. ALLOCATED(DstParamData%nCpldRods)) THEN + ALLOCATE(DstParamData%nCpldRods(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%nCpldRods.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF DstParamData%nCpldRods = SrcParamData%nCpldRods +ENDIF +IF (ALLOCATED(SrcParamData%nCpldCons)) THEN + i1_l = LBOUND(SrcParamData%nCpldCons,1) + i1_u = UBOUND(SrcParamData%nCpldCons,1) + IF (.NOT. ALLOCATED(DstParamData%nCpldCons)) THEN + ALLOCATE(DstParamData%nCpldCons(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%nCpldCons.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF DstParamData%nCpldCons = SrcParamData%nCpldCons +ENDIF DstParamData%NConns = SrcParamData%NConns DstParamData%NAnchs = SrcParamData%NAnchs DstParamData%g = SrcParamData%g @@ -9965,6 +10037,20 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) DstParamData%MDUnOut = SrcParamData%MDUnOut DstParamData%WaterKin = SrcParamData%WaterKin DstParamData%nTurbines = SrcParamData%nTurbines +IF (ALLOCATED(SrcParamData%TurbineRefPos)) THEN + i1_l = LBOUND(SrcParamData%TurbineRefPos,1) + i1_u = UBOUND(SrcParamData%TurbineRefPos,1) + i2_l = LBOUND(SrcParamData%TurbineRefPos,2) + i2_u = UBOUND(SrcParamData%TurbineRefPos,2) + IF (.NOT. ALLOCATED(DstParamData%TurbineRefPos)) THEN + ALLOCATE(DstParamData%TurbineRefPos(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%TurbineRefPos.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%TurbineRefPos = SrcParamData%TurbineRefPos +ENDIF IF (ALLOCATED(SrcParamData%ux)) THEN i1_l = LBOUND(SrcParamData%ux,1) i1_u = UBOUND(SrcParamData%ux,1) @@ -10207,12 +10293,24 @@ SUBROUTINE MD_DestroyParam( ParamData, ErrStat, ErrMsg ) ! ErrStat = ErrID_None ErrMsg = "" +IF (ALLOCATED(ParamData%nCpldBodies)) THEN + DEALLOCATE(ParamData%nCpldBodies) +ENDIF +IF (ALLOCATED(ParamData%nCpldRods)) THEN + DEALLOCATE(ParamData%nCpldRods) +ENDIF +IF (ALLOCATED(ParamData%nCpldCons)) THEN + DEALLOCATE(ParamData%nCpldCons) +ENDIF IF (ALLOCATED(ParamData%OutParam)) THEN DO i1 = LBOUND(ParamData%OutParam,1), UBOUND(ParamData%OutParam,1) CALL MD_Destroyoutparmtype( ParamData%OutParam(i1), ErrStat, ErrMsg ) ENDDO DEALLOCATE(ParamData%OutParam) ENDIF +IF (ALLOCATED(ParamData%TurbineRefPos)) THEN + DEALLOCATE(ParamData%TurbineRefPos) +ENDIF IF (ALLOCATED(ParamData%ux)) THEN DEALLOCATE(ParamData%ux) ENDIF @@ -10307,9 +10405,21 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Int_BufSz = Int_BufSz + 1 ! nFreeBodies Int_BufSz = Int_BufSz + 1 ! nFreeRods Int_BufSz = Int_BufSz + 1 ! nFreeCons - Int_BufSz = Int_BufSz + 1 ! nCpldBodies - Int_BufSz = Int_BufSz + 1 ! nCpldRods - Int_BufSz = Int_BufSz + 1 ! nCpldCons + Int_BufSz = Int_BufSz + 1 ! nCpldBodies allocated yes/no + IF ( ALLOCATED(InData%nCpldBodies) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! nCpldBodies upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%nCpldBodies) ! nCpldBodies + END IF + Int_BufSz = Int_BufSz + 1 ! nCpldRods allocated yes/no + IF ( ALLOCATED(InData%nCpldRods) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! nCpldRods upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%nCpldRods) ! nCpldRods + END IF + Int_BufSz = Int_BufSz + 1 ! nCpldCons allocated yes/no + IF ( ALLOCATED(InData%nCpldCons) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! nCpldCons upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%nCpldCons) ! nCpldCons + END IF Int_BufSz = Int_BufSz + 1 ! NConns Int_BufSz = Int_BufSz + 1 ! NAnchs Db_BufSz = Db_BufSz + 1 ! g @@ -10349,6 +10459,11 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Int_BufSz = Int_BufSz + 1 ! MDUnOut Int_BufSz = Int_BufSz + 1 ! WaterKin Int_BufSz = Int_BufSz + 1 ! nTurbines + Int_BufSz = Int_BufSz + 1 ! TurbineRefPos allocated yes/no + IF ( ALLOCATED(InData%TurbineRefPos) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! TurbineRefPos upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%TurbineRefPos) ! TurbineRefPos + END IF Int_BufSz = Int_BufSz + 1 ! ux allocated yes/no IF ( ALLOCATED(InData%ux) ) THEN Int_BufSz = Int_BufSz + 2*4 ! ux upper/lower bounds for each dimension @@ -10478,12 +10593,51 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%nFreeCons Int_Xferred = Int_Xferred + 1 - IntKiBuf(Int_Xferred) = InData%nCpldBodies + IF ( .NOT. ALLOCATED(InData%nCpldBodies) ) THEN + IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 - IntKiBuf(Int_Xferred) = InData%nCpldRods + ELSE + IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf(Int_Xferred) = InData%nCpldCons + IntKiBuf( Int_Xferred ) = LBOUND(InData%nCpldBodies,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%nCpldBodies,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%nCpldBodies,1), UBOUND(InData%nCpldBodies,1) + IntKiBuf(Int_Xferred) = InData%nCpldBodies(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%nCpldRods) ) THEN + IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%nCpldRods,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%nCpldRods,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%nCpldRods,1), UBOUND(InData%nCpldRods,1) + IntKiBuf(Int_Xferred) = InData%nCpldRods(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%nCpldCons) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%nCpldCons,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%nCpldCons,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%nCpldCons,1), UBOUND(InData%nCpldCons,1) + IntKiBuf(Int_Xferred) = InData%nCpldCons(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF IntKiBuf(Int_Xferred) = InData%NConns Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%NAnchs @@ -10559,6 +10713,26 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%nTurbines Int_Xferred = Int_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%TurbineRefPos) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%TurbineRefPos,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%TurbineRefPos,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%TurbineRefPos,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%TurbineRefPos,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%TurbineRefPos,2), UBOUND(InData%TurbineRefPos,2) + DO i1 = LBOUND(InData%TurbineRefPos,1), UBOUND(InData%TurbineRefPos,1) + ReKiBuf(Re_Xferred) = InData%TurbineRefPos(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF IF ( .NOT. ALLOCATED(InData%ux) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -10966,12 +11140,60 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg Int_Xferred = Int_Xferred + 1 OutData%nFreeCons = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 - OutData%nCpldBodies = IntKiBuf(Int_Xferred) + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! nCpldBodies not allocated Int_Xferred = Int_Xferred + 1 - OutData%nCpldRods = IntKiBuf(Int_Xferred) + ELSE Int_Xferred = Int_Xferred + 1 - OutData%nCpldCons = IntKiBuf(Int_Xferred) + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%nCpldBodies)) DEALLOCATE(OutData%nCpldBodies) + ALLOCATE(OutData%nCpldBodies(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%nCpldBodies.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%nCpldBodies,1), UBOUND(OutData%nCpldBodies,1) + OutData%nCpldBodies(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! nCpldRods not allocated Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%nCpldRods)) DEALLOCATE(OutData%nCpldRods) + ALLOCATE(OutData%nCpldRods(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%nCpldRods.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%nCpldRods,1), UBOUND(OutData%nCpldRods,1) + OutData%nCpldRods(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! nCpldCons not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%nCpldCons)) DEALLOCATE(OutData%nCpldCons) + ALLOCATE(OutData%nCpldCons(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%nCpldCons.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%nCpldCons,1), UBOUND(OutData%nCpldCons,1) + OutData%nCpldCons(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF OutData%NConns = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 OutData%NAnchs = IntKiBuf(Int_Xferred) @@ -11062,6 +11284,29 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg Int_Xferred = Int_Xferred + 1 OutData%nTurbines = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! TurbineRefPos not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%TurbineRefPos)) DEALLOCATE(OutData%TurbineRefPos) + ALLOCATE(OutData%TurbineRefPos(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%TurbineRefPos.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%TurbineRefPos,2), UBOUND(OutData%TurbineRefPos,2) + DO i1 = LBOUND(OutData%TurbineRefPos,1), UBOUND(OutData%TurbineRefPos,1) + OutData%TurbineRefPos(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ux not allocated Int_Xferred = Int_Xferred + 1 ELSE From 82d54d03b7e9a3854a3a371d1321fc4f2d792196 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Sun, 11 Jul 2021 01:24:33 -0600 Subject: [PATCH 046/242] MoorDyn_IO fixing bug when specifying output channel node number --- modules/moordyn/src/MoorDyn_IO.f90 | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index 5f725f3e08..8b177f156b 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -364,7 +364,7 @@ SUBROUTINE MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat, ErrMsg ) ! fairlead tension case (updated) <<<<<<<<<<<<<<<<<<<<<<<<<<< these are not currently working - need new way to find ObjID IF (let1 == 'FAIRTEN') THEN - p%OutParam(I)%OType = 1 ! connection object type + p%OutParam(I)%OType = 1 ! line object type p%OutParam(I)%QType = Ten ! tension quantity type p%OutParam(I)%Units = UnitList(Ten) ! set units according to QType READ (num1,*) oID ! this is the line number @@ -373,7 +373,7 @@ SUBROUTINE MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat, ErrMsg ) ! achor tension case ELSE IF (let1 == 'ANCHTEN') THEN - p%OutParam(I)%OType = 1 ! connectoin object type + p%OutParam(I)%OType = 1 ! line object type p%OutParam(I)%QType = Ten ! tension quantity type p%OutParam(I)%Units = UnitList(Ten) ! set units according to QType READ (num1,*) oID ! this is the line number @@ -390,7 +390,7 @@ SUBROUTINE MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat, ErrMsg ) p%OutParam(I)%OType = 1 ! Line object type ! for now we'll just assume the next character(s) are "n" to represent node number or "s" to represent segment number READ (num2,*) nID ! node or segment ID - p%OutParam%NodeID = nID + p%OutParam(I)%NodeID = nID qVal = let3 ! quantity type string ! Connect case @@ -402,11 +402,11 @@ SUBROUTINE MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat, ErrMsg ) ELSE IF (let1(1:1) == 'R') THEN ! Look for R?xxx or Rod?xxx p%OutParam(I)%OType = 3 ! Rod object type IF (LEN_TRIM(let3)== 0) THEN ! No third character cluster indicates this is a whole-rod channel - p%OutParam%NodeID = 0 + p%OutParam(I)%NodeID = 0 qVal = let2 ! quantity type string ELSE READ (num2,*) nID ! rod node ID - p%OutParam%NodeID = nID + p%OutParam(I)%NodeID = nID qVal = let3 ! quantity type string END IF From 64373ba9b5b78fb1fae8a62361f40b7dfaa53bc5 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Tue, 13 Jul 2021 15:39:51 -0600 Subject: [PATCH 047/242] Finishing up merging of MDv2 and fast-farm branches. Compiles and runs. --- glue-codes/fast-farm/src/FAST_Farm_Subs.f90 | 26 ++++---- modules/moordyn/src/MoorDyn.f90 | 73 ++++++++++++--------- modules/moordyn/src/MoorDyn_Registry.txt | 3 +- modules/moordyn/src/MoorDyn_Types.f90 | 15 ++++- vs-build/FAST-farm/FAST-Farm.vfproj | 12 ++-- 5 files changed, 78 insertions(+), 51 deletions(-) diff --git a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 index b21e476c93..8350571459 100644 --- a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 +++ b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 @@ -1765,7 +1765,7 @@ SUBROUTINE Farm_InitMD( farm, ErrStat, ErrMsg ) ! These aren't currently handled at the FAST.Farm level, so just give the farm's MoorDyn default values, which can be overwridden by its input file MD_InitInp%g = 9.81 MD_InitInp%rhoW = 1025.0 - MD_InitInp%WtrDepth = 0.0 + MD_InitInp%WtrDepth = 0.0 !TODO: eventually connect this to a global depth input variable <<< ! allocate MoorDyn inputs (assuming size 2 for linear interpolation/extrapolation... > @@ -1815,26 +1815,26 @@ SUBROUTINE Farm_InitMD( farm, ErrStat, ErrMsg ) !if (farm%MD%p%NFairs(nt) > 0 ) then ! only set up a mesh map if MoorDyn has connections to this turbine ! loads - CALL MeshMapCreate( farm%MD%y%PtFairleadLoad(nt), & + CALL MeshMapCreate( farm%MD%y%CoupledLoads(nt), & farm%FWrap(nt)%m%Turbine%ED%Input(1)%PlatformPtMesh, farm%m%MD_2_FWrap(nt), ErrStat2, ErrMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':MD_2_FWrap' ) ! kinematics CALL MeshMapCreate( farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh, & - farm%MD%Input(1)%PtFairleadDisplacement(nt), farm%m%FWrap_2_MD(nt), ErrStat2, ErrMsg2 ) + farm%MD%Input(1)%CoupledKinematics(nt), farm%m%FWrap_2_MD(nt), ErrStat2, ErrMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':FWrap_2_MD' ) ! SubDyn alternative: - !CALL MeshMapCreate( farm%MD%y%PtFairleadLoad(nt), & + !CALL MeshMapCreate( farm%MD%y%CoupledLoads(nt), & ! farm%FWrap(nt)%m%Turbine%SD%Input(1)%LMesh, farm%m%MD_2_FWrap, ErrStat2, ErrMsg2 ) ! !CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':MD_2_FWrap' ) ! !CALL MeshMapCreate( farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh, & - ! farm%MD%Input(1)%PtFairleadDisplacement(nt), farm%m%FWrap_2_MD, ErrStat2, ErrMsg2 ) + ! farm%MD%Input(1)%CoupledKinematics(nt), farm%m%FWrap_2_MD, ErrStat2, ErrMsg2 ) ! !CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':FWrap_2_MD' ) !end if @@ -1894,13 +1894,13 @@ subroutine FARM_MD_Increment(t, n, farm, ErrStat, ErrMsg) do nt = 1,farm%p%NumTurbines !if (farm%MD%p%NFairs(nt) > 0 ) then - CALL Transfer_Point_to_Point( farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh, farm%MD%Input(1)%PtFairleadDisplacement(nt), & + CALL Transfer_Point_to_Point( farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh, farm%MD%Input(1)%CoupledKinematics(nt), & farm%m%FWrap_2_MD(nt), ErrStat2, ErrMsg2 ) - CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat, ErrMsg,RoutineName//'u_MD%PtFairleadDisplacement' ) + CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat, ErrMsg,RoutineName//'u_MD%CoupledKinematics' ) ! SubDyn alternative - !CALL Transfer_Point_to_Point( farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh, farm%MD%Input(1)%PtFairleadDisplacement(nt), farm%m%FWrap_2_MD(nt), ErrStat, ErrMsg ) + !CALL Transfer_Point_to_Point( farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh, farm%MD%Input(1)%CoupledKinematics(nt), farm%m%FWrap_2_MD(nt), ErrStat, ErrMsg ) !end if end do @@ -1921,12 +1921,12 @@ subroutine FARM_MD_Increment(t, n, farm, ErrStat, ErrMsg) ! ----- map MD load outputs to each turbine's substructure ----- (taken from U FullOpt1...) do nt = 1,farm%p%NumTurbines - if (farm%MD%p%NFairs(nt) > 0 ) then ! only map loads if MoorDyn has connections to this turbine + if (farm%MD%p%nCpldCons(nt) > 0 ) then ! only map loads if MoorDyn has connections to this turbine (currently considering only Point connections <<< ) ! mapping - CALL Transfer_Point_to_Point( farm%MD%y%PtFairleadLoad(nt), farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_2, & + CALL Transfer_Point_to_Point( farm%MD%y%CoupledLoads(nt), farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_2, & farm%m%MD_2_FWrap(nt), ErrStat2, ErrMsg2, & - farm%MD%Input(1)%PtFairleadDisplacement(nt), farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh ) !u_MD and y_ED contain the displacements needed for moment calculations + farm%MD%Input(1)%CoupledKinematics(nt), farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh ) !u_MD and y_ED contain the displacements needed for moment calculations CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) @@ -1938,9 +1938,9 @@ subroutine FARM_MD_Increment(t, n, farm, ErrStat, ErrMsg) + farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_2%Moment ! SubDyn alternative - !CALL Transfer_Point_to_Point( farm%MD%y%PtFairleadLoad(nt), farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2, & + !CALL Transfer_Point_to_Point( farm%MD%y%CoupledLoads(nt), farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2, & ! farm%m%MD_2_FWrap(nt), ErrStat2, ErrMsg2, & - ! farm%MD%Input(1)%PtFairleadDisplacement(nt), farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh ) !u_MD and y_SD contain the displacements needed for moment calculations + ! farm%MD%Input(1)%CoupledKinematics(nt), farm%FWrap(nt)%m%Turbine%SD%y%y2Mesh ) !u_MD and y_SD contain the displacements needed for moment calculations ! !farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Force = farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Force + farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2%Force !farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Moment = farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh%Moment + farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2%Moment diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 9989640d11..5d7a31d050 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -173,6 +173,9 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er InputFileDat%threshIC = 0.01_ReKi p%WaterKin = 0_IntKi !p%dtOut = 0.0_DbKi + + + m%PtfmInit = InitInp%PtfmInit(:,1) ! is this copying necssary in case this is an individual instance in FAST.Farm? @@ -500,6 +503,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er RETURN END IF + !TODO: add check if %name is maximum length, which might indicate the full name was too long <<< + ! process stiffness, damping, and bending coefficients (which might use lookup tables) CALL getCoefficientOrCurve(tempString1, m%LineTypeList(l)%EA, & m%LineTypeList(l)%nEApoints, & @@ -725,12 +730,12 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er end if else - CALL SetErrStat( ErrID_Severe, "Body ID out of bounds for Rod "//trim(Num2LStr(l))//".", ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, "Body ID out of bounds for Rod "//trim(Num2LStr(l))//".", ErrStat, ErrMsg, RoutineName ) return end if else - CALL SetErrStat( ErrID_Severe, "No number provided for Rod "//trim(Num2LStr(l))//" Body attachment.", ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, "No number provided for Rod "//trim(Num2LStr(l))//" Body attachment.", ErrStat, ErrMsg, RoutineName ) return end if @@ -769,7 +774,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else - CALL SetErrStat( ErrID_Severe, "Unidentified Type/BodyID for Rod "//trim(Num2LStr(l))//": "//trim(tempString1), ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, "Unidentified Type/BodyID for Rod "//trim(Num2LStr(l))//": "//trim(tempString1), ErrStat, ErrMsg, RoutineName ) return end if @@ -780,7 +785,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er m%RodList(l)%PropsIdNum = J EXIT IF (J == p%nRodTypes) THEN ! call an error if there is no match - CALL SetErrStat( ErrID_Severe, 'Unable to find matching rod type name for Rod '//trim(Num2LStr(l))//": "//trim(tempString2), ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, 'Unable to find matching rod type name for Rod '//trim(Num2LStr(l))//": "//trim(tempString2), ErrStat, ErrMsg, RoutineName ) END IF END IF END DO @@ -896,11 +901,11 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL Body_AddConnect(m%BodyList(J), l, tempArray(1:3)) ! add connection l to Ground body else - CALL SetErrStat( ErrID_Severe, "Body ID out of bounds for Connection "//trim(Num2LStr(l))//".", ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, "Body ID out of bounds for Connection "//trim(Num2LStr(l))//".", ErrStat, ErrMsg, RoutineName ) return end if else - CALL SetErrStat( ErrID_Severe, "No number provided for Connection "//trim(Num2LStr(l))//" Body attachment.", ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, "No number provided for Connection "//trim(Num2LStr(l))//" Body attachment.", ErrStat, ErrMsg, RoutineName ) return end if @@ -924,20 +929,28 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if ((let1 == "TURBINE") .or. (let1 == "T")) then ! turbine-coupled in FAST.Farm case - K = scan(TempString , '1234567890' ) ! find index of first number in the string - READ(TempString(K:), *) iTurb ! convert to int, representing turbine index - if (iTurb > p%nTurbines) then - call CheckError( ErrID_Fatal, 'Error: Turbine[n] Connect types can only be used with FAST.Farm and must not exceed the number of turbines.' ) - return + if (len_trim(num1) > 0) then + READ(num1, *) J ! convert to int, representing parent body index + + if ((J <= p%nTurbines) .and. (J > 0)) then + + m%ConnectList(l)%TypeNum = -1 ! -J ! typeNum < 0 indicates -turbine number <<<< NOT USED, RIGHT?? + p%nCpldCons(J) = p%nCpldCons(J) + 1 ! increment counter for the appropriate turbine + m%CpldConIs(p%nCpldCons(J),J) = l + print *, ' added connection ', l, ' as fairlead for turbine ', J + + + else + CALL SetErrStat( ErrID_Fatal, "Turbine ID out of bounds for Connection "//trim(Num2LStr(l))//".", ErrStat, ErrMsg, RoutineName ) + return + end if + else + CALL SetErrStat( ErrID_Fatal, "No number provided for Connection "//trim(Num2LStr(l))//" Turbine attachment.", ErrStat, ErrMsg, RoutineName ) + return end if - - m%ConnectList(I)%TypeNum = -iTurb ! typeNum < 0 indicates -turbine number <<<< NOT USED, RIGHT?? - p%nCpldCons(iTurb) = p%nCpldCons(iTurb) + 1 ! increment counter for the appropriate turbine - m%CpldConIs(p%nCpldCons(iTurb),iTurb) = l - print *, ' added connection ', I, ' as fairlead for turbine ', iTurb - + else - CALL SetErrStat( ErrID_Severe, "Unidentified Type/BodyID for Connection "//trim(Num2LStr(l))//": "//trim(tempString2), ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, "Unidentified Type/BodyID for Connection "//trim(Num2LStr(l))//": "//trim(tempString2), ErrStat, ErrMsg, RoutineName ) return end if @@ -1003,7 +1016,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er m%LineList(l)%PropsIdNum = J EXIT IF (J == p%nLineTypes) THEN ! call an error if there is no match - CALL SetErrStat( ErrID_Severe, 'Unable to find matching line type name for Line '//trim(Num2LStr(l)), ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, 'Unable to find matching line type name for Line '//trim(Num2LStr(l)), ErrStat, ErrMsg, RoutineName ) END IF END IF END DO @@ -1020,7 +1033,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er call DecomposeString(tempString2, let1, num1, let2, num2, let3) if (len_trim(num1)<1) then - CALL SetErrStat( ErrID_Severe, "Error: no number provided for line "//trim(Num2LStr(l))//" end A attachment.", ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, "Error: no number provided for line "//trim(Num2LStr(l))//" end A attachment.", ErrStat, ErrMsg, RoutineName ) return end if @@ -1035,11 +1048,11 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if (let2 == "B") then CALL Rod_AddLine(m%RodList(J), l, 0, 1) ! add line l (end A, denoted by 0) to rod J (end B, denoted by 1) else - CALL SetErrStat( ErrID_Severe, "Error: rod end (A or B) must be specified for line "//trim(Num2LStr(l))//" end A attachment. Instead seeing "//let2, ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, "Error: rod end (A or B) must be specified for line "//trim(Num2LStr(l))//" end A attachment. Instead seeing "//let2, ErrStat, ErrMsg, RoutineName ) return end if else - CALL SetErrStat( ErrID_Severe, "Error: rod connection ID out of bounds for line "//trim(Num2LStr(l))//" end A attachment.", ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, "Error: rod connection ID out of bounds for line "//trim(Num2LStr(l))//" end A attachment.", ErrStat, ErrMsg, RoutineName ) return end if @@ -1049,7 +1062,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er if ((J <= p%nConnects) .and. (J > 0)) then CALL Connect_AddLine(m%ConnectList(J), l, 0) ! add line l (end A, denoted by 0) to connection J else - CALL SetErrStat( ErrID_Severe, "Error: connection out of bounds for line "//trim(Num2LStr(l))//" end A attachment.", ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, "Error: connection out of bounds for line "//trim(Num2LStr(l))//" end A attachment.", ErrStat, ErrMsg, RoutineName ) return end if @@ -1061,7 +1074,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er call DecomposeString(tempString3, let1, num1, let2, num2, let3) if (len_trim(num1)<1) then - CALL SetErrStat( ErrID_Severe, "Error: no number provided for line "//trim(Num2LStr(l))//" end B attachment.", ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, "Error: no number provided for line "//trim(Num2LStr(l))//" end B attachment.", ErrStat, ErrMsg, RoutineName ) return end if @@ -1076,11 +1089,11 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if (let2 == "B") then CALL Rod_AddLine(m%RodList(J), l, 1, 1) ! add line l (end B, denoted by 1) to rod J (end B, denoted by 1) else - CALL SetErrStat( ErrID_Severe, "Error: rod end (A or B) must be specified for line "//trim(Num2LStr(l))//" end B attachment. Instead seeing "//let2, ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, "Error: rod end (A or B) must be specified for line "//trim(Num2LStr(l))//" end B attachment. Instead seeing "//let2, ErrStat, ErrMsg, RoutineName ) return end if else - CALL SetErrStat( ErrID_Severe, "Error: rod connection ID out of bounds for line "//trim(Num2LStr(l))//" end B attachment.", ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, "Error: rod connection ID out of bounds for line "//trim(Num2LStr(l))//" end B attachment.", ErrStat, ErrMsg, RoutineName ) return end if @@ -1090,7 +1103,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er if ((J <= p%nConnects) .and. (J > 0)) then CALL Connect_AddLine(m%ConnectList(J), l, 1) ! add line l (end B, denoted by 1) to connection J else - CALL SetErrStat( ErrID_Severe, "Error: connection out of bounds for line "//trim(Num2LStr(l))//" end B attachment.", ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, "Error: connection out of bounds for line "//trim(Num2LStr(l))//" end B attachment.", ErrStat, ErrMsg, RoutineName ) return end if @@ -1572,9 +1585,9 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN - u%CoupledKinematics(iTurb)%TranslationDisp(1,i) = InitInp%PtfmInit(1,iTurb) + OrMat(1,1)*rRef(1) + OrMat(2,1)*rRef(2) + OrMat(3,1)*rRef(3) - rRef(1) - u%CoupledKinematics(iTurb)%TranslationDisp(2,i) = InitInp%PtfmInit(2,iTurb) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2) - u%CoupledKinematics(iTurb)%TranslationDisp(3,i) = InitInp%PtfmInit(3,iTurb) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) + u%CoupledKinematics(iTurb)%TranslationDisp(1,J) = InitInp%PtfmInit(1,iTurb) + OrMat(1,1)*rRef(1) + OrMat(2,1)*rRef(2) + OrMat(3,1)*rRef(3) - rRef(1) + u%CoupledKinematics(iTurb)%TranslationDisp(2,J) = InitInp%PtfmInit(2,iTurb) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2) + u%CoupledKinematics(iTurb)%TranslationDisp(3,J) = InitInp%PtfmInit(3,iTurb) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) ! set absolute initial positions in MoorDyn m%ConnectList(m%CpldConIs(l,iTurb))%r = u%CoupledKinematics(iTurb)%Position(:,iTurb) + u%CoupledKinematics(iTurb)%TranslationDisp(:,iTurb) + p%TurbineRefPos(:,iTurb) diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 2a61250e4e..4fbe326435 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -54,7 +54,7 @@ typedef ^ ^ DbKi WaveTime # ====================================== Internal data types ======================================================================== # line properties from line dictionary input typedef ^ MD_LineProp IntKi IdNum - - - "integer identifier of this set of line properties" -typedef ^ ^ CHARACTER(10) name - - - "name/identifier of this set of line properties" +typedef ^ ^ CHARACTER(20) name - - - "name/identifier of this set of line properties" typedef ^ ^ DbKi d - - - "volume-equivalent diameter" "[m]" typedef ^ ^ DbKi w - - - "per-length weight in air" "[kg/m]" typedef ^ ^ DbKi EA - - - "axial stiffness" "[N]" @@ -325,6 +325,7 @@ typedef ^ ^ MD_ContinuousStateType xTemp - typedef ^ ^ MD_ContinuousStateType xdTemp - - - "contains temporary state derivative vector used in integration (put here so it's only allocated once)" typedef ^ ^ DbKi zeros6 {6} - - "array of zeros for convenience" typedef ^ ^ DbKi MDWrOutput {:} - - "Data from time step to be written to a MoorDyn output file" +typedef ^ ^ ReKi PtfmInit {6} - - "initial position of platform for an individual (non-farm) MD instance" - ## ============================== Parameters ============================================================================================================================================ diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index 77007e0800..a4b683c161 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -66,7 +66,7 @@ MODULE MoorDyn_Types ! ========= MD_LineProp ======= TYPE, PUBLIC :: MD_LineProp INTEGER(IntKi) :: IdNum !< integer identifier of this set of line properties [-] - CHARACTER(10) :: name !< name/identifier of this set of line properties [-] + CHARACTER(20) :: name !< name/identifier of this set of line properties [-] REAL(DbKi) :: d !< volume-equivalent diameter [[m]] REAL(DbKi) :: w !< per-length weight in air [[kg/m]] REAL(DbKi) :: EA !< axial stiffness [[N]] @@ -353,6 +353,7 @@ MODULE MoorDyn_Types TYPE(MD_ContinuousStateType) :: xdTemp !< contains temporary state derivative vector used in integration (put here so it's only allocated once) [-] REAL(DbKi) , DIMENSION(1:6) :: zeros6 !< array of zeros for convenience [-] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: MDWrOutput !< Data from time step to be written to a MoorDyn output file [-] + REAL(ReKi) , DIMENSION(1:6) :: PtfmInit !< initial position of platform for an individual (non-farm) MD instance [-] END TYPE MD_MiscVarType ! ======================= ! ========= MD_ParameterType ======= @@ -8026,6 +8027,7 @@ SUBROUTINE MD_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) END IF DstMiscData%MDWrOutput = SrcMiscData%MDWrOutput ENDIF + DstMiscData%PtfmInit = SrcMiscData%PtfmInit END SUBROUTINE MD_CopyMisc SUBROUTINE MD_DestroyMisc( MiscData, ErrStat, ErrMsg ) @@ -8455,6 +8457,7 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_BufSz = Int_BufSz + 2*1 ! MDWrOutput upper/lower bounds for each dimension Db_BufSz = Db_BufSz + SIZE(InData%MDWrOutput) ! MDWrOutput END IF + Re_BufSz = Re_BufSz + SIZE(InData%PtfmInit) ! PtfmInit IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) IF (ErrStat2 /= 0) THEN @@ -9101,6 +9104,10 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Db_Xferred = Db_Xferred + 1 END DO END IF + DO i1 = LBOUND(InData%PtfmInit,1), UBOUND(InData%PtfmInit,1) + ReKiBuf(Re_Xferred) = InData%PtfmInit(i1) + Re_Xferred = Re_Xferred + 1 + END DO END SUBROUTINE MD_PackMisc SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) @@ -9938,6 +9945,12 @@ SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) Db_Xferred = Db_Xferred + 1 END DO END IF + i1_l = LBOUND(OutData%PtfmInit,1) + i1_u = UBOUND(OutData%PtfmInit,1) + DO i1 = LBOUND(OutData%PtfmInit,1), UBOUND(OutData%PtfmInit,1) + OutData%PtfmInit(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO END SUBROUTINE MD_UnPackMisc SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) diff --git a/vs-build/FAST-farm/FAST-Farm.vfproj b/vs-build/FAST-farm/FAST-Farm.vfproj index f3baaa575d..aa9af0b4cb 100644 --- a/vs-build/FAST-farm/FAST-Farm.vfproj +++ b/vs-build/FAST-farm/FAST-Farm.vfproj @@ -5,7 +5,7 @@ - + @@ -15,7 +15,7 @@ - + @@ -25,7 +25,7 @@ - + @@ -35,7 +35,7 @@ - + @@ -45,7 +45,7 @@ - + @@ -55,7 +55,7 @@ - + From dc92d7c152024cf46bae381251484d96dcc83fd1 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Tue, 13 Jul 2021 22:35:44 -0600 Subject: [PATCH 048/242] New approach succeeds in applying shared mooring forces: - After discussions with Andy, set up new FAST input PlatformPtMesh u_ED_PlatformPtMesh_MDf to receive loads from shared mooring system in FAST.Farm. If running from FAST.Farm, loads will be added from this to ED. (Not currently set up for SD.) - To control whether this is used, made FarmIntegration a parameter in FAST, so it can be checked at any point. - Adjusted the farm-level MD code in FAST_Farm_Subs so that mooring loads are mapped to this new u_ED_PlatformPtMesh_MDf mesh. - Initial tests show the mooring system is having an effect now. --- glue-codes/fast-farm/src/FAST_Farm_Subs.f90 | 20 ++- .../openfast-library/src/FAST_Registry.txt | 5 +- modules/openfast-library/src/FAST_Solver.f90 | 29 ++++- modules/openfast-library/src/FAST_Subs.f90 | 3 +- modules/openfast-library/src/FAST_Types.f90 | 123 ++++++++++++++++-- 5 files changed, 152 insertions(+), 28 deletions(-) diff --git a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 index 8350571459..de4bc927d4 100644 --- a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 +++ b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 @@ -1816,7 +1816,7 @@ SUBROUTINE Farm_InitMD( farm, ErrStat, ErrMsg ) ! loads CALL MeshMapCreate( farm%MD%y%CoupledLoads(nt), & - farm%FWrap(nt)%m%Turbine%ED%Input(1)%PlatformPtMesh, farm%m%MD_2_FWrap(nt), ErrStat2, ErrMsg2 ) + farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_MDf, farm%m%MD_2_FWrap(nt), ErrStat2, ErrMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':MD_2_FWrap' ) @@ -1923,20 +1923,18 @@ subroutine FARM_MD_Increment(t, n, farm, ErrStat, ErrMsg) if (farm%MD%p%nCpldCons(nt) > 0 ) then ! only map loads if MoorDyn has connections to this turbine (currently considering only Point connections <<< ) + ! copy the MD output mesh for this turbine into a copy mesh within the FAST instance + !CALL MeshCopy ( farm%MD%y%CoupledLoads(nt), farm%FWrap(nt)%m%Turbine%MeshMapData%u_FarmMD_CoupledLoads, MESH_NEWCOPY, ErrStat2, ErrMsg2 ) + ! CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':MeshCopy CoupledLoads' ) + + ! mapping - CALL Transfer_Point_to_Point( farm%MD%y%CoupledLoads(nt), farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_2, & + CALL Transfer_Point_to_Point( farm%MD%y%CoupledLoads(nt), farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_MDf, & farm%m%MD_2_FWrap(nt), ErrStat2, ErrMsg2, & farm%MD%Input(1)%CoupledKinematics(nt), farm%FWrap(nt)%m%Turbine%ED%y%PlatformPtMesh ) !u_MD and y_ED contain the displacements needed for moment calculations - - CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) - ! adding to ElastoDyn mesh that will be a future input (I'm assuming MeshMapData%u_ED_PlatformPtMesh makes its way into ElastoDyn inputs somehow) - farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh%Force = farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh%Force & - + farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_2%Force - - farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh%Moment = farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh%Moment & - + farm%FWrap(nt)%m%Turbine%MeshMapData%u_ED_PlatformPtMesh_2%Moment - + CALL SetErrStat(ErrStat2,ErrMsg2, ErrStat, ErrMsg, RoutineName) + ! SubDyn alternative !CALL Transfer_Point_to_Point( farm%MD%y%CoupledLoads(nt), farm%FWrap(nt)%m%Turbine%MeshMapData%u_SD_LMesh_2, & ! farm%m%MD_2_FWrap(nt), ErrStat2, ErrMsg2, & diff --git a/modules/openfast-library/src/FAST_Registry.txt b/modules/openfast-library/src/FAST_Registry.txt index bf9292d785..a8496c7a27 100644 --- a/modules/openfast-library/src/FAST_Registry.txt +++ b/modules/openfast-library/src/FAST_Registry.txt @@ -118,6 +118,8 @@ typedef ^ FAST_ParameterType IntKi CompIce - - - "Compute ice loading (switch) { typedef ^ FAST_ParameterType LOGICAL UseDWM - - - "Use the DWM module in AeroDyn" - typedef ^ FAST_ParameterType LOGICAL Linearize - - - "Linearization analysis (flag)" - typedef ^ FAST_ParameterType IntKi WaveFieldMod - - - "Wave field handling (-) (switch) 0: use individual HydroDyn inputs without adjustment, 1: adjust wave phases based on turbine offsets from farm origin" - +typedef ^ FAST_ParameterType logical FarmIntegration - .false. - "whether this is called from FAST.Farm (or another program that doesn't want FAST to call all of the init stuff first)" - +typedef ^ FAST_ParameterType SiKi TurbinePos {3} - - "Initial position of turbine base (origin used for graphics)" m # Input file names: typedef ^ FAST_ParameterType CHARACTER(1024) EDFile - - - "The name of the ElastoDyn input file" - @@ -157,7 +159,6 @@ typedef ^ FAST_ParameterType CHARACTER(1024) VTK_OutFileRoot - "''" - "The rootn typedef ^ FAST_ParameterType INTEGER VTK_tWidth - - - "Width of number of files for leading zeros in file name format" - typedef ^ FAST_ParameterType DbKi VTK_fps - - - "number of frames per second to output VTK data" - typedef ^ FAST_ParameterType FAST_VTK_SurfaceType VTK_surface - - - "Data for VTK surface visualization" -typedef ^ FAST_ParameterType SiKi TurbinePos {3} - - "Initial position of turbine base (origin used for graphics)" m typedef ^ FAST_ParameterType CHARACTER(4) Tdesc - - - "description of turbine ID (for FAST.Farm) screen printing" # Parameters for linearization @@ -633,6 +634,7 @@ typedef ^ FAST_ModuleMapType MeshType u_ED_NacelleLoads - - - "copy of ED input typedef ^ FAST_ModuleMapType MeshType u_ED_PlatformPtMesh - - - "copy of ED input mesh" typedef ^ FAST_ModuleMapType MeshType u_ED_PlatformPtMesh_2 - - - "copy of ED input mesh (used only for temporary storage)" typedef ^ FAST_ModuleMapType MeshType u_ED_PlatformPtMesh_3 - - - "copy of ED input mesh (used only for temporary storage)" +typedef ^ FAST_ModuleMapType MeshType u_ED_PlatformPtMesh_MDf - - - "copy of ED input mesh used to store loads from farm-level MD" typedef ^ FAST_ModuleMapType MeshType u_ED_TowerPtloads - - - "copy of ED input mesh" typedef ^ FAST_ModuleMapType MeshType u_ED_BladePtLoads {:} - - "copy of ED input mesh" typedef ^ FAST_ModuleMapType MeshType u_SD_TPMesh - - - "copy of SD input mesh" @@ -649,6 +651,7 @@ typedef ^ FAST_ModuleMapType MeshType y_BD_BldMotion_4Loads {:} - - "BD blade mo typedef ^ FAST_ModuleMapType MeshType u_BD_Distrload {:} - - "copy of BD DistrLoad input meshes" typedef ^ FAST_ModuleMapType MeshType u_Orca_PtfmMesh - - - "copy of Orca PtfmMesh input mesh" typedef ^ FAST_ModuleMapType MeshType u_ExtPtfm_PtfmMesh - - - "copy of ExtPtfm_MCKF PtfmMesh input mesh" +#typedef ^ FAST_ModuleMapType MeshType u_FarmMD_CoupledLoads - - - "FAST-internal copy of MoorDyn's CoupledLoads output mesh for use with shared moorings in FAST.Farm" # ..... FAST_ExternalInput data ....................................................................................................... typedef FAST FAST_ExternInputType ReKi GenTrq - - - "generator torque input from Simulink/Labview" typedef ^ FAST_ExternInputType ReKi ElecPwr - - - "electric power input from Simulink/Labview" diff --git a/modules/openfast-library/src/FAST_Solver.f90 b/modules/openfast-library/src/FAST_Solver.f90 index 690a431d47..3bc9eb1a6a 100644 --- a/modules/openfast-library/src/FAST_Solver.f90 +++ b/modules/openfast-library/src/FAST_Solver.f90 @@ -2024,6 +2024,7 @@ SUBROUTINE U_ED_HD_Residual( y_ED2, y_HD2, u_IN, U_Resid) !.................. ! Set mooring line inputs (which don't have acceleration fields) !.................. + !TODO: MoorDyn input mesh now has acceleration fields, and they are used in some uncommon cases. Is this an issue? <<< IF ( p_FAST%CompMooring == Module_MAP ) THEN @@ -2058,7 +2059,14 @@ SUBROUTINE U_ED_HD_Residual( y_ED2, y_HD2, u_IN, U_Resid) MeshMapData%u_ED_PlatformPtMesh%Moment = 0.0_ReKi END IF - + + + ! add farm-level mooring loads if applicable >>> note: not yet set up for SubDyn <<< + IF (p_FAST%FarmIntegration) THEN + MeshMapData%u_ED_PlatformPtMesh%Force = MeshMapData%u_ED_PlatformPtMesh%Force + MeshMapData%u_ED_PlatformPtMesh_MDf%Force + MeshMapData%u_ED_PlatformPtMesh%Moment = MeshMapData%u_ED_PlatformPtMesh%Moment + MeshMapData%u_ED_PlatformPtMesh_MDf%Moment + END IF + ! Map motions for ServodDyn Structural control (TMD) if used and forces from the TMD to the platform IF ( p_FAST%CompServo == Module_SrvD .and. p_FAST%CompSub /= Module_SD ) THEN @@ -3290,7 +3298,14 @@ SUBROUTINE U_FullOpt1_Residual( y_ED2, y_SD2, y_HD2, y_BD2, y_Orca2, y_ExtPtfm2, MeshMapData%u_ED_PlatformPtMesh%Force = MeshMapData%u_ED_PlatformPtMesh%Force + MeshMapData%u_ED_PlatformPtMesh_2%Force MeshMapData%u_ED_PlatformPtMesh%Moment = MeshMapData%u_ED_PlatformPtMesh%Moment + MeshMapData%u_ED_PlatformPtMesh_2%Moment END IF - + + + ! add farm-level mooring loads if applicable + IF (p_FAST%FarmIntegration) THEN + MeshMapData%u_ED_PlatformPtMesh%Force = MeshMapData%u_ED_PlatformPtMesh%Force + MeshMapData%u_ED_PlatformPtMesh_MDf%Force + MeshMapData%u_ED_PlatformPtMesh%Moment = MeshMapData%u_ED_PlatformPtMesh%Moment + MeshMapData%u_ED_PlatformPtMesh_MDf%Moment + END IF + ! Map the forces from the platform mounted TMD (from ServoDyn) to the platform reference point IF ( p_FAST%CompServo == Module_SrvD .and. p_FAST%CompSub /= Module_SD .and. allocated(y_SrvD%SStCLoadMesh)) THEN @@ -4893,6 +4908,16 @@ SUBROUTINE InitModuleMappings(p_FAST, ED, BD, AD14, AD, HD, SD, ExtPtfm, SrvD, M CALL MeshCopy ( ED%Input(1)%PlatformPtMesh, MeshMapData%u_ED_PlatformPtMesh_3, MESH_NEWCOPY, ErrStat2, ErrMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':u_ED_PlatformPtMesh_3' ) + ! for now, setting up this additional load mesh for farm-level MD loads if in FAST.Farm (TODO: add more checks/handling) <<< + if (p_FAST%FarmIntegration) then + CALL MeshCopy ( ED%Input(1)%PlatformPtMesh, MeshMapData%u_ED_PlatformPtMesh_MDf, MESH_NEWCOPY, ErrStat2, ErrMsg2 ) + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName//':u_ED_PlatformPtMesh_MDf' ) + + ! need to initialize to zero? + MeshMapData%u_ED_PlatformPtMesh_MDf%Force = 0.0_ReKi + MeshMapData%u_ED_PlatformPtMesh_MDf%Moment = 0.0_ReKi + end if + IF ( p_FAST%CompElast == Module_BD ) THEN diff --git a/modules/openfast-library/src/FAST_Subs.f90 b/modules/openfast-library/src/FAST_Subs.f90 index fdc4ccd5e1..4f3a43b775 100644 --- a/modules/openfast-library/src/FAST_Subs.f90 +++ b/modules/openfast-library/src/FAST_Subs.f90 @@ -190,9 +190,10 @@ SUBROUTINE FAST_InitializeAll( t_initial, p_FAST, y_FAST, m_FAST, ED, BD, SrvD, END IF ! ... Open and read input files ... - ! also, set turbine reference position for graphics output + ! also, set applicable farm paramters and turbine reference position also for graphics output p_FAST%UseSC = .FALSE. if (PRESENT(ExternInitData)) then + p_FAST%FarmIntegration = ExternInitData%FarmIntegration p_FAST%TurbinePos = ExternInitData%TurbinePos p_FAST%WaveFieldMod = ExternInitData%WaveFieldMod if( (ExternInitData%NumSC2CtrlGlob .gt. 0) .or. (ExternInitData%NumSC2Ctrl .gt. 0) .or. (ExternInitData%NumCtrl2SC .gt. 0)) then diff --git a/modules/openfast-library/src/FAST_Types.f90 b/modules/openfast-library/src/FAST_Types.f90 index 11036e7b2e..8dd136549c 100644 --- a/modules/openfast-library/src/FAST_Types.f90 +++ b/modules/openfast-library/src/FAST_Types.f90 @@ -162,6 +162,8 @@ MODULE FAST_Types LOGICAL :: UseDWM !< Use the DWM module in AeroDyn [-] LOGICAL :: Linearize !< Linearization analysis (flag) [-] INTEGER(IntKi) :: WaveFieldMod !< Wave field handling (-) (switch) 0: use individual HydroDyn inputs without adjustment, 1: adjust wave phases based on turbine offsets from farm origin [-] + LOGICAL :: FarmIntegration = .false. !< whether this is called from FAST.Farm (or another program that doesn't want FAST to call all of the init stuff first) [-] + REAL(SiKi) , DIMENSION(1:3) :: TurbinePos !< Initial position of turbine base (origin used for graphics) [m] CHARACTER(1024) :: EDFile !< The name of the ElastoDyn input file [-] CHARACTER(1024) , DIMENSION(MaxNBlades) :: BDBldFile !< Name of files containing BeamDyn inputs for each blade [-] CHARACTER(1024) :: InflowFile !< Name of file containing inflow wind input parameters [-] @@ -197,7 +199,6 @@ MODULE FAST_Types INTEGER(IntKi) :: VTK_tWidth !< Width of number of files for leading zeros in file name format [-] REAL(DbKi) :: VTK_fps !< number of frames per second to output VTK data [-] TYPE(FAST_VTK_SurfaceType) :: VTK_surface !< Data for VTK surface visualization [-] - REAL(SiKi) , DIMENSION(1:3) :: TurbinePos !< Initial position of turbine base (origin used for graphics) [m] CHARACTER(4) :: Tdesc !< description of turbine ID (for FAST.Farm) screen printing [-] LOGICAL :: CalcSteady !< Calculate a steady-state periodic operating point before linearization [unused if Linearize=False] [-] INTEGER(IntKi) :: TrimCase !< Controller parameter to be trimmed {1:yaw; 2:torque; 3:pitch} [unused if Linearize=False; used only if CalcSteady=True] [-] @@ -672,6 +673,7 @@ MODULE FAST_Types TYPE(MeshType) :: u_ED_PlatformPtMesh !< copy of ED input mesh [-] TYPE(MeshType) :: u_ED_PlatformPtMesh_2 !< copy of ED input mesh (used only for temporary storage) [-] TYPE(MeshType) :: u_ED_PlatformPtMesh_3 !< copy of ED input mesh (used only for temporary storage) [-] + TYPE(MeshType) :: u_ED_PlatformPtMesh_MDf !< copy of ED input mesh used to store loads from farm-level MD [-] TYPE(MeshType) :: u_ED_TowerPtloads !< copy of ED input mesh [-] TYPE(MeshType) , DIMENSION(:), ALLOCATABLE :: u_ED_BladePtLoads !< copy of ED input mesh [-] TYPE(MeshType) :: u_SD_TPMesh !< copy of SD input mesh [-] @@ -2136,6 +2138,8 @@ SUBROUTINE FAST_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg DstParamData%UseDWM = SrcParamData%UseDWM DstParamData%Linearize = SrcParamData%Linearize DstParamData%WaveFieldMod = SrcParamData%WaveFieldMod + DstParamData%FarmIntegration = SrcParamData%FarmIntegration + DstParamData%TurbinePos = SrcParamData%TurbinePos DstParamData%EDFile = SrcParamData%EDFile DstParamData%BDBldFile = SrcParamData%BDBldFile DstParamData%InflowFile = SrcParamData%InflowFile @@ -2173,7 +2177,6 @@ SUBROUTINE FAST_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg CALL FAST_Copyvtk_surfacetype( SrcParamData%VTK_surface, DstParamData%VTK_surface, CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) IF (ErrStat>=AbortErrLev) RETURN - DstParamData%TurbinePos = SrcParamData%TurbinePos DstParamData%Tdesc = SrcParamData%Tdesc DstParamData%CalcSteady = SrcParamData%CalcSteady DstParamData%TrimCase = SrcParamData%TrimCase @@ -2270,6 +2273,8 @@ SUBROUTINE FAST_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Int_BufSz = Int_BufSz + 1 ! UseDWM Int_BufSz = Int_BufSz + 1 ! Linearize Int_BufSz = Int_BufSz + 1 ! WaveFieldMod + Int_BufSz = Int_BufSz + 1 ! FarmIntegration + Re_BufSz = Re_BufSz + SIZE(InData%TurbinePos) ! TurbinePos Int_BufSz = Int_BufSz + 1*LEN(InData%EDFile) ! EDFile Int_BufSz = Int_BufSz + SIZE(InData%BDBldFile)*LEN(InData%BDBldFile) ! BDBldFile Int_BufSz = Int_BufSz + 1*LEN(InData%InflowFile) ! InflowFile @@ -2322,7 +2327,6 @@ SUBROUTINE FAST_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Int_BufSz = Int_BufSz + SIZE( Int_Buf ) DEALLOCATE(Int_Buf) END IF - Re_BufSz = Re_BufSz + SIZE(InData%TurbinePos) ! TurbinePos Int_BufSz = Int_BufSz + 1*LEN(InData%Tdesc) ! Tdesc Int_BufSz = Int_BufSz + 1 ! CalcSteady Int_BufSz = Int_BufSz + 1 ! TrimCase @@ -2444,6 +2448,12 @@ SUBROUTINE FAST_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%WaveFieldMod Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = TRANSFER(InData%FarmIntegration, IntKiBuf(1)) + Int_Xferred = Int_Xferred + 1 + DO i1 = LBOUND(InData%TurbinePos,1), UBOUND(InData%TurbinePos,1) + ReKiBuf(Re_Xferred) = InData%TurbinePos(i1) + Re_Xferred = Re_Xferred + 1 + END DO DO I = 1, LEN(InData%EDFile) IntKiBuf(Int_Xferred) = ICHAR(InData%EDFile(I:I), IntKi) Int_Xferred = Int_Xferred + 1 @@ -2572,10 +2582,6 @@ SUBROUTINE FAST_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, ELSE IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 ENDIF - DO i1 = LBOUND(InData%TurbinePos,1), UBOUND(InData%TurbinePos,1) - ReKiBuf(Re_Xferred) = InData%TurbinePos(i1) - Re_Xferred = Re_Xferred + 1 - END DO DO I = 1, LEN(InData%Tdesc) IntKiBuf(Int_Xferred) = ICHAR(InData%Tdesc(I:I), IntKi) Int_Xferred = Int_Xferred + 1 @@ -2739,6 +2745,14 @@ SUBROUTINE FAST_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMs Int_Xferred = Int_Xferred + 1 OutData%WaveFieldMod = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 + OutData%FarmIntegration = TRANSFER(IntKiBuf(Int_Xferred), OutData%FarmIntegration) + Int_Xferred = Int_Xferred + 1 + i1_l = LBOUND(OutData%TurbinePos,1) + i1_u = UBOUND(OutData%TurbinePos,1) + DO i1 = LBOUND(OutData%TurbinePos,1), UBOUND(OutData%TurbinePos,1) + OutData%TurbinePos(i1) = REAL(ReKiBuf(Re_Xferred), SiKi) + Re_Xferred = Re_Xferred + 1 + END DO DO I = 1, LEN(OutData%EDFile) OutData%EDFile(I:I) = CHAR(IntKiBuf(Int_Xferred)) Int_Xferred = Int_Xferred + 1 @@ -2881,12 +2895,6 @@ SUBROUTINE FAST_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMs IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) - i1_l = LBOUND(OutData%TurbinePos,1) - i1_u = UBOUND(OutData%TurbinePos,1) - DO i1 = LBOUND(OutData%TurbinePos,1), UBOUND(OutData%TurbinePos,1) - OutData%TurbinePos(i1) = REAL(ReKiBuf(Re_Xferred), SiKi) - Re_Xferred = Re_Xferred + 1 - END DO DO I = 1, LEN(OutData%Tdesc) OutData%Tdesc(I:I) = CHAR(IntKiBuf(Int_Xferred)) Int_Xferred = Int_Xferred + 1 @@ -36545,6 +36553,9 @@ SUBROUTINE FAST_CopyModuleMapType( SrcModuleMapTypeData, DstModuleMapTypeData, C CALL MeshCopy( SrcModuleMapTypeData%u_ED_PlatformPtMesh_3, DstModuleMapTypeData%u_ED_PlatformPtMesh_3, CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat>=AbortErrLev) RETURN + CALL MeshCopy( SrcModuleMapTypeData%u_ED_PlatformPtMesh_MDf, DstModuleMapTypeData%u_ED_PlatformPtMesh_MDf, CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat>=AbortErrLev) RETURN CALL MeshCopy( SrcModuleMapTypeData%u_ED_TowerPtloads, DstModuleMapTypeData%u_ED_TowerPtloads, CtrlCode, ErrStat2, ErrMsg2 ) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat>=AbortErrLev) RETURN @@ -36820,6 +36831,7 @@ SUBROUTINE FAST_DestroyModuleMapType( ModuleMapTypeData, ErrStat, ErrMsg ) CALL MeshDestroy( ModuleMapTypeData%u_ED_PlatformPtMesh, ErrStat, ErrMsg ) CALL MeshDestroy( ModuleMapTypeData%u_ED_PlatformPtMesh_2, ErrStat, ErrMsg ) CALL MeshDestroy( ModuleMapTypeData%u_ED_PlatformPtMesh_3, ErrStat, ErrMsg ) + CALL MeshDestroy( ModuleMapTypeData%u_ED_PlatformPtMesh_MDf, ErrStat, ErrMsg ) CALL MeshDestroy( ModuleMapTypeData%u_ED_TowerPtloads, ErrStat, ErrMsg ) IF (ALLOCATED(ModuleMapTypeData%u_ED_BladePtLoads)) THEN DO i1 = LBOUND(ModuleMapTypeData%u_ED_BladePtLoads,1), UBOUND(ModuleMapTypeData%u_ED_BladePtLoads,1) @@ -37857,6 +37869,23 @@ SUBROUTINE FAST_PackModuleMapType( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, Int_BufSz = Int_BufSz + SIZE( Int_Buf ) DEALLOCATE(Int_Buf) END IF + Int_BufSz = Int_BufSz + 3 ! u_ED_PlatformPtMesh_MDf: size of buffers for each call to pack subtype + CALL MeshPack( InData%u_ED_PlatformPtMesh_MDf, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, .TRUE. ) ! u_ED_PlatformPtMesh_MDf + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! u_ED_PlatformPtMesh_MDf + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! u_ED_PlatformPtMesh_MDf + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! u_ED_PlatformPtMesh_MDf + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF Int_BufSz = Int_BufSz + 3 ! u_ED_TowerPtloads: size of buffers for each call to pack subtype CALL MeshPack( InData%u_ED_TowerPtloads, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, .TRUE. ) ! u_ED_TowerPtloads CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) @@ -39814,6 +39843,34 @@ SUBROUTINE FAST_PackModuleMapType( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + CALL MeshPack( InData%u_ED_PlatformPtMesh_MDf, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2, OnlySize ) ! u_ED_PlatformPtMesh_MDf + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + IF(ALLOCATED(Re_Buf)) THEN IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf @@ -42649,6 +42706,46 @@ SUBROUTINE FAST_UnPackModuleMapType( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrSta CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) IF (ErrStat >= AbortErrLev) RETURN + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL MeshUnpack( OutData%u_ED_PlatformPtMesh_MDf, Re_Buf, Db_Buf, Int_Buf, ErrStat2, ErrMsg2 ) ! u_ED_PlatformPtMesh_MDf + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) From a300ec073fc71b5d8e65ce973222c4090aab832a Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Mon, 16 Aug 2021 17:05:34 -0600 Subject: [PATCH 049/242] Enabling MoorDyn dtOut and FAST.Farm thread/timer output: - Added dtOut option to MoorDyn so that an output interval can be specified that is larger than the time step. - Enabled/expanded multithreading output messages that indicate what threads are running and how long each part of the process takes -- now also for the shared mooring mode. There is now a new "printthreads" preprocessor definition that turns these all on/off rather than having to comment/ uncomment each print statement. --- glue-codes/fast-farm/src/FAST_Farm_Subs.f90 | 101 +++++++++++++++----- modules/moordyn/src/MoorDyn.f90 | 9 +- modules/moordyn/src/MoorDyn_IO.f90 | 36 +++++-- modules/moordyn/src/MoorDyn_Registry.txt | 2 + modules/moordyn/src/MoorDyn_Types.f90 | 14 +++ 5 files changed, 127 insertions(+), 35 deletions(-) diff --git a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 index de4bc927d4..74d28b12ad 100644 --- a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 +++ b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 @@ -1720,7 +1720,7 @@ SUBROUTINE Farm_InitMD( farm, ErrStat, ErrMsg ) ELSE IF ( farm%p%dt_mooring > farm%p%DT_low ) THEN ErrStat = ErrID_Fatal - ErrMsg = "The mooring coupling time step ("//TRIM(Num2LStr(farm%p%dt_mooring))// & + ErrMsg = "The farm mooring coupling time step ("//TRIM(Num2LStr(farm%p%dt_mooring))// & " s) cannot be larger than FAST.Farm time step ("//TRIM(Num2LStr(farm%p%DT_low))//" s)." ELSE ! calculate the number of FAST-MoorDyn subcycles: @@ -2131,7 +2131,7 @@ subroutine FARM_UpdateStates(t, n, farm, ErrStat, ErrMsg) INTEGER(IntKi), ALLOCATABLE :: ErrStatF(:) ! Temporary Error status for FAST CHARACTER(ErrMsgLen), ALLOCATABLE :: ErrMsgF (:) ! Temporary Error message for FAST CHARACTER(*), PARAMETER :: RoutineName = 'FARM_UpdateStates' -! REAL(DbKi) :: tm1,tm2,tm3 + REAL(DbKi) :: tm1,tm2,tm3, tm01, tm02, tm03, tmSF, tmSM ! timer variables ErrStat = ErrID_None ErrMsg = "" @@ -2142,6 +2142,12 @@ subroutine FARM_UpdateStates(t, n, farm, ErrStat, ErrMsg) if (errStat2 /= 0) call SetErrStat ( ErrID_Fatal, 'Could not allocate memory for ErrMsgF.', errStat, errMsg, RoutineName ) if (ErrStat >= AbortErrLev) return + + #ifdef _OPENMP + #define printthreads + #endif + + !....................................................................................... ! update module states (steps 1. and 2. and 3. and 4. can be done in parallel) !....................................................................................... @@ -2182,60 +2188,75 @@ subroutine FARM_UpdateStates(t, n, farm, ErrStat, ErrMsg) ! Original case: no shared moorings if (farm%p%MooringMod == 0) then - !#ifdef _OPENMP - ! tm1 = omp_get_wtime() - !#endif + #ifdef printthreads + tm1 = omp_get_wtime() + #endif !$OMP PARALLEL DO DEFAULT(Shared) Private(nt) !Private(nt,tm2,tm3) DO nt = 1,farm%p%NumTurbines+1 if(nt.ne.farm%p%NumTurbines+1) then - !#ifdef _OPENMP - ! tm3 = omp_get_wtime() - !#endif + #ifdef printthreads + tm3 = omp_get_wtime() + #endif call FWrap_Increment( t, n, farm%FWrap(nt)%u, farm%FWrap(nt)%p, farm%FWrap(nt)%x, farm%FWrap(nt)%xd, farm%FWrap(nt)%z, & farm%FWrap(nt)%OtherSt, farm%FWrap(nt)%y, farm%FWrap(nt)%m, ErrStatF(nt), ErrMsgF(nt) ) - !#ifdef _OPENMP - ! tm2 = omp_get_wtime() - ! write(*,*) ' FWrap_Increment for turbine #'//trim(num2lstr(nt))//' using thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' - !#endif + #ifdef printthreads + tm2 = omp_get_wtime() + write(*,*) ' FWrap_Increment for turbine #'//trim(num2lstr(nt))//' using thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' + #endif else - !#ifdef _OPENMP - ! tm3 = omp_get_wtime() - !#endif + #ifdef printthreads + tm3 = omp_get_wtime() + #endif call AWAE_UpdateStates( t, n, farm%AWAE%u, farm%AWAE%p, farm%AWAE%x, farm%AWAE%xd, farm%AWAE%z, & farm%AWAE%OtherSt, farm%AWAE%m, ErrStatAWAE, ErrMsgAWAE ) - !#ifdef _OPENMP - ! tm2 = omp_get_wtime() - ! write(*,*) ' AWAE_UpdateStates using thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' - !#endif + #ifdef printthreads + tm2 = omp_get_wtime() + write(*,*) ' AWAE_UpdateStates using thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' + #endif endif END DO !$OMP END PARALLEL DO - !#ifdef _OPENMP - ! tm2 = omp_get_wtime() - ! write(*,*) 'Total Farm_US took '//trim(num2lstr(tm2-tm1))//' seconds.' - !#endif + #ifdef printthreads + tm2 = omp_get_wtime() + write(*,*) 'Total Farm_US took '//trim(num2lstr(tm2-tm1))//' seconds.' + #endif ! Farm-level moorings case using MoorDyn else if (farm%p%MooringMod == 3) then + #ifdef printthreads + tm1 = omp_get_wtime() + #endif + ! Set up two parallel sections - one for FAST-MoorDyn steps (FAST portion in parallel for each step), and the other for AWAE. !$OMP PARALLEL SECTIONS DEFAULT(Shared) + ! The first section, for looping through FAST and farm-level MoorDyn time steps !$OMP SECTION + #ifdef printthreads + tm3 = omp_get_wtime() + tmSF = 0.0_DbKi + tmSM = 0.0_DbKi + #endif + ! This is the FAST-MoorDyn farm-level substepping loop do n_ss = 1, farm%p%n_mooring ! do n_mooring substeps (number of FAST/FarmMD steps per Farm time step) n_FMD = n*farm%p%n_mooring + n_ss - 1 ! number of the current time step of the call to FAST and MoorDyn t2 = t + farm%p%DT_mooring*(n_ss - 1) ! current time in the loop + #ifdef printthreads + tm01 = omp_get_wtime() + #endif + ! A nested parallel for loop to call each instance of OpenFAST in parallel !$OMP PARALLEL DO DEFAULT(Shared) Private(nt) DO nt = 1,farm%p%NumTurbines @@ -2243,19 +2264,53 @@ subroutine FARM_UpdateStates(t, n, farm, ErrStat, ErrMsg) farm%FWrap(nt)%OtherSt, farm%FWrap(nt)%y, farm%FWrap(nt)%m, ErrStatF(nt), ErrMsgF(nt) ) END DO !$OMP END PARALLEL DO + + #ifdef printthreads + tm02 = omp_get_wtime() + #endif ! call farm-level MoorDyn time step here (can't multithread this with FAST since it needs inputs from all FAST instances) call Farm_MD_Increment( t2, n_FMD, farm, ErrStatMD, ErrMsgMD) call SetErrStat(ErrStatMD, ErrMsgMD, ErrStat, ErrMsg, 'FARM_UpdateStates') ! MD error status <<<<< + + #ifdef printthreads + tm03 = omp_get_wtime() + tmSF = tmSF + tm02-tm01 + tmSM = tmSM + tm03-tm02 + #endif + end do ! n_ss substepping + #ifdef printthreads + tm2 = omp_get_wtime() + write(*,*) ' Turbine and support structure simulations with parent thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' + write(*,*) ' Time on FAST sims: '//trim(num2lstr(tmSF))//' s. Time on Farm MoorDyn: '//trim(num2lstr(tmSM))//' seconds' + #endif + + ! The second section, for updating AWAE states on a separate thread in parallel with the FAST/MoorDyn time stepping !$OMP SECTION + + #ifdef printthreads + tm3 = omp_get_wtime() + #endif + call AWAE_UpdateStates( t, n, farm%AWAE%u, farm%AWAE%p, farm%AWAE%x, farm%AWAE%xd, farm%AWAE%z, & farm%AWAE%OtherSt, farm%AWAE%m, ErrStatAWAE, ErrMsgAWAE ) + #ifdef printthreads + tm2 = omp_get_wtime() + write(*,*) ' AWAE_UpdateStates using thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' + #endif + + !$OMP END PARALLEL SECTIONS + #ifdef printthreads + tm2 = omp_get_wtime() + write(*,*) 'Total Farm_US took '//trim(num2lstr(tm2-tm1))//' seconds.' + #endif + else CALL SetErrStat( ErrID_Fatal, 'MooringMod must be 0 or 3.', ErrStat, ErrMsg, RoutineName ) end if diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 5d7a31d050..659befcb8e 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -30,7 +30,7 @@ MODULE MoorDyn PRIVATE - TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a10', '9 July 2021' ) + TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a11', '2 August 2021' ) INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output @@ -164,6 +164,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er p%g = InitInp%g p%rhoW = InitInp%rhoW p%WtrDpth = InitInp%WtrDepth + ! TODO: add MSL2SWL from OpenFAST <<<< ! set the following to some defaults p%kBot = 3.0E6 p%cBot = 3.0E5 @@ -172,7 +173,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er InputFileDat%CdScaleIC = 4.0_ReKi InputFileDat%threshIC = 0.01_ReKi p%WaterKin = 0_IntKi - !p%dtOut = 0.0_DbKi + p%dtOut = 0.0_DbKi m%PtfmInit = InitInp%PtfmInit(:,1) ! is this copying necssary in case this is an individual instance in FAST.Farm? @@ -1271,8 +1272,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er read (OptValue,*) InputFileDat%threshIC else if ( OptString == 'WATERKIN') then read (OptValue,*) p%WaterKin - !else if ( OptString == 'DTOUT') then - ! read (OptValue,*) p%dtOut + else if ( OptString == 'DTOUT') then + read (OptValue,*) p%dtOut else CALL SetErrStat( ErrID_Warn, 'unable to interpret input '//trim(OptString), ErrStat, ErrMsg, RoutineName ) end if diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index 8b177f156b..aa33774b26 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -1227,13 +1227,30 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) END DO ! I, loop through OutParam + END IF - ! Write the output parameters to the file + ! check if this is a repeated time step, in which case exit instead of writing a duplicate line to the output files + if (Time <= m%LastOutTime) then + return + else + m%LastOutTime = Time + end if - Frmt = '(F10.4,'//TRIM(Int2LStr(p%NumOuts))//'(A1,e12.5))' ! should evenutally use user specified format? + ! if using a certain output time step, check whether we should output, and exit the subroutine if not + if (p%dtOut > 0) then + !if (Time < (floor((Time-p%dtCoupling)/p%dtOut) + 1.0)*p%dtOut) then + if ( abs(MOD( Time - 0.5*p%dtOut, p%dtOut) - 0.5*p%dtOut) >= 0.5*p%dtCoupling) then + return + end if + end if + ! What the above does is say if ((dtOut==0) || (t >= (floor((t-dtC)/dtOut) + 1.0)*dtOut)), continue to writing files + if ( p%NumOuts > 0_IntKi ) then + + ! Write the output parameters to the file + Frmt = '(F10.4,'//TRIM(Int2LStr(p%NumOuts))//'(A1,e12.5))' ! should evenutally use user specified format? + WRITE(p%MDUnOut,Frmt) Time, ( p%Delim, y%WriteOutput(I), I=1,p%NumOuts ) - END IF @@ -1252,10 +1269,13 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) + (m%LineList(I)%N + 1)*SUM(m%LineList(I)%OutFlagList(7:9)) & + m%LineList(I)%N*SUM(m%LineList(I)%OutFlagList(10:18)) + if (m%LineList(I)%OutFlagList(2) == 1) THEN ! if node positions are included, make them using a float format for higher precision + Frmt = '(F10.4,'//TRIM(Int2LStr(3*(m%LineList(I)%N + 1)))//'(A1,F12.4)),'//TRIM(Int2LStr(LineNumOuts - 3*(m%LineList(I)%N - 1)))//'(A1,e12.5))' + else + Frmt = '(F10.4,'//TRIM(Int2LStr(LineNumOuts))//'(A1,e12.5))' ! should evenutally use user specified format? + end if - Frmt = '(F10.4,'//TRIM(Int2LStr(LineNumOuts))//'(A1,e12.5))' ! should evenutally use user specified format? - - L = 1 ! start of index of line output file at first entry + L = 1 ! start of index of line output file at first entry 12345.7890 ! Time ! m%LineList(I)%LineWrOutput(L) = Time @@ -1543,9 +1563,9 @@ FUNCTION Line_GetNodeTen(Line, i, p) result(NodeTen) REAL(DbKi) :: Tmag_squared if (i==0) then - NodeTen = sqrt( Line%Fnet(1,i)**2 + Line%Fnet(2,i) + (Line%Fnet(3,i) + Line%M(1,1,i)*(-p%g))**2 ) + NodeTen = sqrt( Line%Fnet(1,i)**2 + Line%Fnet(2,i)**2 + (Line%Fnet(3,i) + Line%M(1,1,i)*(-p%g))**2 ) else if (i==Line%N) then - NodeTen = sqrt( Line%Fnet(1,i)**2 + Line%Fnet(2,i) + (Line%Fnet(3,i) + Line%M(1,1,i)*(-p%g))**2 ) + NodeTen = sqrt( Line%Fnet(1,i)**2 + Line%Fnet(2,i)**2 + (Line%Fnet(3,i) + Line%M(1,1,i)*(-p%g))**2 ) else Tmag_squared = 0.0_DbKi DO J=1,3 diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 4fbe326435..2390c81636 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -325,6 +325,7 @@ typedef ^ ^ MD_ContinuousStateType xTemp - typedef ^ ^ MD_ContinuousStateType xdTemp - - - "contains temporary state derivative vector used in integration (put here so it's only allocated once)" typedef ^ ^ DbKi zeros6 {6} - - "array of zeros for convenience" typedef ^ ^ DbKi MDWrOutput {:} - - "Data from time step to be written to a MoorDyn output file" +typedef ^ ^ DbKi LastOutTime - - - "Time of last writing to MD output files" typedef ^ ^ ReKi PtfmInit {6} - - "initial position of platform for an individual (non-farm) MD instance" - @@ -354,6 +355,7 @@ typedef ^ ^ DbKi cBot - typedef ^ ^ DbKi dtM0 - - - "desired mooring model time step" "[s]" typedef ^ ^ DbKi dtCoupling - - - "coupling time step that MoorDyn should expect" "[s]" typedef ^ ^ IntKi NumOuts - - - "Number of parameters in the output list (number of outputs requested)" - +typedef ^ ^ DbKi dtOut - - - "interval for writing output file lines" "[s]" typedef ^ ^ CHARACTER(1024) RootName - - - "RootName for writing output files" - typedef ^ ^ MD_OutParmType OutParam {:} - - "Names and units (and other characteristics) of all requested output parameters" - typedef ^ ^ CHARACTER(1) Delim - - - "Column delimiter for output text files" - diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index a4b683c161..712c293ec4 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -353,6 +353,7 @@ MODULE MoorDyn_Types TYPE(MD_ContinuousStateType) :: xdTemp !< contains temporary state derivative vector used in integration (put here so it's only allocated once) [-] REAL(DbKi) , DIMENSION(1:6) :: zeros6 !< array of zeros for convenience [-] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: MDWrOutput !< Data from time step to be written to a MoorDyn output file [-] + REAL(DbKi) :: LastOutTime !< Time of last writing to MD output files [-] REAL(ReKi) , DIMENSION(1:6) :: PtfmInit !< initial position of platform for an individual (non-farm) MD instance [-] END TYPE MD_MiscVarType ! ======================= @@ -383,6 +384,7 @@ MODULE MoorDyn_Types REAL(DbKi) :: dtM0 !< desired mooring model time step [[s]] REAL(DbKi) :: dtCoupling !< coupling time step that MoorDyn should expect [[s]] INTEGER(IntKi) :: NumOuts !< Number of parameters in the output list (number of outputs requested) [-] + REAL(DbKi) :: dtOut !< interval for writing output file lines [[s]] CHARACTER(1024) :: RootName !< RootName for writing output files [-] TYPE(MD_OutParmType) , DIMENSION(:), ALLOCATABLE :: OutParam !< Names and units (and other characteristics) of all requested output parameters [-] CHARACTER(1) :: Delim !< Column delimiter for output text files [-] @@ -8027,6 +8029,7 @@ SUBROUTINE MD_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) END IF DstMiscData%MDWrOutput = SrcMiscData%MDWrOutput ENDIF + DstMiscData%LastOutTime = SrcMiscData%LastOutTime DstMiscData%PtfmInit = SrcMiscData%PtfmInit END SUBROUTINE MD_CopyMisc @@ -8457,6 +8460,7 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_BufSz = Int_BufSz + 2*1 ! MDWrOutput upper/lower bounds for each dimension Db_BufSz = Db_BufSz + SIZE(InData%MDWrOutput) ! MDWrOutput END IF + Db_BufSz = Db_BufSz + 1 ! LastOutTime Re_BufSz = Re_BufSz + SIZE(InData%PtfmInit) ! PtfmInit IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) @@ -9104,6 +9108,8 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Db_Xferred = Db_Xferred + 1 END DO END IF + DbKiBuf(Db_Xferred) = InData%LastOutTime + Db_Xferred = Db_Xferred + 1 DO i1 = LBOUND(InData%PtfmInit,1), UBOUND(InData%PtfmInit,1) ReKiBuf(Re_Xferred) = InData%PtfmInit(i1) Re_Xferred = Re_Xferred + 1 @@ -9945,6 +9951,8 @@ SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) Db_Xferred = Db_Xferred + 1 END DO END IF + OutData%LastOutTime = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 i1_l = LBOUND(OutData%PtfmInit,1) i1_u = UBOUND(OutData%PtfmInit,1) DO i1 = LBOUND(OutData%PtfmInit,1), UBOUND(OutData%PtfmInit,1) @@ -10029,6 +10037,7 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) DstParamData%dtM0 = SrcParamData%dtM0 DstParamData%dtCoupling = SrcParamData%dtCoupling DstParamData%NumOuts = SrcParamData%NumOuts + DstParamData%dtOut = SrcParamData%dtOut DstParamData%RootName = SrcParamData%RootName IF (ALLOCATED(SrcParamData%OutParam)) THEN i1_l = LBOUND(SrcParamData%OutParam,1) @@ -10443,6 +10452,7 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Db_BufSz = Db_BufSz + 1 ! dtM0 Db_BufSz = Db_BufSz + 1 ! dtCoupling Int_BufSz = Int_BufSz + 1 ! NumOuts + Db_BufSz = Db_BufSz + 1 ! dtOut Int_BufSz = Int_BufSz + 1*LEN(InData%RootName) ! RootName Int_BufSz = Int_BufSz + 1 ! OutParam allocated yes/no IF ( ALLOCATED(InData%OutParam) ) THEN @@ -10671,6 +10681,8 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Db_Xferred = Db_Xferred + 1 IntKiBuf(Int_Xferred) = InData%NumOuts Int_Xferred = Int_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%dtOut + Db_Xferred = Db_Xferred + 1 DO I = 1, LEN(InData%RootName) IntKiBuf(Int_Xferred) = ICHAR(InData%RootName(I:I), IntKi) Int_Xferred = Int_Xferred + 1 @@ -11227,6 +11239,8 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg Db_Xferred = Db_Xferred + 1 OutData%NumOuts = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 + OutData%dtOut = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 DO I = 1, LEN(OutData%RootName) OutData%RootName(I:I) = CHAR(IntKiBuf(Int_Xferred)) Int_Xferred = Int_Xferred + 1 From 191070f3b0f5bbddb4de7de322c9fdaa3950f951 Mon Sep 17 00:00:00 2001 From: shousner Date: Mon, 20 Sep 2021 16:18:37 -0600 Subject: [PATCH 050/242] Replacing MoorDyn depth parameter with option for bathymetry - Created a new subroutine in MoorDyn_IO called getDepthOrBathymetry to create a depth matrix to describe the bathymetry - - Will either be a 1x1 matrix with the constant depth (existing capability) - - Or can be an mxn matrix with each element describing the depth at different locations - The reading of the water depth options in the MoorDyn input file can now have an option to be a text file name - The bathymetry grid is then called when running the GetStateDerivs of Lines and Rods, using the x and y positions of the line ends - It interpolates it using a getBathymetry subroutine, which calls getInterpNums and calculate2Dinterpolation - - calculate2Dinterpolation is also a newly created subroutine --- modules/moordyn/src/MoorDyn.f90 | 86 +++++++++++++++++++--- modules/moordyn/src/MoorDyn_IO.f90 | 72 ++++++++++++++++++ modules/moordyn/src/MoorDyn_bathymetry.txt | 8 ++ 3 files changed, 156 insertions(+), 10 deletions(-) create mode 100644 modules/moordyn/src/MoorDyn_bathymetry.txt diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 659befcb8e..75b1507744 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -1256,8 +1256,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er read (OptValue,*) p%g else if ( OptString == 'RHOW') then read (OptValue,*) p%rhoW - else if ( OptString == 'WTRDPTH') then - read (OptValue,*) p%WtrDpth + ! else if ( OptString == 'WTRDPTH') then + ! read (OptValue,*) p%WtrDpth + else if ( OptString == 'WTRDPTH') then ! can be either a p or a m, same as the x and y values, probably p + CALL getDepthOrBathymetry(OptValue, m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, m%BathGrid_npoints) else if ( OptString == 'KBOT') then read (OptValue,*) p%kBot else if ( OptString == 'CBOT') then @@ -4234,14 +4236,24 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, ! F-K force from fluid acceleration not implemented yet ! bottom contact (stiffness and damping, vertical-only for now) - updated Nov 24 for general case where anchor and fairlead ends may deal with bottom contact forces + + ! interpolate the local depth from the bathymetry grid + CALL getBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, Line%r(1,I), Line%r(2,I), depth) - IF (Line%r(3,I) < -p%WtrDpth) THEN + IF (Line%r(3,I) < -depth) THEN IF (I==0) THEN - Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*( Line%l(I+1) ) + Line%B(3,I) = ( (-depth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*( Line%l(I+1) ) ELSE IF (I==N) THEN - Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) ) + Line%B(3,I) = ( (-depth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) ) ELSE - Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) + Line%l(I+1) ) + Line%B(3,I) = ( (-depth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) + Line%l(I+1) ) + ! IF (Line%r(3,I) < -p%WtrDpth) THEN + ! IF (I==0) THEN + ! Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*( Line%l(I+1) ) + ! ELSE IF (I==N) THEN + ! Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) ) + ! ELSE + ! Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) + Line%l(I+1) ) @@ -5593,14 +5605,21 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) Rod%Pd(:,I) = 0.0_DbKi ! assuming zero for sides for now, until taper comes into play ! bottom contact (stiffness and damping, vertical-only for now) - updated Nov 24 for general case where anchor and fairlead ends may deal with bottom contact forces - IF (Rod%r(3,I) < -p%WtrDpth) THEN + IF (Rod%r(3,I) < -depth) THEN IF (I==0) THEN - Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*( Rod%l(I+1) ) + Rod%B(3,I) = ( (-depth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*( Rod%l(I+1) ) ELSE IF (I==N) THEN - Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) ) + Rod%B(3,I) = ( (-depth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) ) ELSE - Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) + Rod%l(I+1) ) + Rod%B(3,I) = ( (-depth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) + Rod%l(I+1) ) END IF + ! IF (I==0) THEN + ! Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*( Rod%l(I+1) ) + ! ELSE IF (I==N) THEN + ! Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) ) + ! ELSE + ! Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) + Rod%l(I+1) ) + ! END IF ELSE Rod%B(3,I) = 0.0_DbKi END IF @@ -6871,6 +6890,26 @@ SUBROUTINE getWaveKin(p, x, y, z, t, tindex, U, Ud, zeta, PDyn) END SUBROUTINE + + + SUBROUTINE getBathymetry(BathymetryGrid, BathGrid_Xs, BathGrid_Ys, LineX, LineY, depth) + + REAL(DbKi), INTENT(IN ) :: BathymetryGrid(:,:) ! need colons or some sort of dimension setting + REAL(DbKi), INTENT(IN ) :: BathGrid_Xs(:) + REAL(DbKi), INTENT(IN ) :: BathGrid_Ys(:) + REAL(DbKi), INTENT(IN ) :: LineX + REAL(DbKi), INTENT(IN ) :: LineY + REAL(DbKi), INTENT( OUT) :: depth + + INTEGER(IntKi) :: ix, iy ! indeces for interpolation + Real(DbKi) :: fx, fy ! interpolation fractions + + CALL getInterpNums(BathGrid_Xs, LineX, 1, ix, fx) + CALL getInterpNums(BathGrid_Ys, LineY, 1, iy, fy) + + CALL calculate2Dinterpolation(BathymetryGrid, ix, iy, fx, fy, depth) + + END SUBROUTINE getBathymetry SUBROUTINE getInterpNums(xlist, xin, istart, i, fout) @@ -7022,7 +7061,34 @@ SUBROUTINE calculate3Dinterpolation(f, ix0, iy0, iz0, fx, fy, fz, c) END SUBROUTINE + SUBROUTINE calculate2Dinterpolation(f, ix0, iy0, fx, fy, c) + REAL(DbKi), INTENT (IN ) :: f(:,:) ! data array + INTEGER(IntKi), INTENT (IN ) :: ix0, iy0 ! indices for interpolation + REAL(DbKi), INTENT (IN ) :: fx, fy ! interpolation fractions + REAL(DbKi), INTENT ( OUT) :: c ! the output value + + INTEGER(IntKi) :: ix1, iy1 ! second indices + REAL(DbKi) :: c00, c01, c10, c11, c0, c1 + ! handle end case conditions + IF (fx == 0) THEN + ix1 = ix0 + ELSE + ix1 = min(ix0+1,size(f,2)) ! don't overstep bounds + END IF + IF (fy == 0) THEN + iy1 = iy0 + ELSE + iy1 = min(iy0+1,size(f,1)) ! don't overstep bounds + END IF + c00 = f(iy0, ix0) + c01 = f(iy1, ix0) + c10 = f(iy0, ix1) + c11 = f(iy1, ix1) + c0 = c00 *(1.0-fx) + c10 *fx + c1 = c01 *(1.0-fx) + c11 *fx + c = c0 *(1.0-fy) + c1 *fy + END SUBROUTINE calculate2Dinterpolation ! ============ below are some math convenience functions =============== ! should add error checking if I keep these, but hopefully there are existing NWTCLib functions to replace them diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index aa33774b26..1217df4a74 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -110,6 +110,78 @@ MODULE MoorDyn_IO CONTAINS + SUBROUTINE getDepthOrBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, BathGrid_npoints) + + CHARACTER(40), INTENT(IN ) :: inputString + REAL(DbKi), INTENT( OUT) :: BathGrid (:,:) + REAL(DbKi), INTENT( OUT) :: BathGrid_Xs (:) + REAL(DbKi), INTENT( OUT) :: BathGrid_Ys (:) + REAL(DbKi), INTENT( OUT) :: BathGrid_npoints + + INTEGER(IntKi), INTENT( OUT) :: ErrStat3 ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg3 ! Error message if ErrStat /= ErrID_None + + INTEGER(IntKi) :: I + INTEGER(IntKi) :: UnCoef ! unit number for coefficient input file + + INTEGER(IntKi) :: ErrStat4 + CHARACTER(120) :: ErrMsg4 + CHARACTER(120) :: Line2 + + CHARACTER(20) :: nGridX_string ! string to temporarily hold the nGridX string from Line2 + CHARACTER(20) :: nGridY_string ! string to temporarily hold the nGridY string from Line3 + INTEGER(IntKi) :: nGridX ! integer of the size of BathGrid_Xs + INTEGER(IntKi) :: nGridY ! integer of the size of BathGrid_Ys + + + IF (SCAN(inputString, "abcdfghijklmnopqrstuvwxyzABCDFGHIJKLMNOPQRSTUVWXYZ") == 0) THEN + ! If the input does not have any of these string values, let's treat it as a number but store in a matrix + ALLOCATE(BathGrid(1,1), STAT=ErrStat4) + READ(inputString, *, IOSTAT=ErrStat4) BathGrid(1,1) + + ELSE ! otherwise interpret the input as a file name to load the bathymetry lookup data from + PRINT *, "found a letter in the depth value so will try to load a bathymetry file" + + ! load lookup table data from file + CALL GetNewUnit( UnCoef ) ! unit number for coefficient input file + CALL OpenFInpFile( UnCoef, TRIM(inputString), ErrStat4, ErrMsg4 ) + + READ(UnCoef,'(A)',IOSTAT=ErrStat4) Line1 ! skip the first title line + READ(UnCoef,*,IOSTAT=ErrStat4) nGridX_string, nGridX ! read in the second line as the number of x values in the BathGrid + READ(UnCoef,*,IOSTAT=ErrStat4) nGridY_string, nGridY ! read in the third line as the number of y values in the BathGrid + + ! Allocate the bathymetry matrix and associated grid x and y values + ALLOCATE(BathGrid(nGridX, nGridY), STAT=ErrStat4) + ALLOCATE(BathGrid_Xs(nGridX), STAT=ErrStat4) + ALLOCATE(BathGrid_Ys(nGridY), STAT=ErrStat4) + + DO I = 1, nGridY ! loop through each line in the rest of the bathymetry file + + READ(UnCoef,'(A)',IOSTAT=ErrStat4) Line2 ! read into a line and call it Line2 + IF (ErrStat4 > 0) EXIT + + IF (I==1) THEN ! if it's the first line in the Bathymetry Grid, then it's a list of all the x values + READ(Line2, *,IOSTAT=ErrStat4) BathGrid_Xs + ELSE ! if it's not the first line, then the first value is a y value and the rest are the depth values + READ(Line2, *,IOSTAT=ErrStat4) BathGrid_Ys(I-1), BathGrid(I-1,:) + ENDIF + + END DO + + IF (I < 2) THEN + ErrStat3 = ErrID_Fatal + ErrMssg3 = "Less than the minimum of 2 data lines found in file "//TRIM(inputString)// + CLOSE (UnCoef) + RETURN + ELSE + BathGrid_npoints = nGridX*nGridY ! save the number of points in the grid + CLOSE (UnCoef) + END IF + + END IF + + END SUBROUTINE getDepthOrBathymetry + ! read in stiffness/damping coefficient or load nonlinear data file if applicable SUBROUTINE getCoefficientOrCurve(inputString, LineProp_c, LineProp_npoints, LineProp_Xs, LineProp_Ys, ErrStat3, ErrMsg3) diff --git a/modules/moordyn/src/MoorDyn_bathymetry.txt b/modules/moordyn/src/MoorDyn_bathymetry.txt new file mode 100644 index 0000000000..bfe4ffbbbd --- /dev/null +++ b/modules/moordyn/src/MoorDyn_bathymetry.txt @@ -0,0 +1,8 @@ +--- MoorDyn Bathymetry Input File --- +nGridX 4 +nGridY 4 + -800 -10 10 800 +-800 400 400 500 500 + -10 400 400 500 500 + 10 600 600 600 600 + 800 600 600 600 600 \ No newline at end of file From 5801d166fd44facf9f04834126fb36fca7c2290c Mon Sep 17 00:00:00 2001 From: shousner Date: Tue, 21 Sep 2021 11:58:29 -0600 Subject: [PATCH 051/242] Debugging error fixes: allocatable issue, syntax errors, etc. - Renaming subroutines: getDepthOrBathymetry is now getBathymetry - - getBathymetry is now getDepthFromBathymetry - Changed the intent of all the bathymetry grid variables in getBathymetry to INTENT(INOUT) - Changed Line1 to Line2 - Extra 's' in ErrMsg3. Also added the error variables to the input/output list in getBathymetry - Added in the new bathymetry MiscVars to the MoorDyn_Registry file --- modules/moordyn/src/MoorDyn.f90 | 10 +++++----- modules/moordyn/src/MoorDyn_IO.f90 | 16 ++++++++-------- modules/moordyn/src/MoorDyn_Registry.txt | 4 ++++ 3 files changed, 17 insertions(+), 13 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 75b1507744..b76313b826 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -1258,8 +1258,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er read (OptValue,*) p%rhoW ! else if ( OptString == 'WTRDPTH') then ! read (OptValue,*) p%WtrDpth - else if ( OptString == 'WTRDPTH') then ! can be either a p or a m, same as the x and y values, probably p - CALL getDepthOrBathymetry(OptValue, m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, m%BathGrid_npoints) + else if ( OptString == 'WTRDPTH') then + CALL getBathymetry(OptValue, m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, m%BathGrid_npoints, ErrStat2, ErrMsg2) else if ( OptString == 'KBOT') then read (OptValue,*) p%kBot else if ( OptString == 'CBOT') then @@ -4238,7 +4238,7 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, ! bottom contact (stiffness and damping, vertical-only for now) - updated Nov 24 for general case where anchor and fairlead ends may deal with bottom contact forces ! interpolate the local depth from the bathymetry grid - CALL getBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, Line%r(1,I), Line%r(2,I), depth) + CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, Line%r(1,I), Line%r(2,I), depth) IF (Line%r(3,I) < -depth) THEN IF (I==0) THEN @@ -6892,7 +6892,7 @@ SUBROUTINE getWaveKin(p, x, y, z, t, tindex, U, Ud, zeta, PDyn) END SUBROUTINE - SUBROUTINE getBathymetry(BathymetryGrid, BathGrid_Xs, BathGrid_Ys, LineX, LineY, depth) + SUBROUTINE getDepthFromBathymetry(BathymetryGrid, BathGrid_Xs, BathGrid_Ys, LineX, LineY, depth) REAL(DbKi), INTENT(IN ) :: BathymetryGrid(:,:) ! need colons or some sort of dimension setting REAL(DbKi), INTENT(IN ) :: BathGrid_Xs(:) @@ -6909,7 +6909,7 @@ SUBROUTINE getBathymetry(BathymetryGrid, BathGrid_Xs, BathGrid_Ys, LineX, LineY, CALL calculate2Dinterpolation(BathymetryGrid, ix, iy, fx, fy, depth) - END SUBROUTINE getBathymetry + END SUBROUTINE getDepthFromBathymetry SUBROUTINE getInterpNums(xlist, xin, istart, i, fout) diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index 1217df4a74..0c07613b8e 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -110,13 +110,13 @@ MODULE MoorDyn_IO CONTAINS - SUBROUTINE getDepthOrBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, BathGrid_npoints) + SUBROUTINE getBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, BathGrid_npoints, ErrStat3, ErrMsg3) CHARACTER(40), INTENT(IN ) :: inputString - REAL(DbKi), INTENT( OUT) :: BathGrid (:,:) - REAL(DbKi), INTENT( OUT) :: BathGrid_Xs (:) - REAL(DbKi), INTENT( OUT) :: BathGrid_Ys (:) - REAL(DbKi), INTENT( OUT) :: BathGrid_npoints + REAL(DbKi), INTENT(INOUT) :: BathGrid (:,:) + REAL(DbKi), INTENT(INOUT) :: BathGrid_Xs (:) + REAL(DbKi), INTENT(INOUT) :: BathGrid_Ys (:) + REAL(IntKi), INTENT(INOUT) :: BathGrid_npoints INTEGER(IntKi), INTENT( OUT) :: ErrStat3 ! Error status of the operation CHARACTER(*), INTENT( OUT) :: ErrMsg3 ! Error message if ErrStat /= ErrID_None @@ -146,7 +146,7 @@ SUBROUTINE getDepthOrBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, CALL GetNewUnit( UnCoef ) ! unit number for coefficient input file CALL OpenFInpFile( UnCoef, TRIM(inputString), ErrStat4, ErrMsg4 ) - READ(UnCoef,'(A)',IOSTAT=ErrStat4) Line1 ! skip the first title line + READ(UnCoef,'(A)',IOSTAT=ErrStat4) Line2 ! skip the first title line READ(UnCoef,*,IOSTAT=ErrStat4) nGridX_string, nGridX ! read in the second line as the number of x values in the BathGrid READ(UnCoef,*,IOSTAT=ErrStat4) nGridY_string, nGridY ! read in the third line as the number of y values in the BathGrid @@ -170,7 +170,7 @@ SUBROUTINE getDepthOrBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, IF (I < 2) THEN ErrStat3 = ErrID_Fatal - ErrMssg3 = "Less than the minimum of 2 data lines found in file "//TRIM(inputString)// + ErrMsg3 = "Less than the minimum of 2 data lines found in file "//TRIM(inputString) CLOSE (UnCoef) RETURN ELSE @@ -180,7 +180,7 @@ SUBROUTINE getDepthOrBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, END IF - END SUBROUTINE getDepthOrBathymetry + END SUBROUTINE getBathymetry ! read in stiffness/damping coefficient or load nonlinear data file if applicable diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 2390c81636..392bff6007 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -327,6 +327,10 @@ typedef ^ ^ DbKi zeros6 {6} typedef ^ ^ DbKi MDWrOutput {:} - - "Data from time step to be written to a MoorDyn output file" typedef ^ ^ DbKi LastOutTime - - - "Time of last writing to MD output files" typedef ^ ^ ReKi PtfmInit {6} - - "initial position of platform for an individual (non-farm) MD instance" - +typedef ^ ^ DbKi BathGrid {:}{:} - - "matrix describing the bathymetry in a grid of x's and y's" +typedef ^ ^ DbKi BathGrid_Xs {:} - - "array of x-coordinates in the bathymetry grid" +typedef ^ ^ DbKi BathGrid_Ys {:} - - "array of y-coordinates in the bathymetry grid" +typedef ^ ^ IntKi BathGrid_npoints {:} - - "number of grid points to describe the bathymetry grid" ## ============================== Parameters ============================================================================================================================================ From d4a9e6ff2d9404cab179dd7c38c76c5364382dde Mon Sep 17 00:00:00 2001 From: shousner Date: Tue, 21 Sep 2021 13:41:22 -0600 Subject: [PATCH 052/242] Third iteration of fixes for compilation: depth and BathGrid names - Needed to add local depth variable declaration to Line and Rod subroutines that use water depth - Added a call to getDepthFromBathymetry in the Rod_DoRHS subroutine, as well as Line_GetStateDerivs - Made the bathymetry grid variables ALLOCATABLE in getBathymetry - Fixed wrong name of BathymetryGrid in MoorDyn_Registry --- modules/moordyn/src/MoorDyn.f90 | 7 +++++++ modules/moordyn/src/MoorDyn_IO.f90 | 10 +++++----- modules/moordyn/src/MoorDyn_Registry.txt | 2 +- 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index b76313b826..ab73ef942d 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -4089,6 +4089,8 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, Real(DbKi) :: MagVp ! Real(DbKi) :: MagVq ! + Real(DbKi) :: depth ! local water depth interpolated from bathymetry grid + N = Line%N ! for convenience d = Line%d @@ -5447,6 +5449,8 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) Real(DbKi) :: Fcentripetal(3) ! centripetal force Real(DbKi) :: Mcentripetal(3) ! centripetal moment + Real(DbKi) :: depth ! local interpolated depth from bathymetry grid + N = Rod%N @@ -5605,6 +5609,9 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) Rod%Pd(:,I) = 0.0_DbKi ! assuming zero for sides for now, until taper comes into play ! bottom contact (stiffness and damping, vertical-only for now) - updated Nov 24 for general case where anchor and fairlead ends may deal with bottom contact forces + ! interpolate the local depth from the bathymetry grid + CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, Rod%r(1,I), Rod%r(2,I), depth) + IF (Rod%r(3,I) < -depth) THEN IF (I==0) THEN Rod%B(3,I) = ( (-depth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*( Rod%l(I+1) ) diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index 0c07613b8e..e501407102 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -112,11 +112,11 @@ MODULE MoorDyn_IO SUBROUTINE getBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, BathGrid_npoints, ErrStat3, ErrMsg3) - CHARACTER(40), INTENT(IN ) :: inputString - REAL(DbKi), INTENT(INOUT) :: BathGrid (:,:) - REAL(DbKi), INTENT(INOUT) :: BathGrid_Xs (:) - REAL(DbKi), INTENT(INOUT) :: BathGrid_Ys (:) - REAL(IntKi), INTENT(INOUT) :: BathGrid_npoints + CHARACTER(40), INTENT(IN ) :: inputString + REAL(DbKi), ALLOCATABLE, INTENT(INOUT) :: BathGrid (:,:) + REAL(DbKi), ALLOCATABLE, INTENT(INOUT) :: BathGrid_Xs (:) + REAL(DbKi), ALLOCATABLE, INTENT(INOUT) :: BathGrid_Ys (:) + REAL(IntKi), INTENT(INOUT) :: BathGrid_npoints INTEGER(IntKi), INTENT( OUT) :: ErrStat3 ! Error status of the operation CHARACTER(*), INTENT( OUT) :: ErrMsg3 ! Error message if ErrStat /= ErrID_None diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 392bff6007..dc0fdc7653 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -327,7 +327,7 @@ typedef ^ ^ DbKi zeros6 {6} typedef ^ ^ DbKi MDWrOutput {:} - - "Data from time step to be written to a MoorDyn output file" typedef ^ ^ DbKi LastOutTime - - - "Time of last writing to MD output files" typedef ^ ^ ReKi PtfmInit {6} - - "initial position of platform for an individual (non-farm) MD instance" - -typedef ^ ^ DbKi BathGrid {:}{:} - - "matrix describing the bathymetry in a grid of x's and y's" +typedef ^ ^ DbKi BathymetryGrid {:}{:} - - "matrix describing the bathymetry in a grid of x's and y's" typedef ^ ^ DbKi BathGrid_Xs {:} - - "array of x-coordinates in the bathymetry grid" typedef ^ ^ DbKi BathGrid_Ys {:} - - "array of y-coordinates in the bathymetry grid" typedef ^ ^ IntKi BathGrid_npoints {:} - - "number of grid points to describe the bathymetry grid" From 9c45c6f3e60152be7e8ad66881b892855792b0c7 Mon Sep 17 00:00:00 2001 From: shousner Date: Thu, 23 Sep 2021 08:53:00 -0600 Subject: [PATCH 053/242] Some touchups to MoorDyn to get it to compile and run - For now, I just took out the BathGrid_npoints variable since it was causing trouble since it wasn't being allocated anywhere yet and we weren't even using it for anything - - Had to make BathGrid_npoints an integer type, but commented out - Had to make the getBathymetry subroutine in MoorDyn_IO public to use in MoorDyn.f90 - Had to change the character string size of inputString in getBathymetry - Had to allocate BathGrid_Xs and BathGrid_Ys and set to zero in the case where a constant depth is used - Needed one more loop in setting up the bathymetry grid, nGridY+1 - MoorDyn_Types seems to have been updated with these new changes - Looks like the MAP_dll and FAST_registry vcxproj files were also updated --- modules/moordyn/src/MoorDyn.f90 | 3 +- modules/moordyn/src/MoorDyn_IO.f90 | 18 +- modules/moordyn/src/MoorDyn_Types.f90 | 228 ++++++++++++++++++++++++ vs-build/MAPlib/MAP_dll.vcxproj | 10 +- vs-build/Registry/FAST_Registry.vcxproj | 8 +- 5 files changed, 252 insertions(+), 15 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index ab73ef942d..ebb27623bd 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -1259,7 +1259,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! else if ( OptString == 'WTRDPTH') then ! read (OptValue,*) p%WtrDpth else if ( OptString == 'WTRDPTH') then - CALL getBathymetry(OptValue, m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, m%BathGrid_npoints, ErrStat2, ErrMsg2) + CALL getBathymetry(OptValue, m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, ErrStat2, ErrMsg2) + ! CALL getBathymetry(OptValue, m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, m%BathGrid_npoints, ErrStat2, ErrMsg2) else if ( OptString == 'KBOT') then read (OptValue,*) p%kBot else if ( OptString == 'CBOT') then diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index e501407102..183a808d77 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -99,6 +99,7 @@ MODULE MoorDyn_IO ! PUBLIC :: MDIO_ReadInput + PUBLIC :: getBathymetry PUBLIC :: getCoefficientOrCurve PUBLIC :: DecomposeString PUBLIC :: MDIO_OpenOutput @@ -110,13 +111,14 @@ MODULE MoorDyn_IO CONTAINS - SUBROUTINE getBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, BathGrid_npoints, ErrStat3, ErrMsg3) + SUBROUTINE getBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, ErrStat3, ErrMsg3) + ! SUBROUTINE getBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, BathGrid_npoints, ErrStat3, ErrMsg3) - CHARACTER(40), INTENT(IN ) :: inputString + CHARACTER(20), INTENT(IN ) :: inputString REAL(DbKi), ALLOCATABLE, INTENT(INOUT) :: BathGrid (:,:) REAL(DbKi), ALLOCATABLE, INTENT(INOUT) :: BathGrid_Xs (:) REAL(DbKi), ALLOCATABLE, INTENT(INOUT) :: BathGrid_Ys (:) - REAL(IntKi), INTENT(INOUT) :: BathGrid_npoints + ! INTEGER(IntKi), INTENT(INOUT) :: BathGrid_npoints INTEGER(IntKi), INTENT( OUT) :: ErrStat3 ! Error status of the operation CHARACTER(*), INTENT( OUT) :: ErrMsg3 ! Error message if ErrStat /= ErrID_None @@ -138,6 +140,12 @@ SUBROUTINE getBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, BathGr ! If the input does not have any of these string values, let's treat it as a number but store in a matrix ALLOCATE(BathGrid(1,1), STAT=ErrStat4) READ(inputString, *, IOSTAT=ErrStat4) BathGrid(1,1) + + ALLOCATE(BathGrid_Xs(1), STAT=ErrStat4) + BathGrid_Xs(1) = 0.0_DbKi + + ALLOCATE(BathGrid_Ys(1), STAT=ErrStat4) + BathGrid_Ys(1) = 0.0_DbKi ELSE ! otherwise interpret the input as a file name to load the bathymetry lookup data from PRINT *, "found a letter in the depth value so will try to load a bathymetry file" @@ -155,7 +163,7 @@ SUBROUTINE getBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, BathGr ALLOCATE(BathGrid_Xs(nGridX), STAT=ErrStat4) ALLOCATE(BathGrid_Ys(nGridY), STAT=ErrStat4) - DO I = 1, nGridY ! loop through each line in the rest of the bathymetry file + DO I = 1, nGridY+1 ! loop through each line in the rest of the bathymetry file READ(UnCoef,'(A)',IOSTAT=ErrStat4) Line2 ! read into a line and call it Line2 IF (ErrStat4 > 0) EXIT @@ -174,7 +182,7 @@ SUBROUTINE getBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, BathGr CLOSE (UnCoef) RETURN ELSE - BathGrid_npoints = nGridX*nGridY ! save the number of points in the grid + ! BathGrid_npoints = nGridX*nGridY ! save the number of points in the grid CLOSE (UnCoef) END IF diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index 712c293ec4..379d1c9ae1 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -355,6 +355,10 @@ MODULE MoorDyn_Types REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: MDWrOutput !< Data from time step to be written to a MoorDyn output file [-] REAL(DbKi) :: LastOutTime !< Time of last writing to MD output files [-] REAL(ReKi) , DIMENSION(1:6) :: PtfmInit !< initial position of platform for an individual (non-farm) MD instance [-] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: BathymetryGrid !< matrix describing the bathymetry in a grid of x's and y's [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: BathGrid_Xs !< array of x-coordinates in the bathymetry grid [-] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: BathGrid_Ys !< array of y-coordinates in the bathymetry grid [-] + INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: BathGrid_npoints !< number of grid points to describe the bathymetry grid [-] END TYPE MD_MiscVarType ! ======================= ! ========= MD_ParameterType ======= @@ -8031,6 +8035,56 @@ SUBROUTINE MD_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) ENDIF DstMiscData%LastOutTime = SrcMiscData%LastOutTime DstMiscData%PtfmInit = SrcMiscData%PtfmInit +IF (ALLOCATED(SrcMiscData%BathymetryGrid)) THEN + i1_l = LBOUND(SrcMiscData%BathymetryGrid,1) + i1_u = UBOUND(SrcMiscData%BathymetryGrid,1) + i2_l = LBOUND(SrcMiscData%BathymetryGrid,2) + i2_u = UBOUND(SrcMiscData%BathymetryGrid,2) + IF (.NOT. ALLOCATED(DstMiscData%BathymetryGrid)) THEN + ALLOCATE(DstMiscData%BathymetryGrid(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%BathymetryGrid.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%BathymetryGrid = SrcMiscData%BathymetryGrid +ENDIF +IF (ALLOCATED(SrcMiscData%BathGrid_Xs)) THEN + i1_l = LBOUND(SrcMiscData%BathGrid_Xs,1) + i1_u = UBOUND(SrcMiscData%BathGrid_Xs,1) + IF (.NOT. ALLOCATED(DstMiscData%BathGrid_Xs)) THEN + ALLOCATE(DstMiscData%BathGrid_Xs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%BathGrid_Xs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%BathGrid_Xs = SrcMiscData%BathGrid_Xs +ENDIF +IF (ALLOCATED(SrcMiscData%BathGrid_Ys)) THEN + i1_l = LBOUND(SrcMiscData%BathGrid_Ys,1) + i1_u = UBOUND(SrcMiscData%BathGrid_Ys,1) + IF (.NOT. ALLOCATED(DstMiscData%BathGrid_Ys)) THEN + ALLOCATE(DstMiscData%BathGrid_Ys(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%BathGrid_Ys.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%BathGrid_Ys = SrcMiscData%BathGrid_Ys +ENDIF +IF (ALLOCATED(SrcMiscData%BathGrid_npoints)) THEN + i1_l = LBOUND(SrcMiscData%BathGrid_npoints,1) + i1_u = UBOUND(SrcMiscData%BathGrid_npoints,1) + IF (.NOT. ALLOCATED(DstMiscData%BathGrid_npoints)) THEN + ALLOCATE(DstMiscData%BathGrid_npoints(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%BathGrid_npoints.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%BathGrid_npoints = SrcMiscData%BathGrid_npoints +ENDIF END SUBROUTINE MD_CopyMisc SUBROUTINE MD_DestroyMisc( MiscData, ErrStat, ErrMsg ) @@ -8131,6 +8185,18 @@ SUBROUTINE MD_DestroyMisc( MiscData, ErrStat, ErrMsg ) CALL MD_DestroyContState( MiscData%xdTemp, ErrStat, ErrMsg ) IF (ALLOCATED(MiscData%MDWrOutput)) THEN DEALLOCATE(MiscData%MDWrOutput) +ENDIF +IF (ALLOCATED(MiscData%BathymetryGrid)) THEN + DEALLOCATE(MiscData%BathymetryGrid) +ENDIF +IF (ALLOCATED(MiscData%BathGrid_Xs)) THEN + DEALLOCATE(MiscData%BathGrid_Xs) +ENDIF +IF (ALLOCATED(MiscData%BathGrid_Ys)) THEN + DEALLOCATE(MiscData%BathGrid_Ys) +ENDIF +IF (ALLOCATED(MiscData%BathGrid_npoints)) THEN + DEALLOCATE(MiscData%BathGrid_npoints) ENDIF END SUBROUTINE MD_DestroyMisc @@ -8462,6 +8528,26 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz END IF Db_BufSz = Db_BufSz + 1 ! LastOutTime Re_BufSz = Re_BufSz + SIZE(InData%PtfmInit) ! PtfmInit + Int_BufSz = Int_BufSz + 1 ! BathymetryGrid allocated yes/no + IF ( ALLOCATED(InData%BathymetryGrid) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! BathymetryGrid upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%BathymetryGrid) ! BathymetryGrid + END IF + Int_BufSz = Int_BufSz + 1 ! BathGrid_Xs allocated yes/no + IF ( ALLOCATED(InData%BathGrid_Xs) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! BathGrid_Xs upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%BathGrid_Xs) ! BathGrid_Xs + END IF + Int_BufSz = Int_BufSz + 1 ! BathGrid_Ys allocated yes/no + IF ( ALLOCATED(InData%BathGrid_Ys) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! BathGrid_Ys upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%BathGrid_Ys) ! BathGrid_Ys + END IF + Int_BufSz = Int_BufSz + 1 ! BathGrid_npoints allocated yes/no + IF ( ALLOCATED(InData%BathGrid_npoints) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! BathGrid_npoints upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%BathGrid_npoints) ! BathGrid_npoints + END IF IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) IF (ErrStat2 /= 0) THEN @@ -9114,6 +9200,71 @@ SUBROUTINE MD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz ReKiBuf(Re_Xferred) = InData%PtfmInit(i1) Re_Xferred = Re_Xferred + 1 END DO + IF ( .NOT. ALLOCATED(InData%BathymetryGrid) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%BathymetryGrid,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%BathymetryGrid,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%BathymetryGrid,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%BathymetryGrid,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%BathymetryGrid,2), UBOUND(InData%BathymetryGrid,2) + DO i1 = LBOUND(InData%BathymetryGrid,1), UBOUND(InData%BathymetryGrid,1) + DbKiBuf(Db_Xferred) = InData%BathymetryGrid(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%BathGrid_Xs) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%BathGrid_Xs,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%BathGrid_Xs,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%BathGrid_Xs,1), UBOUND(InData%BathGrid_Xs,1) + DbKiBuf(Db_Xferred) = InData%BathGrid_Xs(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%BathGrid_Ys) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%BathGrid_Ys,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%BathGrid_Ys,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%BathGrid_Ys,1), UBOUND(InData%BathGrid_Ys,1) + DbKiBuf(Db_Xferred) = InData%BathGrid_Ys(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%BathGrid_npoints) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%BathGrid_npoints,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%BathGrid_npoints,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%BathGrid_npoints,1), UBOUND(InData%BathGrid_npoints,1) + IntKiBuf(Int_Xferred) = InData%BathGrid_npoints(i1) + Int_Xferred = Int_Xferred + 1 + END DO + END IF END SUBROUTINE MD_PackMisc SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) @@ -9959,6 +10110,83 @@ SUBROUTINE MD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) OutData%PtfmInit(i1) = ReKiBuf(Re_Xferred) Re_Xferred = Re_Xferred + 1 END DO + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! BathymetryGrid not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%BathymetryGrid)) DEALLOCATE(OutData%BathymetryGrid) + ALLOCATE(OutData%BathymetryGrid(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%BathymetryGrid.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%BathymetryGrid,2), UBOUND(OutData%BathymetryGrid,2) + DO i1 = LBOUND(OutData%BathymetryGrid,1), UBOUND(OutData%BathymetryGrid,1) + OutData%BathymetryGrid(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! BathGrid_Xs not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%BathGrid_Xs)) DEALLOCATE(OutData%BathGrid_Xs) + ALLOCATE(OutData%BathGrid_Xs(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%BathGrid_Xs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%BathGrid_Xs,1), UBOUND(OutData%BathGrid_Xs,1) + OutData%BathGrid_Xs(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! BathGrid_Ys not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%BathGrid_Ys)) DEALLOCATE(OutData%BathGrid_Ys) + ALLOCATE(OutData%BathGrid_Ys(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%BathGrid_Ys.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%BathGrid_Ys,1), UBOUND(OutData%BathGrid_Ys,1) + OutData%BathGrid_Ys(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! BathGrid_npoints not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%BathGrid_npoints)) DEALLOCATE(OutData%BathGrid_npoints) + ALLOCATE(OutData%BathGrid_npoints(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%BathGrid_npoints.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%BathGrid_npoints,1), UBOUND(OutData%BathGrid_npoints,1) + OutData%BathGrid_npoints(i1) = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + END DO + END IF END SUBROUTINE MD_UnPackMisc SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) diff --git a/vs-build/MAPlib/MAP_dll.vcxproj b/vs-build/MAPlib/MAP_dll.vcxproj index 7956ac13cd..0c7c4198f0 100644 --- a/vs-build/MAPlib/MAP_dll.vcxproj +++ b/vs-build/MAPlib/MAP_dll.vcxproj @@ -22,32 +22,32 @@ {BF86702A-CB17-4050-8AE9-078CDC5910D3} Win32Proj MAP_DLL - 8.1 + 10.0 StaticLibrary true - v140 + v142 Unicode StaticLibrary true - v140 + v142 Unicode StaticLibrary false - v140 + v142 true Unicode StaticLibrary false - v140 + v142 true Unicode diff --git a/vs-build/Registry/FAST_Registry.vcxproj b/vs-build/Registry/FAST_Registry.vcxproj index 40649f85f0..792f8ce14e 100644 --- a/vs-build/Registry/FAST_Registry.vcxproj +++ b/vs-build/Registry/FAST_Registry.vcxproj @@ -28,27 +28,27 @@ Application true Unicode - v140 + v142 Application true Unicode - v140 + v142 Application false true Unicode - v140 + v142 Application false true Unicode - v140 + v142 From d5620011affcc2fc943096ab99c51e2452a78ee3 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Mon, 27 Sep 2021 15:51:27 -0600 Subject: [PATCH 054/242] Corrections in Rod end hydrodynamic added mass and inertia: - Rod end hydrodynamic added mass at each end was previously calculated based on full sphere volume. Now it's calculated based on the volume of a hemisphere at each end. - Rod end Froude-Krylov force appeared to have a rogue 0.5 factor. It is now removed. --- modules/moordyn/src/MoorDyn.f90 | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 659befcb8e..aedfa62f15 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -5642,7 +5642,7 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ! >>> what about rotational drag?? <<< eqn will be Pi* Rod%d**4/16.0 omega_rel?^2... *0.5 * Cd... ! Froud-Krylov force - Rod%Aq(:,I) = Rod%Aq(:,I) + VOF * p%rhoW*(1.0+Rod%CaEnd)*0.5* (2.0/3.0*Pi*Rod%d**3 /8) * aq + Rod%Aq(:,I) = Rod%Aq(:,I) + VOF * p%rhoW*(1.0+Rod%CaEnd)* (2.0/3.0*Pi*Rod%d**3 /8.0) * aq ! dynamic pressure force Rod%Pd(:,I) = Rod%Pd(:,I) + VOF * 0.25* Pi*Rod%d*Rod%d * Rod%PDyn(I) * Rod%q @@ -5651,9 +5651,9 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) DO J=1,3 DO K=1,3 IF (J==K) THEN - Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* (Pi*Rod%d**3/6.0) * Rod%CaEnd*Rod%q(J)*Rod%q(K) + Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) ELSE - Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* (Pi*Rod%d**3/6.0) * Rod%CaEnd*Rod%q(J)*Rod%q(K) + Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) END IF END DO END DO @@ -5674,7 +5674,7 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) Rod%Dq(:,I) = Rod%Dq(:,I) + VOF * 0.25* Pi*Rod%d*Rod%d * p%rhoW*Rod%CdEnd * MagVq * Vq ! Froud-Krylov force - Rod%Aq(:,I) = Rod%Aq(:,I) + VOF * p%rhoW*(1.0+Rod%CaEnd)*0.5* (2.0/3.0*Pi*Rod%d**3 /8) * aq + Rod%Aq(:,I) = Rod%Aq(:,I) + VOF * p%rhoW*(1.0+Rod%CaEnd)* (2.0/3.0*Pi*Rod%d**3 /8.0) * aq ! dynamic pressure force Rod%Pd(:,I) = Rod%Pd(:,I) - VOF * 0.25* Pi*Rod%d*Rod%d * Rod%PDyn(I) * Rod%q @@ -5683,9 +5683,9 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) DO J=1,3 DO K=1,3 IF (J==K) THEN - Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* (Pi*Rod%d**3/6.0) * Rod%CaEnd*Rod%q(J)*Rod%q(K) + Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) ELSE - Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* (Pi*Rod%d**3/6.0) * Rod%CaEnd*Rod%q(J)*Rod%q(K) + Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) END IF END DO END DO From 41f16628fabe517a520b547f34d465df6e6b646a Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Wed, 29 Sep 2021 18:32:26 -0600 Subject: [PATCH 055/242] Implemented simple viscoelastic capability: - Added ElasticMod for Line and LineProp objects. 1 is the normal linear elasticity model, 2 is the new viscoelastic model that has both static and dynamic stiffnesses and dampings in series. - Added a new set of states for the static portion of stretch in each segment. It is dl_S in the Line object and stored in the last N entries of a line's state vector. Only when ElasticMod==2. - For using ElasticMod=2, a pair of values should be provided for a line type's EA and BA entries. This is done by entering two numbers separated by a bar ("|") with no space between. The new MDIO SplitByBar subroutine parses this. When not using this option there is not change to the input file. - ToDo: continue verification, clean up, add more error checks. --- modules/moordyn/src/MoorDyn.f90 | 216 ++++++++++++++--- modules/moordyn/src/MoorDyn_IO.f90 | 30 +++ modules/moordyn/src/MoorDyn_Registry.txt | 25 +- modules/moordyn/src/MoorDyn_Types.f90 | 285 ++++++++++++++++++++++- 4 files changed, 503 insertions(+), 53 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 659befcb8e..d1aad0736b 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -30,7 +30,7 @@ MODULE MoorDyn PRIVATE - TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a11', '2 August 2021' ) + TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a12', '28 September 2021' ) INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output @@ -112,6 +112,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CHARACTER(40) :: TempString2 ! CHARACTER(40) :: TempString3 ! CHARACTER(40) :: TempString4 ! + CHARACTER(40) :: TempStrings(6) ! Array of 6 strings used when parsing comma-separated items CHARACTER(1024) :: FileName ! @@ -506,15 +507,40 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !TODO: add check if %name is maximum length, which might indicate the full name was too long <<< - ! process stiffness, damping, and bending coefficients (which might use lookup tables) - CALL getCoefficientOrCurve(tempString1, m%LineTypeList(l)%EA, & + ! process stiffness coefficients + CALL SplitByBars(tempString1, N, tempStrings) + if (N > 2) then + CALL SetErrStat( ErrID_Fatal, 'A line type EA entry can have at most 2 (comma-separated) values.', ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + else if (N==2) then ! visco-elastic case! + m%LineTypeList(l)%ElasticMod = 2 + read(tempStrings(2), *) m%LineTypeList(l)%EA_D + else + m%LineTypeList(l)%ElasticMod = 1 ! normal case + end if + ! get the regular/static coefficient or relation in all cases (can be from a lookup table) + CALL getCoefficientOrCurve(tempStrings(1), m%LineTypeList(l)%EA, & m%LineTypeList(l)%nEApoints, & m%LineTypeList(l)%stiffXs, & m%LineTypeList(l)%stiffYs, ErrStat2, ErrMsg2) - CALL getCoefficientOrCurve(tempString2, m%LineTypeList(l)%BA, & - m%LineTypeList(l)%nBpoints, & + + ! process damping coefficients + CALL SplitByBars(tempString2, N, tempStrings) + if (N > m%LineTypeList(l)%ElasticMod) then + CALL SetErrStat( ErrID_Fatal, 'A line type BA entry cannot have more (comma-separated) values its EA entry.', ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + else if (N==2) then ! visco-elastic case when two BA values provided + read(tempStrings(2), *) m%LineTypeList(l)%BA_D + else if (m%LineTypeList(l)%ElasticMod == 2) then ! case where there is no dynamic damping for viscoelastic model (will it work)? + print *, "Warning, viscoelastic model being used with zero damping on the dynamic stiffness." + end if + ! get the regular/static coefficient or relation in all cases (can be from a lookup table?) + CALL getCoefficientOrCurve(tempStrings(1), m%LineTypeList(l)%BA, & + m%LineTypeList(l)%nBApoints, & m%LineTypeList(l)%dampXs, & m%LineTypeList(l)%dampYs, ErrStat2, ErrMsg2) + + ! process bending stiffness coefficients (which might use lookup tables) CALL getCoefficientOrCurve(tempString3, m%LineTypeList(l)%EI, & m%LineTypeList(l)%nEIpoints, & m%LineTypeList(l)%bstiffXs, & @@ -1024,9 +1050,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! account for states of line m%LineStateIs1(l) = Nx + 1 - m%LineStateIsN(l) = Nx + 6*m%LineList(l)%N - 6 - Nx = Nx + 6*(m%LineList(l)%N - 1) - + if (m%LineTypeList(m%LineList(l)%PropsIdNum)%ElasticMod == 2) then + Nx = Nx + 7*m%LineList(l)%N - 6 ! if using viscoelastic model, need one more state per segment + m%LineStateIsN(l) = Nx + else + Nx = Nx + 6*m%LineList(l)%N - 6 ! normal case, just 6 states per internal node + m%LineStateIsN(l) = Nx + end if + ! Process attachment identfiers and attach line ends ! First for the anchor (or end A)... @@ -2017,6 +2048,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er END DO ! print *, m%LineList(l)%r(:,I) END DO + + ! if using viscoelastic model, initialize the internal states + if (m%LineList(l)%ElasticMod == 2) then + do I = 1,N + x%states(m%LineStateIs1(l) + 6*N-6 + I-1) = m%LineList(l)%dl_S(I) ! should be zero + end do + end if + END DO !l = 1, p%NLines @@ -2901,6 +2940,8 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er ! call ground body to update all the fixed things CALL Body_DoRHS(m%GroundBody, m, p) + + !print *, t, m%LineList(1)%T(1,9), m%LineList(1)%T(2,9), m%LineList(1)%T(3,9), m%LineList(3)%T(1,9), m%LineList(3)%T(2,9), m%LineList(3)%T(3,9) END SUBROUTINE MD_CalcContStateDeriv @@ -3181,11 +3222,11 @@ SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) INTEGER, INTENT( INOUT ) :: ErrStat ! returns a non-zero value when an error occurs CHARACTER(*), INTENT( INOUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None - INTEGER(4) :: J ! Generic index - INTEGER(4) :: K ! Generic index + INTEGER(4) :: I, J, K ! Generic index INTEGER(IntKi) :: N REAL(DbKi) :: temp + N = Line%N ! number of segments in this line (for code readability) ! -------------- save some section properties to the line object itself ----------------- @@ -3194,12 +3235,38 @@ SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) Line%rho = LineProp%w/(Pi/4.0 * Line%d * Line%d) Line%EA = LineProp%EA + ! note: Line%BA is set later + Line%EA_D = LineProp%EA_D + Line%BA_D = LineProp%BA_D + !Line%EI = LineProp%EI <<< for bending stiffness Line%Can = LineProp%Can Line%Cat = LineProp%Cat Line%Cdn = LineProp%Cdn Line%Cdt = LineProp%Cdt + ! copy over elasticity data + Line%ElasticMod = LineProp%ElasticMod + + Line%nEApoints = LineProp%nEApoints + DO I = 1,Line%nEApoints + Line%stiffXs(I) = LineProp%stiffXs(I) + Line%stiffYs(I) = LineProp%stiffYs(I) ! note: this does not convert to E (not EA) like done in C version + END DO + + Line%nBApoints = LineProp%nBApoints + DO I = 1,Line%nBApoints + Line%dampXs(I) = LineProp%dampXs(I) + Line%dampYs(I) = LineProp%dampYs(I) + END DO + + Line%nEIpoints = LineProp%nEIpoints + DO I = 1,Line%nEIpoints + Line%bstiffXs(I) = LineProp%bstiffXs(I) + Line%bstiffYs(I) = LineProp%bstiffYs(I) ! copy over + END DO + + ! Specify specific internal damping coefficient (BA) for this line. ! Will be equal to inputted BA of LineType if input value is positive. ! If input value is negative, it is considered to be desired damping ratio (zeta) @@ -3230,11 +3297,23 @@ SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) !CALL CleanUp() RETURN END IF - - ! allocate node tangent vectors - ALLOCATE ( Line%q(3, 0:N), STAT = ErrStat ) + + ! if using viscoelastic model, allocate additional state quantities + if (Line%ElasticMod == 2) then + ALLOCATE ( Line%dl_S(N), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) THEN + ErrMsg = ' Error allocating dl_S array.' + !CALL CleanUp() + RETURN + END IF + ! initialize to zero + Line%dl_S = 0.0_DbKi + end if + + ! allocate node and segment tangent vectors + ALLOCATE ( Line%q(3, 0:N), Line%qs(3, N), STAT = ErrStat ) IF ( ErrStat /= ErrID_None ) THEN - ErrMsg = ' Error allocating q array.' + ErrMsg = ' Error allocating q or qs array.' !CALL CleanUp() RETURN END IF @@ -4046,6 +4125,13 @@ SUBROUTINE Line_SetState(Line, X, t) END DO END DO + + ! if using viscoelastic model, also set the static stiffness stretch + if (Line%ElasticMod == 2) then + do I=1,Line%N + Line%dl_S(I) = X( 6*Line%N-6 + I) ! these will be the last N entries in the state vector + end do + end if END SUBROUTINE Line_SetState !-------------------------------------------------------------- @@ -4086,6 +4172,12 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, Real(DbKi) :: SumSqVq ! Real(DbKi) :: MagVp ! Real(DbKi) :: MagVq ! + Real(DbKi) :: MagT ! tension stiffness force magnitude + Real(DbKi) :: MagTd ! tension damping force magnitude + Real(DbKi) :: Xi ! used in interpolating from lookup table + Real(DbKi) :: Yi ! used in interpolating from lookup table + Real(DbKi) :: dl ! stretch of a segment [m] + Real(DbKi) :: ld_S ! rate of change of static stiffness portion of segment [m/s] N = Line%N ! for convenience @@ -4104,14 +4196,15 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, - ! calculate instantaneous (stretched) segment lengths and rates << should add catch here for if lstr is ever zero + ! -------------------- calculate various kinematic quantities --------------------------- DO I = 1, N - Sum1 = 0.0_DbKi - DO J = 1, 3 - Sum1 = Sum1 + (Line%r(J,I) - Line%r(J,I-1)) * (Line%r(J,I) - Line%r(J,I-1)) - END DO - Line%lstr(I) = sqrt(Sum1) ! stretched segment length - + + + ! calculate current (Stretched) segment lengths and unit tangent vectors (qs) for each segment (this is used for bending calculations) + CALL UnitVector(Line%r(:,I-1), Line%r(:,I), Line%qs(:,I), Line%lstr(I)) + + ! should add catch here for if lstr is ever zero + Sum1 = 0.0_DbKi DO J = 1, 3 Sum1 = Sum1 + (Line%r(J,I) - Line%r(J,I-1))*(Line%rd(J,I) - Line%rd(J,I-1)) @@ -4169,25 +4262,74 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, ! loop through the segments DO I = 1, N + + ! handle nonlinear stiffness if needed + if (Line%nEApoints > 0) then + + Xi = Line%l(I)/Line%lstr(I) - 1.0 ! strain rate based on inputs + Yi = 0.0_DbKi + + ! find stress based on strain + if (Xi < 0.0) then ! if negative strain (compression), zero stress + Yi = 0.0_DbKi + else if (Xi < Line%stiffXs(1)) then ! if strain below first data point, interpolate from zero + Yi = Xi * Line%stiffYs(1)/Line%stiffXs(1) + else if (Xi >= Line%stiffXs(Line%nEApoints)) then ! if strain exceeds last data point, use last data point + Yi = Line%stiffYs(Line%nEApoints) + else ! otherwise we're in range of the table so interpolate! + do J=1, Line%nEApoints-1 ! go through lookup table until next entry exceeds inputted strain rate + if (Line%stiffXs(J+1) > Xi) then + Yi = Line%stiffYs(J) + (Xi-Line%stiffXs(J)) * (Line%stiffYs(J+1)-Line%stiffYs(J))/(Line%stiffXs(J+1)-Line%stiffXs(J)) + exit + end if + end do + end if - ! line tension, inherently including possibility of dynamic length changes in l term - IF (Line%lstr(I)/Line%l(I) > 1.0) THEN - DO J = 1, 3 - Line%T(J,I) = Line%EA *( 1.0/Line%l(I) - 1.0/Line%lstr(I) ) * (Line%r(J,I)-Line%r(J,I-1)) - END DO - ELSE - DO J = 1, 3 - Line%T(J,I) = 0.0_DbKi ! cable can't "push" - END DO - END if + ! calculate a young's modulus equivalent value based on stress/strain + Line%EA = Yi/Xi + end if + + + ! >>>> could do similar as above for nonlinear damping or bending stiffness <<<< + if (Line%nBApoints > 0) print *, 'Nonlinear elastic damping not yet implemented' + if (Line%nEIpoints > 0) print *, 'Nonlinear bending stiffness not yet implemented' + + + ! basic elasticity model + if (Line%ElasticMod == 1) then + ! line tension, inherently including possibility of dynamic length changes in l term + if (Line%lstr(I)/Line%l(I) > 1.0) then + MagT = Line%EA *( Line%lstr(I)/Line%l(I) - 1.0 ) + else + MagT = 0.0_DbKi ! cable can't "push" + end if + ! line internal damping force based on line-specific BA value, including possibility of dynamic length changes in l and ld terms + MagTd = Line%BA* ( Line%lstrd(I) - Line%lstr(I)*Line%ld(I)/Line%l(I) )/Line%l(I) + + ! viscoelastic model + else if (Line%ElasticMod == 2) then + + dl = Line%lstr(I) - Line%l(I) ! delta l of this segment + + ld_S = (Line%EA_D*dl - (Line%EA_D + Line%EA)*Line%dl_S(I) + Line%BA_D*Line%lstrd(I)) /( Line%BA_D + Line%BA) ! rate of change of static stiffness portion [m/s] + + !MagT = (Line%EA*Line%dl_S(I) + Line%BA*ld_S)/ Line%l(I) ! compute tension based on static portion (dynamic portion would give same) + MagT = Line%EA*Line%dl_S(I)/ Line%l(I) + MagTd = Line%BA*ld_S / Line%l(I) + + ! update state derivative for static stiffness stretch (last N entries in the state vector) + Xd( 6*N-6 + I) = ld_S + + end if - ! line internal damping force based on line-specific BA value, including possibility of dynamic length changes in l and ld terms - DO J = 1, 3 + + do J = 1, 3 + !Line%T(J,I) = Line%EA *( 1.0/Line%l(I) - 1.0/Line%lstr(I) ) * (Line%r(J,I)-Line%r(J,I-1)) + Line%T(J,I) = MagT * Line%qs(J,I) !Line%Td(J,I) = Line%BA* ( Line%lstrd(I) / Line%l(I) ) * (Line%r(J,I)-Line%r(J,I-1)) / Line%lstr(I) ! note new form of damping coefficient, BA rather than Cint - Line%Td(J,I) = Line%BA* ( Line%lstrd(I) - Line%lstr(I)*Line%ld(I)/Line%l(I) )/Line%l(I) * (Line%r(J,I)-Line%r(J,I-1)) / Line%lstr(I) - END DO - END DO - + Line%Td(J,I) = MagTd * Line%qs(J,I) + end do + end do ! loop through the nodes diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index aa33774b26..b80fb96264 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -100,6 +100,7 @@ MODULE MoorDyn_IO ! PUBLIC :: MDIO_ReadInput PUBLIC :: getCoefficientOrCurve + PUBLIC :: SplitByBars PUBLIC :: DecomposeString PUBLIC :: MDIO_OpenOutput PUBLIC :: MDIO_CloseOutput @@ -178,7 +179,35 @@ SUBROUTINE getCoefficientOrCurve(inputString, LineProp_c, LineProp_npoints, Line END SUBROUTINE getCoefficientOrCurve + ! Split a string into separate strings by the bar (|) symbol + SUBROUTINE SplitByBars(instring, n, outstrings) + + CHARACTER(*), INTENT(INOUT) :: instring + INTEGER(IntKi), INTENT( OUT) :: n + CHARACTER(40), INTENT(INOUT) :: outstrings(6) ! array of output strings. Up to 6 strings can be read + + INTEGER :: pos1, pos2, i + + n = 0 + pos1=1 + + DO + pos2 = INDEX(instring(pos1:), "|") ! find index of next comma + IF (pos2 == 0) THEN ! if there isn't another comma, read the last entry and call it done (this could be the only entry if no commas) + n = n + 1 + outstrings(n) = instring(pos1:) + EXIT + END IF + n = n + 1 + if (n > 6) then + print *, "ERROR - SplitByBars cannot do more than 6 entries" + end if + outstrings(n) = instring(pos1:pos1+pos2-2) + pos1 = pos2+pos1 + END DO + END SUBROUTINE SplitByBars + ! Split a string into separate letter strings and integers. Letters are converted to uppercase. SUBROUTINE DecomposeString(outWord, let1, num1, let2, num2, let3) @@ -370,6 +399,7 @@ SUBROUTINE MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat, ErrMsg ) READ (num1,*) oID ! this is the line number p%OutParam(I)%ObjID = oID ! record the ID of the line p%OutParam(I)%NodeID = m%LineList(oID)%N ! specify node N (end B, fairlead) + ! >>> should check validity of ObjID and NodeID <<< ! achor tension case ELSE IF (let1 == 'ANCHTEN') THEN diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 2390c81636..eed8b296f4 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -58,16 +58,19 @@ typedef ^ ^ CHARACTER(20) name - typedef ^ ^ DbKi d - - - "volume-equivalent diameter" "[m]" typedef ^ ^ DbKi w - - - "per-length weight in air" "[kg/m]" typedef ^ ^ DbKi EA - - - "axial stiffness" "[N]" +typedef ^ ^ DbKi EA_D - - - "axial stiffness" "[N]" typedef ^ ^ DbKi BA - - - "internal damping coefficient times area" "[N-s]" +typedef ^ ^ DbKi BA_D - - - "internal damping coefficient times area" "[N-s]" typedef ^ ^ DbKi EI - - - "bending stiffness" "[N-m]" typedef ^ ^ DbKi Can - - - "transverse added mass coefficient" typedef ^ ^ DbKi Cat - - - "tangential added mass coefficient" typedef ^ ^ DbKi Cdn - - - "transverse drag coefficient" typedef ^ ^ DbKi Cdt - - - "tangential drag coefficient" +typedef ^ ^ IntKi ElasticMod - - - "Which elasticity model to use: {0 basic, 1 viscoelastic, 2 future SYCOM} " - typedef ^ ^ IntKi nEApoints - 0 - "number of values in stress-strain lookup table (0 means using constant E)" typedef ^ ^ DbKi stiffXs {30} - - "x array for stress-strain lookup table (up to nCoef)" typedef ^ ^ DbKi stiffYs {30} - - "y array for stress-strain lookup table" -typedef ^ ^ IntKi nBpoints - 0 - "number of values in stress-strainrate lookup table (0 means using constant c)" +typedef ^ ^ IntKi nBApoints - 0 - "number of values in stress-strainrate lookup table (0 means using constant c)" typedef ^ ^ DbKi dampXs {30} - - "x array for stress-strainrate lookup table (up to nCoef)" typedef ^ ^ DbKi dampYs {30} - - "y array for stress-strainrate lookup table " typedef ^ ^ IntKi nEIpoints - 0 - "number of values in bending stress-strain lookup table (0 means using constant E)" @@ -205,6 +208,7 @@ typedef ^ ^ DbKi RodWrOutput {:} typedef ^ MD_Line IntKi IdNum - - - "integer identifier of this Line" #typedef ^ ^ CHARACTER(10) type - - - "type of line. should match one of LineProp names" typedef ^ ^ IntKi PropsIdNum - - - "the IdNum of the associated line properties" - +typedef ^ ^ IntKi ElasticMod - - - "Which elasticity model to use: {0 basic, 1 viscoelastic, 2 future SYCOM} " - typedef ^ ^ IntKi OutFlagList {20} - - "array specifying what line quantities should be output (1 vs 0)" - typedef ^ ^ IntKi CtrlChan - 0 - "index of control channel that will drive line active tensioning (0 for none)" - typedef ^ ^ IntKi FairConnect - - - "IdNum of Connection at fairlead" @@ -215,21 +219,34 @@ typedef ^ ^ IntKi endTypeB - typedef ^ ^ DbKi UnstrLen - - - "unstretched length of the line" - typedef ^ ^ DbKi rho - - - "density" "[kg/m3]" typedef ^ ^ DbKi d - - - "volume-equivalent diameter" "[m]" -typedef ^ ^ DbKi EA - - - "stiffness" "[N]" -typedef ^ ^ DbKi EI - - - "bending stiffness" "[N-m]" -typedef ^ ^ DbKi BA - - - "internal damping coefficient times area for this line only" "[N-s]" +typedef ^ ^ DbKi EA - 0 - "stiffness" "[N]" +typedef ^ ^ DbKi EA_D - 0 - "dynamic stiffness when using viscoelastic model" "[N]" +typedef ^ ^ DbKi BA - 0 - "internal damping coefficient times area for this line only" "[N-s]" +typedef ^ ^ DbKi BA_D - 0 - "dynamic internal damping coefficient times area when using viscoelastic model" "[N-s]" +typedef ^ ^ DbKi EI - 0 - "bending stiffness" "[N-m]" typedef ^ ^ DbKi Can - - - "" "[-]" typedef ^ ^ DbKi Cat - - - "" "[-]" typedef ^ ^ DbKi Cdn - - - "" "[-]" typedef ^ ^ DbKi Cdt - - - "" "[-]" +typedef ^ ^ IntKi nEApoints - 0 - "number of values in stress-strain lookup table (0 means using constant E)" +typedef ^ ^ DbKi stiffXs {30} - - "x array for stress-strain lookup table (up to nCoef)" +typedef ^ ^ DbKi stiffYs {30} - - "y array for stress-strain lookup table" +typedef ^ ^ IntKi nBApoints - 0 - "number of values in stress-strainrate lookup table (0 means using constant c)" +typedef ^ ^ DbKi dampXs {30} - - "x array for stress-strainrate lookup table (up to nCoef)" +typedef ^ ^ DbKi dampYs {30} - - "y array for stress-strainrate lookup table " +typedef ^ ^ IntKi nEIpoints - 0 - "number of values in bending stress-strain lookup table (0 means using constant E)" +typedef ^ ^ DbKi bstiffXs {30} - - "x array for stress-strain lookup table (up to nCoef)" +typedef ^ ^ DbKi bstiffYs {30} - - "y array for stress-strain lookup table" typedef ^ ^ DbKi time - - - "current time" "[s]" typedef ^ ^ DbKi r {:}{:} - - "node positions" - typedef ^ ^ DbKi rd {:}{:} - - "node velocities" - typedef ^ ^ DbKi q {:}{:} - - "node tangent vectors" - +typedef ^ ^ DbKi qs {:}{:} - - "segment tangent vectors" - typedef ^ ^ DbKi l {:} - - "segment unstretched length" "[m]" typedef ^ ^ DbKi ld {:} - - "segment unstretched length rate of change (used in active tensioning)" "[m]" typedef ^ ^ DbKi lstr {:} - - "segment stretched length" "[m]" typedef ^ ^ DbKi lstrd {:} - - "segment change in stretched length" "[m/s]" +typedef ^ ^ DbKi dl_S {:} - - "segment stretch attributed to static stiffness portion" "[m]" typedef ^ ^ DbKi V {:} - - "segment volume" "[m^3]" typedef ^ ^ DbKi U {:}{:} - - "water velocity at node" "[m/s]" typedef ^ ^ DbKi Ud {:}{:} - - "water acceleration at node" "[m/s^2]" diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index 712c293ec4..cae84f15db 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -70,16 +70,19 @@ MODULE MoorDyn_Types REAL(DbKi) :: d !< volume-equivalent diameter [[m]] REAL(DbKi) :: w !< per-length weight in air [[kg/m]] REAL(DbKi) :: EA !< axial stiffness [[N]] + REAL(DbKi) :: EA_D !< axial stiffness [[N]] REAL(DbKi) :: BA !< internal damping coefficient times area [[N-s]] + REAL(DbKi) :: BA_D !< internal damping coefficient times area [[N-s]] REAL(DbKi) :: EI !< bending stiffness [[N-m]] REAL(DbKi) :: Can !< transverse added mass coefficient [-] REAL(DbKi) :: Cat !< tangential added mass coefficient [-] REAL(DbKi) :: Cdn !< transverse drag coefficient [-] REAL(DbKi) :: Cdt !< tangential drag coefficient [-] + INTEGER(IntKi) :: ElasticMod !< Which elasticity model to use: {0 basic, 1 viscoelastic, 2 future SYCOM} [-] INTEGER(IntKi) :: nEApoints = 0 !< number of values in stress-strain lookup table (0 means using constant E) [-] REAL(DbKi) , DIMENSION(1:30) :: stiffXs !< x array for stress-strain lookup table (up to nCoef) [-] REAL(DbKi) , DIMENSION(1:30) :: stiffYs !< y array for stress-strain lookup table [-] - INTEGER(IntKi) :: nBpoints = 0 !< number of values in stress-strainrate lookup table (0 means using constant c) [-] + INTEGER(IntKi) :: nBApoints = 0 !< number of values in stress-strainrate lookup table (0 means using constant c) [-] REAL(DbKi) , DIMENSION(1:30) :: dampXs !< x array for stress-strainrate lookup table (up to nCoef) [-] REAL(DbKi) , DIMENSION(1:30) :: dampYs !< y array for stress-strainrate lookup table [-] INTEGER(IntKi) :: nEIpoints = 0 !< number of values in bending stress-strain lookup table (0 means using constant E) [-] @@ -225,6 +228,7 @@ MODULE MoorDyn_Types TYPE, PUBLIC :: MD_Line INTEGER(IntKi) :: IdNum !< integer identifier of this Line [-] INTEGER(IntKi) :: PropsIdNum !< the IdNum of the associated line properties [-] + INTEGER(IntKi) :: ElasticMod !< Which elasticity model to use: {0 basic, 1 viscoelastic, 2 future SYCOM} [-] INTEGER(IntKi) , DIMENSION(1:20) :: OutFlagList !< array specifying what line quantities should be output (1 vs 0) [-] INTEGER(IntKi) :: CtrlChan = 0 !< index of control channel that will drive line active tensioning (0 for none) [-] INTEGER(IntKi) :: FairConnect !< IdNum of Connection at fairlead [-] @@ -235,21 +239,34 @@ MODULE MoorDyn_Types REAL(DbKi) :: UnstrLen !< unstretched length of the line [-] REAL(DbKi) :: rho !< density [[kg/m3]] REAL(DbKi) :: d !< volume-equivalent diameter [[m]] - REAL(DbKi) :: EA !< stiffness [[N]] - REAL(DbKi) :: EI !< bending stiffness [[N-m]] - REAL(DbKi) :: BA !< internal damping coefficient times area for this line only [[N-s]] + REAL(DbKi) :: EA = 0 !< stiffness [[N]] + REAL(DbKi) :: EA_D = 0 !< dynamic stiffness when using viscoelastic model [[N]] + REAL(DbKi) :: BA = 0 !< internal damping coefficient times area for this line only [[N-s]] + REAL(DbKi) :: BA_D = 0 !< dynamic internal damping coefficient times area when using viscoelastic model [[N-s]] + REAL(DbKi) :: EI = 0 !< bending stiffness [[N-m]] REAL(DbKi) :: Can !< [[-]] REAL(DbKi) :: Cat !< [[-]] REAL(DbKi) :: Cdn !< [[-]] REAL(DbKi) :: Cdt !< [[-]] + INTEGER(IntKi) :: nEApoints = 0 !< number of values in stress-strain lookup table (0 means using constant E) [-] + REAL(DbKi) , DIMENSION(1:30) :: stiffXs !< x array for stress-strain lookup table (up to nCoef) [-] + REAL(DbKi) , DIMENSION(1:30) :: stiffYs !< y array for stress-strain lookup table [-] + INTEGER(IntKi) :: nBApoints = 0 !< number of values in stress-strainrate lookup table (0 means using constant c) [-] + REAL(DbKi) , DIMENSION(1:30) :: dampXs !< x array for stress-strainrate lookup table (up to nCoef) [-] + REAL(DbKi) , DIMENSION(1:30) :: dampYs !< y array for stress-strainrate lookup table [-] + INTEGER(IntKi) :: nEIpoints = 0 !< number of values in bending stress-strain lookup table (0 means using constant E) [-] + REAL(DbKi) , DIMENSION(1:30) :: bstiffXs !< x array for stress-strain lookup table (up to nCoef) [-] + REAL(DbKi) , DIMENSION(1:30) :: bstiffYs !< y array for stress-strain lookup table [-] REAL(DbKi) :: time !< current time [[s]] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: r !< node positions [-] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: rd !< node velocities [-] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: q !< node tangent vectors [-] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: qs !< segment tangent vectors [-] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: l !< segment unstretched length [[m]] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: ld !< segment unstretched length rate of change (used in active tensioning) [[m]] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: lstr !< segment stretched length [[m]] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: lstrd !< segment change in stretched length [[m/s]] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: dl_S !< segment stretch attributed to static stiffness portion [[m]] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: V !< segment volume [[m^3]] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: U !< water velocity at node [[m/s]] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Ud !< water acceleration at node [[m/s^2]] @@ -1398,16 +1415,19 @@ SUBROUTINE MD_CopyLineProp( SrcLinePropData, DstLinePropData, CtrlCode, ErrStat, DstLinePropData%d = SrcLinePropData%d DstLinePropData%w = SrcLinePropData%w DstLinePropData%EA = SrcLinePropData%EA + DstLinePropData%EA_D = SrcLinePropData%EA_D DstLinePropData%BA = SrcLinePropData%BA + DstLinePropData%BA_D = SrcLinePropData%BA_D DstLinePropData%EI = SrcLinePropData%EI DstLinePropData%Can = SrcLinePropData%Can DstLinePropData%Cat = SrcLinePropData%Cat DstLinePropData%Cdn = SrcLinePropData%Cdn DstLinePropData%Cdt = SrcLinePropData%Cdt + DstLinePropData%ElasticMod = SrcLinePropData%ElasticMod DstLinePropData%nEApoints = SrcLinePropData%nEApoints DstLinePropData%stiffXs = SrcLinePropData%stiffXs DstLinePropData%stiffYs = SrcLinePropData%stiffYs - DstLinePropData%nBpoints = SrcLinePropData%nBpoints + DstLinePropData%nBApoints = SrcLinePropData%nBApoints DstLinePropData%dampXs = SrcLinePropData%dampXs DstLinePropData%dampYs = SrcLinePropData%dampYs DstLinePropData%nEIpoints = SrcLinePropData%nEIpoints @@ -1466,16 +1486,19 @@ SUBROUTINE MD_PackLineProp( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Db_BufSz = Db_BufSz + 1 ! d Db_BufSz = Db_BufSz + 1 ! w Db_BufSz = Db_BufSz + 1 ! EA + Db_BufSz = Db_BufSz + 1 ! EA_D Db_BufSz = Db_BufSz + 1 ! BA + Db_BufSz = Db_BufSz + 1 ! BA_D Db_BufSz = Db_BufSz + 1 ! EI Db_BufSz = Db_BufSz + 1 ! Can Db_BufSz = Db_BufSz + 1 ! Cat Db_BufSz = Db_BufSz + 1 ! Cdn Db_BufSz = Db_BufSz + 1 ! Cdt + Int_BufSz = Int_BufSz + 1 ! ElasticMod Int_BufSz = Int_BufSz + 1 ! nEApoints Db_BufSz = Db_BufSz + SIZE(InData%stiffXs) ! stiffXs Db_BufSz = Db_BufSz + SIZE(InData%stiffYs) ! stiffYs - Int_BufSz = Int_BufSz + 1 ! nBpoints + Int_BufSz = Int_BufSz + 1 ! nBApoints Db_BufSz = Db_BufSz + SIZE(InData%dampXs) ! dampXs Db_BufSz = Db_BufSz + SIZE(InData%dampYs) ! dampYs Int_BufSz = Int_BufSz + 1 ! nEIpoints @@ -1520,8 +1543,12 @@ SUBROUTINE MD_PackLineProp( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%EA Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%EA_D + Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%BA Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%BA_D + Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%EI Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%Can @@ -1532,6 +1559,8 @@ SUBROUTINE MD_PackLineProp( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%Cdt Db_Xferred = Db_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%ElasticMod + Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%nEApoints Int_Xferred = Int_Xferred + 1 DO i1 = LBOUND(InData%stiffXs,1), UBOUND(InData%stiffXs,1) @@ -1542,7 +1571,7 @@ SUBROUTINE MD_PackLineProp( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, DbKiBuf(Db_Xferred) = InData%stiffYs(i1) Db_Xferred = Db_Xferred + 1 END DO - IntKiBuf(Int_Xferred) = InData%nBpoints + IntKiBuf(Int_Xferred) = InData%nBApoints Int_Xferred = Int_Xferred + 1 DO i1 = LBOUND(InData%dampXs,1), UBOUND(InData%dampXs,1) DbKiBuf(Db_Xferred) = InData%dampXs(i1) @@ -1603,8 +1632,12 @@ SUBROUTINE MD_UnPackLineProp( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrM Db_Xferred = Db_Xferred + 1 OutData%EA = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 + OutData%EA_D = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 OutData%BA = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 + OutData%BA_D = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 OutData%EI = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 OutData%Can = DbKiBuf(Db_Xferred) @@ -1615,6 +1648,8 @@ SUBROUTINE MD_UnPackLineProp( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrM Db_Xferred = Db_Xferred + 1 OutData%Cdt = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 + OutData%ElasticMod = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 OutData%nEApoints = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 i1_l = LBOUND(OutData%stiffXs,1) @@ -1629,7 +1664,7 @@ SUBROUTINE MD_UnPackLineProp( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrM OutData%stiffYs(i1) = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 END DO - OutData%nBpoints = IntKiBuf(Int_Xferred) + OutData%nBApoints = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 i1_l = LBOUND(OutData%dampXs,1) i1_u = UBOUND(OutData%dampXs,1) @@ -4302,6 +4337,7 @@ SUBROUTINE MD_CopyLine( SrcLineData, DstLineData, CtrlCode, ErrStat, ErrMsg ) ErrMsg = "" DstLineData%IdNum = SrcLineData%IdNum DstLineData%PropsIdNum = SrcLineData%PropsIdNum + DstLineData%ElasticMod = SrcLineData%ElasticMod DstLineData%OutFlagList = SrcLineData%OutFlagList DstLineData%CtrlChan = SrcLineData%CtrlChan DstLineData%FairConnect = SrcLineData%FairConnect @@ -4313,12 +4349,23 @@ SUBROUTINE MD_CopyLine( SrcLineData, DstLineData, CtrlCode, ErrStat, ErrMsg ) DstLineData%rho = SrcLineData%rho DstLineData%d = SrcLineData%d DstLineData%EA = SrcLineData%EA - DstLineData%EI = SrcLineData%EI + DstLineData%EA_D = SrcLineData%EA_D DstLineData%BA = SrcLineData%BA + DstLineData%BA_D = SrcLineData%BA_D + DstLineData%EI = SrcLineData%EI DstLineData%Can = SrcLineData%Can DstLineData%Cat = SrcLineData%Cat DstLineData%Cdn = SrcLineData%Cdn DstLineData%Cdt = SrcLineData%Cdt + DstLineData%nEApoints = SrcLineData%nEApoints + DstLineData%stiffXs = SrcLineData%stiffXs + DstLineData%stiffYs = SrcLineData%stiffYs + DstLineData%nBApoints = SrcLineData%nBApoints + DstLineData%dampXs = SrcLineData%dampXs + DstLineData%dampYs = SrcLineData%dampYs + DstLineData%nEIpoints = SrcLineData%nEIpoints + DstLineData%bstiffXs = SrcLineData%bstiffXs + DstLineData%bstiffYs = SrcLineData%bstiffYs DstLineData%time = SrcLineData%time IF (ALLOCATED(SrcLineData%r)) THEN i1_l = LBOUND(SrcLineData%r,1) @@ -4362,6 +4409,20 @@ SUBROUTINE MD_CopyLine( SrcLineData, DstLineData, CtrlCode, ErrStat, ErrMsg ) END IF DstLineData%q = SrcLineData%q ENDIF +IF (ALLOCATED(SrcLineData%qs)) THEN + i1_l = LBOUND(SrcLineData%qs,1) + i1_u = UBOUND(SrcLineData%qs,1) + i2_l = LBOUND(SrcLineData%qs,2) + i2_u = UBOUND(SrcLineData%qs,2) + IF (.NOT. ALLOCATED(DstLineData%qs)) THEN + ALLOCATE(DstLineData%qs(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%qs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%qs = SrcLineData%qs +ENDIF IF (ALLOCATED(SrcLineData%l)) THEN i1_l = LBOUND(SrcLineData%l,1) i1_u = UBOUND(SrcLineData%l,1) @@ -4410,6 +4471,18 @@ SUBROUTINE MD_CopyLine( SrcLineData, DstLineData, CtrlCode, ErrStat, ErrMsg ) END IF DstLineData%lstrd = SrcLineData%lstrd ENDIF +IF (ALLOCATED(SrcLineData%dl_S)) THEN + i1_l = LBOUND(SrcLineData%dl_S,1) + i1_u = UBOUND(SrcLineData%dl_S,1) + IF (.NOT. ALLOCATED(DstLineData%dl_S)) THEN + ALLOCATE(DstLineData%dl_S(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%dl_S.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%dl_S = SrcLineData%dl_S +ENDIF IF (ALLOCATED(SrcLineData%V)) THEN i1_l = LBOUND(SrcLineData%V,1) i1_u = UBOUND(SrcLineData%V,1) @@ -4667,6 +4740,9 @@ SUBROUTINE MD_DestroyLine( LineData, ErrStat, ErrMsg ) IF (ALLOCATED(LineData%q)) THEN DEALLOCATE(LineData%q) ENDIF +IF (ALLOCATED(LineData%qs)) THEN + DEALLOCATE(LineData%qs) +ENDIF IF (ALLOCATED(LineData%l)) THEN DEALLOCATE(LineData%l) ENDIF @@ -4679,6 +4755,9 @@ SUBROUTINE MD_DestroyLine( LineData, ErrStat, ErrMsg ) IF (ALLOCATED(LineData%lstrd)) THEN DEALLOCATE(LineData%lstrd) ENDIF +IF (ALLOCATED(LineData%dl_S)) THEN + DEALLOCATE(LineData%dl_S) +ENDIF IF (ALLOCATED(LineData%V)) THEN DEALLOCATE(LineData%V) ENDIF @@ -4769,6 +4848,7 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_BufSz = 0 Int_BufSz = Int_BufSz + 1 ! IdNum Int_BufSz = Int_BufSz + 1 ! PropsIdNum + Int_BufSz = Int_BufSz + 1 ! ElasticMod Int_BufSz = Int_BufSz + SIZE(InData%OutFlagList) ! OutFlagList Int_BufSz = Int_BufSz + 1 ! CtrlChan Int_BufSz = Int_BufSz + 1 ! FairConnect @@ -4780,12 +4860,23 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Db_BufSz = Db_BufSz + 1 ! rho Db_BufSz = Db_BufSz + 1 ! d Db_BufSz = Db_BufSz + 1 ! EA - Db_BufSz = Db_BufSz + 1 ! EI + Db_BufSz = Db_BufSz + 1 ! EA_D Db_BufSz = Db_BufSz + 1 ! BA + Db_BufSz = Db_BufSz + 1 ! BA_D + Db_BufSz = Db_BufSz + 1 ! EI Db_BufSz = Db_BufSz + 1 ! Can Db_BufSz = Db_BufSz + 1 ! Cat Db_BufSz = Db_BufSz + 1 ! Cdn Db_BufSz = Db_BufSz + 1 ! Cdt + Int_BufSz = Int_BufSz + 1 ! nEApoints + Db_BufSz = Db_BufSz + SIZE(InData%stiffXs) ! stiffXs + Db_BufSz = Db_BufSz + SIZE(InData%stiffYs) ! stiffYs + Int_BufSz = Int_BufSz + 1 ! nBApoints + Db_BufSz = Db_BufSz + SIZE(InData%dampXs) ! dampXs + Db_BufSz = Db_BufSz + SIZE(InData%dampYs) ! dampYs + Int_BufSz = Int_BufSz + 1 ! nEIpoints + Db_BufSz = Db_BufSz + SIZE(InData%bstiffXs) ! bstiffXs + Db_BufSz = Db_BufSz + SIZE(InData%bstiffYs) ! bstiffYs Db_BufSz = Db_BufSz + 1 ! time Int_BufSz = Int_BufSz + 1 ! r allocated yes/no IF ( ALLOCATED(InData%r) ) THEN @@ -4802,6 +4893,11 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_BufSz = Int_BufSz + 2*2 ! q upper/lower bounds for each dimension Db_BufSz = Db_BufSz + SIZE(InData%q) ! q END IF + Int_BufSz = Int_BufSz + 1 ! qs allocated yes/no + IF ( ALLOCATED(InData%qs) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! qs upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%qs) ! qs + END IF Int_BufSz = Int_BufSz + 1 ! l allocated yes/no IF ( ALLOCATED(InData%l) ) THEN Int_BufSz = Int_BufSz + 2*1 ! l upper/lower bounds for each dimension @@ -4822,6 +4918,11 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_BufSz = Int_BufSz + 2*1 ! lstrd upper/lower bounds for each dimension Db_BufSz = Db_BufSz + SIZE(InData%lstrd) ! lstrd END IF + Int_BufSz = Int_BufSz + 1 ! dl_S allocated yes/no + IF ( ALLOCATED(InData%dl_S) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! dl_S upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%dl_S) ! dl_S + END IF Int_BufSz = Int_BufSz + 1 ! V allocated yes/no IF ( ALLOCATED(InData%V) ) THEN Int_BufSz = Int_BufSz + 2*1 ! V upper/lower bounds for each dimension @@ -4941,6 +5042,8 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%PropsIdNum Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%ElasticMod + Int_Xferred = Int_Xferred + 1 DO i1 = LBOUND(InData%OutFlagList,1), UBOUND(InData%OutFlagList,1) IntKiBuf(Int_Xferred) = InData%OutFlagList(i1) Int_Xferred = Int_Xferred + 1 @@ -4965,10 +5068,14 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%EA Db_Xferred = Db_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%EI + DbKiBuf(Db_Xferred) = InData%EA_D Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%BA Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%BA_D + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%EI + Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%Can Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%Cat @@ -4977,6 +5084,36 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%Cdt Db_Xferred = Db_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nEApoints + Int_Xferred = Int_Xferred + 1 + DO i1 = LBOUND(InData%stiffXs,1), UBOUND(InData%stiffXs,1) + DbKiBuf(Db_Xferred) = InData%stiffXs(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%stiffYs,1), UBOUND(InData%stiffYs,1) + DbKiBuf(Db_Xferred) = InData%stiffYs(i1) + Db_Xferred = Db_Xferred + 1 + END DO + IntKiBuf(Int_Xferred) = InData%nBApoints + Int_Xferred = Int_Xferred + 1 + DO i1 = LBOUND(InData%dampXs,1), UBOUND(InData%dampXs,1) + DbKiBuf(Db_Xferred) = InData%dampXs(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%dampYs,1), UBOUND(InData%dampYs,1) + DbKiBuf(Db_Xferred) = InData%dampYs(i1) + Db_Xferred = Db_Xferred + 1 + END DO + IntKiBuf(Int_Xferred) = InData%nEIpoints + Int_Xferred = Int_Xferred + 1 + DO i1 = LBOUND(InData%bstiffXs,1), UBOUND(InData%bstiffXs,1) + DbKiBuf(Db_Xferred) = InData%bstiffXs(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%bstiffYs,1), UBOUND(InData%bstiffYs,1) + DbKiBuf(Db_Xferred) = InData%bstiffYs(i1) + Db_Xferred = Db_Xferred + 1 + END DO DbKiBuf(Db_Xferred) = InData%time Db_Xferred = Db_Xferred + 1 IF ( .NOT. ALLOCATED(InData%r) ) THEN @@ -5039,6 +5176,26 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz END DO END DO END IF + IF ( .NOT. ALLOCATED(InData%qs) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%qs,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%qs,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%qs,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%qs,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%qs,2), UBOUND(InData%qs,2) + DO i1 = LBOUND(InData%qs,1), UBOUND(InData%qs,1) + DbKiBuf(Db_Xferred) = InData%qs(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF IF ( .NOT. ALLOCATED(InData%l) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -5099,6 +5256,21 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Db_Xferred = Db_Xferred + 1 END DO END IF + IF ( .NOT. ALLOCATED(InData%dl_S) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%dl_S,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%dl_S,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%dl_S,1), UBOUND(InData%dl_S,1) + DbKiBuf(Db_Xferred) = InData%dl_S(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF IF ( .NOT. ALLOCATED(InData%V) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -5474,6 +5646,8 @@ SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) Int_Xferred = Int_Xferred + 1 OutData%PropsIdNum = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 + OutData%ElasticMod = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 i1_l = LBOUND(OutData%OutFlagList,1) i1_u = UBOUND(OutData%OutFlagList,1) DO i1 = LBOUND(OutData%OutFlagList,1), UBOUND(OutData%OutFlagList,1) @@ -5500,10 +5674,14 @@ SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) Db_Xferred = Db_Xferred + 1 OutData%EA = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 - OutData%EI = DbKiBuf(Db_Xferred) + OutData%EA_D = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 OutData%BA = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 + OutData%BA_D = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%EI = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 OutData%Can = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 OutData%Cat = DbKiBuf(Db_Xferred) @@ -5512,6 +5690,48 @@ SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) Db_Xferred = Db_Xferred + 1 OutData%Cdt = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 + OutData%nEApoints = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + i1_l = LBOUND(OutData%stiffXs,1) + i1_u = UBOUND(OutData%stiffXs,1) + DO i1 = LBOUND(OutData%stiffXs,1), UBOUND(OutData%stiffXs,1) + OutData%stiffXs(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%stiffYs,1) + i1_u = UBOUND(OutData%stiffYs,1) + DO i1 = LBOUND(OutData%stiffYs,1), UBOUND(OutData%stiffYs,1) + OutData%stiffYs(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + OutData%nBApoints = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + i1_l = LBOUND(OutData%dampXs,1) + i1_u = UBOUND(OutData%dampXs,1) + DO i1 = LBOUND(OutData%dampXs,1), UBOUND(OutData%dampXs,1) + OutData%dampXs(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%dampYs,1) + i1_u = UBOUND(OutData%dampYs,1) + DO i1 = LBOUND(OutData%dampYs,1), UBOUND(OutData%dampYs,1) + OutData%dampYs(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + OutData%nEIpoints = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + i1_l = LBOUND(OutData%bstiffXs,1) + i1_u = UBOUND(OutData%bstiffXs,1) + DO i1 = LBOUND(OutData%bstiffXs,1), UBOUND(OutData%bstiffXs,1) + OutData%bstiffXs(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%bstiffYs,1) + i1_u = UBOUND(OutData%bstiffYs,1) + DO i1 = LBOUND(OutData%bstiffYs,1), UBOUND(OutData%bstiffYs,1) + OutData%bstiffYs(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO OutData%time = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! r not allocated @@ -5583,6 +5803,29 @@ SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) END DO END DO END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! qs not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%qs)) DEALLOCATE(OutData%qs) + ALLOCATE(OutData%qs(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%qs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%qs,2), UBOUND(OutData%qs,2) + DO i1 = LBOUND(OutData%qs,1), UBOUND(OutData%qs,1) + OutData%qs(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! l not allocated Int_Xferred = Int_Xferred + 1 ELSE @@ -5655,6 +5898,24 @@ SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) Db_Xferred = Db_Xferred + 1 END DO END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! dl_S not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%dl_S)) DEALLOCATE(OutData%dl_S) + ALLOCATE(OutData%dl_S(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%dl_S.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%dl_S,1), UBOUND(OutData%dl_S,1) + OutData%dl_S(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! V not allocated Int_Xferred = Int_Xferred + 1 ELSE From 45463920ba315c80d51ac648bd959b872c3940f1 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Mon, 4 Oct 2021 16:09:10 -0600 Subject: [PATCH 056/242] Created MDv2 driver and tweaked Rod/Body farm positions: - Rewrote the MD driver to work with MDv2. It now works for simple cases. FAST.Farm-like cases to be supported in the future. - Added a missing turbine reference position offset to Body and Rod positions. (Previously it was only there for Connections.) --- modules/moordyn/src/MoorDyn.f90 | 8 +- modules/moordyn/src/MoorDyn_Driver.f90 | 681 +++++++++++++++---------- 2 files changed, 427 insertions(+), 262 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index aedfa62f15..489aeecd29 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -931,11 +931,11 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if ((let1 == "TURBINE") .or. (let1 == "T")) then ! turbine-coupled in FAST.Farm case if (len_trim(num1) > 0) then - READ(num1, *) J ! convert to int, representing parent body index + READ(num1, *) J ! convert to int, representing turbine index if ((J <= p%nTurbines) .and. (J > 0)) then - m%ConnectList(l)%TypeNum = -1 ! -J ! typeNum < 0 indicates -turbine number <<<< NOT USED, RIGHT?? + m%ConnectList(l)%TypeNum = -1 ! set as coupled type p%nCpldCons(J) = p%nCpldCons(J) + 1 ! increment counter for the appropriate turbine m%CpldConIs(p%nCpldCons(J),J) = l print *, ' added connection ', l, ' as fairlead for turbine ', J @@ -2705,7 +2705,7 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er ! any coupled bodies (type -1) DO l = 1,p%nCpldBodies(iTurb) J = J + 1 - r6_in(1:3) = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + r6_in(1:3) = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + p%TurbineRefPos(:,iTurb) !r6_in(4:6) = EulerExtract( TRANSPOSE( u%CoupledKinematics(iTurb)%Orientation(:,:,J) ) ) r6_in(4:6) = EulerExtract( u%CoupledKinematics(iTurb)%Orientation(:,:,J) ) ! <<< changing back v6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationVel(:,J) @@ -2720,7 +2720,7 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er DO l = 1,p%nCpldRods(iTurb) J = J + 1 - r6_in(1:3) = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + r6_in(1:3) = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + p%TurbineRefPos(:,iTurb) r6_in(4:6) = MATMUL( u%CoupledKinematics(iTurb)%Orientation(:,:,J) , (/0.0, 0.0, 1.0/) ) ! <<<< CHECK ! adjustment because rod's rotational entries are a unit vector, q v6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationVel(:,J) v6_in(4:6) = u%CoupledKinematics(iTurb)%RotationVel(:,J) diff --git a/modules/moordyn/src/MoorDyn_Driver.f90 b/modules/moordyn/src/MoorDyn_Driver.f90 index aa0f380906..7fc3d95df3 100644 --- a/modules/moordyn/src/MoorDyn_Driver.f90 +++ b/modules/moordyn/src/MoorDyn_Driver.f90 @@ -26,27 +26,58 @@ PROGRAM MoorDyn_Driver IMPLICIT NONE + TYPE MD_Drvr_InitInput + LOGICAL :: Echo + REAL(DbKi) :: Gravity + REAL(DbKi) :: rhoW + REAL(DbKi) :: WtrDepth + + CHARACTER(1024) :: MDInputFile + CHARACTER(1024) :: OutRootName + REAL(DbKi) :: TMax + REAL(DbKi) :: dtC + + INTEGER :: FarmSize + REAL(DbKi) :: FarmPositions(8,40) + + INTEGER :: InputsMod + CHARACTER(1024) :: InputsFile + INTEGER :: nTurb + CHARACTER(1024) :: positions + END TYPE MD_Drvr_InitInput + + INTEGER(IntKi) :: ErrStat ! Status of error message CHARACTER(1024) :: ErrMsg ! Error message if ErrStat /= ErrID_None - TYPE (MD_InitInputType) :: MD_InitInput - TYPE (MD_ParameterType) :: MD_Parameter - TYPE (MD_ContinuousStateType) :: MD_ContinuousState - TYPE (MD_InitOutputType) :: MD_InitOutput - TYPE (MD_DiscreteStateType) :: MD_DiscreteState - TYPE (MD_ConstraintStateType) :: MD_ConstraintState - TYPE (MD_OtherStateType) :: MD_OtherState - TYPE (MD_MiscVarType) :: MD_MiscVar + INTEGER(IntKi) :: ErrStat2 ! Status of error message + CHARACTER(1024) :: ErrMsg2 ! Error message if ErrStat /= ErrID_None + + CHARACTER(1024) :: drvrFilename ! Filename and path for the driver input file. This is passed in as a command line argument when running the Driver exe. + TYPE(MD_Drvr_InitInput) :: drvrInitInp ! Initialization data for the driver program + INTEGER :: UnIn ! Unit number for the input file + INTEGER :: UnEcho ! The local unit number for this module's echo file + + + TYPE (MD_InitInputType) :: MD_InitInp + TYPE (MD_ParameterType) :: MD_p + TYPE (MD_ContinuousStateType) :: MD_x ! continuous states + TYPE (MD_InitOutputType) :: MD_InitOut + TYPE (MD_DiscreteStateType) :: MD_xd ! discrete states + TYPE (MD_ConstraintStateType) :: MD_xc ! constraint states + TYPE (MD_OtherStateType) :: MD_xo ! other states + TYPE (MD_MiscVarType) :: MD_m - TYPE (MD_InputType), ALLOCATABLE :: MD_Input(:) - REAL(DbKi), DIMENSION(:), ALLOCATABLE :: MD_InputTimes + TYPE (MD_InputType), ALLOCATABLE :: MD_u(:) + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: MD_uTimes - TYPE (MD_OutputType) :: MD_Output ! Output file identifier + TYPE (MD_OutputType) :: MD_y ! Output file identifier INTEGER(IntKi) :: UnPtfmMotIn ! platform motion input file identifier CHARACTER(100) :: Line ! String to temporarially hold value of read line REAL(ReKi), ALLOCATABLE :: PtfmMotIn(:,:) ! Variable for storing time, and DOF time series from driver input file - REAL(ReKi), ALLOCATABLE :: PtfmMot(:,:) ! Variable for storing interpolated DOF time series from driver input file + REAL(ReKi), ALLOCATABLE :: r_in(:,:) ! Variable for storing interpolated DOF time series from driver input file + REAL(ReKi), ALLOCATABLE :: rd_in(:,:) ! Variable for storing 1st derivative of interpolate DOF time series INTEGER(IntKi) :: ntIn ! number of time steps read from driver input file INTEGER(IntKi) :: ncIn ! number of channels read from driver input file INTEGER(IntKi) :: nt ! number of coupling time steps to use in simulation @@ -59,277 +90,297 @@ PROGRAM MoorDyn_Driver INTEGER(IntKi) :: MD_interp_order ! order of interpolation/extrapolation ! Local variables - Integer(IntKi) :: i ! counter for various loops - Integer(IntKi) :: j ! counter for various loops - Integer(IntKi) :: k ! counter for various loops + Integer(IntKi) :: i,j,k,l ! counter for various loops + Integer(IntKi) :: iTurb + Integer(IntKi) :: nTurbines Integer(IntKi) :: iIn integer(intKi) :: Un CHARACTER(20) :: FlagArg ! flag argument from command line - CHARACTER(1024) :: PlatformInitInputFile + !CHARACTER(1024) :: drvrInitInp%%InputsFile CHARACTER(200) :: git_commit ! String containing the current git commit hash TYPE(ProgDesc), PARAMETER :: version = ProgDesc( 'MoorDyn Driver', '', '' ) + + ErrMsg = "" + ErrStat = ErrID_None + UnEcho=-1 + UnIn =-1 + + CALL NWTC_Init( ProgNameIn=version%Name ) - MD_InitInput%FileName = "MoorDyn.dat" ! initialize to empty string to make sure it's input from the command line - CALL CheckArgs( MD_InitInput%FileName, Arg2=PlatformInitInputFile, Flag=FlagArg ) + MD_InitInp%FileName = "MoorDyn.dat" ! initialize to empty string to make sure it's input from the command line + CALL CheckArgs( MD_InitInp%FileName, Arg2=drvrInitInp%InputsFile, Flag=FlagArg ) IF ( LEN( TRIM(FlagArg) ) > 0 ) CALL NormStop() ! Display the copyright notice - CALL DispCopyrightLicense( version%Name, 'Copyright (C) 2020 Matthew Hall' ) + CALL DispCopyrightLicense( version%Name, 'Copyright (C) 2021 NREL, 2019 Matt Hall' ) ! Obtain OpenFAST git commit hash git_commit = QueryGitVersion() ! Tell our users what they're running CALL WrScr( ' Running '//TRIM( version%Name )//' a part of OpenFAST - '//TRIM(git_commit)//NewLine//' linked with '//TRIM( NWTC_Ver%Name )//NewLine ) - ! ------------------------------------------------------------------------- - ! Initialize MoorDyn - - ! ------------------------------------------------------------------------- + + + + + + ! Parse the driver input file and run the simulation based on that file + CALL get_command_argument(1, drvrFilename) + CALL ReadDriverInputFile( drvrFilename, drvrInitInp); + + ! do any initializing and allocating needed in prep for calling MD_Init + + ! set the input file name and other environment terms + !MD_InitInp%NStepWave = 1 ! an arbitrary number > 0 (to set the size of the wave data, which currently contains all zero values) + MD_InitInp%g = drvrInitInp%Gravity + MD_InitInp%rhoW = drvrInitInp%rhoW + MD_InitInp%WtrDepth = drvrInitInp%WtrDepth + MD_InitInp%FileName = drvrInitInp%MDInputFile + MD_InitInp%RootName = drvrInitInp%OutRootName + MD_InitInp%UsePrimaryInputFile = .TRUE. + !MD_InitInp%PassedPrimaryInputData = + MD_InitInp%Echo = drvrInitInp%Echo + !MD_InitInp%OutList = <<<< never used? + MD_InitInp%Linearize = .FALSE. + + TMax = drvrInitInp%TMax + dtC = drvrInitInp%dtC ! desired coupling time step size for communicating with MoorDyn + + ! do OpenFAST vs FAST.Farm related setup + + MD_InitInp%FarmSize = drvrInitInp%FarmSize + + if (drvrInitInp%FarmSize > 0) then ! Check if this MoorDyn instance is being run from FAST.Farm (indicated by FarmSize > 0) + nTurbines = drvrInitInp%FarmSize + else ! FarmSize==0 indicates normal, FAST module mode + nTurbines = 1 ! if a regular FAST module mode, we treat it like a nTurbine=1 farm case + end if + + CALL AllocAry(MD_InitInp%PtfmInit, 6, nTurbines, 'PtfmInit array' , ErrStat2, ErrMsg2); call AbortIfFailed() + CALL AllocAry(MD_InitInp%TurbineRefPos, 3, nTurbines, 'TurbineRefPos array', ErrStat2, ErrMsg2); call AbortIfFailed() + + if (drvrInitInp%FarmSize > 0) then ! if in FAST.Farm mode, specify turbine ref positions and initial positions from driver input file + do J=1,drvrInitInp%FarmSize + MD_InitInp%TurbineRefPos(1,J) = drvrInitInp%FarmPositions(1,J) + MD_InitInp%TurbineRefPos(2,J) = drvrInitInp%FarmPositions(2,J) + MD_InitInp%TurbineRefPos(3,J) = 0.0_DbKi + MD_InitInp%PtfmInit(:,J) = drvrInitInp%FarmPositions(3:8,J) + end do + else ! if in normal OpenFAST mode, zero the initial platform position since the framework doesn't allow much else + MD_InitInp%PtfmInit = 0.0_DbKi + MD_InitInp%TurbineRefPos = 0.0_DbKi + end if - dtC = 0.01 ! desired coupling time step size for communicating with MoorDyn MD_interp_order = 0 - ! MAP: allocate Input and Output arrays; used for interpolation and extrapolation - Allocate(MD_InputTimes(MD_interp_order + 1)) + ! allocate Input and Output arrays; used for interpolation and extrapolation + Allocate(MD_uTimes(MD_interp_order + 1)) ! @bonnie : This is in the FAST developers glue code example, but it's probably not needed here. - Allocate(MD_Input(MD_interp_order + 1)) + Allocate(MD_u(MD_interp_order + 1)) - ! set the input file name and other environment terms. - !MD_InitInput%NStepWave = 1 ! an arbitrary number > 0 (to set the size of the wave data, which currently contains all zero values) - MD_InitInput%g = 9.81 ! This need to be according to g used in ElastoDyn - MD_InitInput%rhoW = 1025 ! This needs to be set according to seawater density in HydroDyn - MD_InitInput%PtfmInit = 0.0 - MD_InitInput%RootName = "MoorDyn.MD" + + if (drvrInitInp%InputsMod > 1) then + ErrStat2 = ErrID_Fatal + ErrMsg2 = ' ERROR: MoorDyn Driver InputsMod must be 0 or 1.' + CALL AbortIfFailed() + end if + + + ! -------------------------------- ----------------------------------- ! fill in the hydrodynamics data - ALLOCATE( MD_InitInput%WaveVel (2,200,3)) - ALLOCATE( MD_InitInput%WaveAcc (2,200,3)) - ALLOCATE( MD_InitInput%WavePDyn(2,200) ) - ALLOCATE( MD_InitInput%WaveElev(2,200) ) - ALLOCATE( MD_InitInput%WaveTime(2) ) - MD_InitInput%WaveVel = 0.0_ReKi - MD_InitInput%WaveAcc = 0.0_ReKi - MD_InitInput%WavePDyn = 0.0_ReKi - MD_InitInput%WaveElev = 0.0_ReKi - MD_InitInput%WaveTime = 0.0_ReKi - DO I = 1,SIZE(MD_InitInput%WaveTime) - MD_InitInput%WaveTime(I) = 600.0*I + ALLOCATE( MD_InitInp%WaveVel (2,200,3)) + ALLOCATE( MD_InitInp%WaveAcc (2,200,3)) + ALLOCATE( MD_InitInp%WavePDyn(2,200) ) + ALLOCATE( MD_InitInp%WaveElev(2,200) ) + ALLOCATE( MD_InitInp%WaveTime(2) ) + MD_InitInp%WaveVel = 0.0_ReKi + MD_InitInp%WaveAcc = 0.0_ReKi + MD_InitInp%WavePDyn = 0.0_ReKi + MD_InitInp%WaveElev = 0.0_ReKi + MD_InitInp%WaveTime = 0.0_ReKi + DO I = 1,SIZE(MD_InitInp%WaveTime) + MD_InitInp%WaveTime(I) = 600.0*I END DO + ! open driver output file >>> not yet used <<< CALL GetNewUnit( Un ) OPEN(Unit=Un,FILE='MD.out',STATUS='UNKNOWN') ! call the initialization routine - CALL MD_Init( MD_InitInput , & - MD_Input(1) , & - MD_Parameter , & - MD_ContinuousState , & - MD_DiscreteState , & - MD_ConstraintState , & - MD_OtherState , & - MD_Output , & - MD_MiscVar , & - dtC , & - MD_InitOutput , & - ErrStat , & - ErrMsg ) - IF ( ErrStat .NE. ErrID_None ) THEN - IF (ErrStat >=AbortErrLev) CALL ProgAbort(ErrMsg) - CALL WrScr( ErrMsg ) - END IF + CALL MD_Init( MD_InitInp, MD_u(1), MD_p, MD_x , MD_xd, MD_xc, MD_xo, MD_y, MD_m, dtC, MD_InitOut, ErrStat2, ErrMsg2 ); call AbortIfFailed() - CALL MD_DestroyInitInput ( MD_InitInput , ErrStat, ErrMsg ) - CALL MD_DestroyInitOutput ( MD_InitOutput , ErrStat, ErrMsg ) + CALL MD_DestroyInitInput ( MD_InitInp , ErrStat2, ErrMsg2 ); call AbortIfFailed() + CALL MD_DestroyInitOutput ( MD_InitOut , ErrStat2, ErrMsg2 ); call AbortIfFailed() - CALL DispNVD( MD_InitOutput%Ver ) + CALL DispNVD( MD_InitOut%Ver ) - ncIn = 6 + size(MD_Input(1)%DeltaL) ! determine number of input channels expected from driver input file time series (DOFs including active tensioning channels) + ! determine number of input channels expected from driver input file time series (DOFs including active tensioning channels) + ncIn = size(MD_u(1)%DeltaL) - ! ------------------------------------------------------------------------- - ! Read in prescribed motions from text file if available - ! (single 6DOF platform for now, plus one active tensioning command) - ! (to be updated for versatile coupling in future) - ! ------------------------------------------------------------------------- - IF( LEN( TRIM(PlatformInitInputFile) ) < 1 ) THEN - ntIn = 0 ! flag to indicate no motion input file - print *, "No MoorDyn Driver input file provided, so using zero values." + do iTurb = 1, MD_p%nTurbines + ncIn = ncIn + MD_p%nCpldBodies(iTurb)*6 + MD_p%nCpldRods(iTurb)*6 + MD_p%nCpldCons(iTurb)*3 + end do + + print *, 'MoorDyn has '//trim(num2lstr(ncIn))//' coupled DOFs and/or active-tensioned inputs.' - ELSE - CALL GetNewUnit( UnPtfmMotIn ) - CALL OpenFInpFile ( UnPtfmMotIn, PlatformInitInputFile, ErrStat, ErrMsg ) - IF (ErrStat /= 0 ) THEN - print *, ErrStat, ErrMsg - STOP - ENDIF - print *, "Reading platform motion input data from ", PlatformInitInputFile + if (drvrInitInp%InputsMod == 1 ) then + + if ( LEN( TRIM(drvrInitInp%InputsFile) ) < 1 ) then + ErrStat2 = ErrID_Fatal + ErrMsg2 = ' ERROR: MoorDyn Driver InputFile cannot be empty if InputsMode is 2.' + CALL AbortIfFailed() + end if + + CALL GetNewUnit( UnPtfmMotIn ) + + CALL OpenFInpFile ( UnPtfmMotIn, drvrInitInp%InputsFile, ErrStat2, ErrMsg2 ); call AbortIfFailed() + + print *, 'Reading platform motion input data from ', trim(drvrInitInp%InputsFile) + print *, 'MD driver is expecting '//trim(num2lstr(ncIn))//' columns of input data, plus time, in motion input file.' ! Read through length of file to find its length i = 1 ! start counter DO - READ(UnPtfmMotIn,'(A)',IOSTAT=ErrStat) Line !read into a line - - - IF (ErrStat /= 0) EXIT + READ(UnPtfmMotIn,'(A)',IOSTAT=ErrStat2) Line !read into a line + IF (ErrStat2 /= 0) EXIT ! break out of the loop if it couldn't read the line (i.e. if at end of file) print *, TRIM(Line) - i = i+1 END DO ! rewind to start of input file to re-read things now that we know how long it is REWIND(UnPtfmMotIn) - ntIn = i-1 ! save number of lines of file + ntIn = i-3 ! save number of lines of file ! allocate space for input motion array (including time column) - ALLOCATE ( PtfmMotIn(ntIn, ncIn+1), STAT = ErrStat ) + ALLOCATE ( PtfmMotIn(ntIn, ncIn+1), STAT=ErrStat2) IF ( ErrStat /= ErrID_None ) THEN - ErrMsg = ' Error allocating space for PtfmMotIn array.' - CALL WrScr( ErrMsg ) - END IF + ErrStat2 = ErrID_Fatal + ErrMsg2 = ' Error allocating space for PtfmMotIn array.' + call AbortIfFailed() + END IF ! read the data in from the file + READ(UnPtfmMotIn,'(A)',IOSTAT=ErrStat2) Line !read into a line + READ(UnPtfmMotIn,'(A)',IOSTAT=ErrStat2) Line !read into a line + DO i = 1, ntIn - READ (UnPtfmMotIn,*,IOSTAT=ErrStat) (PtfmMotIn (i,J), J=1,ncIn+1) + READ (UnPtfmMotIn, *, IOSTAT=ErrStat2) (PtfmMotIn (i,J), J=1,ncIn+1) - IF ( ErrStat /= 0 ) THEN - ErrMsg = ' Error reading the input time-series file. Expecting '//TRIM(Int2LStr(ncIn))//' channels plus time.' - CALL WrScr( ErrMsg ) + IF ( ErrStat2 /= 0 ) THEN + ErrStat2 = ErrID_Fatal + ErrMsg2 = ' Error reading the input time-series file. Expecting '//TRIM(Int2LStr(ncIn))//' channels plus time.' + call AbortIfFailed() END IF END DO - ! Close the inputs file + ! Close the inputs file CLOSE ( UnPtfmMotIn ) print *, "Read ", ntIn, " time steps from input file." print *, PtfmMotIn - - - END IF + ! trim simulation duration to length of input file if needed + if (PtfmMotIn(ntIn, 1) < TMax) then + TMax = PtfmMotIn(ntIn, 1) + end if + - ! ----------------------- specify stepping details ----------------------- - - + ! specify stepping details + nt = tMax/dtC - 1 ! number of coupling time steps - IF (ntIn > 0) THEN - tMax = PtfmMotIn(ntIn, 1) ! save last time step as total sim time - ELSE - tMax = 60 - END IF - + + ! allocate space for processed motion array + ALLOCATE ( r_in(nt, ncIn), rd_in(nt, ncIn), STAT=ErrStat2) + IF ( ErrStat2 /= ErrID_None ) THEN + ErrStat2 = ErrID_Fatal + ErrMsg2 = ' Error allocating space for r_in or rd_in array.' + call AbortIfFailed() + END IF - nt = tMax/dtC - 1 ! number of coupling time steps - CALL WrScr(" ") - print *, "Tmax - ", tMax, " and nt=", nt - CALL WrScr(" ") + ! go through and interpolate inputs to new regular time steps (if nt=0 this array should be left as zeros) - ! allocate space for processed motion array - ALLOCATE ( PtfmMot(nt, ncIn), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN - ErrMsg = ' Error allocating space for PtfmMot array.' - CALL WrScr( ErrMsg ) - END IF - - - ! go through and interpolate inputs to new regular time steps (if nt=0 this array should be left as zeros) - IF (ntIn > 0) THEN - DO i = 1,nt - + DO i = 1,nt t = dtC*(i-1) - ! interpolation routine + ! interpolation routine DO iIn = 1,ntIn-1 - IF (PtfmMotIn(iIn+1, 1) > t) THEN - frac = (t - PtfmMotIn(iIn, 1) )/( PtfmMotIn(iIn+1, 1) - PtfmMotIn(iIn, 1) ) - - ! print *, "t=", t, ", iIn=", iIn, ", frac=", frac + IF (PtfmMotIn(iIn+1, 1) > t) THEN ! find the right two points to interpolate between (remember that the first column of PtfmMotIn is time) + frac = (t - PtfmMotIn(iIn, 1) )/( PtfmMotIn(iIn+1, 1) - PtfmMotIn(iIn, 1) ) ! interpolation fraction (0-1) between two interpolation points DO J=1,ncIn - PtfmMot(i, J) = PtfmMotIn(iIn, J+1) + frac*(PtfmMotIn(iIn+1, J+1) - PtfmMotIn(iIn, J+1)) + ! get interpolated position of coupling point + r_in(i, J) = PtfmMotIn(iIn, J+1) + frac*(PtfmMotIn(iIn+1, J+1) - PtfmMotIn(iIn, J+1)) + + ! use forward different to estimate velocity of coupling point + rd_in(i, J) = (PtfmMotIn(iIn+1, J+1) - PtfmMotIn(iIn, J+1)) / (PtfmMotIn(iIn+1, 1) - PtfmMotIn(iIn, 1)) END DO - EXIT + EXIT ! break out of the loop for this time step once we've done its interpolation END IF END DO - ! print *, t, "s", PtfmMot(i,:) - END DO + ! InputsMod == 1 - - ELSE - PtfmMot = 0.0_Reki - END IF - + else + nt = tMax/dtC - 1 ! number of coupling time steps + end if + + CALL WrScr(" ") + print *, "Tmax - ", tMax, " and nt=", nt + CALL WrScr(" ") ! --------------------------------------------------------------- ! Set the initial input values ! --------------------------------------------------------------- - ! zero the displacements to start with - MD_Input(1)%PtFairleadDisplacement%TranslationDisp = 0.0_ReKi + ! zero the displacements to start with <<< should this be based on what came out of MD_Init instead? <<< + MD_u(1)%CoupledKinematics(1)%TranslationDisp = 0.0_ReKi ! zero the tension commands - MD_Input(1)%DeltaL = 0.0_ReKi - MD_Input(1)%DeltaLdot = 0.0_ReKi + MD_u(1)%DeltaL = 0.0_ReKi + MD_u(1)%DeltaLdot = 0.0_ReKi ! ! zero water inputs (if passing wave info in from glue code) -! MD_Input(1)%U = 0.0 -! MD_Input(1)%Ud = 0.0 -! MD_Input(1)%zeta = 0.0 -! MD_Input(1)%PDyn = 0.0 +! MD_u(1)%U = 0.0 +! MD_u(1)%Ud = 0.0 +! MD_u(1)%zeta = 0.0 +! MD_u(1)%PDyn = 0.0 ! ! now add some current in x for testing -! MD_Input(1)%U(1,:) = 1.0 +! MD_u(1)%U(1,:) = 1.0 - - - DO i = 2, MD_interp_order + 1 - CALL MD_CopyInput( MD_Input(1), MD_Input(i), MESH_NEWCOPY, ErrStat, ErrMsg ) + CALL MD_CopyInput( MD_u(1), MD_u(i), MESH_NEWCOPY, ErrStat2, ErrMsg2 ); call AbortIfFailed() END DO DO i = 1, MD_interp_order + 1 - MD_InputTimes(i) = -(i - 1) * dtC + MD_uTimes(i) = -(i - 1) * dtC ENDDO - + ! get output at initialization (before time stepping) t = 0 + CALL MD_CalcOutput( t, MD_u(1), MD_p, MD_x, MD_xd, MD_xc , MD_xo, MD_y, MD_m, ErrStat2, ErrMsg2 ); call AbortIfFailed() - CALL MD_CalcOutput( t , & - MD_Input(1) , & - MD_Parameter , & - MD_ContinuousState , & - MD_DiscreteState , & - MD_ConstraintState , & - MD_OtherState , & - MD_Output , & - MD_MiscVar , & - ErrStat , & - ErrMsg ) - IF ( ErrStat .NE. ErrID_None ) THEN - IF (ErrStat >=AbortErrLev) CALL ProgAbort(ErrMsg) - CALL WrScr( ErrMsg ) - END IF ! ------------------------------------------------------------------------- - ! BEGIN time marching >>> note that 3 rotational platform DOFs are currently neglected <<< + ! BEGIN time marching ! ------------------------------------------------------------------------- - ! TODO: add rotational DOFs, update coupling points, add filtering, and add velocity and acceleration <<<< - print *,"Doing time marching now..." DO i = 1,nt @@ -339,84 +390,88 @@ PROGRAM MoorDyn_Driver t = dtC*(i-1) print *, t - - MD_InputTimes(1) = t + dtC - !MD_InputTimes(2) = MD_InputTimes(1) - dtC - !MD_InputTimes(3) = MD_InputTimes(2) - dtC - - ! apply platform translations (neglecting rotations for now) - DO J = 1,MD_Parameter%nCpldCons - MD_Input(1)%PtFairleadDisplacement%TranslationDisp(1,J) = PtfmMot(i, 1) - MD_Input(1)%PtFairleadDisplacement%TranslationDisp(2,J) = PtfmMot(i, 2) - MD_Input(1)%PtFairleadDisplacement%TranslationDisp(3,J) = PtfmMot(i, 3) - END DO - - !MD_Input(1)%PtFairleadDisplacement%TranslationDisp(1,1) = PtfmMot(i, 1) - !MD_Input(1)%PtFairleadDisplacement%TranslationDisp(1,2) = PtfmMot(i, 2) - !MD_Input(1)%PtFairleadDisplacement%TranslationDisp(1,3) = PtfmMot(i, 3) + + MD_uTimes(1) = t + dtC + !MD_uTimes(2) = MD_uTimes(1) - dtC + !MD_uTimes(3) = MD_uTimes(2) - dtC + + ! update coupled object kinematics iff we're reading input time series + if (drvrInitInp%InputsMod == 1 ) then + + DO iTurb = 1, MD_p%nTurbines + + J = 1 ! J is the index of the coupling points in the input mesh CoupledKinematics + ! any coupled bodies (type -1) + DO l = 1,MD_p%nCpldBodies(iTurb) + MD_u(1)%CoupledKinematics(iTurb)%TranslationDisp(:,J) = r_in(i, J:J+2) + MD_u(1)%CoupledKinematics(iTurb)%Orientation( :,:,J) = TRANSPOSE( EulerConstruct( r_in(i, J+3:J+5) ) ) ! full Euler angle approach <<<< need to check order + MD_u(1)%CoupledKinematics(iTurb)%TranslationVel( :,J) = rd_in(i, J:J+2) + MD_u(1)%CoupledKinematics(iTurb)%RotationVel( :,J) = rd_in(i, J+3:J+5) + !a6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationAcc(:,J) + !a6_in(4:6) = u%CoupledKinematics(iTurb)%RotationAcc(:,J) + + J = J + 6 + END DO + + ! any coupled rods (type -1 or -2) >>> need to make rotations ignored if it's a pinned rod <<< + DO l = 1,MD_p%nCpldRods(iTurb) + + MD_u(1)%CoupledKinematics(iTurb)%TranslationDisp(:,J) = r_in(i, J:J+2) + MD_u(1)%CoupledKinematics(iTurb)%Orientation( :,:,J) = TRANSPOSE( EulerConstruct( r_in(i, J+3:J+5) ) ) + MD_u(1)%CoupledKinematics(iTurb)%TranslationVel( :,J) = rd_in(i, J:J+2) + MD_u(1)%CoupledKinematics(iTurb)%RotationVel( :,J) = rd_in(i, J+3:J+5) + + J = J + 6 + END DO + + ! any coupled points (type -1) + DO l = 1, MD_p%nCpldCons(iTurb) + + MD_u(1)%CoupledKinematics(iTurb)%TranslationDisp(:,J) = r_in(i, J:J+2) + MD_u(1)%CoupledKinematics(iTurb)%TranslationVel( :,J) = rd_in(i, J:J+2) + + !u%CoupledKinematics(iTurb)%TranslationVel(:,J) + !u%CoupledKinematics(iTurb)%TranslationAcc(:,J) + + !print *, u%PtFairleadDisplacement%Position(:,l) + u%PtFairleadDisplacement%TranslationDisp(:,l) + !print *, u%PtFairleadDisplacement%TranslationVel(:,l) + + J = J + 3 + END DO + + end do ! iTurb + + ! also provide any active tensioning commands + do l = 1, size(MD_u(1)%DeltaL) + + MD_u(1)%DeltaL( j) = r_in(i, J) + MD_u(1)%DeltaLdot(j) = rd_in(i, J) - !MD_Input(2)%PtFairleadDisplacement%TranslationDisp(1,1) = .001*n_t_global - !MD_Input(3)%PtFairleadDisplacement%TranslationDisp(1,1) = .001*n_t_global + J = J + 1 + end do - ! what about velocities?? + end if ! InputsMod == 1 - ! also provide any active tensioning commands (just using delta L, and finite differencing to get derivative) - DO j = 1,ncIn-6 + ! >>> otherwise, mesh kinematics should all still be zero ... maybe worth checking <<< - MD_Input(1)%DeltaL(j) = PtfmMot(i, 6+j) - - IF (i>1) then - MD_Input(1)%DeltaLdot(j) = (PtfmMot(i, 6+j) - PtfmMot(i-1, 6+j))/dtC - ELSE - MD_Input(1)%DeltaLdot(j) = 0.0_ReKi - END IF - - END DO ! --------------------------------- update states --------------------------------- - CALL MD_UpdateStates( t , & - nt , & - MD_Input , & - MD_InputTimes , & - MD_Parameter , & - MD_ContinuousState , & - MD_DiscreteState , & - MD_ConstraintState , & - MD_OtherState , & - MD_MiscVar , & - ErrStat , & - ErrMsg ) - IF ( ErrStat .NE. ErrID_None ) THEN - IF (ErrStat >=AbortErrLev) CALL ProgAbort(ErrMsg) - CALL WrScr( ErrMsg ) - EXIT - END IF + CALL MD_UpdateStates( t, nt, MD_u, MD_uTimes, MD_p, MD_x, MD_xd, MD_xc, MD_xo, MD_m, ErrStat2, ErrMsg2 ); call AbortIfFailed() + ! update the global time step by one delta t <<<< ??? why? t = t + dtC ! --------------------------------- calculate outputs --------------------------------- - CALL MD_CalcOutput( t , & - MD_Input(1) , & - MD_Parameter , & - MD_ContinuousState , & - MD_DiscreteState , & - MD_ConstraintState , & - MD_OtherState , & - MD_Output , & - MD_MiscVar , & - ErrStat , & - ErrMsg ) - IF ( ErrStat .NE. ErrID_None ) THEN - IF (ErrStat >=AbortErrLev) CALL ProgAbort(ErrMsg) - CALL WrScr( ErrMsg ) - END IF - - - WRITE(Un,100) t, MD_Input(1)%PtFairleadDisplacement%TranslationDisp(1,1), & - ((MD_Output%PtFairleadLoad%Force(k,j), k=1,3),j=1,3) + CALL MD_CalcOutput( t, MD_u(1), MD_p, MD_x, MD_xd, MD_xc, MD_xo, MD_y, MD_m, ErrStat2, ErrMsg2 ); call AbortIfFailed() + + + ! >>> should make output vector to hold and print outputs <<< + !WRITE(Un, *) t, MD_u(1)%CoupledKinematics(1)%TranslationDisp(1,1), ((MD_y%CoupledLoads(1)%Force(k,j), k=1,3),j=1,3) !WRITE(*,*) t_global + ! FORMAT(2(1X,F8.3),9(1X,E12.5)) + END DO @@ -425,34 +480,144 @@ PROGRAM MoorDyn_Driver ! ------------------------------------------------------------------------- ! Destroy all objects - CALL MD_End( MD_Input(1) , & - MD_Parameter , & - MD_ContinuousState , & - MD_DiscreteState , & - MD_ConstraintState , & - MD_OtherState , & - MD_Output , & - MD_MiscVar , & - ErrStat , & - ErrMsg ) - IF ( ErrStat .NE. ErrID_None ) THEN - IF (ErrStat >=AbortErrLev) CALL ProgAbort(ErrMsg) - CALL WrScr( ErrMsg ) - END IF + CALL MD_End( MD_u(1), MD_p, MD_x, MD_xd, MD_xc , MD_xo, MD_y, MD_m, ErrStat2, ErrMsg2 ); call AbortIfFailed() do j = 2,MD_interp_order+1 - call MD_DestroyInput( MD_Input(j), ErrStat, ErrMsg) + call MD_DestroyInput( MD_u(j), ErrStat, ErrMsg) end do - DEALLOCATE(MD_Input) - DEALLOCATE(MD_InputTimes) + DEALLOCATE(MD_u) + DEALLOCATE(MD_uTimes) - IF (ALLOCATED(PtfmMot) ) DEALLOCATE(PtfmMot ) + IF (ALLOCATED(r_in) ) DEALLOCATE(r_in ) IF (ALLOCATED(PtfmMotIn)) DEALLOCATE(PtfmMotIn) CALL WrScr( "Program has ended" ) close (un) -100 FORMAT(2(1X,F8.3),9(1X,E12.5)) - - END PROGRAM + +CONTAINS + + SUBROUTINE AbortIfFailed() + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'MoorDyn_Driver') + IF ( ErrStat /= ErrID_None ) THEN + CALL WrScr( ErrMsg2 ) + CALL WrScr( 'hi1') + CALL WrScr( ErrMsg ) + CALL WrScr( 'hi1') + END IF + if (ErrStat >= AbortErrLev) then + call CleanUp() + STOP + endif + END SUBROUTINE AbortIfFailed + + LOGICAL FUNCTION Failed() + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'OutSummary') + Failed = ErrStat >= AbortErrLev + if (Failed) call CleanUp() + END FUNCTION Failed + + SUBROUTINE CleanUp() + if(UnEcho>0) CLOSE(UnEcho) + if(UnEcho>0) CLOSE( UnIn) + if(allocated(MD_u)) deallocate(MD_u) + END SUBROUTINE CleanUp + + !------------------------------------------------------------------------------------------------------------------------------- + SUBROUTINE ReadDriverInputFile( inputFile, InitInp) + CHARACTER(*), INTENT( IN ) :: inputFile + TYPE(MD_Drvr_InitInput), INTENT( OUT ) :: InitInp + ! Local variables + INTEGER :: I ! generic integer for counting + INTEGER :: J ! generic integer for counting + CHARACTER( 2) :: strI ! string version of the loop counter + + CHARACTER(1024) :: EchoFile ! Name of MoorDyn echo file + CHARACTER(1024) :: Line ! String to temporarially hold value of read line + CHARACTER(1024) :: TmpPath ! Temporary storage for relative path name + CHARACTER(1024) :: TmpFmt ! Temporary storage for format statement + CHARACTER(1024) :: FileName ! Name of MoorDyn input file + CHARACTER(1024) :: FilePath ! Path Name of MoorDyn input file + + UnEcho=-1 + UnIn =-1 + + FileName = TRIM(inputFile) + + CALL GetNewUnit( UnIn ) + CALL OpenFInpFile( UnIn, FileName, ErrStat2, ErrMsg2); + call AbortIfFailed() + + CALL WrScr( 'Opening MoorDyn Driver input file: '//FileName ) + + ! Read until "echo" + CALL ReadCom( UnIn, FileName, 'MoorDyn Driver input file header line 1', ErrStat2, ErrMsg2); call AbortIfFailed() + CALL ReadCom( UnIn, FileName, 'MoorDyn Driver input file header line 2', ErrStat2, ErrMsg2); call AbortIfFailed() + CALL ReadVar ( UnIn, FileName, InitInp%Echo, 'Echo', 'Echo Input', ErrStat2, ErrMsg2); call AbortIfFailed() + ! If we echo, we rewind + IF ( InitInp%Echo ) THEN + EchoFile = TRIM(FileName)//'.echo' + CALL GetNewUnit( UnEcho ) + CALL OpenEcho ( UnEcho, EchoFile, ErrStat, ErrMsg ); call AbortIfFailed() + REWIND(UnIn) + CALL ReadCom( UnIn, FileName, 'MoorDyn Driver input file header line 1', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + CALL ReadCom( UnIn, FileName, 'MoorDyn Driver input file header line 2', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + CALL ReadVar ( UnIn, FileName, InitInp%Echo, 'Echo', 'Echo the input file data', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + END IF + !---------------------- ENVIRONMENTAL CONDITIONS ------------------------------------------------- + CALL ReadCom( UnIn, FileName, 'Environmental conditions header', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + CALL ReadVar( UnIn, FileName, InitInp%Gravity, 'Gravity', 'Gravity', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + CALL ReadVar( UnIn, FileName, InitInp%rhoW , 'rhoW', 'water density', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + CALL ReadVar( UnIn, FileName, InitInp%WtrDepth, 'WtrDepth', 'water depth', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + !---------------------- MoorDyn ------------------------------------------------------------------- + CALL ReadCom( UnIn, FileName, 'MoorDyn header', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + CALL ReadVar( UnIn, FileName, InitInp%MDInputFile, 'MDInputFile', 'MoorDyn input filename', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + CALL ReadVar( UnIn, FileName, InitInp%OutRootName, 'OutRootName', 'MoorDyn output root filename', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + CALL ReadVar( UnIn, FileName, InitInp%TMax , 'Tmax', 'Simulation time duration', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + CALL ReadVar( UnIn, FileName, InitInp%dtC , 'dtC', 'Time step size for calling MoorDyn', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + ! ---------------------- FAST.Farm ---------------------------------------------------------------- + CALL ReadCom( UnIn, FileName, 'FAST.Farm header', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + CALL ReadVar( UnIn, FileName, InitInp%FarmSize , 'NumTurbines', 'number of turbines in FAST.Farm', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + CALL ReadCom( UnIn, FileName, 'FAST.Farm table header line 1', ErrStat2, ErrMsg2); call AbortIfFailed() + CALL ReadCom( UnIn, FileName, 'FAST.Farm table header line 2', ErrStat2, ErrMsg2); call AbortIfFailed() + do J=1,InitInp%FarmSize + CALL ReadAry( UnIn, FileName, InitInp%FarmPositions(:,J), 8, "FarmPositions", "FAST.Farm position inputs", ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + end do + !---------------------- INPUTS ------------------------------------------------------------------- + CALL ReadCom( UnIn, FileName, 'INPUTS header', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + CALL ReadVar( UnIn, FileName, InitInp%InputsMod , 'InputsMod', 'Mode for the inputs - zero/steady/time-series', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + CALL ReadVar( UnIn, FileName, InitInp%InputsFile, 'InputsFile', 'Filename for the MoorDyn inputs', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + CALL ReadCom( UnIn, FileName, 'Header line saying next line will be a list of coupled positions' , ErrStat2, ErrMsg2); call AbortIfFailed() + CALL ReadVar( UnIn, FileName, InitInp%positions, 'positions', 'List of positions when InputsMod=1', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + + ! done reading + if(UnEcho>0) CLOSE( UnEcho ) + if(UnIn>0) CLOSE( UnIn ) + + ! Perform input checks and triggers + !CALL GetPath( FileName, FilePath ) + !IF ( PathIsRelative( InitInp%MDInputFile ) ) then + ! InitInp%MDInputFile = TRIM(FilePath)//TRIM(InitInp%MDInputFile) + !END IF + !IF ( PathIsRelative( InitInp%OutRootName ) ) then + ! InitInp%OutRootName = TRIM(FilePath)//TRIM(InitInp%OutRootName) + !endif + !IF ( PathIsRelative( InitInp%InputsFile ) ) then + ! InitInp%InputsFile = TRIM(FilePath)//TRIM(InitInp%InputsFile) + !endif + + END SUBROUTINE ReadDriverInputFile + + subroutine print_help() + print '(a)', 'usage: ' + print '(a)', '' + print '(a)', 'MoorDynDriver.exe driverfilename' + print '(a)', '' + print '(a)', 'Where driverfilename is the name of the MoorDyn driver input file.' + print '(a)', '' + end subroutine print_help +!---------------------------------------------------------------------------------------------------------------------------------- + + +END PROGRAM From c505683354580df0db7aef0b3889fed1a2d89f63 Mon Sep 17 00:00:00 2001 From: Stein Date: Wed, 6 Oct 2021 14:55:31 -0600 Subject: [PATCH 057/242] Fixed inputString/OptValue character length issue that was causing long-named bathymetry files not to work. Also added an error statement call --- modules/moordyn/src/MoorDyn.f90 | 2 +- modules/moordyn/src/MoorDyn_IO.f90 | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index a158f79e52..73115f5cb1 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -106,7 +106,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CHARACTER(1024) :: Line ! String to temporarially hold value of read line CHARACTER(20) :: LineOutString ! String to temporarially hold characters specifying line output options CHARACTER(20) :: OptString ! String to temporarially hold name of option variable - CHARACTER(20) :: OptValue ! String to temporarially hold value of options variable input + CHARACTER(40) :: OptValue ! String to temporarially hold value of options variable input INTEGER(IntKi) :: nOpts ! number of options lines in input file CHARACTER(40) :: TempString1 ! CHARACTER(40) :: TempString2 ! diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index 183a808d77..fc1fb1e385 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -114,7 +114,7 @@ MODULE MoorDyn_IO SUBROUTINE getBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, ErrStat3, ErrMsg3) ! SUBROUTINE getBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, BathGrid_npoints, ErrStat3, ErrMsg3) - CHARACTER(20), INTENT(IN ) :: inputString + CHARACTER(40), INTENT(IN ) :: inputString REAL(DbKi), ALLOCATABLE, INTENT(INOUT) :: BathGrid (:,:) REAL(DbKi), ALLOCATABLE, INTENT(INOUT) :: BathGrid_Xs (:) REAL(DbKi), ALLOCATABLE, INTENT(INOUT) :: BathGrid_Ys (:) @@ -153,6 +153,7 @@ SUBROUTINE getBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, ErrSta ! load lookup table data from file CALL GetNewUnit( UnCoef ) ! unit number for coefficient input file CALL OpenFInpFile( UnCoef, TRIM(inputString), ErrStat4, ErrMsg4 ) + cALL SetErrStat(ErrStat4, ErrMsg4, ErrStat3, ErrMsg3, 'MDIO_getBathymetry') READ(UnCoef,'(A)',IOSTAT=ErrStat4) Line2 ! skip the first title line READ(UnCoef,*,IOSTAT=ErrStat4) nGridX_string, nGridX ! read in the second line as the number of x values in the BathGrid From 4654dc1255552fa5fe8bc17d6e5710ddb60cde06 Mon Sep 17 00:00:00 2001 From: Stein Date: Wed, 6 Oct 2021 14:56:20 -0600 Subject: [PATCH 058/242] Adding .vs folders to git ignore --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index 3827d6e222..2c10260ef8 100644 --- a/.gitignore +++ b/.gitignore @@ -49,3 +49,6 @@ vs-build/ *.aux *.nlo *.log + +# .vs folders +.vs From 45d4d9e6b56493f70063a1a536849645dd50c1bb Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Thu, 7 Oct 2021 16:27:56 -0600 Subject: [PATCH 059/242] Added .fstf input validation and MD driver improvements: - Added validation for new FAST.Farm input file entries for new shared moorings and array-wide wave kinematics features. - Improvements to MD driver including less screen output and correction of rotation handling. --- glue-codes/fast-farm/src/FAST_Farm_Subs.f90 | 20 +++++++++++- modules/moordyn/src/MoorDyn_Driver.f90 | 34 ++++++++++++++++----- 2 files changed, 46 insertions(+), 8 deletions(-) diff --git a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 index 74d28b12ad..6ce10a896c 100644 --- a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 +++ b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 @@ -1421,6 +1421,12 @@ SUBROUTINE Farm_ValidateInput( p, WD_InitInp, AWAE_InitInp, SC_InitInp, ErrStat, ErrStat = ErrID_None ErrMsg = "" + + ! --- SIMULATION CONTROL --- + IF ((p%WaveFieldMod .ne. 1) .and. (p%WaveFieldMod .ne. 2)) CALL SetErrStat(ErrID_Fatal,'WaveFieldMod must be 1 or 2.',ErrStat,ErrMsg,RoutineName) + IF ((p%MooringMod .ne. 0) .and. (p%MooringMod .ne. 3)) CALL SetErrStat(ErrID_Fatal,'MooringMod must be 0 or 3.',ErrStat,ErrMsg,RoutineName) + + IF (p%DT_low <= 0.0_ReKi) CALL SetErrStat(ErrID_Fatal,'DT_low must be positive.',ErrStat,ErrMsg,RoutineName) IF (p%DT_high <= 0.0_ReKi) CALL SetErrStat(ErrID_Fatal,'DT_high must be positive.',ErrStat,ErrMsg,RoutineName) IF (p%TMax < 0.0_ReKi) CALL SetErrStat(ErrID_Fatal,'TMax must not be negative.',ErrStat,ErrMsg,RoutineName) @@ -1429,7 +1435,10 @@ SUBROUTINE Farm_ValidateInput( p, WD_InitInp, AWAE_InitInp, SC_InitInp, ErrStat, ! --- SUPER CONTROLLER --- ! TODO : Verify that the DLL file exists - + ! --- SHARED MOORING SYSTEM --- + ! TODO : Verify that p%MD_FileName file exists + if ((p%DT_mooring <= 0.0_ReKi) .or. p%DT_mooring > p%DT_high)) CALL SetErrStat(ErrID_Fatal,'DT_mooring must be greater than zero and no greater than dt_high.',ErrStat,ErrMsg,RoutineName) + ! --- WAKE DYNAMICS --- IF (WD_InitInp%dr <= 0.0_ReKi) CALL SetErrStat(ErrID_Fatal,'dr (radial increment) must be larger than 0.',ErrStat,ErrMsg,RoutineName) IF (WD_InitInp%NumRadii < 2) CALL SetErrStat(ErrID_Fatal,'NumRadii (number of radii) must be at least 2.',ErrStat,ErrMsg,RoutineName) @@ -2764,6 +2773,15 @@ subroutine FARM_End(farm, ErrStat, ErrMsg) end if + !-------------- + ! 5. End farm-level MoorDyn + if (farm%p%MooringMod == 3) then + call MD_End(farm%MD%Input(1), farm%MD%p, farm%MD%x, farm%MD%xd, farm%MD%z, farm%MD%OtherSt, farm%MD%y, farm%MD%m, ErrStat2, ErrMsg2) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + !TODO: any related items need to be cleared? + end if + + !....................................................................................... ! close output file !....................................................................................... diff --git a/modules/moordyn/src/MoorDyn_Driver.f90 b/modules/moordyn/src/MoorDyn_Driver.f90 index 7fc3d95df3..b7dcbc9097 100644 --- a/modules/moordyn/src/MoorDyn_Driver.f90 +++ b/modules/moordyn/src/MoorDyn_Driver.f90 @@ -95,13 +95,23 @@ PROGRAM MoorDyn_Driver Integer(IntKi) :: nTurbines Integer(IntKi) :: iIn integer(intKi) :: Un - + + ! data for SimStatus/RunTimes: + REAL(DbKi) :: PrevSimTime !< Previous time message was written to screen (s > 0) + REAL(ReKi) :: PrevClockTime !< Previous clock time in seconds past midnight + INTEGER :: SimStrtTime (8) !< An array containing the elements of the start time (after initialization). + INTEGER :: ProgStrtTime (8) !< An array containing the elements of the program start time (before initialization). + REAL(ReKi) :: SimStrtCPU !< User CPU time for simulation (without intialization) + REAL(ReKi) :: ProgStrtCPU !< User CPU time for program (with intialization) + + CHARACTER(20) :: FlagArg ! flag argument from command line !CHARACTER(1024) :: drvrInitInp%%InputsFile CHARACTER(200) :: git_commit ! String containing the current git commit hash TYPE(ProgDesc), PARAMETER :: version = ProgDesc( 'MoorDyn Driver', '', '' ) + ErrMsg = "" ErrStat = ErrID_None UnEcho=-1 @@ -122,7 +132,10 @@ PROGRAM MoorDyn_Driver CALL WrScr( ' Running '//TRIM( version%Name )//' a part of OpenFAST - '//TRIM(git_commit)//NewLine//' linked with '//TRIM( NWTC_Ver%Name )//NewLine ) - + + CALL DATE_AND_TIME ( Values=ProgStrtTime ) ! Let's time the whole simulation + CALL CPU_TIME ( ProgStrtCPU ) ! Initial time (this zeros the start time when used as a MATLAB function) + @@ -252,7 +265,7 @@ PROGRAM MoorDyn_Driver READ(UnPtfmMotIn,'(A)',IOSTAT=ErrStat2) Line !read into a line IF (ErrStat2 /= 0) EXIT ! break out of the loop if it couldn't read the line (i.e. if at end of file) - print *, TRIM(Line) + !print *, TRIM(Line) i = i+1 END DO @@ -288,7 +301,7 @@ PROGRAM MoorDyn_Driver CLOSE ( UnPtfmMotIn ) print *, "Read ", ntIn, " time steps from input file." - print *, PtfmMotIn + !print *, PtfmMotIn ! trim simulation duration to length of input file if needed if (PtfmMotIn(ntIn, 1) < TMax) then @@ -382,6 +395,8 @@ PROGRAM MoorDyn_Driver ! ------------------------------------------------------------------------- print *,"Doing time marching now..." + + CALL SimStatus_FirstTime( PrevSimTime, PrevClockTime, SimStrtTime, SimStrtCPU, t, tMax ) DO i = 1,nt @@ -389,7 +404,10 @@ PROGRAM MoorDyn_Driver t = dtC*(i-1) - print *, t + + if ( MOD( i, 20 ) == 0 ) THEN + CALL SimStatus( PrevSimTime, PrevClockTime, t, tMax ) + end if MD_uTimes(1) = t + dtC !MD_uTimes(2) = MD_uTimes(1) - dtC @@ -404,7 +422,7 @@ PROGRAM MoorDyn_Driver ! any coupled bodies (type -1) DO l = 1,MD_p%nCpldBodies(iTurb) MD_u(1)%CoupledKinematics(iTurb)%TranslationDisp(:,J) = r_in(i, J:J+2) - MD_u(1)%CoupledKinematics(iTurb)%Orientation( :,:,J) = TRANSPOSE( EulerConstruct( r_in(i, J+3:J+5) ) ) ! full Euler angle approach <<<< need to check order + MD_u(1)%CoupledKinematics(iTurb)%Orientation( :,:,J) = EulerConstruct( r_in(i, J+3:J+5) ) ! full Euler angle approach MD_u(1)%CoupledKinematics(iTurb)%TranslationVel( :,J) = rd_in(i, J:J+2) MD_u(1)%CoupledKinematics(iTurb)%RotationVel( :,J) = rd_in(i, J+3:J+5) !a6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationAcc(:,J) @@ -417,7 +435,7 @@ PROGRAM MoorDyn_Driver DO l = 1,MD_p%nCpldRods(iTurb) MD_u(1)%CoupledKinematics(iTurb)%TranslationDisp(:,J) = r_in(i, J:J+2) - MD_u(1)%CoupledKinematics(iTurb)%Orientation( :,:,J) = TRANSPOSE( EulerConstruct( r_in(i, J+3:J+5) ) ) + MD_u(1)%CoupledKinematics(iTurb)%Orientation( :,:,J) = EulerConstruct( r_in(i, J+3:J+5) ) MD_u(1)%CoupledKinematics(iTurb)%TranslationVel( :,J) = rd_in(i, J:J+2) MD_u(1)%CoupledKinematics(iTurb)%RotationVel( :,J) = rd_in(i, J+3:J+5) @@ -479,6 +497,8 @@ PROGRAM MoorDyn_Driver ! END time marching ! ------------------------------------------------------------------------- + CALL RunTimes( ProgStrtTime, ProgStrtCPU, SimStrtTime, SimStrtCPU, t ) + ! Destroy all objects CALL MD_End( MD_u(1), MD_p, MD_x, MD_xd, MD_xc , MD_xo, MD_y, MD_m, ErrStat2, ErrMsg2 ); call AbortIfFailed() From 7c241b45ee7a3250a1ab62c3b8afffc320555394 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Wed, 13 Oct 2021 14:49:03 -0600 Subject: [PATCH 060/242] MD viscoelastic adjustment to input overall static and dynamic stiffnesses --- modules/moordyn/src/MoorDyn.f90 | 23 +++++++++++++---------- modules/moordyn/src/MoorDyn_Registry.txt | 2 +- 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index d1aad0736b..fd94fe0257 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -2052,7 +2052,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! if using viscoelastic model, initialize the internal states if (m%LineList(l)%ElasticMod == 2) then do I = 1,N - x%states(m%LineStateIs1(l) + 6*N-6 + I-1) = m%LineList(l)%dl_S(I) ! should be zero + x%states(m%LineStateIs1(l) + 6*N-6 + I-1) = m%LineList(l)%dl_1(I) ! should be zero end do end if @@ -3300,14 +3300,14 @@ SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) ! if using viscoelastic model, allocate additional state quantities if (Line%ElasticMod == 2) then - ALLOCATE ( Line%dl_S(N), STAT = ErrStat ) + ALLOCATE ( Line%dl_1(N), STAT = ErrStat ) IF ( ErrStat /= ErrID_None ) THEN - ErrMsg = ' Error allocating dl_S array.' + ErrMsg = ' Error allocating dl_1 array.' !CALL CleanUp() RETURN END IF ! initialize to zero - Line%dl_S = 0.0_DbKi + Line%dl_1 = 0.0_DbKi end if ! allocate node and segment tangent vectors @@ -4129,7 +4129,7 @@ SUBROUTINE Line_SetState(Line, X, t) ! if using viscoelastic model, also set the static stiffness stretch if (Line%ElasticMod == 2) then do I=1,Line%N - Line%dl_S(I) = X( 6*Line%N-6 + I) ! these will be the last N entries in the state vector + Line%dl_1(I) = X( 6*Line%N-6 + I) ! these will be the last N entries in the state vector end do end if @@ -4177,7 +4177,8 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, Real(DbKi) :: Xi ! used in interpolating from lookup table Real(DbKi) :: Yi ! used in interpolating from lookup table Real(DbKi) :: dl ! stretch of a segment [m] - Real(DbKi) :: ld_S ! rate of change of static stiffness portion of segment [m/s] + Real(DbKi) :: ld_1 ! rate of change of static stiffness portion of segment [m/s] + Real(DbKi) :: EA_1 ! stiffness of 'static stiffness' portion of segment, combines with dynamic stiffness to give static stiffnes [m/s] N = Line%N ! for convenience @@ -4309,16 +4310,18 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, ! viscoelastic model else if (Line%ElasticMod == 2) then + EA_1 = Line%EA_D*Line%EA/(Line%EA_D - Line%EA)! calculated EA_1 which is the stiffness in series with EA_D that will result in the desired static stiffness of EA_S + dl = Line%lstr(I) - Line%l(I) ! delta l of this segment - ld_S = (Line%EA_D*dl - (Line%EA_D + Line%EA)*Line%dl_S(I) + Line%BA_D*Line%lstrd(I)) /( Line%BA_D + Line%BA) ! rate of change of static stiffness portion [m/s] + ld_1 = (Line%EA_D*dl - (Line%EA_D + EA_1)*Line%dl_1(I) + Line%BA_D*Line%lstrd(I)) /( Line%BA_D + Line%BA) ! rate of change of static stiffness portion [m/s] !MagT = (Line%EA*Line%dl_S(I) + Line%BA*ld_S)/ Line%l(I) ! compute tension based on static portion (dynamic portion would give same) - MagT = Line%EA*Line%dl_S(I)/ Line%l(I) - MagTd = Line%BA*ld_S / Line%l(I) + MagT = EA_1*Line%dl_1(I)/ Line%l(I) + MagTd = Line%BA*ld_1 / Line%l(I) ! update state derivative for static stiffness stretch (last N entries in the state vector) - Xd( 6*N-6 + I) = ld_S + Xd( 6*N-6 + I) = ld_1 end if diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index eed8b296f4..94c192dc43 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -246,7 +246,7 @@ typedef ^ ^ DbKi l {:} typedef ^ ^ DbKi ld {:} - - "segment unstretched length rate of change (used in active tensioning)" "[m]" typedef ^ ^ DbKi lstr {:} - - "segment stretched length" "[m]" typedef ^ ^ DbKi lstrd {:} - - "segment change in stretched length" "[m/s]" -typedef ^ ^ DbKi dl_S {:} - - "segment stretch attributed to static stiffness portion" "[m]" +typedef ^ ^ DbKi dl_1 {:} - - "segment stretch attributed to static stiffness portion" "[m]" typedef ^ ^ DbKi V {:} - - "segment volume" "[m^3]" typedef ^ ^ DbKi U {:}{:} - - "water velocity at node" "[m/s]" typedef ^ ^ DbKi Ud {:}{:} - - "water acceleration at node" "[m/s^2]" From 3770d3e498b6261f8c4bf7ce796e2266a718e670 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Wed, 13 Oct 2021 23:09:57 -0600 Subject: [PATCH 061/242] Fixed MD driver to support nonzero initial positions: - Modified MD driver input file format so that initial positions are always specified. These should match ElastoDyn platform initial positions if applicable. - Corrected initialization of Bodies in MoorDyn to support nonzero initial positions and orientations. --- modules/moordyn/src/MoorDyn.f90 | 59 +++++++++++++++----------- modules/moordyn/src/MoorDyn_Driver.f90 | 42 +++++++----------- 2 files changed, 49 insertions(+), 52 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 489aeecd29..7cf43a3878 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -30,7 +30,7 @@ MODULE MoorDyn PRIVATE - TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a11', '2 August 2021' ) + TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a13', '13 October 2021' ) INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output @@ -80,6 +80,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er INTEGER(IntKi) :: N ! convenience integer for readability: number of segments in the line REAL(ReKi) :: rPos(3) ! array for setting fairlead reference positions in mesh REAL(ReKi) :: OrMat(3,3) ! rotation matrix for setting fairlead positions correctly if there is initial platform rotation + REAL(ReKi) :: OrMat2(3,3) REAL(DbKi), ALLOCATABLE :: FairTensIC(:,:)! array of size nCpldCons, 3 to store three latest fairlead tensions of each line CHARACTER(20) :: TempString ! temporary string for incidental use INTEGER(IntKi) :: ErrStat2 ! Error status of the operation @@ -1518,6 +1519,11 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! Go through each turbine and set up its mesh and initial positions of coupled objects DO iTurb = 1,p%nTurbines + ! calculate rotation matrix for the initial orientation provided for this turbine + CALL SmllRotTrans('PtfmInit', InitInp%PtfmInit(4,iTurb),InitInp%PtfmInit(5,iTurb),InitInp%PtfmInit(6,iTurb), OrMat, '', ErrStat2, ErrMsg2) + CALL CheckError( ErrStat2, ErrMsg2 ) + IF (ErrStat >= AbortErrLev) RETURN + ! count number of coupling nodes needed for the mesh of this turbine K = p%nCpldBodies(iTurb) + p%nCpldRods(iTurb) + p%nCpldCons(iTurb) if (K == 0) K = 1 ! Always have at least one node (it will be a dummy node if no fairleads are attached) @@ -1536,56 +1542,64 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! note: in MoorDyn-F v2, the points in the mesh correspond in order to all the coupled bodies, then rods, then connections ! >>> make sure all coupled objects have been offset correctly by the PtfmInit values, including if it's a farm situation -- below or where the objects are first created <<<< + J = 0 ! this is the counter through the mesh points for each turbine DO l = 1,p%nCpldBodies(iTurb) - J = J + 1 rRef = m%BodyList(m%CpldBodyIs(l,iTurb))%r6 ! for now set reference position as per input file <<< + !OrMatRef = CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef(1:3), ErrStat2, ErrMsg2) ! defaults to identity orientation matrix - u%CoupledKinematics(iTurb)%TranslationDisp(:,J) = 0.0_ReKi ! no displacement from reference position + + ! calculate initial point relative position, adjusted due to initial platform translations + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) = InitInp%PtfmInit(1:3,iTurb) - rRef(1:3) + + OrMat2 = MATMUL(OrMat, TRANSPOSE( EulerConstruct( rRef(4:6)))) + u%CoupledKinematics(iTurb)%Orientation(:,:,J) = OrMat2 - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN + ! set absolute initial positions in MoorDyn + m%BodyList(m%CpldBodyIs(l,iTurb))%r6(1:3) = u%CoupledKinematics(iTurb)%Position(:,iTurb) + u%CoupledKinematics(iTurb)%TranslationDisp(:,iTurb) + p%TurbineRefPos(:,iTurb) + m%BodyList(m%CpldBodyIs(l,iTurb))%r6(4:6) = EulerExtract(MATMUL(OrMat, TRANSPOSE( EulerConstruct( rRef(4:6))))) ! apply rotation from PtfmInit onto input file's body orientation to get its true initial orientation - ! ! Apply offsets due to initial platform rotations and translations (fixed Jun 19, 2015) - ! CALL SmllRotTrans('body rotation matrix', InitInp%PtfmInit(4),InitInp%PtfmInit(5),InitInp%PtfmInit(6), OrMat, '', ErrStat2, ErrMsg2) - ! u%CoupledKinematics%TranslationDisp(1, i) = InitInp%PtfmInit(1) + OrMat(1,1)*rRef(1) + OrMat(2,1)*rRef(2) + OrMat(3,1)*rRef(3) - rRef(1) - ! u%CoupledKinematics%TranslationDisp(2, i) = InitInp%PtfmInit(2) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2) - ! u%CoupledKinematics%TranslationDisp(3, i) = InitInp%PtfmInit(3) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) - CALL MeshConstructElement(u%CoupledKinematics(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, J) ! set node as point element + + ! lastly, do this to initialize any attached Rods or Points and set their positions CALL Body_InitializeUnfree( m%BodyList(m%CpldBodyIs(l,iTurb)), m ) END DO DO l = 1,p%nCpldRods(iTurb) ! keeping this one simple for now, positioning at whatever is specified in input file <<<<< should change to glue code! - J = J + 1 rRef = m%RodList(m%CpldRodIs(l,iTurb))%r6 ! for now set reference position as per input file <<< + CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef(1:3), ErrStat2, ErrMsg2) ! defaults to identity orientation matrix + + ! calculate initial point relative position, adjusted due to initial platform rotations and translations <<< could convert to array math + u%CoupledKinematics(iTurb)%TranslationDisp(1,J) = InitInp%PtfmInit(1,iTurb) + OrMat(1,1)*rRef(1) + OrMat(2,1)*rRef(2) + OrMat(3,1)*rRef(3) - rRef(1) + u%CoupledKinematics(iTurb)%TranslationDisp(2,J) = InitInp%PtfmInit(2,iTurb) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2) + u%CoupledKinematics(iTurb)%TranslationDisp(3,J) = InitInp%PtfmInit(3,iTurb) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) + + ! set absolute initial positions in MoorDyn + m%RodList(m%CpldRodIs(l,iTurb))%r6(1:3) = u%CoupledKinematics(iTurb)%Position(:,iTurb) + u%CoupledKinematics(iTurb)%TranslationDisp(:,iTurb) + p%TurbineRefPos(:,iTurb) + + ! >>> still need to set Rod initial orientations accounting for PtfmInit rotation <<< - CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef, ErrStat2, ErrMsg2) ! defaults to identity orientation matrix - u%CoupledKinematics(iTurb)%TranslationDisp(:,J) = 0.0_ReKi ! no displacement from reference position CALL MeshConstructElement(u%CoupledKinematics(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, J) + ! lastly, do this to set the attached line endpoint positions: CALL Rod_SetKinematics(m%RodList(m%CpldRodIs(l,iTurb)), DBLE(rRef), m%zeros6, m%zeros6, 0.0_DbKi, m) END DO DO l = 1,p%nCpldCons(iTurb) ! keeping this one simple for now, positioning at whatever is specified by glue code <<< J = J + 1 - ! set reference position as per input file + ! set reference position as per input file <<< what about turbine positions in array? rRef(1:3) = m%ConnectList(m%CpldConIs(l,iTurb))%r - CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef, ErrStat2, ErrMsg2) + CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef(1:3), ErrStat2, ErrMsg2) ! calculate initial point relative position, adjusted due to initial platform rotations and translations <<< could convert to array math - CALL SmllRotTrans('PtfmInit', InitInp%PtfmInit(4,iTurb),InitInp%PtfmInit(5,iTurb),InitInp%PtfmInit(6,iTurb), OrMat, '', ErrStat2, ErrMsg2) - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN - u%CoupledKinematics(iTurb)%TranslationDisp(1,J) = InitInp%PtfmInit(1,iTurb) + OrMat(1,1)*rRef(1) + OrMat(2,1)*rRef(2) + OrMat(3,1)*rRef(3) - rRef(1) u%CoupledKinematics(iTurb)%TranslationDisp(2,J) = InitInp%PtfmInit(2,iTurb) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2) u%CoupledKinematics(iTurb)%TranslationDisp(3,J) = InitInp%PtfmInit(3,iTurb) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) @@ -1593,11 +1607,6 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! set absolute initial positions in MoorDyn m%ConnectList(m%CpldConIs(l,iTurb))%r = u%CoupledKinematics(iTurb)%Position(:,iTurb) + u%CoupledKinematics(iTurb)%TranslationDisp(:,iTurb) + p%TurbineRefPos(:,iTurb) - - !print *, 'Fairlead ', i, ' z TranslationDisp at start is ', u%PtFairleadDisplacement(iTurb)%TranslationDisp(3,i) - !print *, 'Fairlead ', i, ' z Position at start is ', u%PtFairleadDisplacement(iTurb)%Position(3,i) - ! <<<< - CALL MeshConstructElement(u%CoupledKinematics(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, J) ! lastly, do this to set the attached line endpoint positions: diff --git a/modules/moordyn/src/MoorDyn_Driver.f90 b/modules/moordyn/src/MoorDyn_Driver.f90 index b7dcbc9097..6b9a629ca4 100644 --- a/modules/moordyn/src/MoorDyn_Driver.f90 +++ b/modules/moordyn/src/MoorDyn_Driver.f90 @@ -43,7 +43,6 @@ PROGRAM MoorDyn_Driver INTEGER :: InputsMod CHARACTER(1024) :: InputsFile INTEGER :: nTurb - CHARACTER(1024) :: positions END TYPE MD_Drvr_InitInput @@ -174,19 +173,13 @@ PROGRAM MoorDyn_Driver CALL AllocAry(MD_InitInp%PtfmInit, 6, nTurbines, 'PtfmInit array' , ErrStat2, ErrMsg2); call AbortIfFailed() CALL AllocAry(MD_InitInp%TurbineRefPos, 3, nTurbines, 'TurbineRefPos array', ErrStat2, ErrMsg2); call AbortIfFailed() - if (drvrInitInp%FarmSize > 0) then ! if in FAST.Farm mode, specify turbine ref positions and initial positions from driver input file - do J=1,drvrInitInp%FarmSize - MD_InitInp%TurbineRefPos(1,J) = drvrInitInp%FarmPositions(1,J) - MD_InitInp%TurbineRefPos(2,J) = drvrInitInp%FarmPositions(2,J) - MD_InitInp%TurbineRefPos(3,J) = 0.0_DbKi - MD_InitInp%PtfmInit(:,J) = drvrInitInp%FarmPositions(3:8,J) - end do - else ! if in normal OpenFAST mode, zero the initial platform position since the framework doesn't allow much else - MD_InitInp%PtfmInit = 0.0_DbKi - MD_InitInp%TurbineRefPos = 0.0_DbKi - end if - - + do J=1,nTurbines + MD_InitInp%TurbineRefPos(1,J) = drvrInitInp%FarmPositions(1,J) + MD_InitInp%TurbineRefPos(2,J) = drvrInitInp%FarmPositions(2,J) + MD_InitInp%TurbineRefPos(3,J) = 0.0_DbKi + MD_InitInp%PtfmInit(:,J) = drvrInitInp%FarmPositions(3:8,J) + end do + MD_interp_order = 0 ! allocate Input and Output arrays; used for interpolation and extrapolation @@ -596,21 +589,16 @@ SUBROUTINE ReadDriverInputFile( inputFile, InitInp) CALL ReadVar( UnIn, FileName, InitInp%OutRootName, 'OutRootName', 'MoorDyn output root filename', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() CALL ReadVar( UnIn, FileName, InitInp%TMax , 'Tmax', 'Simulation time duration', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() CALL ReadVar( UnIn, FileName, InitInp%dtC , 'dtC', 'Time step size for calling MoorDyn', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() - ! ---------------------- FAST.Farm ---------------------------------------------------------------- - CALL ReadCom( UnIn, FileName, 'FAST.Farm header', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + CALL ReadVar( UnIn, FileName, InitInp%InputsMod , 'InputsMode', 'Mode for the inputs - zero/steady/time-series', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() + CALL ReadVar( UnIn, FileName, InitInp%InputsFile , 'InputsFile', 'Filename for the MoorDyn inputs', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() CALL ReadVar( UnIn, FileName, InitInp%FarmSize , 'NumTurbines', 'number of turbines in FAST.Farm', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() - CALL ReadCom( UnIn, FileName, 'FAST.Farm table header line 1', ErrStat2, ErrMsg2); call AbortIfFailed() - CALL ReadCom( UnIn, FileName, 'FAST.Farm table header line 2', ErrStat2, ErrMsg2); call AbortIfFailed() - do J=1,InitInp%FarmSize + CALL ReadCom( UnIn, FileName, 'Initial positions header', ErrStat2, ErrMsg2); call AbortIfFailed() + CALL ReadCom( UnIn, FileName, 'Initial positions table header line 1', ErrStat2, ErrMsg2); call AbortIfFailed() + CALL ReadCom( UnIn, FileName, 'Initial positions table header line 2', ErrStat2, ErrMsg2); call AbortIfFailed() + do J=1,MAX(1,InitInp%FarmSize) CALL ReadAry( UnIn, FileName, InitInp%FarmPositions(:,J), 8, "FarmPositions", "FAST.Farm position inputs", ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() - end do - !---------------------- INPUTS ------------------------------------------------------------------- - CALL ReadCom( UnIn, FileName, 'INPUTS header', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() - CALL ReadVar( UnIn, FileName, InitInp%InputsMod , 'InputsMod', 'Mode for the inputs - zero/steady/time-series', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() - CALL ReadVar( UnIn, FileName, InitInp%InputsFile, 'InputsFile', 'Filename for the MoorDyn inputs', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() - CALL ReadCom( UnIn, FileName, 'Header line saying next line will be a list of coupled positions' , ErrStat2, ErrMsg2); call AbortIfFailed() - CALL ReadVar( UnIn, FileName, InitInp%positions, 'positions', 'List of positions when InputsMod=1', ErrStat2, ErrMsg2, UnEcho); call AbortIfFailed() - + end do + ! done reading if(UnEcho>0) CLOSE( UnEcho ) if(UnIn>0) CLOSE( UnIn ) From e1cf18edabebf62bb3aba136add8e09b845d2cd1 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Thu, 14 Oct 2021 14:20:05 -0600 Subject: [PATCH 062/242] Getting MD driver to give t=0 output --- modules/moordyn/src/MoorDyn.f90 | 2 ++ 1 file changed, 2 insertions(+) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 7cf43a3878..b1439b6a5b 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -2204,6 +2204,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL WrScr(' MoorDyn initialization completed.') + m%LastOutTime = -1.0_DbKi ! set to nonzero to ensure that output happens at the start of simulation at t=0 + ! TODO: add feature for automatic water depth increase based on max anchor depth! CONTAINS From 340610b21663662bd49fabdb5d0e7ea929d00e9e Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Thu, 14 Oct 2021 21:27:52 -0600 Subject: [PATCH 063/242] Reorder input parsing in prep for auto anchor depth: - Set parsing of MD input file settings to happen first so that the bathymetry info is known when the connection/point locations are read in. - To restore support for using depth provided by glue code and not having depth in the input file, changed when getBathymetry is called and adjusted it so that it considers both the glue-code-provided depth and the (optionally) input depth. --- modules/moordyn/src/MoorDyn.f90 | 115 ++++++++++------------- modules/moordyn/src/MoorDyn_Driver.f90 | 11 +-- modules/moordyn/src/MoorDyn_IO.f90 | 24 +++-- modules/moordyn/src/MoorDyn_Registry.txt | 2 +- 4 files changed, 73 insertions(+), 79 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 1d86578ca2..da54b344dd 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -108,6 +108,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CHARACTER(20) :: LineOutString ! String to temporarially hold characters specifying line output options CHARACTER(20) :: OptString ! String to temporarially hold name of option variable CHARACTER(40) :: OptValue ! String to temporarially hold value of options variable input + CHARACTER(40) :: DepthValue ! Temporarily stores the optional WtrDpth setting for MD, which could be a number or a filename INTEGER(IntKi) :: nOpts ! number of options lines in input file CHARACTER(40) :: TempString1 ! CHARACTER(40) :: TempString2 ! @@ -164,7 +165,6 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er p%dtM0 = DTcoupling ! default to the coupling interval (but will likely need to be smaller) p%g = InitInp%g p%rhoW = InitInp%rhoW - p%WtrDpth = InitInp%WtrDepth ! TODO: add MSL2SWL from OpenFAST <<<< ! set the following to some defaults p%kBot = 3.0E6 @@ -175,6 +175,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er InputFileDat%threshIC = 0.01_ReKi p%WaterKin = 0_IntKi p%dtOut = 0.0_DbKi + DepthValue = "" ! Start off as empty string, to only be filled if MD setting is specified (otherwise InitInp%WtrDepth is used) + ! DepthValue and InitInp%WtrDepth are processed later by getBathymetry. m%PtfmInit = InitInp%PtfmInit(:,1) ! is this copying necssary in case this is an individual instance in FAST.Farm? @@ -372,11 +374,52 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if (INDEX(Line, "OPTIONS") > 0) then ! if options header - ! don't skip any lines (no column headers for the options section) - - ! find how many options have been specified + IF (wordy > 0) print *, "Reading Options" + + ! don't skip any lines (no column headers for the options section) + ! process each line in this section Line = NextLine(i) DO while (INDEX(Line, "---") == 0) ! while we DON'T find another header line + + ! parse out entries: value, option keyword + READ(Line,*,IOSTAT=ErrStat2) OptValue, OptString ! look at first two entries, ignore remaining words in line, which should be comments + IF ( ErrStat2 /= 0 ) THEN + CALL SetErrStat( ErrID_Fatal, 'Failed to read options.', ErrStat, ErrMsg, RoutineName ) ! would be nice to specify which line had the error + CALL CleanUp() + RETURN + END IF + + CALL Conv2UC(OptString) + + ! check all possible options types and see if OptString is one of them, in which case set the variable. + if ( OptString == 'DTM') THEN + read (OptValue,*) p%dtM0 + else if ( OptString == 'G') then + read (OptValue,*) p%g + else if ( OptString == 'RHOW') then + read (OptValue,*) p%rhoW + else if ( OptString == 'WTRDPTH') then + read (OptValue,*) DepthValue ! water depth input read in as a string to be processed by getBathymetry + else if ( OptString == 'KBOT') then + read (OptValue,*) p%kBot + else if ( OptString == 'CBOT') then + read (OptValue,*) p%cBot + else if ( OptString == 'DTIC') then + read (OptValue,*) InputFileDat%dtIC + else if ( OptString == 'TMAXIC') then + read (OptValue,*) InputFileDat%TMaxIC + else if ( OptString == 'CDSCALEIC') then + read (OptValue,*) InputFileDat%CdScaleIC + else if ( OptString == 'THRESHIC') then + read (OptValue,*) InputFileDat%threshIC + else if ( OptString == 'WATERKIN') then + read (OptValue,*) p%WaterKin + else if ( OptString == 'DTOUT') then + read (OptValue,*) p%dtOut + else + CALL SetErrStat( ErrID_Warn, 'unable to interpret input '//trim(OptString), ErrStat, ErrMsg, RoutineName ) + end if + nOpts = nOpts + 1 Line = NextLine(i) END DO @@ -410,6 +453,9 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF (wordy > 0) print *, " Identified ", nOpts , "Options in input file." + ! set up seabed bathymetry + CALL getBathymetry(DepthValue, InitInp%WtrDepth, m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, ErrStat2, ErrMsg2) + ! ----------------------------- misc checks to be sorted ----------------------------- @@ -478,6 +524,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er do while ( i <= FileInfo_In%NumLines ) if (INDEX(Line, "---") > 0) then ! look for a header line + !------------------------------------------------------------------------------------------- if ( ( INDEX(Line, "LINE DICTIONARY") > 0) .or. ( INDEX(Line, "LINE TYPES") > 0) ) then ! if line dictionary header @@ -1225,66 +1272,6 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er END DO - !------------------------------------------------------------------------------------------- - else if (INDEX(Line, "OPTIONS") > 0) then ! if options header - - IF (wordy > 0) print *, "Reading Options" - - ! (don't skip any lines) - - ! process each line - DO l = 1,nOpts - - !read into a line - Line = NextLine(i) - - ! parse out entries: value, option keyword - READ(Line,*,IOSTAT=ErrStat2) OptValue, OptString ! look at first two entries, ignore remaining words in line, which should be comments - - - IF ( ErrStat2 /= 0 ) THEN - CALL SetErrStat( ErrID_Fatal, 'Failed to read options.', ErrStat, ErrMsg, RoutineName ) ! would be nice to specify which line had the error - CALL CleanUp() - RETURN - END IF - - CALL Conv2UC(OptString) - - ! check all possible options types and see if OptString is one of them, in which case set the variable. - if ( OptString == 'DTM') THEN - read (OptValue,*) p%dtM0 - else if ( OptString == 'G') then - read (OptValue,*) p%g - else if ( OptString == 'RHOW') then - read (OptValue,*) p%rhoW - ! else if ( OptString == 'WTRDPTH') then - ! read (OptValue,*) p%WtrDpth - else if ( OptString == 'WTRDPTH') then - CALL getBathymetry(OptValue, m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, ErrStat2, ErrMsg2) - ! CALL getBathymetry(OptValue, m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, m%BathGrid_npoints, ErrStat2, ErrMsg2) - else if ( OptString == 'KBOT') then - read (OptValue,*) p%kBot - else if ( OptString == 'CBOT') then - read (OptValue,*) p%cBot - else if ( OptString == 'DTIC') then - read (OptValue,*) InputFileDat%dtIC - else if ( OptString == 'TMAXIC') then - read (OptValue,*) InputFileDat%TMaxIC - else if ( OptString == 'CDSCALEIC') then - read (OptValue,*) InputFileDat%CdScaleIC - else if ( OptString == 'THRESHIC') then - read (OptValue,*) InputFileDat%threshIC - else if ( OptString == 'WATERKIN') then - read (OptValue,*) p%WaterKin - else if ( OptString == 'DTOUT') then - read (OptValue,*) p%dtOut - else - CALL SetErrStat( ErrID_Warn, 'unable to interpret input '//trim(OptString), ErrStat, ErrMsg, RoutineName ) - end if - - END DO - - !------------------------------------------------------------------------------------------- else if (INDEX(Line, "OUTPUT") > 0) then ! if output header diff --git a/modules/moordyn/src/MoorDyn_Driver.f90 b/modules/moordyn/src/MoorDyn_Driver.f90 index 6b9a629ca4..a365415f8e 100644 --- a/modules/moordyn/src/MoorDyn_Driver.f90 +++ b/modules/moordyn/src/MoorDyn_Driver.f90 @@ -353,10 +353,7 @@ PROGRAM MoorDyn_Driver ! --------------------------------------------------------------- ! Set the initial input values ! --------------------------------------------------------------- - - ! zero the displacements to start with <<< should this be based on what came out of MD_Init instead? <<< - MD_u(1)%CoupledKinematics(1)%TranslationDisp = 0.0_ReKi - + ! zero the tension commands MD_u(1)%DeltaL = 0.0_ReKi MD_u(1)%DeltaLdot = 0.0_ReKi @@ -369,13 +366,13 @@ PROGRAM MoorDyn_Driver ! ! now add some current in x for testing ! MD_u(1)%U(1,:) = 1.0 + ! copy inputs to initialize input arrays for higher interp orders if applicable (it's not) DO i = 2, MD_interp_order + 1 CALL MD_CopyInput( MD_u(1), MD_u(i), MESH_NEWCOPY, ErrStat2, ErrMsg2 ); call AbortIfFailed() - END DO - + END DO DO i = 1, MD_interp_order + 1 MD_uTimes(i) = -(i - 1) * dtC - ENDDO + END DO ! get output at initialization (before time stepping) t = 0 diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index fc1fb1e385..7a8371fd57 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -111,17 +111,16 @@ MODULE MoorDyn_IO CONTAINS - SUBROUTINE getBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, ErrStat3, ErrMsg3) + SUBROUTINE getBathymetry(inputString, defaultDepth, BathGrid, BathGrid_Xs, BathGrid_Ys, ErrStat3, ErrMsg3) ! SUBROUTINE getBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, BathGrid_npoints, ErrStat3, ErrMsg3) - CHARACTER(40), INTENT(IN ) :: inputString + CHARACTER(40), INTENT(IN ) :: inputString ! string describing water depth or bathymetry filename + REAL(ReKi), INTENT(IN ) :: defaultDepth ! depth to use if inputString is empty REAL(DbKi), ALLOCATABLE, INTENT(INOUT) :: BathGrid (:,:) REAL(DbKi), ALLOCATABLE, INTENT(INOUT) :: BathGrid_Xs (:) REAL(DbKi), ALLOCATABLE, INTENT(INOUT) :: BathGrid_Ys (:) - ! INTEGER(IntKi), INTENT(INOUT) :: BathGrid_npoints - - INTEGER(IntKi), INTENT( OUT) :: ErrStat3 ! Error status of the operation - CHARACTER(*), INTENT( OUT) :: ErrMsg3 ! Error message if ErrStat /= ErrID_None + INTEGER(IntKi), INTENT( OUT) :: ErrStat3 ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg3 ! Error message if ErrStat /= ErrID_None INTEGER(IntKi) :: I INTEGER(IntKi) :: UnCoef ! unit number for coefficient input file @@ -136,7 +135,18 @@ SUBROUTINE getBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, ErrSta INTEGER(IntKi) :: nGridY ! integer of the size of BathGrid_Ys - IF (SCAN(inputString, "abcdfghijklmnopqrstuvwxyzABCDFGHIJKLMNOPQRSTUVWXYZ") == 0) THEN + IF (LEN_TRIM(inputString) == 0) THEN + ! If the input is empty (not provided), make the 1x1 bathymetry grid using the default depth + ALLOCATE(BathGrid(1,1), STAT=ErrStat4) + BathGrid(1,1) = DBLE(defaultDepth) + + ALLOCATE(BathGrid_Xs(1), STAT=ErrStat4) + BathGrid_Xs(1) = 0.0_DbKi + + ALLOCATE(BathGrid_Ys(1), STAT=ErrStat4) + BathGrid_Ys(1) = 0.0_DbKi + + ELSE IF (SCAN(inputString, "abcdfghijklmnopqrstuvwxyzABCDFGHIJKLMNOPQRSTUVWXYZ") == 0) THEN ! If the input does not have any of these string values, let's treat it as a number but store in a matrix ALLOCATE(BathGrid(1,1), STAT=ErrStat4) READ(inputString, *, IOSTAT=ErrStat4) BathGrid(1,1) diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index dc0fdc7653..2a6f7d34ba 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -353,7 +353,7 @@ typedef ^ ^ IntKi NConns - typedef ^ ^ IntKi NAnchs - 0 - "number of Anchor type Connections" "" typedef ^ ^ DbKi g - 9.81 - "gravitational constant (positive)" "[m/s^2]" typedef ^ ^ DbKi rhoW - 1025 - "density of seawater" "[kg/m^3]" -typedef ^ ^ DbKi WtrDpth - - - "water depth" "[m]" +#typedef ^ ^ DbKi WtrDpth - - - "water depth" "[m]" typedef ^ ^ DbKi kBot - - - "bottom stiffness" "[Pa/m]" typedef ^ ^ DbKi cBot - - - "bottom damping" "[Pa-s/m]" typedef ^ ^ DbKi dtM0 - - - "desired mooring model time step" "[s]" From 91373f056fc68fea277b02ad80a285934fc5616a Mon Sep 17 00:00:00 2001 From: shousner Date: Fri, 15 Oct 2021 10:19:53 -0600 Subject: [PATCH 064/242] First pass at setting anchor depth based on bathymetry --- modules/moordyn/src/MoorDyn.f90 | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 1d86578ca2..6864ee9c54 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -861,8 +861,18 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! parse out entries: Node Type X Y Z M V FX FY FZ CdA Ca IF (ErrStat2 == 0) THEN READ(Line,*,IOSTAT=ErrStat2) m%ConnectList(l)%IdNum, tempString1, tempArray(1), & - tempArray(2), tempArray(3), m%ConnectList(l)%conM, & + tempArray(2), tempString4, m%ConnectList(l)%conM, & m%ConnectList(l)%conV, m%ConnectList(l)%conCdA, m%ConnectList(l)%conCa + + IF (SCAN(tempString4, "seabed") == 0) THEN + ! if the tempString of the anchor depth value does not have any letters that are found in the word seabed, it's a scalar depth value + READ(tempString4, *, IOSTAT=ErrStat2) tempArray(3) + ELSE ! otherwise interpret the anchor depth value as a 'seabed' input, meaning the anchor should be on the seabed wherever the bathymetry says it should be + PRINT *, "Anchor depth set for seabed; Finding the right seabed depth based on bathymetry" + + CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, tempArray(1), tempArray(2), tempArray(3)) + + END IF ! not used m%ConnectList(l)%conFX = 0.0_DbKi From 75dfc12c9767646ea1974535b4fc2582ae9a56f2 Mon Sep 17 00:00:00 2001 From: shousner Date: Fri, 15 Oct 2021 12:38:24 -0600 Subject: [PATCH 065/242] Added temporary depth variable to make negative and store in tempArray --- modules/moordyn/src/MoorDyn.f90 | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index c3a587f608..f1227fac29 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -115,6 +115,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CHARACTER(40) :: TempString3 ! CHARACTER(40) :: TempString4 ! CHARACTER(1024) :: FileName ! + + REAL(DbKi) :: depth ! local water depth interpolated from bathymetry grid CHARACTER(25) :: let1 ! strings used for splitting and parsing identifiers @@ -917,7 +919,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ELSE ! otherwise interpret the anchor depth value as a 'seabed' input, meaning the anchor should be on the seabed wherever the bathymetry says it should be PRINT *, "Anchor depth set for seabed; Finding the right seabed depth based on bathymetry" - CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, tempArray(1), tempArray(2), tempArray(3)) + CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, tempArray(1), tempArray(2), depth) + tempArray(3) = -depth END IF From d22feca8f4ac2c2b30cb0be813160d4171aa735e Mon Sep 17 00:00:00 2001 From: shousner Date: Fri, 15 Oct 2021 12:39:17 -0600 Subject: [PATCH 066/242] Some water depth variables changed in MoorDyn_Types I'm assuming because I recompiled/rebuilt with Matt's new changes without the water depth variable --- modules/moordyn/src/MoorDyn_Types.f90 | 7 ------- 1 file changed, 7 deletions(-) diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index 379d1c9ae1..b7a1034bd5 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -382,7 +382,6 @@ MODULE MoorDyn_Types INTEGER(IntKi) :: NAnchs = 0 !< number of Anchor type Connections [] REAL(DbKi) :: g = 9.81 !< gravitational constant (positive) [[m/s^2]] REAL(DbKi) :: rhoW = 1025 !< density of seawater [[kg/m^3]] - REAL(DbKi) :: WtrDpth !< water depth [[m]] REAL(DbKi) :: kBot !< bottom stiffness [[Pa/m]] REAL(DbKi) :: cBot !< bottom damping [[Pa-s/m]] REAL(DbKi) :: dtM0 !< desired mooring model time step [[s]] @@ -10259,7 +10258,6 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) DstParamData%NAnchs = SrcParamData%NAnchs DstParamData%g = SrcParamData%g DstParamData%rhoW = SrcParamData%rhoW - DstParamData%WtrDpth = SrcParamData%WtrDpth DstParamData%kBot = SrcParamData%kBot DstParamData%cBot = SrcParamData%cBot DstParamData%dtM0 = SrcParamData%dtM0 @@ -10674,7 +10672,6 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Int_BufSz = Int_BufSz + 1 ! NAnchs Db_BufSz = Db_BufSz + 1 ! g Db_BufSz = Db_BufSz + 1 ! rhoW - Db_BufSz = Db_BufSz + 1 ! WtrDpth Db_BufSz = Db_BufSz + 1 ! kBot Db_BufSz = Db_BufSz + 1 ! cBot Db_BufSz = Db_BufSz + 1 ! dtM0 @@ -10897,8 +10894,6 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%rhoW Db_Xferred = Db_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%WtrDpth - Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%kBot Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%cBot @@ -11455,8 +11450,6 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg Db_Xferred = Db_Xferred + 1 OutData%rhoW = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 - OutData%WtrDpth = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 OutData%kBot = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 OutData%cBot = DbKiBuf(Db_Xferred) From 1fd6bf6a1236c2510cfff8b3c1335d92eb69b2d3 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Fri, 15 Oct 2021 22:42:12 -0600 Subject: [PATCH 067/242] Remove double-counted weight in fairten outputs --- modules/moordyn/src/MoorDyn.f90 | 4 +++- modules/moordyn/src/MoorDyn_IO.f90 | 6 +++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index f1227fac29..75395c5822 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -30,7 +30,7 @@ MODULE MoorDyn PRIVATE - TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a13', '13 October 2021' ) + TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a14', '15 October 2021' ) INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output @@ -4735,6 +4735,8 @@ SUBROUTINE Connect_DoRHS(Connect, m, p) Connect%M (J,J) = Connect%M (J,J) + Connect%conV*p%rhoW*Connect%conCa; ! add added mass END DO + + ! would this sub ever need to include the m*a inertial term? Is it ever called for coupled connects? <<< END SUBROUTINE Connect_DoRHS !===================================================================== diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index 7a8371fd57..0a03736e87 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -547,7 +547,7 @@ SUBROUTINE MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat, ErrMsg ) ELSE IF (qVal == 'AZ') THEN p%OutParam(I)%QType = AccZ p%OutParam(I)%Units = UnitList(AccZ) - ELSE IF ((qVal == 'T') .or. (qval == 'Ten')) THEN + ELSE IF ((qVal == 'T') .or. (qVal == 'TEN')) THEN p%OutParam(I)%QType = Ten p%OutParam(I)%Units = UnitList(Ten) ELSE IF (qVal == 'FX') THEN @@ -1654,9 +1654,9 @@ FUNCTION Line_GetNodeTen(Line, i, p) result(NodeTen) REAL(DbKi) :: Tmag_squared if (i==0) then - NodeTen = sqrt( Line%Fnet(1,i)**2 + Line%Fnet(2,i)**2 + (Line%Fnet(3,i) + Line%M(1,1,i)*(-p%g))**2 ) + NodeTen = sqrt( Line%Fnet(1,i)**2 + Line%Fnet(2,i)**2 + Line%Fnet(3,i)**2 ) ! if an end node, use Fnet which already includes weight else if (i==Line%N) then - NodeTen = sqrt( Line%Fnet(1,i)**2 + Line%Fnet(2,i)**2 + (Line%Fnet(3,i) + Line%M(1,1,i)*(-p%g))**2 ) + NodeTen = sqrt( Line%Fnet(1,i)**2 + Line%Fnet(2,i)**2 + Line%Fnet(3,i)**2 ) else Tmag_squared = 0.0_DbKi DO J=1,3 From 3a1173b2469cde453b938ab8b5977ee189ccc2ee Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Tue, 19 Oct 2021 14:45:30 -0600 Subject: [PATCH 068/242] Restructuring of MoorDyn source and wave/current addition: - Split MoorDyn source code into separate modules for each object type, as well as a "Misc" module for other internal subroutines. This follows the MD C structure and makes the dependencies clearer. - Added most of the code to let MoorDyn handle its own wave and current kinematics independently based on a wave elevation time series and a user-input current profile. This provides a crucial capability for a project without risking delay from dependence on SeaState development. The wave elevation handling mirrors the current WaveMod=5 option in HydroDyn (and adapts much of the same code, albiet streamlined). The current profile capability could potentially be added to HydroDyn or SeaState in future. - Wave and current capabilities are not yet tested. - A new MND_InitInp parameter, Tmax, needs to be added in the glue code. --- modules/moordyn/src/MoorDyn.f90 | 4624 +--------------------- modules/moordyn/src/MoorDyn_Driver.f90 | 11 +- modules/moordyn/src/MoorDyn_IO.f90 | 6 +- modules/moordyn/src/MoorDyn_Registry.txt | 44 +- modules/moordyn/src/MoorDyn_Types.f90 | 1080 ++--- 5 files changed, 808 insertions(+), 4957 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 936b4f6bb2..0c211d6435 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -23,6 +23,11 @@ MODULE MoorDyn USE MoorDyn_Types USE MoorDyn_IO USE NWTC_Library + USE MoorDyn_Line + USE MoorDyn_Point + USE MoorDyn_Rod + USE MoorDyn_Body + USE MoorDyn_Misc !USE WAVES, only: WaveGrid_n, WaveGrid_x0, WaveGrid_dx, WaveGrid_nx, WaveGrid_y0, WaveGrid_dy, WaveGrid_ny, WaveGrid_nz ! seeing if I can get waves data here directly... @@ -109,6 +114,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CHARACTER(20) :: OptString ! String to temporarially hold name of option variable CHARACTER(40) :: OptValue ! String to temporarially hold value of options variable input CHARACTER(40) :: DepthValue ! Temporarily stores the optional WtrDpth setting for MD, which could be a number or a filename + CHARACTER(40) :: WaterKinValue ! Temporarily stores the optional WaterKin setting for MD, which is typically a filename INTEGER(IntKi) :: nOpts ! number of options lines in input file CHARACTER(40) :: TempString1 ! CHARACTER(40) :: TempString2 ! @@ -166,6 +172,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! set default values for the simulation settings ! these defaults are based on the glue code p%dtM0 = DTcoupling ! default to the coupling interval (but will likely need to be smaller) + p%Tmax = InitInp%Tmax p%g = InitInp%g p%rhoW = InitInp%rhoW ! TODO: add MSL2SWL from OpenFAST <<<< @@ -176,11 +183,12 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er InputFileDat%TMaxIC = 60.0_DbKi InputFileDat%CdScaleIC = 4.0_ReKi InputFileDat%threshIC = 0.01_ReKi - p%WaterKin = 0_IntKi + p%WaveKin = 0_IntKi + p%Current = 0_IntKi p%dtOut = 0.0_DbKi DepthValue = "" ! Start off as empty string, to only be filled if MD setting is specified (otherwise InitInp%WtrDepth is used) ! DepthValue and InitInp%WtrDepth are processed later by getBathymetry. - + WaterKinValue = "" m%PtfmInit = InitInp%PtfmInit(:,1) ! is this copying necssary in case this is an individual instance in FAST.Farm? @@ -416,7 +424,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if ( OptString == 'THRESHIC') then read (OptValue,*) InputFileDat%threshIC else if ( OptString == 'WATERKIN') then - read (OptValue,*) p%WaterKin + read (OptValue,*) WaterKinValue else if ( OptString == 'DTOUT') then read (OptValue,*) p%dtOut else @@ -457,7 +465,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! set up seabed bathymetry - CALL getBathymetry(DepthValue, InitInp%WtrDepth, m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, ErrStat2, ErrMsg2) + CALL setupBathymetry(DepthValue, InitInp%WtrDepth, m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, ErrStat2, ErrMsg2) + CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, 0.0_DbKi, 0.0_DbKi, p%WtrDpth) ! set depth at 0,0 as nominal for waves etc + + + ! set up wave and current kinematics + CALL setupWaterKin(WaterKinValue, p, InitInp%Tmax, ErrStat2, ErrMsg2) + ! ----------------------------- misc checks to be sorted ----------------------------- @@ -1721,268 +1735,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er endif - ! ----------------------------- Arrays for wave kinematics ----------------------------- + ! >>> set up wave stuff here??? <<< - m%WaveTi = 1 ! set initial wave grid time interpolation index to 1 to start with - - ! :::::::::::::: BELOW WILL BE USED EVENTUALLY WHEN WAVE INFO IS AN INPUT :::::::::::::::::: - ! ! The rAll array contains all nodes or reference points in the system - ! ! (x,y,z global coordinates for each) in the order of bodies, rods, points, internal line nodes. - ! - ! ! count the number of nodes to use for passing wave kinematics - ! J=0 - ! ! Body reference point coordinates - ! J = J + p%nBodies - ! ! Rod node coordinates (including ends) - ! DO l = 1, p%nRods - ! J = J + (m%RodList(l)%N + 1) - ! END DO - ! ! Point reference point coordinates - ! J = J + p%nConnects - ! ! Line internal node coordinates - ! DO l = 1, p%nLines - ! J = J + (m%LineList(l)%N - 1) - ! END DO - ! - ! ! allocate all relevant arrays - ! ! allocate state vector and temporary state vectors based on size just calculated - ! ALLOCATE ( y%rAll(3,J), u%U(3,J), u%Ud(3,J), u%zeta(J), u%PDyn(J), STAT = ErrStat ) - ! IF ( ErrStat /= ErrID_None ) THEN - ! ErrMsg = ' Error allocating wave kinematics vectors.' - ! RETURN - ! END IF - ! - ! - ! ! go through the nodes and fill in the data (this should maybe be turned into a global function) - ! J=0 - ! ! Body reference point coordinates - ! DO I = 1, p%nBodies - ! J = J + 1 - ! y%rAll(:,J) = m%BodyList(I)%r6(1:3) - ! END DO - ! ! Rod node coordinates - ! DO I = 1, p%nRods - ! DO K = 0,m%RodList(I)%N - ! J = J + 1 - ! y%rAll(:,J) = m%RodList(I)%r(:,K) - ! END DO - ! END DO - ! ! Point reference point coordinates - ! DO I = 1, p%nConnects - ! J = J + 1 - ! y%rAll(:,J) = m%ConnectList(I)%r - ! END DO - ! ! Line internal node coordinates - ! DO I = 1, p%nLines - ! DO K = 1,m%LineList(I)%N-1 - ! J = J + 1 - ! y%rAll(:,J) = m%LineList(I)%r(:,K) - ! END DO - ! END DO - - ! :::::::::::::::: the above might be used eventually. For now, let's store wave info grids within this module ::::::::::::::::: - ! ! allocate arrays - ! ntWave = SIZE(InitInp%WaveTime) - ! ALLOCATE ( p%ux (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ! ALLOCATE ( p%uy (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ! ALLOCATE ( p%uz (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ! ALLOCATE ( p%ax (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ! ALLOCATE ( p%ay (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ! ALLOCATE ( p%az (ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ! ALLOCATE ( p%PDyn(ntWave,WaveGrid_nz,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) - ! ALLOCATE ( p%zeta(ntWave,WaveGrid_ny,WaveGrid_nx), STAT = ErrStat2 ) ! 2D grid over x and y only - ! ALLOCATE ( p%px(WaveGrid_nx), STAT = ErrStat2 ) - ! ALLOCATE ( p%py(WaveGrid_ny), STAT = ErrStat2 ) - ! ALLOCATE ( p%pz(WaveGrid_nz), STAT = ErrStat2 ) - ! ALLOCATE ( p%tWave(ntWave), STAT = ErrStat2 ) - ! - ! ! get grid and time info (currenltly this is hard-coded to match what's in HydroDyn_Input - ! DO I=1,WaveGrid_nz - ! p%pz(I) = 1.0 - 2.0**(WaveGrid_nz-I) ! -127, -63, -31, -15, -7, -3, -1, 0 - ! END DO - ! DO J = 1,WaveGrid_ny - ! p%py(J) = WaveGrid_y0 + WaveGrid_dy*(J-1) - ! END DO - ! DO K = 1,WaveGrid_nx - ! p%px(K) = WaveGrid_x0 + WaveGrid_dx*(K-1) - ! END DO - ! - ! p%tWave = InitInp%WaveTime - ! - ! ! fill in the grid data (the for loops match those in HydroDyn_Input) - ! DO I=1,WaveGrid_nz - ! DO J = 1,WaveGrid_ny - ! DO K = 1,WaveGrid_nx - ! Itemp = (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node on 3D grid - ! - ! p%ux (:,I,J,K) = InitInp%WaveVel( :,Itemp,1) ! note: indices are t, z, y, x - ! p%uy (:,I,J,K) = InitInp%WaveVel( :,Itemp,2) - ! p%uz (:,I,J,K) = InitInp%WaveVel( :,Itemp,3) - ! p%ax (:,I,J,K) = InitInp%WaveAcc( :,Itemp,1) - ! p%ay (:,I,J,K) = InitInp%WaveAcc( :,Itemp,2) - ! p%az (:,I,J,K) = InitInp%WaveAcc( :,Itemp,3) - ! p%PDyn(:,I,J,K) = InitInp%WavePDyn(:,Itemp) - ! END DO - ! END DO - ! END DO - ! - ! ! fill in the grid data (the for loops match those in HydroDyn_Input) - ! DO J = 1,WaveGrid_ny - ! DO K = 1,WaveGrid_nx - ! Itemp = (J-1)*WaveGrid_nx + K ! index of actual node on surface 2D grid - ! p%zeta(:,J,K) = InitInp%WaveElev(:,Itemp) - ! END DO - ! END DO - ! - ! - ! ! write the date to an output file for testing purposes - ! - ! CALL GetNewUnit( UnOut) - ! - ! CALL OpenFOutFile ( UnOut, "waves.txt", ErrStat, ErrMsg ) - ! IF ( ErrStat > ErrID_None ) THEN - ! ErrMsg = ' Error opening wave grid file: '//TRIM(ErrMsg) - ! ErrStat = ErrID_Fatal - ! RETURN - ! END IF - ! - ! WRITE(UnOut, *, IOSTAT=ErrStat2) TRIM( 'MoorDyn v2 wave/current kinematics grid file' ) - ! WRITE(UnOut, *, IOSTAT=ErrStat2) TRIM( '---------------------------------------------' ) - ! WRITE(UnOut, *, IOSTAT=ErrStat2) TRIM( 'The following 6 lines (4-9) specify the input type then the inputs for x, then, y, then z coordinates.' ) - ! - ! WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - X input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) - ! Frmt = '('//TRIM(Int2LStr(5))//'(A1,e10.4))' - ! WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%px(I))), I=1,WaveGrid_nx ) - ! - ! WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - Y input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) - ! Frmt = '('//TRIM(Int2LStr(5))//'(A1,e10.4))' - ! WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%py(I))), I=1,WaveGrid_ny ) - ! - ! WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - Z input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) - ! Frmt = '('//TRIM(Int2LStr(8))//'(A1,e10.4))' - ! WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%pz(I))), I=1,WaveGrid_nz ) - ! - ! CLOSE(UnOut, IOSTAT = ErrStat ) - ! IF ( ErrStat /= 0 ) THEN - ! ErrMsg = 'Error closing wave grid file' - ! END IF - ! - ! - ! CALL GetNewUnit( UnOut) - ! - ! CALL OpenFOutFile ( UnOut, "wave data.txt", ErrStat, ErrMsg ) - ! IF ( ErrStat > ErrID_None ) THEN - ! ErrMsg = ' Error opening wave grid file: '//TRIM(ErrMsg) - ! ErrStat = ErrID_Fatal - ! RETURN - ! END IF - ! - ! ! write channel labels - ! - ! - ! ! time - ! WRITE(UnOut,"(A10)", IOSTAT=ErrStat2, advance="no") "Time" - ! - ! DO J = 1,WaveGrid_ny !y - ! DO K = 1,WaveGrid_nx !x - ! WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ze0", Num2Lstr(J+10*K) - ! END DO - ! END DO - ! DO I=1,WaveGrid_nz !z - ! DO J = 1,WaveGrid_ny !y - ! DO K = 1,WaveGrid_nx !x - ! WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ux", Num2Lstr(I+10*J+100*K) - ! WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " uy", Num2Lstr(I+10*J+100*K) - ! WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " uz", Num2Lstr(I+10*J+100*K) - ! END DO - ! END DO - ! END DO - ! DO I=1,WaveGrid_nz !z - ! DO J = 1,WaveGrid_ny !y - ! DO K = 1,WaveGrid_nx !x - ! WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ax", Num2Lstr(I+10*J+100*K) - ! WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ay", Num2Lstr(I+10*J+100*K) - ! WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " az", Num2Lstr(I+10*J+100*K) - ! END DO - ! END DO - ! END DO - ! DO I=1,WaveGrid_nz !z - ! DO J = 1,WaveGrid_ny !y - ! DO K = 1,WaveGrid_nx !x - ! WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " pd", Num2Lstr(I+10*J+100*K) - ! END DO - ! END DO - ! END DO - ! - ! ! end the line - ! WRITE(UnOut, "(A1)", IOSTAT=ErrStat2) " " - ! - ! - ! - ! DO l=1, SIZE(InitInp%WaveTime) ! loop through all time steps - ! - ! ! time - ! WRITE(UnOut,"(F10.4)", IOSTAT=ErrStat2, advance="no") p%tWave(l) - ! !WRITE(UnOut,"(F10.4)", IOSTAT=ErrStat2, advance="no") InitInp%WaveTime(l) - ! - ! ! wave elevation (all slices for now, to check) - ! DO J = 1,WaveGrid_ny !y - ! DO K = 1,WaveGrid_nx !x - ! Itemp = (J-1)*WaveGrid_nx + K ! index of actual node - ! - ! WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%zeta(l,J,K) - ! END DO - ! END DO - ! - ! ! wave velocities - ! DO I=1,WaveGrid_nz !z - ! DO J = 1,WaveGrid_ny !y - ! DO K = 1,WaveGrid_nx !x - ! Itemp = (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node - ! - ! WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%ux(l,I,J,K) - ! WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%uy(l,I,J,K) - ! WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%uz(l,I,J,K) - ! END DO - ! END DO - ! END DO - ! - ! ! wave accelerations - ! DO I=1,WaveGrid_nz !z - ! DO J = 1,WaveGrid_ny !y - ! DO K = 1,WaveGrid_nx !x - ! Itemp = (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node - ! - ! WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%ax(l,I,J,K) - ! WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%ay(l,I,J,K) - ! WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%az(l,I,J,K) - ! END DO - ! END DO - ! END DO - ! - ! ! dynamic pressure - ! DO I=1,WaveGrid_nz !z - ! DO J = 1,WaveGrid_ny !y - ! DO K = 1,WaveGrid_nx !x - ! Itemp = (I-1)*WaveGrid_nx*WaveGrid_ny + (J-1)*WaveGrid_nx + K ! index of actual node - ! - ! WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%PDyn(l,I,J,K) - ! END DO - ! END DO - ! END DO - ! - ! ! end the line - ! WRITE(UnOut, "(A1)", IOSTAT=ErrStat2) " " - ! - ! - ! END DO - ! - ! - ! CLOSE(UnOut, IOSTAT = ErrStat ) - ! IF ( ErrStat /= 0 ) THEN - ! ErrMsg = 'Error closing wave grid file' - ! END IF + m%WaveTi = 1 ! set initial wave grid time interpolation index to 1 to start with ! Frmt = '(A10,'//TRIM(Int2LStr(p%NumOuts))//'(A1,A12))' @@ -2529,7 +2285,7 @@ SUBROUTINE MD_CalcOutput( t, u, p, x, xd, z, other, y, m, ErrStat, ErrMsg ) ! END DO - ! ! go through nodes and apply wave kinematics from driver + ! ! go through nodes and apply wave kinematics from driver (if water kinematics were passed in at each node in future) ! IF (p%WaterKin > 0) THEN ! ! J=0 @@ -2608,7 +2364,7 @@ SUBROUTINE MD_CalcOutput( t, u, p, x, xd, z, other, y, m, ErrStat, ErrMsg ) end do - ! ! write all node positions to the node positons output array + ! ! write all node positions to the node positons output array (if water kinematics were passed in at each node in future) ! ! go through the nodes and fill in the data (this should maybe be turned into a global function) ! J=0 ! ! Body reference point coordinates @@ -2839,7 +2595,7 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er END DO - ! ! go through nodes and apply wave kinematics from driver + ! ! go through nodes and apply wave kinematics from driver (if water kinematics were passed in at each node in future) ! IF (p%WaterKin > 0) THEN ! ! J=0 @@ -3225,3503 +2981,141 @@ END SUBROUTINE TimeStep - !----------------------------------------------------------------------- - ! >>>>>>>>>>>>>> rename/reorganize this subroutine >>>>>>>>>>>>> - SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) - ! allocate arrays in line object - - TYPE(MD_Line), INTENT(INOUT) :: Line ! the single line object of interest - TYPE(MD_LineProp), INTENT(INOUT) :: LineProp ! the single line property set for the line of interest - REAL(DbKi), INTENT(IN) :: rhoW - INTEGER, INTENT( INOUT ) :: ErrStat ! returns a non-zero value when an error occurs - CHARACTER(*), INTENT( INOUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None - - INTEGER(4) :: I, J, K ! Generic index - INTEGER(IntKi) :: N - REAL(DbKi) :: temp - - - N = Line%N ! number of segments in this line (for code readability) - - ! -------------- save some section properties to the line object itself ----------------- - - Line%d = LineProp%d - Line%rho = LineProp%w/(Pi/4.0 * Line%d * Line%d) - - Line%EA = LineProp%EA - ! note: Line%BA is set later - Line%EA_D = LineProp%EA_D - Line%BA_D = LineProp%BA_D - !Line%EI = LineProp%EI <<< for bending stiffness - - Line%Can = LineProp%Can - Line%Cat = LineProp%Cat - Line%Cdn = LineProp%Cdn - Line%Cdt = LineProp%Cdt - - ! copy over elasticity data - Line%ElasticMod = LineProp%ElasticMod - - Line%nEApoints = LineProp%nEApoints - DO I = 1,Line%nEApoints - Line%stiffXs(I) = LineProp%stiffXs(I) - Line%stiffYs(I) = LineProp%stiffYs(I) ! note: this does not convert to E (not EA) like done in C version - END DO - - Line%nBApoints = LineProp%nBApoints - DO I = 1,Line%nBApoints - Line%dampXs(I) = LineProp%dampXs(I) - Line%dampYs(I) = LineProp%dampYs(I) - END DO - - Line%nEIpoints = LineProp%nEIpoints - DO I = 1,Line%nEIpoints - Line%bstiffXs(I) = LineProp%bstiffXs(I) - Line%bstiffYs(I) = LineProp%bstiffYs(I) ! copy over - END DO - - - ! Specify specific internal damping coefficient (BA) for this line. - ! Will be equal to inputted BA of LineType if input value is positive. - ! If input value is negative, it is considered to be desired damping ratio (zeta) - ! from which the line's BA can be calculated based on the segment natural frequency. - IF (LineProp%BA < 0) THEN - ! - we assume desired damping coefficient is zeta = -LineProp%BA - ! - highest axial vibration mode of a segment is wn = sqrt(k/m) = 2N/UnstrLen*sqrt(EA/w) - Line%BA = -LineProp%BA * Line%UnstrLen / Line%N * SQRT(LineProp%EA * LineProp%w) - IF (wordy > 1) print *, 'Based on zeta, BA set to ', Line%BA - - IF (wordy > 1) print *, 'Negative BA input detected, treating as -zeta. For zeta = ', -LineProp%BA, ', setting BA to ', Line%BA - - ELSE - Line%BA = LineProp%BA - IF (wordy > 1) temp = Line%N * Line%BA / Line%UnstrLen * SQRT(1.0/(LineProp%EA * LineProp%w)) - IF (wordy > 1) print *, 'BA set as input to ', Line%BA, '. Corresponding zeta is ', temp - END IF - - !temp = 2*Line%N / Line%UnstrLen * sqrt( LineProp%EA / LineProp%w) / TwoPi - !print *, 'Segment natural frequency is ', temp, ' Hz' - - - - ! allocate node positions and velocities (NOTE: these arrays start at ZERO) - ALLOCATE ( Line%r(3, 0:N), Line%rd(3, 0:N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN - ErrMsg = ' Error allocating r and rd arrays.' - !CALL CleanUp() - RETURN - END IF - - ! if using viscoelastic model, allocate additional state quantities - if (Line%ElasticMod == 2) then - ALLOCATE ( Line%dl_1(N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN - ErrMsg = ' Error allocating dl_1 array.' - !CALL CleanUp() - RETURN - END IF - ! initialize to zero - Line%dl_1 = 0.0_DbKi - end if - - ! allocate node and segment tangent vectors - ALLOCATE ( Line%q(3, 0:N), Line%qs(3, N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN - ErrMsg = ' Error allocating q or qs array.' - !CALL CleanUp() - RETURN - END IF - - ! allocate segment scalar quantities - ALLOCATE ( Line%l(N), Line%ld(N), Line%lstr(N), Line%lstrd(N), Line%V(N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN - ErrMsg = ' Error allocating segment scalar quantity arrays.' - !CALL CleanUp() - RETURN - END IF - - ! assign values for l, ld, and V - DO J=1,N - Line%l(J) = Line%UnstrLen/REAL(N, DbKi) - Line%ld(J)= 0.0_DbKi - Line%V(J) = Line%l(J)*0.25*Pi*LineProp%d*LineProp%d - END DO - - ! allocate water related vectors - ALLOCATE ( Line%U(3, 0:N), Line%Ud(3, 0:N), Line%zeta(0:N), Line%PDyn(0:N), STAT = ErrStat ) - ! set to zero initially (important of wave kinematics are not being used) - Line%U = 0.0_DbKi - Line%Ud = 0.0_DbKi - Line%zeta = 0.0_DbKi - Line%PDyn = 0.0_DbKi - - ! allocate segment tension and internal damping force vectors - ALLOCATE ( Line%T(3, N), Line%Td(3, N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN - ErrMsg = ' Error allocating T and Td arrays.' - !CALL CleanUp() - RETURN - END IF - - ! allocate node force vectors - ALLOCATE ( Line%W(3, 0:N), Line%Dp(3, 0:N), Line%Dq(3, 0:N), Line%Ap(3, 0:N), & - Line%Aq(3, 0:N), Line%B(3, 0:N), Line%Fnet(3, 0:N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN - ErrMsg = ' Error allocating node force arrays.' - !CALL CleanUp() - RETURN - END IF - - ! set gravity and bottom contact forces to zero initially (because the horizontal components should remain at zero) - DO J = 0,N - DO K = 1,3 - Line%W(K,J) = 0.0_DbKi - Line%B(K,J) = 0.0_DbKi - END DO - END DO - - ! allocate mass and inverse mass matrices for each node (including ends) - ALLOCATE ( Line%S(3, 3, 0:N), Line%M(3, 3, 0:N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN - ErrMsg = ' Error allocating T and Td arrays.' - !CALL CleanUp() - RETURN - END IF - - - - ! need to add cleanup sub <<< - - - END SUBROUTINE SetupLine - !-------------------------------------------------------------- - - - - - - !----------------------------------------------------------------------------------------======= - SUBROUTINE Line_Initialize (Line, LineProp, rhoW, ErrStat, ErrMsg) - ! calculate initial profile of the line using quasi-static model - - TYPE(MD_Line), INTENT(INOUT) :: Line ! the single line object of interest - TYPE(MD_LineProp), INTENT(INOUT) :: LineProp ! the single line property set for the line of interest - REAL(DbKi), INTENT(IN) :: rhoW - INTEGER, INTENT( INOUT ) :: ErrStat ! returns a non-zero value when an error occurs - CHARACTER(*), INTENT( INOUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None - - REAL(DbKi) :: COSPhi ! Cosine of the angle between the xi-axis of the inertia frame and the X-axis of the local coordinate system of the current mooring line (-) - REAL(DbKi) :: SINPhi ! Sine of the angle between the xi-axis of the inertia frame and the X-axis of the local coordinate system of the current mooring line (-) - REAL(DbKi) :: XF ! Horizontal distance between anchor and fairlead of the current mooring line (meters) - REAL(DbKi) :: ZF ! Vertical distance between anchor and fairlead of the current mooring line (meters) - INTEGER(4) :: I ! Generic index - INTEGER(4) :: J ! Generic index - - - INTEGER(IntKi) :: ErrStat2 ! Error status of the operation - CHARACTER(ErrMsgLen) :: ErrMsg2 ! Error message if ErrStat2 /= ErrID_None - REAL(DbKi) :: WetWeight - REAL(DbKi) :: SeabedCD = 0.0_DbKi - REAL(DbKi) :: TenTol = 0.0001_DbKi - REAL(DbKi), ALLOCATABLE :: LSNodes(:) - REAL(DbKi), ALLOCATABLE :: LNodesX(:) - REAL(DbKi), ALLOCATABLE :: LNodesZ(:) - INTEGER(IntKi) :: N - - - N = Line%N ! for convenience - - ! try to calculate initial line profile using catenary routine (from FAST v.7) - ! note: much of this function is adapted from the FAST source code - - ! Transform the fairlead location from the inertial frame coordinate system - ! to the local coordinate system of the current line (this coordinate - ! system lies at the current anchor, Z being vertical, and X directed from - ! current anchor to the current fairlead). Also, compute the orientation - ! of this local coordinate system: - - XF = SQRT( ( Line%r(1,N) - Line%r(1,0) )**2.0 + ( Line%r(2,N) - Line%r(2,0) )**2.0 ) - ZF = Line%r(3,N) - Line%r(3,0) - - IF ( XF == 0.0 ) THEN ! .TRUE. if the current mooring line is exactly vertical; thus, the solution below is ill-conditioned because the orientation is undefined; so set it such that the tensions and nodal positions are only vertical - COSPhi = 0.0_DbKi - SINPhi = 0.0_DbKi - ELSE ! The current mooring line must not be vertical; use simple trigonometry - COSPhi = ( Line%r(1,N) - Line%r(1,0) )/XF - SINPhi = ( Line%r(2,N) - Line%r(2,0) )/XF - ENDIF - - WetWeight = LineProp%w - 0.25*Pi*LineProp%d*LineProp%d*rhoW - - !LineNodes = Line%N + 1 ! number of nodes in line for catenary model to worry about - - ! allocate temporary arrays for catenary routine - ALLOCATE ( LSNodes(N+1), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN - ErrMsg = ' Error allocating LSNodes array.' - CALL CleanUp() - RETURN - END IF - - ALLOCATE ( LNodesX(N+1), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN - ErrMsg = ' Error allocating LNodesX array.' - CALL CleanUp() - RETURN - END IF - - ALLOCATE ( LNodesZ(N+1), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN - ErrMsg = ' Error allocating LNodesZ array.' - CALL CleanUp() - RETURN - END IF - - ! Assign node arc length locations - LSNodes(1) = 0.0_DbKi - DO I=2,N - LSNodes(I) = LSNodes(I-1) + Line%l(I-1) ! note: l index is because line segment indices start at 1 - END DO - LSNodes(N+1) = Line%UnstrLen ! ensure the last node length isn't longer than the line due to numerical error - - ! Solve the analytical, static equilibrium equations for a catenary (or - ! taut) mooring line with seabed interaction in order to find the - ! horizontal and vertical tensions at the fairlead in the local coordinate - ! system of the current line: - ! NOTE: The values for the horizontal and vertical tensions at the fairlead - ! from the previous time step are used as the initial guess values at - ! at this time step (because the LAnchHTe(:) and LAnchVTe(:) arrays - ! are stored in a module and thus their values are saved from CALL to - ! CALL). - - - CALL Catenary ( XF , ZF , Line%UnstrLen, LineProp%EA , & - WetWeight , SeabedCD, TenTol, (N+1) , & - LSNodes, LNodesX, LNodesZ , ErrStat2, ErrMsg2) - - IF (ErrStat2 == ErrID_None) THEN ! if it worked, use it - ! Transform the positions of each node on the current line from the local - ! coordinate system of the current line to the inertial frame coordinate - ! system: - - DO J = 0,N ! Loop through all nodes per line where the line position and tension can be output - Line%r(1,J) = Line%r(1,0) + LNodesX(J+1)*COSPhi - Line%r(2,J) = Line%r(2,0) + LNodesX(J+1)*SINPhi - Line%r(3,J) = Line%r(3,0) + LNodesZ(J+1) - ENDDO ! J - All nodes per line where the line position and tension can be output - - - ELSE ! if there is a problem with the catenary approach, just stretch the nodes linearly between fairlead and anchor - - CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Line_Initialize') - -! print *, "Node positions: " - - DO J = 0,N ! Loop through all nodes per line where the line position and tension can be output - Line%r(1,J) = Line%r(1,0) + (Line%r(1,N) - Line%r(1,0))*REAL(J, DbKi)/REAL(N, DbKi) - Line%r(2,J) = Line%r(2,0) + (Line%r(2,N) - Line%r(2,0))*REAL(J, DbKi)/REAL(N, DbKi) - Line%r(3,J) = Line%r(3,0) + (Line%r(3,N) - Line%r(3,0))*REAL(J, DbKi)/REAL(N, DbKi) - -! print*, Line%r(:,J) - ENDDO - -! print*,"FYI line end A and B node coords are" -! print*, Line%r(:,0) -! print*, Line%r(:,N) - ENDIF - - - - CALL CleanUp() ! deallocate temporary arrays - - - - CONTAINS - - - !----------------------------------------------------------------------- - SUBROUTINE CleanUp() - ! deallocate temporary arrays - - IF (ALLOCATED(LSNodes)) DEALLOCATE(LSNodes) - IF (ALLOCATED(LNodesX)) DEALLOCATE(LNodesX) - IF (ALLOCATED(LNodesZ)) DEALLOCATE(LNodesZ) - - END SUBROUTINE CleanUp - !----------------------------------------------------------------------- - - - !----------------------------------------------------------------------- - SUBROUTINE Catenary ( XF_In, ZF_In, L_In , EA_In, & - W_In , CB_In, Tol_In, N , & - s_In , X_In , Z_In , ErrStat, ErrMsg ) - - ! This subroutine is copied from FAST v7 with minor modifications - - ! This routine solves the analytical, static equilibrium equations - ! for a catenary (or taut) mooring line with seabed interaction. - ! Stretching of the line is accounted for, but bending stiffness - ! is not. Given the mooring line properties and the fairlead - ! position relative to the anchor, this routine finds the line - ! configuration and tensions. Since the analytical solution - ! involves two nonlinear equations (XF and ZF) in two unknowns - ! (HF and VF), a Newton-Raphson iteration scheme is implemented in - ! order to solve for the solution. The values of HF and VF that - ! are passed into this routine are used as the initial guess in - ! the iteration. The Newton-Raphson iteration is only accurate in - ! double precision, so all of the input/output arguments are - ! converteds to/from double precision from/to default precision. - - ! >>>> TO DO: streamline this function, if it's still to be used at all <<<< - - ! USE Precision - - - IMPLICIT NONE - - - ! Passed Variables: - - INTEGER(4), INTENT(IN ) :: N ! Number of nodes where the line position and tension can be output (-) - - REAL(DbKi), INTENT(IN ) :: CB_In ! Coefficient of seabed static friction drag (a negative value indicates no seabed) (-) - REAL(DbKi), INTENT(IN ) :: EA_In ! Extensional stiffness of line (N) - ! REAL(DbKi), INTENT( OUT) :: HA_In ! Effective horizontal tension in line at the anchor (N) - ! REAL(DbKi), INTENT(INOUT) :: HF_In ! Effective horizontal tension in line at the fairlead (N) - REAL(DbKi), INTENT(IN ) :: L_In ! Unstretched length of line (meters) - REAL(DbKi), INTENT(IN ) :: s_In (N) ! Unstretched arc distance along line from anchor to each node where the line position and tension can be output (meters) - ! REAL(DbKi), INTENT( OUT) :: Te_In (N) ! Effective line tensions at each node (N) - REAL(DbKi), INTENT(IN ) :: Tol_In ! Convergence tolerance within Newton-Raphson iteration specified as a fraction of tension (-) - ! REAL(DbKi), INTENT( OUT) :: VA_In ! Effective vertical tension in line at the anchor (N) - ! REAL(DbKi), INTENT(INOUT) :: VF_In ! Effective vertical tension in line at the fairlead (N) - REAL(DbKi), INTENT(IN ) :: W_In ! Weight of line in fluid per unit length (N/m) - REAL(DbKi), INTENT( OUT) :: X_In (N) ! Horizontal locations of each line node relative to the anchor (meters) - REAL(DbKi), INTENT(IN ) :: XF_In ! Horizontal distance between anchor and fairlead (meters) - REAL(DbKi), INTENT( OUT) :: Z_In (N) ! Vertical locations of each line node relative to the anchor (meters) - REAL(DbKi), INTENT(IN ) :: ZF_In ! Vertical distance between anchor and fairlead (meters) - INTEGER, INTENT( OUT ) :: ErrStat ! returns a non-zero value when an error occurs - CHARACTER(*), INTENT( OUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None - - - ! Local Variables: - - REAL(DbKi) :: CB ! Coefficient of seabed static friction (a negative value indicates no seabed) (-) - REAL(DbKi) :: CBOvrEA ! = CB/EA - REAL(DbKi) :: DET ! Determinant of the Jacobian matrix (m^2/N^2) - REAL(DbKi) :: dHF ! Increment in HF predicted by Newton-Raphson (N) - REAL(DbKi) :: dVF ! Increment in VF predicted by Newton-Raphson (N) - REAL(DbKi) :: dXFdHF ! Partial derivative of the calculated horizontal distance with respect to the horizontal fairlead tension (m/N): dXF(HF,VF)/dHF - REAL(DbKi) :: dXFdVF ! Partial derivative of the calculated horizontal distance with respect to the vertical fairlead tension (m/N): dXF(HF,VF)/dVF - REAL(DbKi) :: dZFdHF ! Partial derivative of the calculated vertical distance with respect to the horizontal fairlead tension (m/N): dZF(HF,VF)/dHF - REAL(DbKi) :: dZFdVF ! Partial derivative of the calculated vertical distance with respect to the vertical fairlead tension (m/N): dZF(HF,VF)/dVF - REAL(DbKi) :: EA ! Extensional stiffness of line (N) - REAL(DbKi) :: EXF ! Error function between calculated and known horizontal distance (meters): XF(HF,VF) - XF - REAL(DbKi) :: EZF ! Error function between calculated and known vertical distance (meters): ZF(HF,VF) - ZF - REAL(DbKi) :: HA ! Effective horizontal tension in line at the anchor (N) - REAL(DbKi) :: HF ! Effective horizontal tension in line at the fairlead (N) - REAL(DbKi) :: HFOvrW ! = HF/W - REAL(DbKi) :: HFOvrWEA ! = HF/WEA - REAL(DbKi) :: L ! Unstretched length of line (meters) - REAL(DbKi) :: Lamda0 ! Catenary parameter used to generate the initial guesses of the horizontal and vertical tensions at the fairlead for the Newton-Raphson iteration (-) - REAL(DbKi) :: LMax ! Maximum stretched length of the line with seabed interaction beyond which the line would have to double-back on itself; here the line forms an "L" between the anchor and fairlead (i.e. it is horizontal along the seabed from the anchor, then vertical to the fairlead) (meters) - REAL(DbKi) :: LMinVFOvrW ! = L - VF/W - REAL(DbKi) :: LOvrEA ! = L/EA - REAL(DbKi) :: s (N) ! Unstretched arc distance along line from anchor to each node where the line position and tension can be output (meters) - REAL(DbKi) :: sOvrEA ! = s(I)/EA - REAL(DbKi) :: SQRT1VFOvrHF2 ! = SQRT( 1.0_DbKi + VFOvrHF2 ) - REAL(DbKi) :: SQRT1VFMinWLOvrHF2 ! = SQRT( 1.0_DbKi + VFMinWLOvrHF2 ) - REAL(DbKi) :: SQRT1VFMinWLsOvrHF2 ! = SQRT( 1.0_DbKi + VFMinWLsOvrHF*VFMinWLsOvrHF ) - REAL(DbKi) :: Te (N) ! Effective line tensions at each node (N) - REAL(DbKi) :: Tol ! Convergence tolerance within Newton-Raphson iteration specified as a fraction of tension (-) - REAL(DbKi) :: VA ! Effective vertical tension in line at the anchor (N) - REAL(DbKi) :: VF ! Effective vertical tension in line at the fairlead (N) - REAL(DbKi) :: VFMinWL ! = VF - WL - REAL(DbKi) :: VFMinWLOvrHF ! = VFMinWL/HF - REAL(DbKi) :: VFMinWLOvrHF2 ! = VFMinWLOvrHF*VFMinWLOvrHF - REAL(DbKi) :: VFMinWLs ! = VFMinWL + Ws - REAL(DbKi) :: VFMinWLsOvrHF ! = VFMinWLs/HF - REAL(DbKi) :: VFOvrHF ! = VF/HF - REAL(DbKi) :: VFOvrHF2 ! = VFOvrHF*VFOvrHF - REAL(DbKi) :: VFOvrWEA ! = VF/WEA - REAL(DbKi) :: W ! Weight of line in fluid per unit length (N/m) - REAL(DbKi) :: WEA ! = W*EA - REAL(DbKi) :: WL ! Total weight of line in fluid (N): W*L - REAL(DbKi) :: Ws ! = W*s(I) - REAL(DbKi) :: X (N) ! Horizontal locations of each line node relative to the anchor (meters) - REAL(DbKi) :: XF ! Horizontal distance between anchor and fairlead (meters) - REAL(DbKi) :: XF2 ! = XF*XF - REAL(DbKi) :: Z (N) ! Vertical locations of each line node relative to the anchor (meters) - REAL(DbKi) :: ZF ! Vertical distance between anchor and fairlead (meters) - REAL(DbKi) :: ZF2 ! = ZF*ZF - - INTEGER(4) :: I ! Index for counting iterations or looping through line nodes (-) - INTEGER(4) :: MaxIter ! Maximum number of Newton-Raphson iterations possible before giving up (-) - - LOGICAL :: FirstIter ! Flag to determine whether or not this is the first time through the Newton-Raphson interation (flag) - - - ErrStat = ERrId_None - - - ! The Newton-Raphson iteration is only accurate in double precision, so - ! convert the input arguments into double precision: - - CB = REAL( CB_In , DbKi ) - EA = REAL( EA_In , DbKi ) - HF = 0.0_DbKi ! = REAL( HF_In , DbKi ) - L = REAL( L_In , DbKi ) - s (:) = REAL( s_In (:), DbKi ) - Tol = REAL( Tol_In , DbKi ) - VF = 0.0_DbKi ! keeping this for some error catching functionality? (at first glance) ! VF = REAL( VF_In , DbKi ) - W = REAL( W_In , DbKi ) - XF = REAL( XF_In , DbKi ) - ZF = REAL( ZF_In , DbKi ) - - - - ! HF and VF cannot be initialized to zero when a portion of the line rests on the seabed and the anchor tension is nonzero - - ! Generate the initial guess values for the horizontal and vertical tensions - ! at the fairlead in the Newton-Raphson iteration for the catenary mooring - ! line solution. Use starting values documented in: Peyrot, Alain H. and - ! Goulois, A. M., "Analysis Of Cable Structures," Computers & Structures, - ! Vol. 10, 1979, pp. 805-813: - XF2 = XF*XF - ZF2 = ZF*ZF - - IF ( XF == 0.0_DbKi ) THEN ! .TRUE. if the current mooring line is exactly vertical - Lamda0 = 1.0D+06 - ELSEIF ( L <= SQRT( XF2 + ZF2 ) ) THEN ! .TRUE. if the current mooring line is taut - Lamda0 = 0.2_DbKi - ELSE ! The current mooring line must be slack and not vertical - Lamda0 = SQRT( 3.0_DbKi*( ( L**2 - ZF2 )/XF2 - 1.0_DbKi ) ) - ENDIF - - HF = ABS( 0.5_DbKi*W* XF/ Lamda0 ) - VF = 0.5_DbKi*W*( ZF/TANH(Lamda0) + L ) - - - ! Abort when there is no solution or when the only possible solution is - ! illogical: - - IF ( Tol <= EPSILON(TOL) ) THEN ! .TRUE. when the convergence tolerance is specified incorrectly - ErrStat = ErrID_Warn - ErrMsg = ' Convergence tolerance must be greater than zero in routine Catenary().' - return - ELSEIF ( XF < 0.0_DbKi ) THEN ! .TRUE. only when the local coordinate system is not computed correctly - ErrStat = ErrID_Warn - ErrMsg = ' The horizontal distance between an anchor and its'// & - ' fairlead must not be less than zero in routine Catenary().' - return - - ELSEIF ( ZF < 0.0_DbKi ) THEN ! .TRUE. if the fairlead has passed below its anchor - ErrStat = ErrID_Warn - ErrMsg = " A line's fairlead is defined as below its anchor. You may need to swap a line's fairlead and anchor end nodes." - return - - ELSEIF ( L <= 0.0_DbKi ) THEN ! .TRUE. when the unstretched line length is specified incorrectly - ErrStat = ErrID_Warn - ErrMsg = ' Unstretched length of line must be greater than zero in routine Catenary().' - return - - ELSEIF ( EA <= 0.0_DbKi ) THEN ! .TRUE. when the unstretched line length is specified incorrectly - ErrStat = ErrID_Warn - ErrMsg = ' Extensional stiffness of line must be greater than zero in routine Catenary().' - return - - ELSEIF ( W == 0.0_DbKi ) THEN ! .TRUE. when the weight of the line in fluid is zero so that catenary solution is ill-conditioned - ErrStat = ErrID_Warn - ErrMsg = ' The weight of the line in fluid must not be zero. '// & - ' Routine Catenary() cannot solve quasi-static mooring line solution.' - return - - - ELSEIF ( W > 0.0_DbKi ) THEN ! .TRUE. when the line will sink in fluid - - LMax = XF - EA/W + SQRT( (EA/W)*(EA/W) + 2.0_DbKi*ZF*EA/W ) ! Compute the maximum stretched length of the line with seabed interaction beyond which the line would have to double-back on itself; here the line forms an "L" between the anchor and fairlead (i.e. it is horizontal along the seabed from the anchor, then vertical to the fairlead) - - IF ( ( L >= LMax ) .AND. ( CB >= 0.0_DbKi ) ) then ! .TRUE. if the line is as long or longer than its maximum possible value with seabed interaction - ErrStat = ErrID_Warn - ErrMsg = ' Unstretched mooring line length too large. '// & - ' Routine Catenary() cannot solve quasi-static mooring line solution.' - return - END IF - - ENDIF - - - ! Initialize some commonly used terms that don't depend on the iteration: - - WL = W *L - WEA = W *EA - LOvrEA = L /EA - CBOvrEA = CB /EA - MaxIter = INT(1.0_DbKi/Tol) ! Smaller tolerances may take more iterations, so choose a maximum inversely proportional to the tolerance - - - - ! To avoid an ill-conditioned situation, ensure that the initial guess for - ! HF is not less than or equal to zero. Similarly, avoid the problems - ! associated with having exactly vertical (so that HF is zero) or exactly - ! horizontal (so that VF is zero) lines by setting the minimum values - ! equal to the tolerance. This prevents us from needing to implement - ! the known limiting solutions for vertical or horizontal lines (and thus - ! complicating this routine): - - HF = MAX( HF, Tol ) - XF = MAX( XF, Tol ) - ZF = MAX( ZF, TOl ) - - - - ! Solve the analytical, static equilibrium equations for a catenary (or - ! taut) mooring line with seabed interaction: - - ! Begin Newton-Raphson iteration: - - I = 1 ! Initialize iteration counter - FirstIter = .TRUE. ! Initialize iteration flag - - DO - - - ! Initialize some commonly used terms that depend on HF and VF: - - VFMinWL = VF - WL - LMinVFOvrW = L - VF/W - HFOvrW = HF/W - HFOvrWEA = HF/WEA - VFOvrWEA = VF/WEA - VFOvrHF = VF/HF - VFMinWLOvrHF = VFMinWL/HF - VFOvrHF2 = VFOvrHF *VFOvrHF - VFMinWLOvrHF2 = VFMinWLOvrHF*VFMinWLOvrHF - SQRT1VFOvrHF2 = SQRT( 1.0_DbKi + VFOvrHF2 ) - SQRT1VFMinWLOvrHF2 = SQRT( 1.0_DbKi + VFMinWLOvrHF2 ) - - - ! Compute the error functions (to be zeroed) and the Jacobian matrix - ! (these depend on the anticipated configuration of the mooring line): - - IF ( ( CB < 0.0_DbKi ) .OR. ( W < 0.0_DbKi ) .OR. ( VFMinWL > 0.0_DbKi ) ) THEN ! .TRUE. when no portion of the line rests on the seabed - - EXF = ( LOG( VFOvrHF + SQRT1VFOvrHF2 ) & - - LOG( VFMinWLOvrHF + SQRT1VFMinWLOvrHF2 ) )*HFOvrW & - + LOvrEA* HF - XF - EZF = ( SQRT1VFOvrHF2 & - - SQRT1VFMinWLOvrHF2 )*HFOvrW & - + LOvrEA*( VF - 0.5_DbKi*WL ) - ZF - - dXFdHF = ( LOG( VFOvrHF + SQRT1VFOvrHF2 ) & - - LOG( VFMinWLOvrHF + SQRT1VFMinWLOvrHF2 ) )/ W & - - ( ( VFOvrHF + VFOvrHF2 /SQRT1VFOvrHF2 )/( VFOvrHF + SQRT1VFOvrHF2 ) & - - ( VFMinWLOvrHF + VFMinWLOvrHF2/SQRT1VFMinWLOvrHF2 )/( VFMinWLOvrHF + SQRT1VFMinWLOvrHF2 ) )/ W & - + LOvrEA - dXFdVF = ( ( 1.0_DbKi + VFOvrHF /SQRT1VFOvrHF2 )/( VFOvrHF + SQRT1VFOvrHF2 ) & - - ( 1.0_DbKi + VFMinWLOvrHF /SQRT1VFMinWLOvrHF2 )/( VFMinWLOvrHF + SQRT1VFMinWLOvrHF2 ) )/ W - dZFdHF = ( SQRT1VFOvrHF2 & - - SQRT1VFMinWLOvrHF2 )/ W & - - ( VFOvrHF2 /SQRT1VFOvrHF2 & - - VFMinWLOvrHF2/SQRT1VFMinWLOvrHF2 )/ W - dZFdVF = ( VFOvrHF /SQRT1VFOvrHF2 & - - VFMinWLOvrHF /SQRT1VFMinWLOvrHF2 )/ W & - + LOvrEA - - - ELSEIF ( -CB*VFMinWL < HF ) THEN ! .TRUE. when a portion of the line rests on the seabed and the anchor tension is nonzero - - EXF = LOG( VFOvrHF + SQRT1VFOvrHF2 ) *HFOvrW & - - 0.5_DbKi*CBOvrEA*W* LMinVFOvrW*LMinVFOvrW & - + LOvrEA* HF + LMinVFOvrW - XF - EZF = ( SQRT1VFOvrHF2 - 1.0_DbKi )*HFOvrW & - + 0.5_DbKi*VF*VFOvrWEA - ZF - - dXFdHF = LOG( VFOvrHF + SQRT1VFOvrHF2 ) / W & - - ( ( VFOvrHF + VFOvrHF2 /SQRT1VFOvrHF2 )/( VFOvrHF + SQRT1VFOvrHF2 ) )/ W & - + LOvrEA - dXFdVF = ( ( 1.0_DbKi + VFOvrHF /SQRT1VFOvrHF2 )/( VFOvrHF + SQRT1VFOvrHF2 ) )/ W & - + CBOvrEA*LMinVFOvrW - 1.0_DbKi/W - dZFdHF = ( SQRT1VFOvrHF2 - 1.0_DbKi & - - VFOvrHF2 /SQRT1VFOvrHF2 )/ W - dZFdVF = ( VFOvrHF /SQRT1VFOvrHF2 )/ W & - + VFOvrWEA - - - ELSE ! 0.0_DbKi < HF <= -CB*VFMinWL ! A portion of the line must rest on the seabed and the anchor tension is zero - - EXF = LOG( VFOvrHF + SQRT1VFOvrHF2 ) *HFOvrW & - - 0.5_DbKi*CBOvrEA*W*( LMinVFOvrW*LMinVFOvrW - ( LMinVFOvrW - HFOvrW/CB )*( LMinVFOvrW - HFOvrW/CB ) ) & - + LOvrEA* HF + LMinVFOvrW - XF - EZF = ( SQRT1VFOvrHF2 - 1.0_DbKi )*HFOvrW & - + 0.5_DbKi*VF*VFOvrWEA - ZF - - dXFdHF = LOG( VFOvrHF + SQRT1VFOvrHF2 ) / W & - - ( ( VFOvrHF + VFOvrHF2 /SQRT1VFOvrHF2 )/( VFOvrHF + SQRT1VFOvrHF2 ) )/ W & - + LOvrEA - ( LMinVFOvrW - HFOvrW/CB )/EA - dXFdVF = ( ( 1.0_DbKi + VFOvrHF /SQRT1VFOvrHF2 )/( VFOvrHF + SQRT1VFOvrHF2 ) )/ W & - + HFOvrWEA - 1.0_DbKi/W - dZFdHF = ( SQRT1VFOvrHF2 - 1.0_DbKi & - - VFOvrHF2 /SQRT1VFOvrHF2 )/ W - dZFdVF = ( VFOvrHF /SQRT1VFOvrHF2 )/ W & - + VFOvrWEA - - - ENDIF - - - ! Compute the determinant of the Jacobian matrix and the incremental - ! tensions predicted by Newton-Raphson: - - - DET = dXFdHF*dZFdVF - dXFdVF*dZFdHF - - if ( EqualRealNos( DET, 0.0_DbKi ) ) then -!bjj: there is a serious problem with the debugger here when DET = 0 - ErrStat = ErrID_Warn - ErrMsg = ' Iteration not convergent (DET is 0). '// & - ' Routine Catenary() cannot solve quasi-static mooring line solution.' - return - endif - - - dHF = ( -dZFdVF*EXF + dXFdVF*EZF )/DET ! This is the incremental change in horizontal tension at the fairlead as predicted by Newton-Raphson - dVF = ( dZFdHF*EXF - dXFdHF*EZF )/DET ! This is the incremental change in vertical tension at the fairlead as predicted by Newton-Raphson +!-------------------------------------------------------------- +! Connection-Specific Subroutines +!-------------------------------------------------------------- - dHF = dHF*( 1.0_DbKi - Tol*I ) ! Reduce dHF by factor (between 1 at I = 1 and 0 at I = MaxIter) that reduces linearly with iteration count to ensure that we converge on a solution even in the case were we obtain a nonconvergent cycle about the correct solution (this happens, for example, if we jump to quickly between a taut and slack catenary) - dVF = dVF*( 1.0_DbKi - Tol*I ) ! Reduce dHF by factor (between 1 at I = 1 and 0 at I = MaxIter) that reduces linearly with iteration count to ensure that we converge on a solution even in the case were we obtain a nonconvergent cycle about the correct solution (this happens, for example, if we jump to quickly between a taut and slack catenary) - dHF = MAX( dHF, ( Tol - 1.0_DbKi )*HF ) ! To avoid an ill-conditioned situation, make sure HF does not go less than or equal to zero by having a lower limit of Tol*HF [NOTE: the value of dHF = ( Tol - 1.0_DbKi )*HF comes from: HF = HF + dHF = Tol*HF when dHF = ( Tol - 1.0_DbKi )*HF] - ! Check if we have converged on a solution, or restart the iteration, or - ! Abort if we cannot find a solution: - IF ( ( ABS(dHF) <= ABS(Tol*HF) ) .AND. ( ABS(dVF) <= ABS(Tol*VF) ) ) THEN ! .TRUE. if we have converged; stop iterating! [The converge tolerance, Tol, is a fraction of tension] +!-------------------------------------------------------------- +! Rod-Specific Subroutines +!-------------------------------------------------------------- - EXIT - ELSEIF ( ( I == MaxIter ) .AND. ( FirstIter ) ) THEN ! .TRUE. if we've iterated MaxIter-times for the first time; - ! Perhaps we failed to converge because our initial guess was too far off. - ! (This could happen, for example, while linearizing a model via large - ! pertubations in the DOFs.) Instead, use starting values documented in: - ! Peyrot, Alain H. and Goulois, A. M., "Analysis Of Cable Structures," - ! Computers & Structures, Vol. 10, 1979, pp. 805-813: - ! NOTE: We don't need to check if the current mooring line is exactly - ! vertical (i.e., we don't need to check if XF == 0.0), because XF is - ! limited by the tolerance above. - XF2 = XF*XF - ZF2 = ZF*ZF - IF ( L <= SQRT( XF2 + ZF2 ) ) THEN ! .TRUE. if the current mooring line is taut - Lamda0 = 0.2_DbKi - ELSE ! The current mooring line must be slack and not vertical - Lamda0 = SQRT( 3.0_DbKi*( ( L*L - ZF2 )/XF2 - 1.0_DbKi ) ) - ENDIF - HF = MAX( ABS( 0.5_DbKi*W* XF/ Lamda0 ), Tol ) ! As above, set the lower limit of the guess value of HF to the tolerance - VF = 0.5_DbKi*W*( ZF/TANH(Lamda0) + L ) +!-------------------------------------------------------------- +! Body-Specific Subroutines +!-------------------------------------------------------------- - ! Restart Newton-Raphson iteration: - I = 0 - FirstIter = .FALSE. - dHF = 0.0_DbKi - dVF = 0.0_DbKi +!++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +! ###### The following four routines are Jacobian routines for linearization capabilities ####### +! If the module does not implement them, set ErrStat = ErrID_Fatal in SD_Init() when InitInp%Linearize is .true. +!---------------------------------------------------------------------------------------------------------------------------------- +!> Routine to compute the Jacobians of the output (Y), continuous- (X), discrete- (Xd), and constraint-state (Z) functions +!! with respect to the inputs (u). The partial derivatives dY/du, dX/du, dXd/du, and DZ/du are returned. +SUBROUTINE MD_JacobianPInput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, dYdu, dXdu, dXddu, dZdu) + REAL(DbKi), INTENT(IN ) :: t !< Time in seconds at operating point + TYPE(MD_InputType), INTENT(INOUT) :: u !< Inputs at operating point (may change to inout if a mesh copy is required) + TYPE(MD_ParameterType), INTENT(IN ) :: p !< Parameters + TYPE(MD_ContinuousStateType), INTENT(IN ) :: x !< Continuous states at operating point + TYPE(MD_DiscreteStateType), INTENT(IN ) :: xd !< Discrete states at operating point + TYPE(MD_ConstraintStateType), INTENT(IN ) :: z !< Constraint states at operating point + TYPE(MD_OtherStateType), INTENT(IN ) :: OtherState !< Other states at operating point + TYPE(MD_OutputType), INTENT(INOUT) :: y !< Output (change to inout if a mesh copy is required); Output fields are not used by this routine, but type is available here so that mesh parameter information (i.e., connectivity) does not have to be recalculated for dYdu. + TYPE(MD_MiscVarType), INTENT(INOUT) :: m !< Misc/optimization variables + INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message if ErrStat /= ErrID_None + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dYdu(:,:) !< Partial derivatives of output functions (Y) wrt the inputs (u) [intent in to avoid deallocation] + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dXdu(:,:) !< Partial derivatives of continuous state functions (X) wrt the inputs (u) [intent in to avoid deallocation] + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dXddu(:,:) !< Partial derivatives of discrete state functions (Xd) wrt the inputs (u) [intent in to avoid deallocation] + REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dZdu(:,:) !< Partial derivatives of constraint state functions (Z) wrt the inputs (u) [intent in to avoid deallocation] + + ! local variables + TYPE(MD_OutputType) :: y_m, y_p + TYPE(MD_ContinuousStateType) :: x_m, x_p + TYPE(MD_InputType) :: u_perturb + REAL(R8Ki) :: delta_p, delta_m ! delta change in input (plus, minus) + INTEGER(IntKi) :: i + integer(intKi) :: ErrStat2 + character(ErrMsgLen) :: ErrMsg2 + character(*), parameter :: RoutineName = 'MD_JacobianPInput' + + ! Initialize ErrStat + ErrStat = ErrID_None + ErrMsg = '' + + ! get OP values here: + call MD_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat2, ErrMsg2 ); if(Failed()) return + + ! make a copy of the inputs to perturb + call MD_CopyInput( u, u_perturb, MESH_NEWCOPY, ErrStat2, ErrMsg2); if(Failed()) return + + IF ( PRESENT( dYdu ) ) THEN + ! Calculate the partial derivative of the output functions (Y) with respect to the inputs (u) here: + if (.not. allocated(dYdu) ) then + call AllocAry(dYdu, p%Jac_ny, size(p%Jac_u_indx,1),'dYdu', ErrStat2, ErrMsg2); if(Failed()) return + end if + ! make a copy of outputs because we will need two for the central difference computations (with orientations) + call MD_CopyOutput( y, y_p, MESH_NEWCOPY, ErrStat2, ErrMsg2); if(Failed()) return + call MD_CopyOutput( y, y_m, MESH_NEWCOPY, ErrStat2, ErrMsg2); if(Failed()) return + do i=1,size(p%Jac_u_indx,1) + ! get u_op + delta_p u + call MD_CopyInput( u, u_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + call MD_Perturb_u( p, i, 1, u_perturb, delta_p ) + ! compute y at u_op + delta_p u + call MD_CalcOutput( t, u_perturb, p, x, xd, z, OtherState, y_p, m, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + ! get u_op - delta_m u + call MD_CopyInput( u, u_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + call MD_Perturb_u( p, i, -1, u_perturb, delta_m ) + ! compute y at u_op - delta_m u + call MD_CalcOutput( t, u_perturb, p, x, xd, z, OtherState, y_m, m, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + ! get central difference: + call MD_Compute_dY( p, y_p, y_m, delta_p, dYdu(:,i) ) + end do + if(Failed()) return + END IF + IF ( PRESENT( dXdu ) ) THEN + if (.not. allocated(dXdu)) then + call AllocAry(dXdu, p%Jac_nx, size(p%Jac_u_indx,1), 'dXdu', ErrStat2, ErrMsg2); if (Failed()) return + endif + do i=1,size(p%Jac_u_indx,1) + ! get u_op + delta u + call MD_CopyInput( u, u_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + call MD_Perturb_u( p, i, 1, u_perturb, delta_p ) + ! compute x at u_op + delta u + call MD_CalcContStateDeriv( t, u_perturb, p, x, xd, z, OtherState, m, x_p, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + ! get u_op - delta u + call MD_CopyInput( u, u_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + call MD_Perturb_u( p, i, -1, u_perturb, delta_m ) + ! compute x at u_op - delta u + call MD_CalcContStateDeriv( t, u_perturb, p, x, xd, z, OtherState, m, x_m, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + ! get central difference: + ! we may have had an error allocating memory, so we'll check + if(Failed()) return + ! get central difference: + call MD_Compute_dX( p, x_p, x_m, delta_p, dXdu(:,i) ) + end do + END IF ! dXdu + IF ( PRESENT( dXddu ) ) THEN + if (allocated(dXddu)) deallocate(dXddu) + END IF + IF ( PRESENT( dZdu ) ) THEN + if (allocated(dZdu)) deallocate(dZdu) + END IF + call CleanUp() +contains - ELSEIF ( ( I == MaxIter ) .AND. ( .NOT. FirstIter ) ) THEN ! .TRUE. if we've iterated as much as we can take without finding a solution; Abort - ErrStat = ErrID_Warn - ErrMsg = ' Iteration not convergent. '// & - ' Routine Catenary() cannot solve quasi-static mooring line solution.' - RETURN + logical function Failed() + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + Failed = ErrStat >= AbortErrLev + if (Failed) call CleanUp() + end function Failed - - ENDIF - - - ! Increment fairlead tensions and iteration counter so we can try again: - - HF = HF + dHF - VF = VF + dVF - - I = I + 1 - - - ENDDO - - - - ! We have found a solution for the tensions at the fairlead! - - ! Now compute the tensions at the anchor and the line position and tension - ! at each node (again, these depend on the configuration of the mooring - ! line): - - IF ( ( CB < 0.0_DbKi ) .OR. ( W < 0.0_DbKi ) .OR. ( VFMinWL > 0.0_DbKi ) ) THEN ! .TRUE. when no portion of the line rests on the seabed - - ! Anchor tensions: - - HA = HF - VA = VFMinWL - - - ! Line position and tension at each node: - - DO I = 1,N ! Loop through all nodes where the line position and tension are to be computed - - IF ( ( s(I) < 0.0_DbKi ) .OR. ( s(I) > L ) ) THEN - ErrStat = ErrID_Warn - ErrMsg = ' All line nodes must be located between the anchor ' & - //'and fairlead (inclusive) in routine Catenary().' - RETURN - END IF - - Ws = W *s(I) ! Initialize - VFMinWLs = VFMinWL + Ws ! some commonly - VFMinWLsOvrHF = VFMinWLs/HF ! used terms - sOvrEA = s(I) /EA ! that depend - SQRT1VFMinWLsOvrHF2 = SQRT( 1.0_DbKi + VFMinWLsOvrHF*VFMinWLsOvrHF ) ! on s(I) - - X (I) = ( LOG( VFMinWLsOvrHF + SQRT1VFMinWLsOvrHF2 ) & - - LOG( VFMinWLOvrHF + SQRT1VFMinWLOvrHF2 ) )*HFOvrW & - + sOvrEA* HF - Z (I) = ( SQRT1VFMinWLsOvrHF2 & - - SQRT1VFMinWLOvrHF2 )*HFOvrW & - + sOvrEA*( VFMinWL + 0.5_DbKi*Ws ) - Te(I) = SQRT( HF*HF + VFMinWLs*VFMinWLs ) - - ENDDO ! I - All nodes where the line position and tension are to be computed - - - ELSEIF ( -CB*VFMinWL < HF ) THEN ! .TRUE. when a portion of the line rests on the seabed and the anchor tension is nonzero - - ! Anchor tensions: - - HA = HF + CB*VFMinWL - VA = 0.0_DbKi - - - ! Line position and tension at each node: - - DO I = 1,N ! Loop through all nodes where the line position and tension are to be computed - - IF ( ( s(I) < 0.0_DbKi ) .OR. ( s(I) > L ) ) THEN - ErrStat = ErrID_Warn - ErrMsg = ' All line nodes must be located between the anchor ' & - //'and fairlead (inclusive) in routine Catenary().' - RETURN - END IF - - Ws = W *s(I) ! Initialize - VFMinWLs = VFMinWL + Ws ! some commonly - VFMinWLsOvrHF = VFMinWLs/HF ! used terms - sOvrEA = s(I) /EA ! that depend - SQRT1VFMinWLsOvrHF2 = SQRT( 1.0_DbKi + VFMinWLsOvrHF*VFMinWLsOvrHF ) ! on s(I) - - IF ( s(I) <= LMinVFOvrW ) THEN ! .TRUE. if this node rests on the seabed and the tension is nonzero - - X (I) = s(I) & - + sOvrEA*( HF + CB*VFMinWL + 0.5_DbKi*Ws*CB ) - Z (I) = 0.0_DbKi - Te(I) = HF + CB*VFMinWLs - - ELSE ! LMinVFOvrW < s <= L ! This node must be above the seabed - - X (I) = LOG( VFMinWLsOvrHF + SQRT1VFMinWLsOvrHF2 ) *HFOvrW & - + sOvrEA* HF + LMinVFOvrW - 0.5_DbKi*CB*VFMinWL*VFMinWL/WEA - Z (I) = ( - 1.0_DbKi + SQRT1VFMinWLsOvrHF2 )*HFOvrW & - + sOvrEA*( VFMinWL + 0.5_DbKi*Ws ) + 0.5_DbKi* VFMinWL*VFMinWL/WEA - Te(I) = SQRT( HF*HF + VFMinWLs*VFMinWLs ) - - ENDIF - - ENDDO ! I - All nodes where the line position and tension are to be computed - - - ELSE ! 0.0_DbKi < HF <= -CB*VFMinWL ! A portion of the line must rest on the seabed and the anchor tension is zero - - ! Anchor tensions: - - HA = 0.0_DbKi - VA = 0.0_DbKi - - - ! Line position and tension at each node: - - DO I = 1,N ! Loop through all nodes where the line position and tension are to be computed - - IF ( ( s(I) < 0.0_DbKi ) .OR. ( s(I) > L ) ) THEN - ErrStat = ErrID_Warn - ErrMsg = ' All line nodes must be located between the anchor ' & - //'and fairlead (inclusive) in routine Catenary().' - RETURN - END IF - - Ws = W *s(I) ! Initialize - VFMinWLs = VFMinWL + Ws ! some commonly - VFMinWLsOvrHF = VFMinWLs/HF ! used terms - sOvrEA = s(I) /EA ! that depend - SQRT1VFMinWLsOvrHF2 = SQRT( 1.0_DbKi + VFMinWLsOvrHF*VFMinWLsOvrHF ) ! on s(I) - - IF ( s(I) <= LMinVFOvrW - HFOvrW/CB ) THEN ! .TRUE. if this node rests on the seabed and the tension is zero - - X (I) = s(I) - Z (I) = 0.0_DbKi - Te(I) = 0.0_DbKi - - ELSEIF ( s(I) <= LMinVFOvrW ) THEN ! .TRUE. if this node rests on the seabed and the tension is nonzero - - X (I) = s(I) - ( LMinVFOvrW - 0.5_DbKi*HFOvrW/CB )*HF/EA & - + sOvrEA*( HF + CB*VFMinWL + 0.5_DbKi*Ws*CB ) + 0.5_DbKi*CB*VFMinWL*VFMinWL/WEA - Z (I) = 0.0_DbKi - Te(I) = HF + CB*VFMinWLs - - ELSE ! LMinVFOvrW < s <= L ! This node must be above the seabed - - X (I) = LOG( VFMinWLsOvrHF + SQRT1VFMinWLsOvrHF2 ) *HFOvrW & - + sOvrEA* HF + LMinVFOvrW - ( LMinVFOvrW - 0.5_DbKi*HFOvrW/CB )*HF/EA - Z (I) = ( - 1.0_DbKi + SQRT1VFMinWLsOvrHF2 )*HFOvrW & - + sOvrEA*( VFMinWL + 0.5_DbKi*Ws ) + 0.5_DbKi* VFMinWL*VFMinWL/WEA - Te(I) = SQRT( HF*HF + VFMinWLs*VFMinWLs ) - - ENDIF - - ENDDO ! I - All nodes where the line position and tension are to be computed - - - ENDIF - - - - ! The Newton-Raphson iteration is only accurate in double precision, so - ! convert the output arguments back into the default precision for real - ! numbers: - - !HA_In = REAL( HA , DbKi ) !mth: for this I only care about returning node positions - !HF_In = REAL( HF , DbKi ) - !Te_In(:) = REAL( Te(:), DbKi ) - !VA_In = REAL( VA , DbKi ) - !VF_In = REAL( VF , DbKi ) - X_In (:) = REAL( X (:), DbKi ) - Z_In (:) = REAL( Z (:), DbKi ) - - END SUBROUTINE Catenary - !----------------------------------------------------------------------- - - - END SUBROUTINE Line_Initialize - !-------------------------------------------------------------- - - - !-------------------------------------------------------------- - SUBROUTINE Line_SetState(Line, X, t) - - TYPE(MD_Line), INTENT(INOUT) :: Line ! the current Line object - Real(DbKi), INTENT(IN ) :: X(:) ! state vector section for this line - Real(DbKi), INTENT(IN ) :: t ! instantaneous time - - INTEGER(IntKi) :: i ! index of segments or nodes along line - INTEGER(IntKi) :: J ! index - - - ! store current time - Line%time = t - - ! set interior node positions and velocities based on state vector - DO I=1,Line%N-1 - DO J=1,3 - - Line%r( J,I) = X( 3*Line%N-3 + 3*I-3 + J) ! get positions - Line%rd(J,I) = X( 3*I-3 + J) ! get velocities - - END DO - END DO - - ! if using viscoelastic model, also set the static stiffness stretch - if (Line%ElasticMod == 2) then - do I=1,Line%N - Line%dl_1(I) = X( 6*Line%N-6 + I) ! these will be the last N entries in the state vector - end do - end if - - END SUBROUTINE Line_SetState - !-------------------------------------------------------------- - - !-------------------------------------------------------------- - SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, AnchMtot) - - TYPE(MD_Line), INTENT(INOUT) :: Line ! the current Line object - Real(DbKi), INTENT(INOUT) :: Xd(:) ! state derivative vector section for this line - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - TYPE(MD_ParameterType), INTENT(IN ) :: p ! Parameters - - ! Real(DbKi), INTENT( IN ) :: X(:) ! state vector, provided - ! Real(DbKi), INTENT( INOUT ) :: Xd(:) ! derivative of state vector, returned ! cahnged to INOUT - ! Real(DbKi), INTENT (IN) :: t ! instantaneous time - ! TYPE(MD_Line), INTENT (INOUT) :: Line ! label for the current line, for convenience - ! TYPE(MD_LineProp), INTENT(IN) :: LineProp ! the single line property set for the line of interest - ! Real(DbKi), INTENT(INOUT) :: FairFtot(:) ! total force on Connect top of line is attached to - ! Real(DbKi), INTENT(INOUT) :: FairMtot(:,:) ! total mass of Connect top of line is attached to - ! Real(DbKi), INTENT(INOUT) :: AnchFtot(:) ! total force on Connect bottom of line is attached to - ! Real(DbKi), INTENT(INOUT) :: AnchMtot(:,:) ! total mass of Connect bottom of line is attached to - - - INTEGER(IntKi) :: i ! index of segments or nodes along line - INTEGER(IntKi) :: J ! index - INTEGER(IntKi) :: K ! index - INTEGER(IntKi) :: N ! number of segments in line - Real(DbKi) :: d ! line diameter - Real(DbKi) :: rho ! line material density [kg/m^3] - Real(DbKi) :: Sum1 ! for summing squares - Real(DbKi) :: dummyLength ! - Real(DbKi) :: m_i ! node mass - Real(DbKi) :: v_i ! node submerged volume - Real(DbKi) :: Vi(3) ! relative water velocity at a given node - Real(DbKi) :: Vp(3) ! transverse relative water velocity component at a given node - Real(DbKi) :: Vq(3) ! tangential relative water velocity component at a given node - Real(DbKi) :: SumSqVp ! - Real(DbKi) :: SumSqVq ! - Real(DbKi) :: MagVp ! - Real(DbKi) :: MagVq ! - Real(DbKi) :: MagT ! tension stiffness force magnitude - Real(DbKi) :: MagTd ! tension damping force magnitude - Real(DbKi) :: Xi ! used in interpolating from lookup table - Real(DbKi) :: Yi ! used in interpolating from lookup table - Real(DbKi) :: dl ! stretch of a segment [m] - Real(DbKi) :: ld_1 ! rate of change of static stiffness portion of segment [m/s] - Real(DbKi) :: EA_1 ! stiffness of 'static stiffness' portion of segment, combines with dynamic stiffness to give static stiffnes [m/s] - - Real(DbKi) :: depth ! local water depth interpolated from bathymetry grid - - - N = Line%N ! for convenience - d = Line%d - rho = Line%rho - - ! note that end node kinematics should have already been set by attached objects - -! ! set end node positions and velocities from connect objects' states -! DO J = 1, 3 -! Line%r( J,N) = m%ConnectList(Line%FairConnect)%r(J) -! Line%r( J,0) = m%ConnectList(Line%AnchConnect)%r(J) -! Line%rd(J,N) = m%ConnectList(Line%FairConnect)%rd(J) -! Line%rd(J,0) = m%ConnectList(Line%AnchConnect)%rd(J) -! END DO - - - - ! -------------------- calculate various kinematic quantities --------------------------- - DO I = 1, N - - - ! calculate current (Stretched) segment lengths and unit tangent vectors (qs) for each segment (this is used for bending calculations) - CALL UnitVector(Line%r(:,I-1), Line%r(:,I), Line%qs(:,I), Line%lstr(I)) - - ! should add catch here for if lstr is ever zero - - Sum1 = 0.0_DbKi - DO J = 1, 3 - Sum1 = Sum1 + (Line%r(J,I) - Line%r(J,I-1))*(Line%rd(J,I) - Line%rd(J,I-1)) - END DO - Line%lstrd(I) = Sum1/Line%lstr(I) ! segment stretched length rate of change - - ! Line%V(I) = Pi/4.0 * d*d*Line%l(I) !volume attributed to segment - END DO - - !calculate unit tangent vectors (q) for each node (including ends) note: I think these are pointing toward 0 rather than N! - CALL UnitVector(Line%r(:,0), Line%r(:,1), Line%q(:,0), dummyLength) ! compute unit vector q - DO I = 1, N-1 - CALL UnitVector(Line%r(:,I-1), Line%r(:,I+1), Line%q(:,I), dummyLength) ! compute unit vector q ... using adjacent two nodes! - END DO - CALL UnitVector(Line%r(:,N-1), Line%r(:,N), Line%q(:,N), dummyLength) ! compute unit vector q - - - ! --------------------------------- apply wave kinematics ------------------------------------ - - ! IF (p%WaterKin > 0) THEN ! wave kinematics interpolated from global grid in Waves object - ! DO i=0,N - ! CALL getWaveKin(p, Line%r(1,i), Line%r(2,i), Line%r(3,i), Line%time, m%WaveTi, Line%U(:,i), Line%Ud(:,i), Line%zeta(i), Line%PDyn(i)) - ! END DO - ! END IF - - - ! --------------- calculate mass (including added mass) matrix for each node ----------------- - DO I = 0, N - IF (I==0) THEN - m_i = Pi/8.0 *d*d*Line%l(1)*rho - v_i = 0.5 *Line%V(1) - ELSE IF (I==N) THEN - m_i = pi/8.0 *d*d*Line%l(N)*rho; - v_i = 0.5*Line%V(N) - ELSE - m_i = pi/8.0 * d*d*rho*(Line%l(I) + Line%l(I+1)) - v_i = 0.5 *(Line%V(I) + Line%V(I+1)) - END IF - - DO J=1,3 - DO K=1,3 - IF (J==K) THEN - Line%M(K,J,I) = m_i + p%rhoW*v_i*( Line%Can*(1 - Line%q(J,I)*Line%q(K,I)) + Line%Cat*Line%q(J,I)*Line%q(K,I) ) - ELSE - Line%M(K,J,I) = p%rhoW*v_i*( Line%Can*(-Line%q(J,I)*Line%q(K,I)) + Line%Cat*Line%q(J,I)*Line%q(K,I) ) - END IF - END DO - END DO - - CALL Inverse3by3(Line%S(:,:,I), Line%M(:,:,I)) ! invert mass matrix - END DO - - - ! ------------------ CALCULATE FORCES ON EACH NODE ---------------------------- - - ! loop through the segments - DO I = 1, N - - ! handle nonlinear stiffness if needed - if (Line%nEApoints > 0) then - - Xi = Line%l(I)/Line%lstr(I) - 1.0 ! strain rate based on inputs - Yi = 0.0_DbKi - - ! find stress based on strain - if (Xi < 0.0) then ! if negative strain (compression), zero stress - Yi = 0.0_DbKi - else if (Xi < Line%stiffXs(1)) then ! if strain below first data point, interpolate from zero - Yi = Xi * Line%stiffYs(1)/Line%stiffXs(1) - else if (Xi >= Line%stiffXs(Line%nEApoints)) then ! if strain exceeds last data point, use last data point - Yi = Line%stiffYs(Line%nEApoints) - else ! otherwise we're in range of the table so interpolate! - do J=1, Line%nEApoints-1 ! go through lookup table until next entry exceeds inputted strain rate - if (Line%stiffXs(J+1) > Xi) then - Yi = Line%stiffYs(J) + (Xi-Line%stiffXs(J)) * (Line%stiffYs(J+1)-Line%stiffYs(J))/(Line%stiffXs(J+1)-Line%stiffXs(J)) - exit - end if - end do - end if - - ! calculate a young's modulus equivalent value based on stress/strain - Line%EA = Yi/Xi - end if - - - ! >>>> could do similar as above for nonlinear damping or bending stiffness <<<< - if (Line%nBApoints > 0) print *, 'Nonlinear elastic damping not yet implemented' - if (Line%nEIpoints > 0) print *, 'Nonlinear bending stiffness not yet implemented' - - - ! basic elasticity model - if (Line%ElasticMod == 1) then - ! line tension, inherently including possibility of dynamic length changes in l term - if (Line%lstr(I)/Line%l(I) > 1.0) then - MagT = Line%EA *( Line%lstr(I)/Line%l(I) - 1.0 ) - else - MagT = 0.0_DbKi ! cable can't "push" - end if - ! line internal damping force based on line-specific BA value, including possibility of dynamic length changes in l and ld terms - MagTd = Line%BA* ( Line%lstrd(I) - Line%lstr(I)*Line%ld(I)/Line%l(I) )/Line%l(I) - - ! viscoelastic model - else if (Line%ElasticMod == 2) then - - EA_1 = Line%EA_D*Line%EA/(Line%EA_D - Line%EA)! calculated EA_1 which is the stiffness in series with EA_D that will result in the desired static stiffness of EA_S - - dl = Line%lstr(I) - Line%l(I) ! delta l of this segment - - ld_1 = (Line%EA_D*dl - (Line%EA_D + EA_1)*Line%dl_1(I) + Line%BA_D*Line%lstrd(I)) /( Line%BA_D + Line%BA) ! rate of change of static stiffness portion [m/s] - - !MagT = (Line%EA*Line%dl_S(I) + Line%BA*ld_S)/ Line%l(I) ! compute tension based on static portion (dynamic portion would give same) - MagT = EA_1*Line%dl_1(I)/ Line%l(I) - MagTd = Line%BA*ld_1 / Line%l(I) - - ! update state derivative for static stiffness stretch (last N entries in the state vector) - Xd( 6*N-6 + I) = ld_1 - - end if - - - do J = 1, 3 - !Line%T(J,I) = Line%EA *( 1.0/Line%l(I) - 1.0/Line%lstr(I) ) * (Line%r(J,I)-Line%r(J,I-1)) - Line%T(J,I) = MagT * Line%qs(J,I) - !Line%Td(J,I) = Line%BA* ( Line%lstrd(I) / Line%l(I) ) * (Line%r(J,I)-Line%r(J,I-1)) / Line%lstr(I) ! note new form of damping coefficient, BA rather than Cint - Line%Td(J,I) = MagTd * Line%qs(J,I) - end do - end do - - - ! loop through the nodes - DO I = 0, N - - !submerged weight (including buoyancy) - IF (I==0) THEN - Line%W(3,I) = Pi/8.0*d*d* Line%l(1)*(rho - p%rhoW) *(-p%g) ! assuming g is positive - ELSE IF (i==N) THEN - Line%W(3,I) = pi/8.0*d*d* Line%l(N)*(rho - p%rhoW) *(-p%g) - ELSE - Line%W(3,I) = pi/8.0*d*d* (Line%l(I)*(rho - p%rhoW) + Line%l(I+1)*(rho - p%rhoW) )*(-p%g) ! left in this form for future free surface handling - END IF - - !relative flow velocities - DO J = 1, 3 - Vi(J) = 0.0 - Line%rd(J,I) ! relative flow velocity over node -- this is where wave velicites would be added - END DO - - ! decomponse relative flow into components - SumSqVp = 0.0_DbKi ! start sums of squares at zero - SumSqVq = 0.0_DbKi - DO J = 1, 3 - Vq(J) = DOT_PRODUCT( Vi , Line%q(:,I) ) * Line%q(J,I); ! tangential relative flow component - Vp(J) = Vi(J) - Vq(J) ! transverse relative flow component - SumSqVq = SumSqVq + Vq(J)*Vq(J) - SumSqVp = SumSqVp + Vp(J)*Vp(J) - END DO - MagVp = sqrt(SumSqVp) ! get magnitudes of flow components - MagVq = sqrt(SumSqVq) - - ! transverse and tangenential drag - IF (I==0) THEN - Line%Dp(:,I) = 0.25*p%rhoW*Line%Cdn* d*Line%l(1) * MagVp * Vp - Line%Dq(:,I) = 0.25*p%rhoW*Line%Cdt* Pi*d*Line%l(1) * MagVq * Vq - ELSE IF (I==N) THEN - Line%Dp(:,I) = 0.25*p%rhoW*Line%Cdn* d*Line%l(N) * MagVp * Vp - Line%Dq(:,I) = 0.25*p%rhoW*Line%Cdt* Pi*d*Line%l(N) * MagVq * Vq - ELSE - Line%Dp(:,I) = 0.25*p%rhoW*Line%Cdn* d*(Line%l(I) + Line%l(I+1)) * MagVp * vp - Line%Dq(:,I) = 0.25*p%rhoW*Line%Cdt* Pi*d*(Line%l(I) + Line%l(I+1)) * MagVq * vq - END IF - - ! F-K force from fluid acceleration not implemented yet - - ! bottom contact (stiffness and damping, vertical-only for now) - updated Nov 24 for general case where anchor and fairlead ends may deal with bottom contact forces - - ! interpolate the local depth from the bathymetry grid - CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, Line%r(1,I), Line%r(2,I), depth) - - IF (Line%r(3,I) < -depth) THEN - IF (I==0) THEN - Line%B(3,I) = ( (-depth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*( Line%l(I+1) ) - ELSE IF (I==N) THEN - Line%B(3,I) = ( (-depth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) ) - ELSE - Line%B(3,I) = ( (-depth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) + Line%l(I+1) ) - ! IF (Line%r(3,I) < -p%WtrDpth) THEN - ! IF (I==0) THEN - ! Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*( Line%l(I+1) ) - ! ELSE IF (I==N) THEN - ! Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) ) - ! ELSE - ! Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) + Line%l(I+1) ) - - - - END IF - ELSE - Line%B(3,I) = 0.0_DbKi - END IF - - ! total forces - IF (I==0) THEN - Line%Fnet(:,I) = Line%T(:,1) + Line%Td(:,1) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) - ELSE IF (I==N) THEN - Line%Fnet(:,I) = -Line%T(:,N) - Line%Td(:,N) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) - ELSE - Line%Fnet(:,I) = Line%T(:,I+1) - Line%T(:,I) + Line%Td(:,I+1) - Line%Td(:,I) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) - END IF - - END DO ! I - done looping through nodes - - ! print *, "line ", Line%IdNum, " has N=", N - ! print *, " and rd shape", shape(Line%rd) - ! print *, " and Xd shape", shape(Xd) - - ! loop through internal nodes and update their states <<< should/could convert to matrix operations instead of all these loops - DO I=1, N-1 - DO J=1,3 - - ! calculate RHS constant (premultiplying force vector by inverse of mass matrix ... i.e. rhs = S*Forces) - Sum1 = 0.0_DbKi ! reset temporary accumulator - DO K = 1, 3 - Sum1 = Sum1 + Line%S(K,J,I) * Line%Fnet(K,I) ! matrix-vector multiplication [S i]{Forces i} << double check indices - END DO ! K - - ! print *, "writing Xd indices ", 3*N-3 + 3*I-3 + J, " and " , 3*I-3 + J - ! - ! print*, Line%rd(J,I) - - ! update states - Xd(3*N-3 + 3*I-3 + J) = Line%rd(J,I); ! dxdt = V (velocities) - Xd( 3*I-3 + J) = Sum1 ! dVdt = RHS * A (accelerations) - - END DO ! J - END DO ! I - - - ! check for NaNs - DO J = 1, 6*(N-1) - IF (Is_NaN(Xd(J))) THEN - print *, "NaN detected at time ", Line%time, " in Line ", Line%IdNum, " in MoorDyn." - IF (wordy > 1) THEN - print *, "state derivatives:" - print *, Xd - - - - print *, "m_i p%rhoW v_i Line%Can Line%Cat" - print *, m_i - print *, p%rhoW - print *, v_i - print *, Line%Can - print *, Line%Cat - - print *, "Line%q" - print *, Line%q - - print *, "Line%r" - print *, Line%r - - - print *, "Here is the mass matrix set" - print *, Line%M - - print *, "Here is the inverted mass matrix set" - print *, Line%S - - print *, "Here is the net force set" - print *, Line%Fnet - END IF - - EXIT - END IF - END DO - - - ! ! add force and mass of end nodes to the Connects they correspond to <<<<<<<<<<<< do this from Connection instead now! - ! DO J = 1,3 - ! FairFtot(J) = FairFtot(J) + Line%F(J,N) - ! AnchFtot(J) = AnchFtot(J) + Line%F(J,0) - ! DO K = 1,3 - ! FairMtot(K,J) = FairMtot(K,J) + Line%M(K,J,N) - ! AnchMtot(K,J) = AnchMtot(K,J) + Line%M(K,J,0) - ! END DO - ! END DO - - END SUBROUTINE Line_GetStateDeriv - !===================================================================== - - - !-------------------------------------------------------------- - SUBROUTINE Line_SetEndKinematics(Line, r_in, rd_in, t, topOfLine) - - TYPE(MD_Line), INTENT(INOUT) :: Line ! the current Line object - Real(DbKi), INTENT(IN ) :: r_in( 3) ! state vector section for this line - Real(DbKi), INTENT(IN ) :: rd_in(3) ! state vector section for this line - Real(DbKi), INTENT(IN ) :: t ! instantaneous time - INTEGER(IntKi), INTENT(IN ) :: topOfLine ! 0 for end A (Node 0), 1 for end B (node N) - - Integer(IntKi) :: I,J - INTEGER(IntKi) :: inode - - IF (topOfLine==1) THEN - inode = Line%N - Line%endTypeB = 0 ! set as ball rather than rigid connection (unless changed later by SetEndOrientation) - ELSE - inode = 0 - Line%endTypeA = 0 ! set as ball rather than rigid connection (unless changed later by SetEndOrientation) - END IF - - !Line%r( :,inode) = r_in - !Line%rd(:,inode) = rd_in - - DO J = 1,3 - Line%r( :,inode) = r_in - Line%rd(:,inode) = rd_in - END DO - - ! print *, "SetEndKinematics of line ", Line%idNum, " top?:", topOfLine - ! print *, r_in - ! print *, Line%r( :,inode), " - confirming, node ", inode - ! print *, rd_in - - Line%time = t - - END SUBROUTINE Line_SetEndKinematics - !-------------------------------------------------------------- - - - ! get force, moment, and mass of line at line end node - !-------------------------------------------------------------- - SUBROUTINE Line_GetEndStuff(Line, Fnet_out, Moment_out, M_out, topOfLine) - - TYPE(MD_Line), INTENT(INOUT) :: Line ! label for the current line, for convenience - REAL(DbKi), INTENT( OUT) :: Fnet_out(3) ! net force on end node - REAL(DbKi), INTENT( OUT) :: Moment_out(3) ! moment on end node (future capability) - REAL(DbKi), INTENT( OUT) :: M_out(3,3) ! mass matrix of end node - INTEGER(IntKi), INTENT(IN ) :: topOfLine ! 0 for end A (Node 0), 1 for end B (node N) - - Integer(IntKi) :: I,J - INTEGER(IntKi) :: inode - - IF (topOfLine==1) THEN ! end B of line - Fnet_out = Line%Fnet(:, Line%N) - Moment_out = Line%endMomentB - M_out = Line%M(:,:, Line%N) - ELSE ! end A of line - Fnet_out = Line%Fnet(:, 0) - Moment_out = Line%endMomentA - M_out = Line%M(:,:, 0) - END IF - - END SUBROUTINE Line_GetEndStuff - !-------------------------------------------------------------- - - - ! set end kinematics of a line that's attached to a Rod, and this includes rotational information - !-------------------------------------------------------------- - SUBROUTINE Line_GetEndSegmentInfo(Line, qEnd, EIout, dlOut, topOfLine) - - TYPE(MD_Line), INTENT(INOUT) :: Line ! label for the current line, for convenience - REAL(DbKi), INTENT( OUT) :: qEnd(3) - REAL(DbKi), INTENT( OUT) :: EIout - REAL(DbKi), INTENT( OUT) :: dlOut - INTEGER(IntKi), INTENT(IN ) :: topOfLine ! 0 for end A (Node 0), 1 for end B (node N) - - EIout = Line%EI; - - if (topOfLine==1) then - CALL UnitVector(Line%r(:,Line%N-1), Line%r(:,Line%N), qEnd, dlOut) ! unit vector of last line segment - else - CALL UnitVector(Line%r(:,0 ), Line%r(:,1 ), qEnd, dlOut) ! unit vector of first line segment - END IF - - END SUBROUTINE Line_GetEndSegmentInfo - !-------------------------------------------------------------- - - - ! set end node unit vector of a line (this is called by an attached to a Rod, only applicable for bending stiffness) - !-------------------------------------------------------------- - SUBROUTINE Line_SetEndOrientation(Line, qin, topOfLine, rodEndB) - - TYPE(MD_Line), INTENT(INOUT) :: Line ! label for the current line, for convenience - REAL(DbKi), INTENT(IN ) :: qin(3) ! the rod's axis unit vector - INTEGER(IntKi), INTENT(IN ) :: topOfLine ! 0 for end A (Node 0), 1 for end B (node N) - INTEGER(IntKi), INTENT(IN ) :: rodEndB ! =0 means the line is attached to Rod end A, =1 means attached to Rod end B (implication for unit vector sign) - - if (topOfLine==1) then - - Line%endTypeB = 1 ! indicate attached to Rod (at every time step, just in case line get detached) - - if (rodEndB==1) then - Line%q(:,Line%N) = -qin ! -----line----->[B<==ROD==A] - else - Line%q(:,Line%N) = qin ! -----line----->[A==ROD==>B] - end if - else - - Line%endTypeA = 1 ! indicate attached to Rod (at every time step, just in case line get detached) ! indicate attached to Rod - - if (rodEndB==1) then - Line%q(:,0 ) = qin ! [A==ROD==>B]-----line-----> - else - Line%q(:,0 ) = -qin ! [B<==ROD==A]-----line-----> - end if - end if - - END SUBROUTINE Line_SetEndOrientation - !-------------------------------------------------------------- - - - -!-------------------------------------------------------------- -! Connection-Specific Subroutines -!-------------------------------------------------------------- - - - - - - !-------------------------------------------------------------- - SUBROUTINE Connect_Initialize(Connect, states, m) - - Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object - Real(DbKi), INTENT(INOUT) :: states(6) ! state vector section for this Connection - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - - INTEGER(IntKi) :: l - - - if (Connect%typeNum == 0) then ! error check - - ! pass kinematics to any attached lines so they have initial positions at this initialization stage - DO l=1,Connect%nAttached - IF (wordy > 1) print *, "Connect ", Connect%IdNum, " setting end kinematics of line ", Connect%attached(l), " to ", Connect%r - CALL Line_SetEndKinematics(m%LineList(Connect%attached(l)), Connect%r, Connect%rd, 0.0_DbKi, Connect%Top(l)) - END DO - - - ! assign initial node kinematics to state vector - states(4:6) = Connect%r - states(1:3) = Connect%rd - - - IF (wordy > 0) print *, "Initialized Connection ", Connect%IdNum - - else - print *," Error: wrong Point type given to Connect_Initialize for number ", Connect%idNum - end if - - END SUBROUTINE Connect_Initialize - !-------------------------------------------------------------- - - - !-------------------------------------------------------------- - SUBROUTINE Connect_SetKinematics(Connect, r_in, rd_in, a_in, t, m) - - Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object - Real(DbKi), INTENT(IN ) :: r_in( 3) ! position - Real(DbKi), INTENT(IN ) :: rd_in(3) ! velocity - Real(DbKi), INTENT(IN ) :: a_in(3) ! acceleration (only used for coupled connects) - Real(DbKi), INTENT(IN ) :: t ! instantaneous time - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - - - INTEGER(IntKi) :: l - - ! store current time - Connect%time = t - - - ! if (Connect%typeNum==0) THEN ! anchor ( <<< to be changed/expanded) ... in MoorDyn F also used for coupled connections - - ! set position and velocity - Connect%r = r_in - Connect%rd = rd_in - Connect%a = a_in - - ! pass latest kinematics to any attached lines - DO l=1,Connect%nAttached - CALL Line_SetEndKinematics(m%LineList(Connect%attached(l)), Connect%r, Connect%rd, t, Connect%Top(l)) - END DO - - ! else - ! - ! PRINT*,"Error: setKinematics called for wrong Connection type. Connection ", Connect%IdNum, " type ", Connect%typeNum - - ! END IF - - - END SUBROUTINE Connect_SetKinematics - !-------------------------------------------------------------- - - !-------------------------------------------------------------- - SUBROUTINE Connect_SetState(Connect, X, t, m) - - Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object - Real(DbKi), INTENT(IN ) :: X(:) ! state vector section for this line - Real(DbKi), INTENT(IN ) :: t ! instantaneous time - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - - INTEGER(IntKi) :: l ! index of segments or nodes along line - INTEGER(IntKi) :: J ! index - - - ! store current time - Connect%time = t - - ! from state values, get r and rdot values - DO J=1,3 - Connect%r( J) = X(3 + J) ! get positions - Connect%rd(J) = X( J) ! get velocities - END DO - - ! pass latest kinematics to any attached lines - DO l=1,Connect%nAttached - CALL Line_SetEndKinematics(m%LineList(Connect%attached(l)), Connect%r, Connect%rd, t, Connect%Top(l)) - END DO - - END SUBROUTINE Connect_SetState - !-------------------------------------------------------------- - - !-------------------------------------------------------------- - SUBROUTINE Connect_GetStateDeriv(Connect, Xd, m, p) - - Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object - Real(DbKi), INTENT(INOUT) :: Xd(:) ! state derivative vector section for this line - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - - TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters - - !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables - - !INTEGER(IntKi) :: l ! index of attached lines - INTEGER(IntKi) :: J ! index - INTEGER(IntKi) :: K ! index - Real(DbKi) :: Sum1 ! for adding things - - Real(DbKi) :: S(3,3) ! inverse mass matrix - - - CALL Connect_DoRHS(Connect, m, p) - -! // solve for accelerations in [M]{a}={f} using LU decomposition -! double M_tot[9]; // serialize total mass matrix for easy processing -! for (int I=0; I<3; I++) for (int J=0; J<3; J++) M_tot[3*I+J]=M[I][J]; -! double LU[9]; // serialized matrix that will hold LU matrices combined -! Crout(3, M_tot, LU); // perform LU decomposition on mass matrix -! double acc[3]; // acceleration vector to solve for -! solveCrout(3, LU, Fnet, acc); // solve for acceleration vector - - ! solve for accelerations in [M]{a}={f} using LU decomposition -! CALL LUsolve(6, M_out, LU_temp, Fnet_out, y_temp, acc) - - - ! invert node mass matrix - CALL Inverse3by3(S, Connect%M) - - ! accelerations - Connect%a = MATMUL(S, Connect%Fnet) - - ! fill in state derivatives - Xd(4:6) = Connect%rd ! dxdt = V (velocities) - Xd(1:3) = Connect%a ! dVdt = RHS * A (accelerations) - - - ! check for NaNs - DO J = 1, 6 - IF (Is_NaN(Xd(J))) THEN - print *, "NaN detected at time ", Connect%time, " in Point ",Connect%IdNum, " in MoorDyn." - IF (wordy > 1) print *, "state derivatives:" - IF (wordy > 1) print *, Xd - EXIT - END IF - END DO - - END SUBROUTINE Connect_GetStateDeriv - !-------------------------------------------------------------- - - !-------------------------------------------------------------- - SUBROUTINE Connect_DoRHS(Connect, m, p) - - Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters - - !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables - - INTEGER(IntKi) :: l ! index of attached lines - INTEGER(IntKi) :: I ! index - INTEGER(IntKi) :: J ! index - INTEGER(IntKi) :: K ! index - - Real(DbKi) :: Fnet_i(3) ! force from an attached line - Real(DbKi) :: Moment_dummy(3) ! dummy vector to hold unused line end moments - Real(DbKi) :: M_i(3,3) ! mass from an attached line - - - ! start with the Connection's own forces including buoyancy and weight, and its own mass - Connect%Fnet(1) = Connect%conFX - Connect%Fnet(2) = Connect%conFY - Connect%Fnet(3) = Connect%conFZ + Connect%conV*p%rhoW*p%g - Connect%conM*p%g - - Connect%M = 0.0_DbKi ! clear (zero) the connect mass matrix - - DO J = 1,3 - Connect%M (J,J) = Connect%conM ! set the diagonals to the self-mass (to start with) - END DO - - - ! print *, "connection number", Connect%IdNum - ! print *, "attached lines: ", Connect%attached - ! print *, "size of line list" , size(m%LineList) - - ! loop through attached lines, adding force and mass contributions - DO l=1,Connect%nAttached - - ! print *, " l", l - ! print *, Connect%attached(l) - ! print *, m%LineList(Connect%attached(l))%Fnet - ! - ! - ! print *, " attached line ID", m%LineList(Connect%attached(l))%IdNum - - CALL Line_GetEndStuff(m%LineList(Connect%attached(l)), Fnet_i, Moment_dummy, M_i, Connect%Top(l)) - - ! sum quantitites - Connect%Fnet = Connect%Fnet + Fnet_i - Connect%M = Connect%M + M_i - - END DO - - - ! XXXWhen this sub is called, any self weight, buoyancy, or external forcing should have already been - ! added by the calling subroutine. The only thing left is any added mass or drag forces from the connection (e.g. float) - ! itself, which will be added below.XXX - - - ! IF (EqualRealNos(t, 0.0_DbKi)) THEN ! this is old: with current IC gen approach, we skip the first call to the line objects, because they're set AFTER the call to the connects - ! - ! DO J = 1,3 - ! Xd(3+J) = X(J) ! velocities - these are unused in integration - ! Xd(J) = 0.0_DbKi ! accelerations - these are unused in integration - ! END DO - ! ELSE - ! ! from state values, get r and rdot values - ! DO J = 1,3 - ! Connect%r(J) = X(3 + J) ! get positions - ! Connect%rd(J) = X(J) ! get velocities - ! END DO - ! END IF - - - ! add any added mass and drag forces from the Connect body itself - DO J = 1,3 - Connect%Fnet(J) = Connect%Fnet(J) - 0.5 * p%rhoW * Connect%rd(J) * abs(Connect%rd(J)) * Connect%conCdA; ! add drag forces - corrected Nov 24 - Connect%M (J,J) = Connect%M (J,J) + Connect%conV*p%rhoW*Connect%conCa; ! add added mass - - END DO - - ! would this sub ever need to include the m*a inertial term? Is it ever called for coupled connects? <<< - - END SUBROUTINE Connect_DoRHS - !===================================================================== - - - ! calculate the force including inertial loads on connect that is coupled - !-------------------------------------------------------------- - SUBROUTINE Connect_GetCoupledForce(Connect, Fnet_out, m, p) - - Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connect object - Real(DbKi), INTENT( OUT) :: Fnet_out(3) ! force and moment vector about rRef - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters - - Real(DbKi) :: F_iner(3) ! inertial force - - IF (Connect%typeNum == -1) then - ! calculate forces and masses of connect - CALL Connect_DoRHS(Connect, m, p) - - ! add inertial loads as appropriate - F_iner = -MATMUL(Connect%M, Connect%a) ! inertial loads - Fnet_out = Connect%Fnet + F_iner ! add inertial loads - - ELSE - print *, "Connect_GetCoupledForce called for wrong (uncoupled) Point type in MoorDyn!" - END IF - - END SUBROUTINE Connect_GetCoupledForce - - - ! calculate the force and mass contributions of the connect on the parent body (only for type 3 connects?) - !-------------------------------------------------------------- - SUBROUTINE Connect_GetNetForceAndMass(Connect, rRef, Fnet_out, M_out, m, p) - - Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connect object - Real(DbKi), INTENT(IN ) :: rRef(3) ! global coordinates of reference point (i.e. the parent body) - Real(DbKi), INTENT( OUT) :: Fnet_out(6) ! force and moment vector about rRef - Real(DbKi), INTENT( OUT) :: M_out(6,6) ! mass and inertia matrix about rRef - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters - - Real(DbKi) :: rRel( 3) ! position of connection relative to the body reference point (global orientation frame) - - - CALL Connect_DoRHS(Connect, m, p) - - rRel = Connect%r - rRef ! vector from body reference point to node - - ! convert net force into 6dof force about body ref point - CALL translateForce3to6DOF(rRel, Connect%Fnet, Fnet_out) - - ! convert mass matrix to 6by6 mass matrix about body ref point - CALL translateMass3to6DOF(rRel, Connect%M, M_out) - - END SUBROUTINE Connect_GetNetForceAndMass - - - - - ! this function handles assigning a line to a connection node - !-------------------------------------------------------------- - SUBROUTINE Connect_AddLine(Connect, lineID, TopOfLine) - - Type(MD_Connect), INTENT (INOUT) :: Connect ! the Connection object - Integer(IntKi), INTENT( IN ) :: lineID - Integer(IntKi), INTENT( IN ) :: TopOfLine - - IF (wordy > 0) Print*, "L", lineID, "->C", Connect%IdNum - - IF (Connect%nAttached <10) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. - Connect%nAttached = Connect%nAttached + 1 ! add the line to the number connected - Connect%Attached(Connect%nAttached) = lineID - Connect%Top(Connect%nAttached) = TopOfLine ! attached to line ... 1 = top/fairlead(end B), 0 = bottom/anchor(end A) - ELSE - Print*, "Too many lines connected to Point ", Connect%IdNum, " in MoorDyn!" - END IF - - END SUBROUTINE Connect_AddLine - - - ! this function handles removing a line from a connection node - !-------------------------------------------------------------- - SUBROUTINE Connect_RemoveLine(Connect, lineID, TopOfLine, rEnd, rdEnd) - - Type(MD_Connect), INTENT (INOUT) :: Connect ! the Connection object - Integer(IntKi), INTENT( IN ) :: lineID - Integer(IntKi), INTENT( OUT) :: TopOfLine - REAL(DbKi), INTENT(INOUT) :: rEnd(3) - REAL(DbKi), INTENT(INOUT) :: rdEnd(3) - - Integer(IntKi) :: l,m,J - - DO l = 1,Connect%nAttached ! look through attached lines - - IF (Connect%Attached(l) == lineID) THEN ! if this is the line's entry in the attachment list - - TopOfLine = Connect%Top(l); ! record which end of the line was attached - - DO m = l,Connect%nAttached-1 - - Connect%Attached(m) = Connect%Attached(m+1) ! move subsequent line links forward one spot in the list to eliminate this line link - Connect%Top( m) = Connect%Top(m+1) - - Connect%nAttached = Connect%nAttached - 1 ! reduce attached line counter by 1 - - ! also pass back the kinematics at the end - DO J = 1,3 - rEnd( J) = Connect%r( J) - rdEnd(J) = Connect%rd(J) - END DO - - print*, "Detached line ", lineID, " from Connection ", Connect%IdNum - - EXIT - END DO - - IF (l == Connect%nAttached) THEN ! detect if line not found - print *, "Error: failed to find line to remove during removeLineFromConnect call to connection ", Connect%IdNum, ". Line ", lineID - END IF - - END IF - - END DO - - END SUBROUTINE Connect_RemoveLine - - - - - - - - -!-------------------------------------------------------------- -! Rod-Specific Subroutines -!-------------------------------------------------------------- - - - - !----------------------------------------------------------------------- - SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) - - TYPE(MD_Rod), INTENT(INOUT) :: Rod ! the single rod object of interest - TYPE(MD_RodProp), INTENT(INOUT) :: RodProp ! the single rod property set for the line of interest - REAL(DbKi), INTENT(IN) :: endCoords(6) - REAL(DbKi), INTENT(IN) :: rhoW - INTEGER, INTENT( INOUT ) :: ErrStat ! returns a non-zero value when an error occurs - CHARACTER(*), INTENT( INOUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None - - INTEGER(4) :: J ! Generic index - INTEGER(4) :: K ! Generic index - INTEGER(IntKi) :: N - - N = Rod%N ! number of segments in this line (for code readability) - - ! -------------- save some section properties to the line object itself ----------------- - - Rod%d = RodProp%d - Rod%rho = RodProp%w/(Pi/4.0 * Rod%d * Rod%d) - - Rod%Can = RodProp%Can - Rod%Cat = RodProp%Cat - Rod%Cdn = RodProp%Cdn - Rod%Cdt = RodProp%Cdt - Rod%CaEnd = RodProp%CaEnd - Rod%CdEnd = RodProp%CdEnd - - - ! allocate node positions and velocities (NOTE: these arrays start at ZERO) - ALLOCATE ( Rod%r(3, 0:N), Rod%rd(3, 0:N), STAT = ErrStat ) ! <<<<<< add error checks here - IF ( ErrStat /= ErrID_None ) print *, "Alloc error 1 in MoorDyn" - - ! allocate segment scalar quantities - ALLOCATE ( Rod%l(N), Rod%V(N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) print *, "Alloc error 2 in MoorDyn" - - ! allocate water related vectors - ALLOCATE ( Rod%U(3, 0:N), Rod%Ud(3, 0:N), Rod%zeta(0:N), Rod%PDyn(0:N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) print *, "Alloc error 3 in MoorDyn" - ! set to zero initially (important of wave kinematics are not being used) - Rod%U = 0.0_DbKi - Rod%Ud = 0.0_DbKi - Rod%zeta = 0.0_DbKi - Rod%PDyn = 0.0_DbKi - - ! allocate node force vectors - ALLOCATE ( Rod%W(3, 0:N), Rod%Bo(3, 0:N), Rod%Dp(3, 0:N), Rod%Dq(3, 0:N), Rod%Ap(3, 0:N), & - Rod%Aq(3, 0:N), Rod%Pd(3, 0:N), Rod%B(3, 0:N), Rod%Fnet(3, 0:N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) print *, "Alloc error 4 in MoorDyn" - - ! allocate mass and inverse mass matrices for each node (including ends) - ALLOCATE ( Rod%M(3, 3, 0:N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) print *, "Alloc error 5 in MoorDyn" - - - - ! ------------------------- set some geometric properties and the starting kinematics ------------------------- - - CALL UnitVector(endCoords(1:3), endCoords(4:6), Rod%q, Rod%UnstrLen) ! get Rod axis direction vector and Rod length - - ! set Rod positions if applicable - if (Rod%typeNum==0) then ! for an independent rod, set the position right off the bat - - Rod%r6(1:3) = endCoords(1:3) ! (end A coordinates) - Rod%v6(1:3) = 0.0_DbKi ! (end A velocity, unrotated axes) - - Rod%r6(4:6) = Rod%q ! (Rod direction unit vector) - Rod%v6(4:6) = 0.0_DbKi ! (rotational velocities about unrotated axes) - - - else if (abs(Rod%typeNum)==1) then ! for a pinned rod, just set the orientation (position will be set later by parent object) - - Rod%r6(4:6) = Rod%q ! (Rod direction unit vector) - Rod%v6(4:6) = 0.0_DbKi ! (rotational velocities about unrotated axes) - - end if - ! otherwise (for a fixed rod) the positions will be set by the parent body or via coupling - - - - ! save mass for future calculations >>>> should calculate I_l and I_r here in future <<<< - Rod%mass = Rod%UnstrLen*RodProp%w - - - ! assign values for l and V - DO J=1,N - Rod%l(J) = Rod%UnstrLen/REAL(N, DbKi) - Rod%V(J) = Rod%l(J)*0.25*Pi*RodProp%d*RodProp%d - END DO - - - - ! set gravity and bottom contact forces to zero initially (because the horizontal components should remain at zero) - DO J = 0,N - DO K = 1,3 - Rod%W(K,J) = 0.0_DbKi - Rod%B(K,J) = 0.0_DbKi - END DO - END DO - - ! >>> why are the above assignments making l V W and B appear as "undefined pointer/array"s??? <<< - - IF (wordy > 0) print *, "Set up Rod ",Rod%IdNum, ", type ", Rod%typeNum - - ! need to add cleanup sub <<< - - END SUBROUTINE Rod_Setup - !-------------------------------------------------------------- - - - - - ! Make output file for Rod and set end kinematics of any attached lines. - ! For free Rods, fill in the initial states into the state vector. - ! Notes: r6 and v6 must already be set. - ! ground- or body-pinned rods have already had setKinematics called to set first 3 elements of r6, v6. - !-------------------------------------------------------------- - SUBROUTINE Rod_Initialize(Rod, states, m) - - TYPE(MD_Rod), INTENT(INOUT) :: Rod ! the rod object - Real(DbKi), INTENT(INOUT) :: states(:) ! state vector section for this line - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - - - INTEGER(IntKi) :: l ! index of segments or nodes along line - REAL(DbKi) :: rRef(3) ! reference position of mesh node - REAL(DbKi) :: OrMat(3,3) ! DCM for body orientation based on r6_in - - IF (wordy > 0) print *, "initializing Rod ", Rod%idNum - - ! the r6 and v6 vectors should have already been set - ! r and rd of ends have already been set by setup function or by parent object <<<<< right? <<<<< - - - ! Pass kinematics to any attached lines (this is just like what a Connection does, except for both ends) - ! so that they have the correct initial positions at this initialization stage. - - if (Rod%typeNum >- 2) CALL Rod_SetDependentKin(Rod, 0.0_DbKi, m) ! don't call this for type -2 coupled Rods as it's already been called - - - ! assign the resulting kinematics to its part of the state vector (only matters if it's an independent Rod) - - if (Rod%typeNum == 0) then ! free Rod type - - states(1:6) = 0.0_DbKi ! zero velocities for initialization - states(7:9) = Rod%r(:,0) ! end A position - states(10:12) = Rod%q ! rod direction unit vector - - else if (abs(Rod%typeNum) ==1 ) then ! pinned rod type (coupled or attached to something previously via setPinKin) - - states(1:3) = 0.0_DbKi ! zero velocities for initialization - states(4:6) = Rod%q ! rod direction unit vector - - end if - - ! note: this may also be called by a coupled rod (type = -1) in which case states will be empty - - - END SUBROUTINE Rod_Initialize - !-------------------------------------------------------------- - - - - - ! set kinematics for Rods ONLY if they are attached to a body (including a coupled body) or coupled (otherwise shouldn't be called) - !-------------------------------------------------------------- - SUBROUTINE Rod_SetKinematics(Rod, r6_in, v6_in, a6_in, t, m) - - Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object - Real(DbKi), INTENT(IN ) :: r6_in(6) ! 6-DOF position - Real(DbKi), INTENT(IN ) :: v6_in(6) ! 6-DOF velocity - Real(DbKi), INTENT(IN ) :: a6_in(6) ! 6-DOF acceleration (only used for coupled rods) - Real(DbKi), INTENT(IN ) :: t ! instantaneous time - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - - INTEGER(IntKi) :: l - - Rod%time = t ! store current time - - - if (abs(Rod%typeNum) == 2) then ! rod rigidly coupled to a body, or ground, or coupling point - Rod%r6 = r6_in - Rod%v6 = v6_in - Rod%a6 = a6_in - - call ScaleVector(Rod%r6(4:6), 1.0_DbKi, Rod%r6(4:6)); ! enforce direction vector to be a unit vector - - ! since this rod has no states and all DOFs have been set, pass its kinematics to dependent Lines - CALL Rod_SetDependentKin(Rod, t, m) - - else if (abs(Rod%typeNum) == 1) then ! rod end A pinned to a body, or ground, or coupling point - - ! set Rod *end A only* kinematics based on BCs (linear model for now) - Rod%r6(1:3) = r6_in(1:3) - Rod%v6(1:3) = v6_in(1:3) - Rod%a6(1:3) = a6_in(1:3) - - - ! Rod is pinned so only end A is specified, rotations are left alone and will be - ! handled, along with passing kinematics to dependent lines, by separate call to setState - - else - print *, "Error: Rod_SetKinematics called for a free Rod in MoorDyn." ! <<< - end if - - - ! update Rod direction unit vector (simply equal to last three entries of r6, presumably these were set elsewhere for pinned Rods) - Rod%q = Rod%r6(4:6) - - - - END SUBROUTINE Rod_SetKinematics - !-------------------------------------------------------------- - - ! pass the latest states to the rod if it has any DOFs/states (then update rod end kinematics including attached lines) - !-------------------------------------------------------------- - SUBROUTINE Rod_SetState(Rod, X, t, m) - - Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object - Real(DbKi), INTENT(IN ) :: X(:) ! state vector section for this line - Real(DbKi), INTENT(IN ) :: t ! instantaneous time - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - - INTEGER(IntKi) :: J ! index - - - ! for a free Rod, there are 12 states: - ! [ x, y, z velocity of end A, then rate of change of u/v/w coordinates of unit vector pointing toward end B, - ! then x, y, z coordinate of end A, u/v/w coordinates of unit vector pointing toward end B] - - ! for a pinned Rod, there are 6 states (rotational only): - ! [ rate of change of u/v/w coordinates of unit vector pointing toward end B, - ! then u/v/w coordinates of unit vector pointing toward end B] - - - ! store current time - Rod%time = t - - - ! copy over state values for potential use during derivative calculations - if (Rod%typeNum == 0) then ! free Rod type - - ! CALL ScaleVector(X(10:12), 1.0, X(10:12)) ! enforce direction vector to be a unit vector <<<< can't do this with FAST frameowrk, could be a problem!! - - ! TODO: add "controller" adjusting state derivatives of X(10:12) to artificially force X(10:12) to remain a unit vector <<<<<<<<<<< - - - Rod%r6(1:3) = X(7:9) ! (end A coordinates) - Rod%v6(1:3) = X(1:3) ! (end A velocity, unrotated axes) - CALL ScaleVector(X(10:12), 1.0_DbKi, Rod%r6(4:6)) !Rod%r6(4:6) = X(10:12) ! (Rod direction unit vector) - Rod%v6(4:6) = X(4:6) ! (rotational velocities about unrotated axes) - - - CALL Rod_SetDependentKin(Rod, t, m) - - else if (abs(Rod%typeNum) == 1) then ! pinned rod type (coupled or attached to something)t previously via setPinKin) - - !CALL ScaleVector(X(4:6), 1.0, X(4:6)) ! enforce direction vector to be a unit vector - - - CALL ScaleVector(X(4:6), 1.0_DbKi, Rod%r6(4:6)) !Rod%r6(3+J) = X(3+J) ! (Rod direction unit vector) - Rod%v6(4:6) = X(1:3) ! (rotational velocities about unrotated axes) - - - CALL Rod_SetDependentKin(Rod, t, m) - - else - print *, "Error: Rod::setState called for a non-free rod type in MoorDyn" ! <<< - end if - - ! update Rod direction unit vector (simply equal to last three entries of r6) - Rod%q = Rod%r6(4:6) - - END SUBROUTINE Rod_SetState - !-------------------------------------------------------------- - - - ! Set the Rod end kinematics then set the kinematics of dependent objects (any attached lines). - ! This also determines the orientation of zero-length rods. - !-------------------------------------------------------------- - SUBROUTINE Rod_SetDependentKin(Rod, t, m) - - Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object - Real(DbKi), INTENT(IN ) :: t ! instantaneous time - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects (for simplicity, since Bodies deal with Rods and Connections) - - INTEGER(IntKi) :: l ! index of segments or nodes along line - INTEGER(IntKi) :: J ! index - INTEGER(IntKi) :: N ! number of segments - - REAL(DbKi) :: qEnd(3) ! unit vector of attached line end segment, following same direction convention as Rod's q vector - REAL(DbKi) :: EIend ! bending stiffness of attached line end segment - REAL(DbKi) :: dlEnd ! stretched length of attached line end segment - REAL(DbKi) :: qMomentSum(3) ! summation of qEnd*EI/dl_stretched (with correct sign) for each attached line - - - ! Initialize variables - qMomentSum = 0.0_DbKi - - ! in future pass accelerations here too? <<<< - - N = Rod%N - - ! from state values, set positions of end nodes - ! end A - Rod%r(:,0) = Rod%r6(1:3) ! positions - Rod%rd(:,0) = Rod%v6(1:3) ! velocities - - !print *, Rod%r6(1:3) - !print *, Rod%r(:,0) - - if (Rod%N > 0) then ! set end B nodes only if the rod isn't zero length - CALL transformKinematicsAtoB(Rod%r6(1:3), Rod%r6(4:6), Rod%UnstrLen, Rod%v6, Rod%r(:,N), Rod%rd(:,N)) ! end B - end if - - ! pass end node kinematics to any attached lines (this is just like what a Connection does, except for both ends) - DO l=1,Rod%nAttachedA - CALL Line_SetEndKinematics(m%LineList(Rod%attachedA(l)), Rod%r(:,0), Rod%rd(:,0), t, Rod%TopA(l)) - END DO - DO l=1,Rod%nAttachedB - CALL Line_SetEndKinematics(m%LineList(Rod%attachedB(l)), Rod%r(:,N), Rod%rd(:,N), t, Rod%TopB(l)) - END DO - - - ! if this is a zero-length Rod, get bending moment-related information from attached lines and compute Rod's equilibrium orientation - if (N==0) then - - DO l=1,Rod%nAttachedA - - CALL Line_GetEndSegmentInfo(m%LineList(Rod%attachedA(l)), qEnd, EIend, dlEnd, Rod%TopA(l)) - - qMomentSum = qMomentSum + qEnd*EIend/dlEnd ! add each component to the summation vector - - END DO - - DO l=1,Rod%nAttachedB - - CALL Line_GetEndSegmentInfo(m%LineList(Rod%attachedB(l)), qEnd, EIend, dlEnd, Rod%TopB(l)) - - qMomentSum = qMomentSum + qEnd*EIend/dlEnd ! add each component to the summation vector - - END DO - - ! solve for line unit vector that balances all moments (unit vector of summation of qEnd*EI/dl_stretched over each line) - CALL ScaleVector(qMomentSum, 1.0_DbKi, Rod%q) - END IF - - ! pass Rod orientation to any attached lines (this is just like what a Connection does, except for both ends) - DO l=1,Rod%nAttachedA - CALL Line_SetEndOrientation(m%LineList(Rod%attachedA(l)), Rod%q, Rod%TopA(l), 0) - END DO - DO l=1,Rod%nAttachedB - CALL Line_SetEndOrientation(m%LineList(Rod%attachedB(l)), Rod%q, Rod%TopB(l), 1) - END DO - - END SUBROUTINE Rod_SetDependentKin - !-------------------------------------------------------------- - - !-------------------------------------------------------------- - SUBROUTINE Rod_GetStateDeriv(Rod, Xd, m, p) - - Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object - Real(DbKi), INTENT(INOUT) :: Xd(:) ! state derivative vector section for this line - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects (for simplicity, since Bodies deal with Rods and Connections) - TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters - - !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables - - INTEGER(IntKi) :: J ! index - - Real(DbKi) :: Fnet (6) ! net force and moment about reference point - Real(DbKi) :: M_out (6,6) ! mass matrix about reference point - - Real(DbKi) :: acc(6) ! 6DOF acceleration vector about reference point - - Real(DbKi) :: Mcpl(3) ! moment in response to end A acceleration due to inertial coupling - - Real(DbKi) :: y_temp (6) ! temporary vector for LU decomposition - Real(DbKi) :: LU_temp(6,6) ! temporary matrix for LU decomposition - - ! Initialize some things to zero - y_temp = 0.0_DbKi -! FIXME: should LU_temp be set to M_out before calling LUsolve????? - LU_temp = 0.0_DbKi - - CALL Rod_GetNetForceAndMass(Rod, Rod%r(:,0), Fnet, M_out, m, p) - - - - ! TODO: add "controller" adjusting state derivatives of X(10:12) to artificially force X(10:12) to remain a unit vector <<<<<<<<<<< - - ! fill in state derivatives - IF (Rod%typeNum == 0) THEN ! free Rod type, 12 states - - ! solve for accelerations in [M]{a}={f} using LU decomposition - CALL LUsolve(6, M_out, LU_temp, Fnet, y_temp, acc) - - Xd(7:9) = Rod%v6(1:3) !Xd[6 + I] = v6[ I]; ! dxdt = V (velocities) - Xd(1:6) = acc !Xd[ I] = acc[ I]; ! dVdt = a (accelerations) - !Xd[3 + I] = acc[3+I]; ! rotational accelerations - - ! rate of change of unit vector components!! CHECK! <<<<< - Xd(10) = - Rod%v6(6)*Rod%r6(5) + Rod%v6(5)*Rod%r6(6) ! i.e. u_dot_x = -omega_z*u_y + omega_y*u_z - Xd(11) = Rod%v6(6)*Rod%r6(4) - Rod%v6(4)*Rod%r6(6) ! i.e. u_dot_y = omega_z*u_x - omega_x*u_z - Xd(12) = -Rod%v6(5)*Rod%r6(4) + Rod%v6(4)*Rod%r6(5) ! i.e. u_dot_z = -omega_y*u_x - omega_x*u_y - - ! store accelerations in case they're useful as output - Rod%a6 = acc - - ELSE ! pinned rod, 6 states (rotational only) - - ! account for moment in response to end A acceleration due to inertial coupling (off-diagonal sub-matrix terms) - !Fnet(4:6) = Fnet(4:6) - MATMUL(M_out(4:6,1:3), Rod%a6(1:3)) ! << 1) THEN - print *, " state derivatives:" - print *, Xd - - print *, "r0" - print *, Rod%r(:,0) - print *, "F" - print *, Fnet - print *, "M" - print *, M_out - print *, "acc" - print *, acc - END IF - - EXIT - END IF - END DO - - END SUBROUTINE Rod_GetStateDeriv - !-------------------------------------------------------------- - - - ! calculate the aggregate 3/6DOF rigid-body loads of a coupled rod including inertial loads - !-------------------------------------------------------------- - SUBROUTINE Rod_GetCoupledForce(Rod, Fnet_out, m, p) - - Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object - Real(DbKi), INTENT( OUT) :: Fnet_out(6) ! force and moment vector - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters - - Real(DbKi) :: F6_iner(6) ! inertial reaction force - - ! do calculations of forces and masses on each rod node - CALL Rod_DoRHS(Rod, m, p) - - ! add inertial loads as appropriate (written out in a redundant way just for clarity, and to support load separation in future) - ! fixed coupled rod - if (Rod%typeNum == -2) then - - F6_iner = -MATMUL(Rod%M6net, Rod%a6) ! inertial loads - Fnet_out = Rod%F6net + F6_iner ! add inertial loads - - ! pinned coupled rod - else if (Rod%typeNum == -1) then - ! inertial loads ... from input translational ... and solved rotational ... acceleration - F6_iner(4:6) = -MATMUL(Rod%M6net(1:3,1:3), Rod%a6(1:3)) - MATMUL(Rod%M6net(1:3,4:6), Rod%a6(4:6)) - Fnet_out(1:3) = Rod%F6net(1:3) + F6_iner(4:6) ! add translational inertial loads - Fnet_out(4:6) = 0.0_DbKi - else - print *, "ERROR, Rod_GetCoupledForce called for wrong (non-coupled) rod type!" - end if - - END SUBROUTINE Rod_GetCoupledForce - !-------------------------------------------------------------- - - - - ! calculate the aggregate 6DOF rigid-body force and mass data of the rod - !-------------------------------------------------------------- - SUBROUTINE Rod_GetNetForceAndMass(Rod, rRef, Fnet_out, M_out, m, p) - - Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object - Real(DbKi), INTENT(IN ) :: rRef(3) ! global coordinates of reference point (end A for free Rods) - Real(DbKi), INTENT( OUT) :: Fnet_out(6) ! force and moment vector about rRef - Real(DbKi), INTENT( OUT) :: M_out(6,6) ! mass and inertia matrix about rRef - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters - - Real(DbKi) :: rRel( 3) ! relative position of each node i from rRef - - ! do calculations of forces and masses on each rod node - CALL Rod_DoRHS(Rod, m, p) - - ! note: Some difference from MoorDyn C here. If this function is called by the Rod itself, the reference point must be end A - - ! shift everything from end A reference to rRef reference point - - rRel = Rod%r(:,0) - rRef ! vector from reference point to end A - - CALL translateForce3to6DOF(rRel, Rod%F6net(1:3), Fnet_out) ! shift net forces - Fnet_out(4:6) = Fnet_out(4:6) + Rod%F6net(4:6) ! add in the existing moments - - CALL translateMass6to6DOF(rRel, Rod%M6net, M_out) ! shift mass matrix to be about ref point - - ! >>> do we need to ensure zero moment is passed if it's pinned? <<< - !if (abs(Rod%typeNum)==1) then - ! Fnet_out(4:6) = 0.0_DbKi - !end if - - - END SUBROUTINE Rod_GetNetForceAndMass - !-------------------------------------------------------------- - - - ! calculate the forces on the rod, including from attached lines - !-------------------------------------------------------------- - SUBROUTINE Rod_DoRHS(Rod, m, p) - - Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rodion object - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters - - !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables - - INTEGER(IntKi) :: l ! index of attached lines - INTEGER(IntKi) :: I,J,K ! index - - - INTEGER(IntKi) :: N ! number of rod elements for convenience - - Real(DbKi) :: phi, beta, sinPhi, cosPhi, tanPhi, sinBeta, cosBeta ! various orientation things - Real(DbKi) :: k_hat(3) ! unit vector (redundant, not used) <<<< - Real(DbKi) :: Ftemp ! temporary force component - Real(DbKi) :: Mtemp ! temporary moment component - - Real(DbKi) :: m_i, v_i ! - Real(DbKi) :: zeta ! wave elevation above/below a given node - !Real(DbKi) :: h0 ! distance along rod centerline from end A to the waterplane - Real(DbKi) :: deltaL ! submerged length of a given segment - Real(DbKi) :: Lsum ! cumulative length along rod axis from bottom - Real(DbKi) :: dL ! length attributed to node - Real(DbKi) :: VOF ! fraction of volume associated with node that is submerged - - Real(DbKi) :: Vi(3) ! relative flow velocity over a node - Real(DbKi) :: SumSqVp, SumSqVq, MagVp, MagVq - Real(DbKi) :: Vp(3), Vq(3) ! transverse and axial components of water velocity at a given node - Real(DbKi) :: ap(3), aq(3) ! transverse and axial components of water acceleration at a given node - Real(DbKi) :: Fnet_i(3) ! force from an attached line - Real(DbKi) :: Mnet_i(3) ! moment from an attached line - Real(DbKi) :: Mass_i(3,3) ! mass from an attached line - - ! used in lumped 6DOF calculations: - Real(DbKi) :: rRel( 3) ! relative position of each node i from rRef - Real(DbKi) :: OrMat(3,3) ! rotation matrix to rotate global z to rod's axis - Real(DbKi) :: F6_i(6) ! a node's contribution to the total force vector - Real(DbKi) :: M6_i(6,6) ! a node's contribution to the total mass matrix - Real(DbKi) :: I_l ! axial inertia of rod - Real(DbKi) :: I_r ! radial inertia of rod about CG - Real(DbKi) :: Imat_l(3,3) ! inertia about CG aligned with Rod axis - Real(DbKi) :: Imat(3,3) ! inertia about CG in global frame - Real(DbKi) :: h_c ! location of CG along axis - Real(DbKi) :: r_c(3) ! 3d location of CG relative to node A - Real(DbKi) :: Fcentripetal(3) ! centripetal force - Real(DbKi) :: Mcentripetal(3) ! centripetal moment - - Real(DbKi) :: depth ! local interpolated depth from bathymetry grid - - - N = Rod%N - - ! ------------------------------ zero some things -------------------------- - - Rod%Mext = 0.0_DbKi ! zero the external moment sum - - Lsum = 0.0_DbKi - - - ! ---------------------------- initial rod and node calculations ------------------------ - - ! calculate some orientation information for the Rod as a whole - call GetOrientationAngles(Rod%r( :,0), Rod%r( :,N), phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat) - - ! save to internal roll and pitch variables for use in output <<< should check these, make Euler angles isntead of independent <<< - Rod%roll = -180.0/Pi * phi*sinBeta - Rod%pitch = 180.0/Pi * phi*cosBeta - - ! set interior node positions and velocities (stretch the nodes between the endpoints linearly) (skipped for zero-length Rods) - DO i=1,N-1 - Rod%r( :,i) = Rod%r( :,0) + (Rod%r( :,N) - Rod%r( :,0)) * (REAL(i)/REAL(N)) - Rod%rd(:,i) = Rod%rd(:,0) + (Rod%rd(:,N) - Rod%rd(:,0)) * (REAL(i)/REAL(N)) - - - Rod%V(i) = 0.25*pi * Rod%d*Rod%d * Rod%l(i) ! volume attributed to segment - END DO - - - ! --------------------------------- apply wave kinematics ------------------------------------ - - ! IF (p%WaterKin == 1) THEN ! wave kinematics interpolated from global grid in Waves object - ! DO i=0,N - ! CALL getWaveKin(p, Rod%r(1,i), Rod%r(2,i), Rod%r(3,i), Rod%time, m%WaveTi, Rod%U(:,i), Rod%Ud(:,i), Rod%zeta(i), Rod%PDyn(i)) - ! !F(i) = 1.0 ! set VOF value to one for now (everything submerged - eventually this should be element-based!!!) <<<< - ! ! <<<< currently F is not being used and instead a VOF variable is used within the node loop - ! END DO - ! END IF - - - ! ! wave kinematics not implemented yet <<< - ! ap = 0.0_DbKi - ! aq = 0.0_DbKi - ! ! set U and Ud herem as well as pDyn and zeta... - ! Rod%U = 0.0_DbKi - ! Rod%Ud = 0.0_DbKi - ! pDyn = 0.0_DbKi - ! zeta = 0.0_DbKi - - ! >>> remember to check for violated conditions, if there are any... <<< - - zeta = Rod%zeta(N)! just use the wave elevation computed at the location of the top node for now - - if ((Rod%r(3,0) < zeta) .and. (Rod%r(3,N) > zeta)) then ! check if it's crossing the water plane (should also add some limits to avoid near-horizontals at some point) - Rod%h0 = (zeta - Rod%r(3,0))/Rod%q(3) ! distance along rod centerline from end A to the waterplane - else if (Rod%r(3,0) < zeta) then - Rod%h0 = Rod%UnstrLen ! fully submerged case <<<<<< remove the 2.0 and double check there are no if statements that get changed <<<< - else - Rod%h0 = 0.0_DbKi ! fully unsubmerged case (ever applicable?) - end if - - - ! -------------------------- loop through all the nodes ----------------------------------- - DO I = 0, N - - - ! ------------------ calculate added mass matrix for each node ------------------------- - - ! get mass and volume considering adjacent segment lengths - IF (I==0) THEN - dL = 0.5*Rod%l(1) - m_i = 0.25*Pi * Rod%d*Rod%d * dL *Rod%rho ! (will be zero for zero-length Rods) - v_i = 0.5 *Rod%V(1) - ELSE IF (I==N) THEN - dL = 0.5*Rod%l(N) - m_i = 0.25*pi * Rod%d*Rod%d * dL *Rod%rho - v_i = 0.5*Rod%V(N) - ELSE - dL = 0.5*(Rod%l(I) + Rod%l(I+1)) - m_i = 0.25*pi * Rod%d*Rod%d * dL *Rod%rho - v_i = 0.5 *(Rod%V(I) + Rod%V(I+1)) - END IF - - ! get scalar for submerged portion - IF (Lsum + dL <= Rod%h0) THEN ! if fully submerged - VOF = 1.0_DbKi - ELSE IF (Lsum < Rod%h0) THEN ! if partially below waterline - VOF = (Rod%h0 - Lsum)/dL - ELSE ! must be out of water - VOF = 0.0_DbKi - END IF - - Lsum = Lsum + dL ! add length attributed to this node to the total - - ! build mass and added mass matrix - DO J=1,3 - DO K=1,3 - IF (J==K) THEN - Rod%M(K,J,I) = m_i + VOF*p%rhoW*v_i*( Rod%Can*(1 - Rod%q(J)*Rod%q(K)) + Rod%Cat*Rod%q(J)*Rod%q(K) ) - ELSE - Rod%M(K,J,I) = VOF*p%rhoW*v_i*( Rod%Can*(-Rod%q(J)*Rod%q(K)) + Rod%Cat*Rod%q(J)*Rod%q(K) ) - END IF - END DO - END DO - - ! <<<< what about accounting for offset of half segment from node location for end nodes? <<<< - - -! CALL Inverse3by3(Rod%S(:,:,I), Rod%M(:,:,I)) ! invert mass matrix - - - ! ------------------ CALCULATE FORCES ON EACH NODE ---------------------------- - - if (N > 0) then ! the following force calculations are only nonzero for finite-length rods (skipping for zero-length Rods) - - ! >>> no nodal axial elasticity loads calculated since it's assumed rigid, but should I calculate tension/compression due to other loads? <<< - - ! weight (now only the dry weight) - Rod%W(:,I) = (/ 0.0_DbKi, 0.0_DbKi, -m_i * p%g /) ! assuming g is positive - - ! buoyance (now calculated based on outside pressure, for submerged portion only) - ! radial buoyancy force from sides - Ftemp = -VOF * 0.25*Pi*dL*Rod%d*Rod%d * p%rhoW*p%g * sinPhi - Rod%Bo(:,I) = (/ Ftemp*cosBeta*cosPhi, Ftemp*sinBeta*cosPhi, -Ftemp*sinPhi /) - - !relative flow velocities - DO J = 1, 3 - Vi(J) = Rod%U(J,I) - Rod%rd(J,I) ! relative flow velocity over node -- this is where wave velicites would be added - END DO - - ! decomponse relative flow into components - SumSqVp = 0.0_DbKi ! start sums of squares at zero - SumSqVq = 0.0_DbKi - DO J = 1, 3 - Vq(J) = DOT_PRODUCT( Vi , Rod%q ) * Rod%q(J); ! tangential relative flow component - Vp(J) = Vi(J) - Vq(J) ! transverse relative flow component - SumSqVq = SumSqVq + Vq(J)*Vq(J) - SumSqVp = SumSqVp + Vp(J)*Vp(J) - END DO - MagVp = sqrt(SumSqVp) ! get magnitudes of flow components - MagVq = sqrt(SumSqVq) - - ! transverse and tangenential drag - Rod%Dp(:,I) = VOF * 0.5*p%rhoW*Rod%Cdn* Rod%d* dL * MagVp * Vp - Rod%Dq(:,I) = 0.0_DbKi ! 0.25*p%rhoW*Rod%Cdt* Pi*Rod%d* dL * MagVq * Vq <<< should these axial side loads be included? - - ! fluid acceleration components for current node - aq = DOT_PRODUCT(Rod%Ud(:,I), Rod%q) * Rod%q ! tangential component of fluid acceleration - ap = Rod%Ud(:,I) - aq ! normal component of fluid acceleration - ! transverse Froude-Krylov force - Rod%Ap(:,I) = VOF * p%rhoW*(1.0+Rod%Can)* v_i * ap ! - ! axial Froude-Krylov force - Rod%Aq(:,I) = 0.0_DbKi ! p%rhoW*(1.0+Rod%Cat)* v_i * aq ! <<< just put a taper-based term here eventually? - - ! dynamic pressure - Rod%Pd(:,I) = 0.0_DbKi ! assuming zero for sides for now, until taper comes into play - - ! bottom contact (stiffness and damping, vertical-only for now) - updated Nov 24 for general case where anchor and fairlead ends may deal with bottom contact forces - ! interpolate the local depth from the bathymetry grid - CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, Rod%r(1,I), Rod%r(2,I), depth) - - IF (Rod%r(3,I) < -depth) THEN - IF (I==0) THEN - Rod%B(3,I) = ( (-depth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*( Rod%l(I+1) ) - ELSE IF (I==N) THEN - Rod%B(3,I) = ( (-depth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) ) - ELSE - Rod%B(3,I) = ( (-depth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) + Rod%l(I+1) ) - END IF - ! IF (I==0) THEN - ! Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*( Rod%l(I+1) ) - ! ELSE IF (I==N) THEN - ! Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) ) - ! ELSE - ! Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) + Rod%l(I+1) ) - ! END IF - ELSE - Rod%B(3,I) = 0.0_DbKi - END IF - - ELSE ! zero-length (N=0) Rod case - - ! >>>>>>>>>>>>>> still need to check handling of zero length rods <<<<<<<<<<<<<<<<<<< - - ! for zero-length rods, make sure various forces are zero - Rod%W = 0.0_DbKi - Rod%Bo = 0.0_DbKi - Rod%Dp = 0.0_DbKi - Rod%Dq= 0.0_DbKi - Rod%B = 0.0_DbKi - Rod%Pd = 0.0_DbKi - - END IF - - - ! ------ now add forces, moments, and added mass from Rod end effects (these can exist even if N==0) ------- - - ! end A - IF ((I==0) .and. (Rod%h0 > 0.0_ReKi)) THEN ! if this is end A and it is submerged - - ! >>> eventually should consider a VOF approach for the ends hTilt = 0.5*Rod%d/cosPhi <<< - - ! buoyancy force - Ftemp = -VOF * 0.25*Pi*Rod%d*Rod%d * p%rhoW*p%g*Rod%r(3,I) - Rod%Bo(:,I) = Rod%Bo(:,I) + (/ Ftemp*cosBeta*sinPhi, Ftemp*sinBeta*sinPhi, Ftemp*cosPhi /) - - ! buoyancy moment - Mtemp = -VOF * 1.0/64.0*Pi*Rod%d**4 * p%rhoW*p%g * sinPhi - Rod%Mext = Rod%Mext + (/ Mtemp*sinBeta, -Mtemp*cosBeta, 0.0_DbKi /) - - ! axial drag - Rod%Dq(:,I) = Rod%Dq(:,I) + VOF * 0.25* Pi*Rod%d*Rod%d * p%rhoW*Rod%CdEnd * MagVq * Vq - - - ! >>> what about rotational drag?? <<< eqn will be Pi* Rod%d**4/16.0 omega_rel?^2... *0.5 * Cd... - - ! Froud-Krylov force - Rod%Aq(:,I) = Rod%Aq(:,I) + VOF * p%rhoW*(1.0+Rod%CaEnd)* (2.0/3.0*Pi*Rod%d**3 /8.0) * aq - - ! dynamic pressure force - Rod%Pd(:,I) = Rod%Pd(:,I) + VOF * 0.25* Pi*Rod%d*Rod%d * Rod%PDyn(I) * Rod%q - - ! added mass - DO J=1,3 - DO K=1,3 - IF (J==K) THEN - Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) - ELSE - Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) - END IF - END DO - END DO - - END IF - - IF ((I==N) .and. (Rod%h0 >= Rod%UnstrLen)) THEN ! if this end B and it is submerged (note, if N=0, both this and previous if statement are true) - - ! buoyancy force - Ftemp = VOF * 0.25*Pi*Rod%d*Rod%d * p%rhoW*p%g*Rod%r(3,I) - Rod%Bo(:,I) = Rod%Bo(:,I) + (/ Ftemp*cosBeta*sinPhi, Ftemp*sinBeta*sinPhi, Ftemp*cosPhi /) - - ! buoyancy moment - Mtemp = VOF * 1.0/64.0*Pi*Rod%d**4 * p%rhoW*p%g * sinPhi - Rod%Mext = Rod%Mext + (/ Mtemp*sinBeta, -Mtemp*cosBeta, 0.0_DbKi /) - - ! axial drag - Rod%Dq(:,I) = Rod%Dq(:,I) + VOF * 0.25* Pi*Rod%d*Rod%d * p%rhoW*Rod%CdEnd * MagVq * Vq - - ! Froud-Krylov force - Rod%Aq(:,I) = Rod%Aq(:,I) + VOF * p%rhoW*(1.0+Rod%CaEnd)* (2.0/3.0*Pi*Rod%d**3 /8.0) * aq - - ! dynamic pressure force - Rod%Pd(:,I) = Rod%Pd(:,I) - VOF * 0.25* Pi*Rod%d*Rod%d * Rod%PDyn(I) * Rod%q - - ! added mass - DO J=1,3 - DO K=1,3 - IF (J==K) THEN - Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) - ELSE - Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) - END IF - END DO - END DO - - END IF - - - - ! ---------------------------- total forces for this node ----------------------------- - - Rod%Fnet(:,I) = Rod%W(:,I) + Rod%Bo(:,I) + Rod%Dp(:,I) + Rod%Dq(:,I) & - + Rod%Ap(:,I) + Rod%Aq(:,I) + Rod%Pd(:,I) + Rod%B(:,I) - - - END DO ! I - done looping through nodes - - - ! ----- add waterplane moment of inertia moment if applicable ----- - IF ((Rod%r(3,0) < zeta) .and. (Rod%r(3,N) > zeta)) then ! check if it's crossing the water plane - Mtemp = 1.0/16.0 *Pi*Rod%d**4 * p%rhoW*p%g * sinPhi * (1.0 + 0.5* tanPhi**2) - Rod%Mext = Rod%Mext + (/ Mtemp*sinBeta, -Mtemp*cosBeta, 0.0_DbKi /) - END IF - - ! ---------------- now add in forces on end nodes from attached lines ------------------ - - ! loop through lines attached to end A - DO l=1,Rod%nAttachedA - - CALL Line_GetEndStuff(m%LineList(Rod%attachedA(l)), Fnet_i, Mnet_i, Mass_i, Rod%TopA(l)) - - ! sum quantitites - Rod%Fnet(:,0)= Rod%Fnet(:,0) + Fnet_i ! total force - Rod%Mext = Rod%Mext + Mnet_i ! externally applied moment - Rod%M(:,:,0) = Rod%M(:,:,0) + Mass_i ! mass at end node - - END DO - - ! loop through lines attached to end B - DO l=1,Rod%nAttachedB - - CALL Line_GetEndStuff(m%LineList(Rod%attachedB(l)), Fnet_i, Mnet_i, Mass_i, Rod%TopB(l)) - - ! sum quantitites - Rod%Fnet(:,N)= Rod%Fnet(:,N) + Fnet_i ! total force - Rod%Mext = Rod%Mext + Mnet_i ! externally applied moment - Rod%M(:,:,N) = Rod%M(:,:,N) + Mass_i ! mass at end node - - END DO - - ! ---------------- now lump everything in 6DOF about end A ----------------------------- - - ! question: do I really want to neglect the rotational inertia/drag/etc across the length of each segment? - - ! make sure 6DOF quantiaties are zeroed before adding them up - Rod%F6net = 0.0_DbKi - Rod%M6net = 0.0_DbKi - - ! now go through each node's contributions, put them about end A, and sum them - DO i = 0,Rod%N - - rRel = Rod%r(:,i) - Rod%r(:,0) ! vector from reference point to node - - ! convert segment net force into 6dof force about body ref point (if the Rod itself, end A) - CALL translateForce3to6DOF(rRel, Rod%Fnet(:,i), F6_i) - - ! convert segment mass matrix to 6by6 mass matrix about body ref point (if the Rod itself, end A) - CALL translateMass3to6DOF(rRel, Rod%M(:,:,i), M6_i) - - ! sum contributions - Rod%F6net = Rod%F6net + F6_i - Rod%M6net = Rod%M6net + M6_i - - END DO - - ! ------------- Calculate some items for the Rod as a whole here ----------------- - - ! >>> could some of these be precalculated just once? <<< - - ! add inertia terms for the Rod assuming it is uniform density (radial terms add to existing matrix which contains parallel-axis-theorem components only) - I_l = 0.125*Rod%mass * Rod%d*Rod%d ! axial moment of inertia - I_r = Rod%mass/12 * (0.75*Rod%d*Rod%d + (Rod%UnstrLen/Rod%N)**2 ) * Rod%N ! summed radial moment of inertia for each segment individually - - !h_c = [value from registry] - - Imat_l(1,1) = I_r ! inertia about CG in local orientations (as if Rod is vertical) - Imat_l(2,2) = I_r - Imat_l(3,3) = I_l - - OrMat = CalcOrientation(phi, beta, 0.0_DbKi) ! get rotation matrix to put things in global rather than rod-axis orientations - - Imat = RotateM3(Imat_l, OrMat) ! rotate to give inertia matrix about CG in global frame - - ! these supplementary inertias can then be added the matrix (these are the terms ASIDE from the parallel axis terms) - Rod%M6net(4:6,4:6) = Rod%M6net(4:6,4:6) + Imat - - - ! now add centripetal and gyroscopic forces/moments, and that should be everything - h_c = 0.5*Rod%UnstrLen ! distance to center of mass - r_c = h_c*Rod%q ! vector to center of mass - - ! note that Rod%v6(4:6) is the rotational velocity vector, omega - Fcentripetal = 0.0_DbKi !<< 0) Print*, "L", lineID, "->R", Rod%IdNum , "b" - - IF (Rod%nAttachedB <10) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. - Rod%nAttachedB = Rod%nAttachedB + 1 ! add the line to the number connected - Rod%AttachedB(Rod%nAttachedB) = lineID - Rod%TopB(Rod%nAttachedB) = TopOfLine ! attached to line ... 1 = top/fairlead(end B), 0 = bottom/anchor(end A) - ELSE - Print*, "too many lines connected to Rod ", Rod%IdNum, " in MoorDyn!" - END IF - - else ! attaching to end A - - IF (wordy > 0) Print*, "L", lineID, "->R", Rod%IdNum , "a" - - IF (Rod%nAttachedA <10) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. - Rod%nAttachedA = Rod%nAttachedA + 1 ! add the line to the number connected - Rod%AttachedA(Rod%nAttachedA) = lineID - Rod%TopA(Rod%nAttachedA) = TopOfLine ! attached to line ... 1 = top/fairlead(end B), 0 = bottom/anchor(end A) - ELSE - Print*, "too many lines connected to Rod ", Rod%IdNum, " in MoorDyn!" - END IF - - end if - - END SUBROUTINE Rod_AddLine - - - ! this function handles removing a line from a connection node - SUBROUTINE Rod_RemoveLine(Rod, lineID, TopOfLine, endB, rEnd, rdEnd) - - Type(MD_Rod), INTENT (INOUT) :: Rod ! the Connection object - - Integer(IntKi), INTENT( IN ) :: lineID - Integer(IntKi), INTENT( OUT) :: TopOfLine - Integer(IntKi), INTENT( IN ) :: endB ! end B if 1, end A if 0 - REAL(DbKi), INTENT(INOUT) :: rEnd(3) - REAL(DbKi), INTENT(INOUT) :: rdEnd(3) - - Integer(IntKi) :: l,m,J - - if (endB==1) then ! attaching to end B - - DO l = 1,Rod%nAttachedB ! look through attached lines - - IF (Rod%AttachedB(l) == lineID) THEN ! if this is the line's entry in the attachment list - - TopOfLine = Rod%TopB(l); ! record which end of the line was attached - - DO m = l,Rod%nAttachedB-1 - - Rod%AttachedB(m) = Rod%AttachedB(m+1) ! move subsequent line links forward one spot in the list to eliminate this line link - Rod%TopB( m) = Rod%TopB(m+1) - - Rod%nAttachedB = Rod%nAttachedB - 1 ! reduce attached line counter by 1 - - ! also pass back the kinematics at the end - DO J = 1,3 - rEnd( J) = Rod%r( J,Rod%N) - rdEnd(J) = Rod%rd(J,Rod%N) - END DO - - print*, "Detached line ", lineID, " from Rod ", Rod%IdNum, " end B" - - EXIT - END DO - - IF (l == Rod%nAttachedB) THEN ! detect if line not found - print *, "Error: failed to find line to remove during RemoveLine call to Rod ", Rod%IdNum, ". Line ", lineID - END IF - END IF - END DO - - else ! attaching to end A - - DO l = 1,Rod%nAttachedA ! look through attached lines - - IF (Rod%AttachedA(l) == lineID) THEN ! if this is the line's entry in the attachment list - - TopOfLine = Rod%TopA(l); ! record which end of the line was attached - - DO m = l,Rod%nAttachedA-1 - - Rod%AttachedA(m) = Rod%AttachedA(m+1) ! move subsequent line links forward one spot in the list to eliminate this line link - Rod%TopA( m) = Rod%TopA(m+1) - - Rod%nAttachedA = Rod%nAttachedA - 1 ! reduce attached line counter by 1 - - ! also pass back the kinematics at the end - DO J = 1,3 - rEnd( J) = Rod%r( J,0) - rdEnd(J) = Rod%rd(J,0) - END DO - - print*, "Detached line ", lineID, " from Rod ", Rod%IdNum, " end A" - - EXIT - END DO - - IF (l == Rod%nAttachedA) THEN ! detect if line not found - print *, "Error: failed to find line to remove during RemoveLine call to Rod ", Rod%IdNum, ". Line ", lineID - END IF - END IF - END DO - - end if - - END SUBROUTINE Rod_RemoveLine - - - - - - - - -!-------------------------------------------------------------- -! Body-Specific Subroutines -!-------------------------------------------------------------- - - - SUBROUTINE Body_Setup( Body, tempArray, rhoW, ErrStat, ErrMsg) - - TYPE(MD_Body), INTENT(INOUT) :: Body ! the single body object of interest - REAL(DbKi), INTENT(IN) :: tempArray(6) ! initial pose of body - REAL(DbKi), INTENT(IN) :: rhoW - INTEGER, INTENT(INOUT ) :: ErrStat ! returns a non-zero value when an error occurs - CHARACTER(*), INTENT(INOUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None - - INTEGER(4) :: J ! Generic index - INTEGER(4) :: K ! Generic index - INTEGER(IntKi) :: N - - REAL(DbKi) :: Mtemp(6,6) - - ! set initial velocity to zero - Body%v6 = 0.0_DbKi - - !also set number of attached rods and points to zero initially - Body%nAttachedC = 0 - Body%nAttachedR = 0 - - ! for now take one entry and apply to all three DOFs just using a single entry for all axes <<<<< - DO J=2,3 - Body%BodyI(J) = Body%BodyI(1) - Body%BodyCdA(J) = Body%BodyCdA(1) - Body%BodyCa(J) = Body%BodyCa(1) - END DO - - ! set up body initial mass matrix (excluding any rods or attachements) - DO J=1,3 - Mtemp(J,J) = Body%bodyM ! fill in mass - Mtemp(3+J,3+J) = Body%bodyI(J) ! fill in inertia - END DO - - CALL TranslateMass6to6DOF(Body%rCG, Mtemp, Body%M0) ! account for potential CG offset <<< is the direction right? <<< - - DO J=1,6 - Body%M0(J,J) = Body%M0(J,J) + Body%bodyV*Body%bodyCa(1) ! add added mass in each direction about ref point (so only diagonals) <<< eventually expand to multi D - END DO - - ! --------------- if this is an independent body (not coupled) ---------- - ! set initial position and orientation of body from input file - Body%r6 = tempArray - - ! calculate orientation matrix based on latest angles - !RotMat(r6[3], r6[4], r6[5], OrMat); - Body%OrMat = TRANSPOSE( EulerConstruct( Body%r6(4:6) ) ) ! full Euler angle approach <<<< need to check order - - IF (wordy > 0) print *, "Set up Body ",Body%IdNum, ", type ", Body%typeNum - - ! need to add cleanup sub <<< - - END SUBROUTINE Body_Setup - -! ! used to initialize bodies that aren't free i.e. don't have states -! !-------------------------------------------------------------- -! SUBROUTINE Body_InitializeUnfree(Body, r6_in, mesh, mesh_index, m) -! -! Type(MD_Body), INTENT(INOUT) :: Body ! the Body object -! Real(DbKi), INTENT(IN ) :: r6_in(6) ! state vector section for this line -! TYPE(MeshType), INTENT(INOUT) :: mesh ! -! Integer(IntKi), INTENT(IN ) :: mesh_index ! index of the node in the mesh for the current object being initialized -! TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects -! -! INTEGER(IntKi) :: l ! index of segments or nodes along line -! REAL(DbKi) :: rRef(3) ! reference position of mesh node -! REAL(DbKi) :: OrMat(3,3) ! DCM for body orientation based on r6_in -! REAL(DbKi) :: dummyStates(12) -! -! -! rRef = 0.0_DbKi ! <<< maybe this should be the offsets of the local platform origins from the global origins in future? And that's what's specificed by the Body input coordinates? -! -! CALL MeshPositionNode(mesh, mesh+index, rRef,ErrStat2,ErrMsg2)! "assign the coordinates (u%PtFairleadDisplacement%Position) of each node in the global coordinate space" -! -! CALL CheckError( ErrStat2, ErrMsg2 ) -! IF (ErrStat >= AbortErrLev) RETURN -! -! ! Apply offsets due to initial platform rotations and translations (fixed Jun 19, 2015) -! CALL SmllRotTrans('body rotation matrix', r6_in(4),r6_in(5),r6_in(6), OrMat, '', ErrStat2, ErrMsg2) -! mesh%TranslationDisp(1, mesh_index) = r6_in(1) + OrMat(1,1)*rRef(1) + OrMat(2,1)*rRef(2) + OrMat(3,1)*rRef(3) - rRef(1) -! mesh%TranslationDisp(2, mesh_index) = r6_in(2) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2) -! mesh%TranslationDisp(3, mesh_index) = r6_in(3) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) -! -! ! what about node point orientation ??? -! -! ! If any Rod is fixed to the body (not pinned), initialize it now because otherwise it won't be initialized -! DO l=1, Body%nAttachedR -! if (m%RodList(Body%attachedR(l))%typeNum == 2) CALL Rod_Initialize(m%RodList(Body%attachedR(l)), dummyStates, m%LineList) -! END DO -! -! ! Note: Connections don't need any initialization -! -! END SUBROUTINE Body_InitializeUnfree -! !-------------------------------------------------------------- - - - ! used to initialize bodies that are free - !-------------------------------------------------------------- - SUBROUTINE Body_Initialize(Body, states, m) - - Type(MD_Body), INTENT(INOUT) :: Body ! the Body object - Real(DbKi), INTENT(INOUT) :: states(:) ! state vector section for this Body - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - - INTEGER(IntKi) :: l ! index of segments or nodes along line - REAL(DbKi) :: dummyStates(12) ! dummy vector to mimic states when initializing a rigidly attached rod - - - ! assign initial body kinematics to state vector - states(7:12) = Body%r6 - states(1:6 ) = Body%v6 - - - ! set positions of any dependent connections and rods now (before they are initialized) - CALL Body_SetDependentKin(Body, 0.0_DbKi, m) - - ! If any Rod is fixed to the body (not pinned), initialize it now because otherwise it won't be initialized - DO l=1, Body%nAttachedR - if (m%RodList(Body%attachedR(l))%typeNum == 2) CALL Rod_Initialize(m%RodList(Body%attachedR(l)), dummyStates, m) - END DO - - ! Note: Connections don't need any initialization - - END SUBROUTINE Body_Initialize - !-------------------------------------------------------------- - - ! used to initialize bodies that are coupled or fixed - !-------------------------------------------------------------- - SUBROUTINE Body_InitializeUnfree(Body, m) - - Type(MD_Body), INTENT(INOUT) :: Body ! the Body object - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - - INTEGER(IntKi) :: l ! index of segments or nodes along line - REAL(DbKi) :: dummyStates(12) ! dummy vector to mimic states when initializing a rigidly attached rod - - - ! set positions of any dependent connections and rods now (before they are initialized) - CALL Body_SetDependentKin(Body, 0.0_DbKi, m) - - ! If any Rod is fixed to the body (not pinned), initialize it now because otherwise it won't be initialized - DO l=1, Body%nAttachedR - if (m%RodList(Body%attachedR(l))%typeNum == 2) CALL Rod_Initialize(m%RodList(Body%attachedR(l)), dummyStates, m) - END DO - - ! Note: Connections don't need any initialization - - END SUBROUTINE Body_InitializeUnfree - !-------------------------------------------------------------- - - - - !-------------------------------------------------------------- - SUBROUTINE Body_SetState(Body, X, t, m) - - Type(MD_Body), INTENT(INOUT) :: Body ! the Body object - Real(DbKi), INTENT(IN ) :: X(:) ! state vector section for this line - Real(DbKi), INTENT(IN ) :: t ! instantaneous time - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - - INTEGER(IntKi) :: l ! index of segments or nodes along line - INTEGER(IntKi) :: J ! index - - ! store current time - Body%time = t - - - - Body%r6 = X(7:12) ! get positions - Body%v6 = X(1:6) ! get velocities - - - ! set positions of any dependent connections and rods - CALL Body_SetDependentKin(Body, t, m) - - END SUBROUTINE Body_SetState - !-------------------------------------------------------------- - - - ! set kinematics for Bodies if they are coupled (or ground) - !-------------------------------------------------------------- - SUBROUTINE Body_SetKinematics(Body, r_in, v_in, a_in, t, m) - - Type(MD_Body), INTENT(INOUT) :: Body ! the Body object - Real(DbKi), INTENT(IN ) :: r_in(6) ! 6-DOF position - Real(DbKi), INTENT(IN ) :: v_in(6) ! 6-DOF velocity - Real(DbKi), INTENT(IN ) :: a_in(6) ! 6-DOF acceleration (only used for coupled rods) - Real(DbKi), INTENT(IN ) :: t ! instantaneous time - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects (for simplicity, since Bodies deal with Rods and Connections) - - - INTEGER(IntKi) :: l - - ! store current time - Body%time = t - - ! if (abs(Body%typeNum) == 2) then ! body coupled in 6 DOF, or ground - Body%r6 = r_in - Body%v6 = v_in - Body%a6 = a_in - - ! since this body has no states and all DOFs have been set, pass its kinematics to dependent attachments - CALL Body_SetDependentKin(Body, t, m) - - ! else if (abs(Body%typeNum) == 1) then ! body pinned at reference point - ! - ! ! set Body *end A only* kinematics based on BCs (linear model for now) - ! Body%r6(1:3) = r_in(1:3) - ! Body%v6(1:3) = v_in(1:3) - ! - ! ! Body is pinned so only ref point posiiton is specified, rotations are left alone and will be - ! ! handled, along with passing kinematics to attached objects, by separate call to setState - ! - ! else - ! print *, "Error: Body_SetKinematics called for a free Body." ! <<< - ! end if - - END SUBROUTINE Body_SetKinematics - !-------------------------------------------------------------- - - - ! set the states (positions and velocities) of any connects or rods that are part of this body - ! also computes the orientation matrix (never skip this sub!) - !-------------------------------------------------------------- - SUBROUTINE Body_SetDependentKin(Body, t, m) - - Type(MD_Body), INTENT(INOUT) :: Body ! the Bodyion object - REAL(DbKi), INTENT(IN ) :: t - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects (for simplicity, since Bodies deal with Rods and Connections) - - INTEGER(IntKi) :: l ! index of attached objects - - Real(DbKi) :: rConnect(3) - Real(DbKi) :: rdConnect(3) - Real(DbKi) :: rRod(6) - Real(DbKi) :: vRod(6) - Real(DbKi) :: aRod(6) - - - - ! calculate orientation matrix based on latest angles - !CALL SmllRotTrans('', Body%r6(4), Body%r6(5), Body%r6(6), Body%TransMat, '', ErrStat2, ErrMsg2) - Body%OrMat = TRANSPOSE( EulerConstruct( Body%r6(4:6) ) ) ! full Euler angle approach <<<< need to check order - - ! set kinematics of any dependent connections - do l = 1,Body%nAttachedC - - CALL transformKinematics(Body%rConnectRel(:,l), Body%r6, Body%OrMat, Body%v6, rConnect, rdConnect) !<<< should double check this function - - ! >>> need to add acceleration terms here too? <<< - - ! pass above to the connection and get it to calculate the forces - CALL Connect_SetKinematics( m%ConnectList(Body%attachedC(l)), rConnect, rdConnect, m%zeros6(1:3), t, m) - end do - - ! set kinematics of any dependent Rods - do l=1,Body%nAttachedR - - ! calculate displaced coordinates/orientation and velocities of each rod <<<<<<<<<<<<< - ! do 3d details of Rod ref point - CALL TransformKinematicsA( Body%r6RodRel(1:3,l), Body%r6(1:3), Body%OrMat, Body%v6, Body%a6, rRod(1:3), vRod(1:3), aRod(1:3)) ! set first three entires (end A translation) of rRod and rdRod - ! does the above function need to take in all 6 elements of r6RodRel?? - - ! do rotational stuff - rRod(4:6) = MATMUL(Body%OrMat, Body%r6RodRel(4:6,l)) !<<<<<< correct? <<<<< rotateVector3(r6RodRel[i]+3, OrMat, rRod+3); ! rotate rod relative unit vector by OrMat to get unit vec in reference coords - vRod(4:6) = Body%v6(4:6) ! transformed rotational velocity. <<< is this okay as is? <<<< - aRod(4:6) = Body%a6(4:6) - - ! pass above to the rod and get it to calculate the forces - CALL Rod_SetKinematics(m%RodList(Body%attachedR(l)), rRod, vRod, aRod, t, m) - end do - - END SUBROUTINE Body_SetDependentKin - !-------------------------------------------------------------- - - ! calculate the aggregate 3/6DOF rigid-body loads of a coupled rod including inertial loads - !-------------------------------------------------------------- - SUBROUTINE Body_GetCoupledForce(Body, Fnet_out, m, p) - - Type(MD_Body), INTENT(INOUT) :: Body ! the Body object - Real(DbKi), INTENT( OUT) :: Fnet_out(6) ! force and moment vector - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters - - Real(DbKi) :: F6_iner(6) ! inertial reaction force - - ! do calculations of forces and masses on the body - CALL Body_DoRHS(Body, m, p) - - ! add inertial loads as appropriate - if (Body%typeNum == -1) then - - F6_iner = 0.0_DbKi !-MATMUL(Body%M, Body%a6) <<<<<<<< why does including F6_iner cause instability??? - Fnet_out = Body%F6net + F6_iner ! add inertial loads - - else - print *, "ERROR, Body_GetCoupledForce called for wrong (non-coupled) body type in MoorDyn!" - end if - - END SUBROUTINE Body_GetCoupledForce - !-------------------------------------------------------------- - - - !-------------------------------------------------------------- - SUBROUTINE Body_GetStateDeriv(Body, Xd, m, p) - - Type(MD_Body), INTENT(INOUT) :: Body ! the Bodyion object - Real(DbKi), INTENT(INOUT) :: Xd(:) ! state derivative vector section for this line - - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters - - INTEGER(IntKi) :: J ! index - - Real(DbKi) :: acc(6) ! 6DOF acceleration vector - - Real(DbKi) :: y_temp (6) ! temporary vector for LU decomposition - Real(DbKi) :: LU_temp(6,6) ! temporary matrix for LU decomposition - - - ! Initialize temp variables - y_temp = 0.0_DbKi -! FIXME: should LU_temp be set to M_out before calling LUsolve????? - LU_temp = 0.0_DbKi - - CALL Body_DoRHS(Body, m, p) - - ! solve for accelerations in [M]{a}={f} using LU decomposition - CALL LUsolve(6, Body%M, LU_temp, Body%F6net, y_temp, acc) - - ! fill in state derivatives - Xd(7:12) = Body%v6 ! dxdt = V (velocities) - Xd(1:6) = acc ! dVdt = a (accelerations) - - ! store accelerations in case they're useful as output - Body%a6 = acc - - ! check for NaNs (should check all state derivatives, not just first 6) - DO J = 1, 6 - IF (Is_NaN(Xd(J))) THEN - print *, "NaN detected at time ", Body%time, " in Body ",Body%IdNum, "in MoorDyn," - IF (wordy > 0) print *, "state derivatives:" - IF (wordy > 0) print *, Xd - EXIT - END IF - END DO - - - END SUBROUTINE Body_GetStateDeriv - !-------------------------------------------------------------- - - !-------------------------------------------------------------- - SUBROUTINE Body_DoRHS(Body, m, p) - - Type(MD_Body), INTENT(INOUT) :: Body ! the Bodyion object - TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects - TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters - - !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables - - INTEGER(IntKi) :: l ! index of attached lines - INTEGER(IntKi) :: I ! index - INTEGER(IntKi) :: J ! index - INTEGER(IntKi) :: K ! index - - Real(DbKi) :: Fgrav(3) ! body weight force - Real(DbKi) :: body_rCGrotated(3) ! instantaneous vector from body ref point to CG - Real(DbKi) :: U(3) ! water velocity - zero for now - Real(DbKi) :: Ud(3) ! water acceleration- zero for now - Real(DbKi) :: vi(6) ! relative water velocity (last 3 terms are rotatonal and will be set to zero - Real(DbKi) :: F6_i(6) ! net force and moments from an attached object - Real(DbKi) :: M6_i(6,6) ! mass and inertia from an attached object - - ! Initialize variables - U = 0.0_DbKi ! Set to zero for now - - ! First, the body's own mass matrix must be adjusted based on its orientation so that - ! we have a mass matrix in the global orientation frame - Body%M = RotateM6(Body%M0, Body%OrMat) - - !gravity on core body - Fgrav(1) = 0.0_DbKi - Fgrav(2) = 0.0_DbKi - Fgrav(3) = Body%bodyV * p%rhow * p%g - Body%bodyM * p%g ! weight+buoyancy vector - - body_rCGrotated = MATMUL(Body%OrMat, Body%rCG) ! rotateVector3(body_rCG, OrMat, body_rCGrotated); ! relative vector to body CG in inertial orientation - CALL translateForce3to6DOF(body_rCGrotated, Fgrav, Body%F6net) ! gravity forces and moments about body ref point given CG location - - - ! --------------------------------- apply wave kinematics ------------------------------------ - !env->waves->getU(r6, t, U); ! call generic function to get water velocities <<<<<<<<< all needs updating - - ! for (int J=0; J<3; J++) - ! Ud[J] = 0.0; ! set water accelerations as zero for now - ! ------------------------------------------------------------------------------------------ - - ! viscous drag calculation (on core body) - vi(1:3) = U - Body%v6(1:3) ! relative flow velocity over body ref point - vi(4:6) = - Body%v6(4:6) ! for rotation, this is just the negative of the body's rotation for now (not allowing flow rotation) - - Body%F6net = Body%F6net + 0.5*p%rhoW * vi * abs(vi) * Body%bodyCdA - ! <<< NOTE, for body this should be fixed to account for orientation!! <<< what about drag in rotational DOFs??? <<<<<<<<<<<<<< - - - - ! Get contributions from any dependent connections - do l = 1,Body%nAttachedC - - ! get net force and mass from Connection on body ref point (global orientation) - CALL Connect_GetNetForceAndMass( m%ConnectList(Body%attachedC(l)), Body%r6(1:3), F6_i, M6_i, m, p) - - ! sum quantitites - Body%F6net = Body%F6net + F6_i - Body%M = Body%M + M6_i - end do - - ! Get contributions from any dependent Rods - do l=1,Body%nAttachedR - - ! get net force and mass from Rod on body ref point (global orientation) - CALL Rod_GetNetForceAndMass(m%RodList(Body%attachedR(l)), Body%r6(1:3), F6_i, M6_i, m, p) - - ! sum quantitites - Body%F6net = Body%F6net + F6_i - Body%M = Body%M + M6_i - end do - - - END SUBROUTINE Body_DoRHS - !===================================================================== - - - - - ! this function handles assigning a connection to a body - !-------------------------------------------------------------- - SUBROUTINE Body_AddConnect(Body, connectID, coords) - - Type(MD_Body), INTENT(INOUT) :: Body ! the Connection object - Integer(IntKi), INTENT(IN ) :: connectID - REAL(DbKi), INTENT(IN ) :: coords(3) - - - IF (wordy > 0) Print*, "C", connectID, "->B", Body%IdNum - - IF(Body%nAttachedC < 30) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. - Body%nAttachedC = Body%nAttachedC + 1 ! increment the number connected - Body%AttachedC(Body%nAttachedC) = connectID - Body%rConnectRel(:,Body%nAttachedC) = coords ! store relative position of connect on body - ELSE - Print*, "too many Points attached to Body ", Body%IdNum, " in MoorDyn!" - END IF - - END SUBROUTINE Body_AddConnect - - - ! this function handles assigning a rod to a body - !-------------------------------------------------------------- - SUBROUTINE Body_AddRod(Body, rodID, coords) - - Type(MD_Body), INTENT(INOUT) :: Body ! the Connection object - Integer(IntKi), INTENT(IN ) :: rodID - REAL(DbKi), INTENT(IN ) :: coords(6) ! positions of rod ends A and B relative to body - - REAL(DbKi) :: tempUnitVec(3) - REAL(DbKi) :: dummyLength - - IF (wordy > 0) Print*, "R", rodID, "->B", Body%IdNum - - IF(Body%nAttachedR < 30) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. - Body%nAttachedR = Body%nAttachedR + 1 ! increment the number connected - - ! store rod ID - Body%AttachedR(Body%nAttachedR) = rodID - - ! store Rod end A relative position and unit vector from end A to B - CALL UnitVector(coords(1:3), coords(4:6), tempUnitVec, dummyLength) - Body%r6RodRel(1:3, Body%nAttachedR) = coords(1:3) - Body%r6RodRel(4:6, Body%nAttachedR) = tempUnitVec - - ELSE - Print*, "too many rods attached to Body ", Body%IdNum, " in MoorDyn" - END IF - - END SUBROUTINE Body_AddRod - - - -!++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -! ###### The following four routines are Jacobian routines for linearization capabilities ####### -! If the module does not implement them, set ErrStat = ErrID_Fatal in SD_Init() when InitInp%Linearize is .true. -!---------------------------------------------------------------------------------------------------------------------------------- -!> Routine to compute the Jacobians of the output (Y), continuous- (X), discrete- (Xd), and constraint-state (Z) functions -!! with respect to the inputs (u). The partial derivatives dY/du, dX/du, dXd/du, and DZ/du are returned. -SUBROUTINE MD_JacobianPInput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, dYdu, dXdu, dXddu, dZdu) - REAL(DbKi), INTENT(IN ) :: t !< Time in seconds at operating point - TYPE(MD_InputType), INTENT(INOUT) :: u !< Inputs at operating point (may change to inout if a mesh copy is required) - TYPE(MD_ParameterType), INTENT(IN ) :: p !< Parameters - TYPE(MD_ContinuousStateType), INTENT(IN ) :: x !< Continuous states at operating point - TYPE(MD_DiscreteStateType), INTENT(IN ) :: xd !< Discrete states at operating point - TYPE(MD_ConstraintStateType), INTENT(IN ) :: z !< Constraint states at operating point - TYPE(MD_OtherStateType), INTENT(IN ) :: OtherState !< Other states at operating point - TYPE(MD_OutputType), INTENT(INOUT) :: y !< Output (change to inout if a mesh copy is required); Output fields are not used by this routine, but type is available here so that mesh parameter information (i.e., connectivity) does not have to be recalculated for dYdu. - TYPE(MD_MiscVarType), INTENT(INOUT) :: m !< Misc/optimization variables - INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status of the operation - CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message if ErrStat /= ErrID_None - REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dYdu(:,:) !< Partial derivatives of output functions (Y) wrt the inputs (u) [intent in to avoid deallocation] - REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dXdu(:,:) !< Partial derivatives of continuous state functions (X) wrt the inputs (u) [intent in to avoid deallocation] - REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dXddu(:,:) !< Partial derivatives of discrete state functions (Xd) wrt the inputs (u) [intent in to avoid deallocation] - REAL(R8Ki), ALLOCATABLE, OPTIONAL, INTENT(INOUT) :: dZdu(:,:) !< Partial derivatives of constraint state functions (Z) wrt the inputs (u) [intent in to avoid deallocation] - - ! local variables - TYPE(MD_OutputType) :: y_m, y_p - TYPE(MD_ContinuousStateType) :: x_m, x_p - TYPE(MD_InputType) :: u_perturb - REAL(R8Ki) :: delta_p, delta_m ! delta change in input (plus, minus) - INTEGER(IntKi) :: i - integer(intKi) :: ErrStat2 - character(ErrMsgLen) :: ErrMsg2 - character(*), parameter :: RoutineName = 'MD_JacobianPInput' - - ! Initialize ErrStat - ErrStat = ErrID_None - ErrMsg = '' - - ! get OP values here: - call MD_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat2, ErrMsg2 ); if(Failed()) return - - ! make a copy of the inputs to perturb - call MD_CopyInput( u, u_perturb, MESH_NEWCOPY, ErrStat2, ErrMsg2); if(Failed()) return - - IF ( PRESENT( dYdu ) ) THEN - ! Calculate the partial derivative of the output functions (Y) with respect to the inputs (u) here: - if (.not. allocated(dYdu) ) then - call AllocAry(dYdu, p%Jac_ny, size(p%Jac_u_indx,1),'dYdu', ErrStat2, ErrMsg2); if(Failed()) return - end if - ! make a copy of outputs because we will need two for the central difference computations (with orientations) - call MD_CopyOutput( y, y_p, MESH_NEWCOPY, ErrStat2, ErrMsg2); if(Failed()) return - call MD_CopyOutput( y, y_m, MESH_NEWCOPY, ErrStat2, ErrMsg2); if(Failed()) return - do i=1,size(p%Jac_u_indx,1) - ! get u_op + delta_p u - call MD_CopyInput( u, u_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) - call MD_Perturb_u( p, i, 1, u_perturb, delta_p ) - ! compute y at u_op + delta_p u - call MD_CalcOutput( t, u_perturb, p, x, xd, z, OtherState, y_p, m, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) - ! get u_op - delta_m u - call MD_CopyInput( u, u_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) - call MD_Perturb_u( p, i, -1, u_perturb, delta_m ) - ! compute y at u_op - delta_m u - call MD_CalcOutput( t, u_perturb, p, x, xd, z, OtherState, y_m, m, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) - ! get central difference: - call MD_Compute_dY( p, y_p, y_m, delta_p, dYdu(:,i) ) - end do - if(Failed()) return - END IF - IF ( PRESENT( dXdu ) ) THEN - if (.not. allocated(dXdu)) then - call AllocAry(dXdu, p%Jac_nx, size(p%Jac_u_indx,1), 'dXdu', ErrStat2, ErrMsg2); if (Failed()) return - endif - do i=1,size(p%Jac_u_indx,1) - ! get u_op + delta u - call MD_CopyInput( u, u_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) - call MD_Perturb_u( p, i, 1, u_perturb, delta_p ) - ! compute x at u_op + delta u - call MD_CalcContStateDeriv( t, u_perturb, p, x, xd, z, OtherState, m, x_p, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) - ! get u_op - delta u - call MD_CopyInput( u, u_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) - call MD_Perturb_u( p, i, -1, u_perturb, delta_m ) - ! compute x at u_op - delta u - call MD_CalcContStateDeriv( t, u_perturb, p, x, xd, z, OtherState, m, x_m, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) - ! get central difference: - ! we may have had an error allocating memory, so we'll check - if(Failed()) return - ! get central difference: - call MD_Compute_dX( p, x_p, x_m, delta_p, dXdu(:,i) ) - end do - END IF ! dXdu - IF ( PRESENT( dXddu ) ) THEN - if (allocated(dXddu)) deallocate(dXddu) - END IF - IF ( PRESENT( dZdu ) ) THEN - if (allocated(dZdu)) deallocate(dZdu) - END IF - call CleanUp() -contains - - logical function Failed() - call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) - Failed = ErrStat >= AbortErrLev - if (Failed) call CleanUp() - end function Failed - - subroutine CleanUp() - call MD_DestroyContState( x_p, ErrStat2, ErrMsg2 ) ! we don't need this any more - call MD_DestroyContState( x_m, ErrStat2, ErrMsg2 ) ! we don't need this any more - call MD_DestroyOutput( y_p, ErrStat2, ErrMsg2 ) - call MD_DestroyOutput( y_m, ErrStat2, ErrMsg2 ) - call MD_DestroyInput(u_perturb, ErrStat2, ErrMsg2 ) - end subroutine cleanup + subroutine CleanUp() + call MD_DestroyContState( x_p, ErrStat2, ErrMsg2 ) ! we don't need this any more + call MD_DestroyContState( x_m, ErrStat2, ErrMsg2 ) ! we don't need this any more + call MD_DestroyOutput( y_p, ErrStat2, ErrMsg2 ) + call MD_DestroyOutput( y_m, ErrStat2, ErrMsg2 ) + call MD_DestroyInput(u_perturb, ErrStat2, ErrMsg2 ) + end subroutine cleanup END SUBROUTINE MD_JacobianPInput !---------------------------------------------------------------------------------------------------------------------------------- @@ -7009,742 +3403,6 @@ END SUBROUTINE MD_GetOP - - - ! :::::::::::::::::::::::::: below are some wave related functions ::::::::::::::::::::::::::::::: - - - ! master function to get wave/water kinematics at a given point -- called by each object fro grid-based data - SUBROUTINE getWaveKin(p, x, y, z, t, tindex, U, Ud, zeta, PDyn) - - ! note, this whole approach assuems that px, py, and pz are in increasing order - - TYPE(MD_ParameterType),INTENT (IN ) :: p ! MoorDyn parameters (contains the wave info for now) - Real(DbKi), INTENT (IN ) :: x - Real(DbKi), INTENT (IN ) :: y - Real(DbKi), INTENT (IN ) :: z - Real(DbKi), INTENT (IN ) :: t - INTEGER(IntKi), INTENT (INOUT) :: tindex ! pass time index to try starting from, returns identified time index - Real(DbKi), INTENT (INOUT) :: U(3) - Real(DbKi), INTENT (INOUT) :: Ud(3) - Real(DbKi), INTENT (INOUT) :: zeta - Real(DbKi), INTENT (INOUT) :: PDyn - - - INTEGER(IntKi) :: ix, iy, iz, it ! indeces for interpolation - INTEGER(IntKi) :: N ! number of rod elements for convenience - Real(DbKi) :: fx, fy, fz, ft ! interpolation fractions - Real(DbKi) :: qt ! used in time step interpolation - - - CALL getInterpNums(p%px , x, 1, ix, fx) - CALL getInterpNums(p%py , y, 1, iy, fy) - CALL getInterpNums(p%pz , z, 1, iz, fz) - CALL getInterpNums(p%tWave, t, tindex, it, ft) - tindex = it - - CALL calculate3Dinterpolation(p%zeta, ix, iy, it, fx, fy, ft, zeta) - - CALL calculate4Dinterpolation(p%PDyn, ix, iy, iz, it, fx, fy, fz, ft, PDyn) - - CALL calculate4Dinterpolation(p%ux, ix, iy, iz, it, fx, fy, fz, ft, U(1) ) - CALL calculate4Dinterpolation(p%uy, ix, iy, iz, it, fx, fy, fz, ft, U(2) ) - CALL calculate4Dinterpolation(p%uz, ix, iy, iz, it, fx, fy, fz, ft, U(3) ) - CALL calculate4Dinterpolation(p%ax, ix, iy, iz, it, fx, fy, fz, ft, Ud(1) ) - CALL calculate4Dinterpolation(p%ay, ix, iy, iz, it, fx, fy, fz, ft, Ud(2) ) - CALL calculate4Dinterpolation(p%az, ix, iy, iz, it, fx, fy, fz, ft, Ud(3) ) - - - END SUBROUTINE - - - SUBROUTINE getDepthFromBathymetry(BathymetryGrid, BathGrid_Xs, BathGrid_Ys, LineX, LineY, depth) - - REAL(DbKi), INTENT(IN ) :: BathymetryGrid(:,:) ! need colons or some sort of dimension setting - REAL(DbKi), INTENT(IN ) :: BathGrid_Xs(:) - REAL(DbKi), INTENT(IN ) :: BathGrid_Ys(:) - REAL(DbKi), INTENT(IN ) :: LineX - REAL(DbKi), INTENT(IN ) :: LineY - REAL(DbKi), INTENT( OUT) :: depth - - INTEGER(IntKi) :: ix, iy ! indeces for interpolation - Real(DbKi) :: fx, fy ! interpolation fractions - - CALL getInterpNums(BathGrid_Xs, LineX, 1, ix, fx) - CALL getInterpNums(BathGrid_Ys, LineY, 1, iy, fy) - - CALL calculate2Dinterpolation(BathymetryGrid, ix, iy, fx, fy, depth) - - END SUBROUTINE getDepthFromBathymetry - - - SUBROUTINE getInterpNums(xlist, xin, istart, i, fout) - - Real(DbKi), INTENT (IN ) :: xlist(:) ! list of x values - Real(DbKi), INTENT (IN ) :: xin ! x value to be interpolated - Integer(IntKi),INTENT (IN ) :: istart ! first lower index to try - Integer(IntKi),INTENT ( OUT) :: i ! lower index to interpolate from - Real(DbKi), INTENT ( OUT) :: fout ! fraction to return such that y* = y[i] + fout*(y[i+1]-y[i]) - - Integer(IntKi) :: i1 - Integer(IntKi) :: nx - - i1 = 1 ! Setting in declaration causes an implied save, which would never allow this routine to find anything at the start of the array. - - nx = SIZE(xlist) - - if (xin <= xlist(1)) THEN ! below lowest data point - i = 1_IntKi - fout = 0.0_DbKi - - else if (xlist(nx) <= xin) THEN ! above highest data point - i = nx - fout = 0.0_DbKi - - else ! within the data range - - IF (xlist(min(istart,nx)) < xin) i1 = istart ! if istart is below the actual value, start with it instead of starting at 1 to save time, but make sure it doesn't overstep the array - - DO i = i1, nx-1 - IF (xlist(i+1) > xin) THEN - fout = (xin - xlist(i) )/( xlist(i+1) - xlist(i) ) - exit - END IF - END DO - END IF - - END SUBROUTINE - - - SUBROUTINE calculate4Dinterpolation(f, ix0, iy0, iz0, it0, fx, fy, fz, ft, c) - - Real(DbKi), INTENT (IN ) :: f(:,:,:,:) ! data array - INTEGER(IntKi), INTENT (IN ) :: ix0, iy0, iz0, it0 ! indices for interpolation - Real(DbKi), INTENT (IN ) :: fx, fy, fz, ft ! interpolation fractions - Real(DbKi), INTENT ( OUT) :: c ! the output value - - INTEGER(IntKi) :: ix1, iy1, iz1, it1 ! second indices - REAL(DbKi) :: c000, c001, c010, c011, c100, c101, c110, c111 - REAL(DbKi) :: c00, c01, c10, c11, c0, c1 - - ! handle end case conditions - if (fx == 0) then - ix1 = ix0 - else - ix1 = min(ix0+1,size(f,4)) ! don't overstep bounds - end if - - if (fy == 0) then - iy1 = iy0 - else - iy1 = min(iy0+1,size(f,3)) ! don't overstep bounds - end if - - if (fz == 0) then - iz1 = iz0 - else - iz1 = min(iz0+1,size(f,2)) ! don't overstep bounds - end if - - if (ft == 0) then - it1 = it0 - else - it1 = min(it0+1,size(f,1)) ! don't overstep bounds - end if - - c000 = f(it0,iz0,iy0,ix0)*(1.0-ft) + f(it1,iz0,iy0,ix0)*ft - c001 = f(it0,iz1,iy0,ix0)*(1.0-ft) + f(it1,iz1,iy0,ix0)*ft - c010 = f(it0,iz0,iy1,ix0)*(1.0-ft) + f(it1,iz0,iy1,ix0)*ft - c011 = f(it0,iz1,iy1,ix0)*(1.0-ft) + f(it1,iz1,iy1,ix0)*ft - c100 = f(it0,iz0,iy0,ix1)*(1.0-ft) + f(it1,iz0,iy0,ix1)*ft - c101 = f(it0,iz1,iy0,ix1)*(1.0-ft) + f(it1,iz1,iy0,ix1)*ft - c110 = f(it0,iz0,iy1,ix1)*(1.0-ft) + f(it1,iz0,iy1,ix1)*ft - c111 = f(it0,iz1,iy1,ix1)*(1.0-ft) + f(it1,iz1,iy1,ix1)*ft - - c00 = c000*(1.0-fx) + c100*fx - c01 = c001*(1.0-fx) + c101*fx - c10 = c010*(1.0-fx) + c110*fx - c11 = c011*(1.0-fx) + c111*fx - - c0 = c00 *(1.0-fy) + c10 *fy - c1 = c01 *(1.0-fy) + c11 *fy - - c = c0 *(1.0-fz) + c1 *fz - - END SUBROUTINE - - - SUBROUTINE calculate3Dinterpolation(f, ix0, iy0, iz0, fx, fy, fz, c) - - Real(DbKi), INTENT (IN ) :: f(:,:,:) ! data array - INTEGER(IntKi), INTENT (IN ) :: ix0, iy0, iz0 ! indices for interpolation - Real(DbKi), INTENT (IN ) :: fx, fy, fz ! interpolation fractions - Real(DbKi), INTENT ( OUT) :: c ! the output value - - INTEGER(IntKi) :: ix1, iy1, iz1 ! second indices - REAL(DbKi) :: c000, c001, c010, c011, c100, c101, c110, c111 - REAL(DbKi) :: c00, c01, c10, c11, c0, c1 - - ! note that "z" could also be "t" - dimension names are arbitrary - - ! handle end case conditions - if (fx == 0) then - ix1 = ix0 - else - ix1 = min(ix0+1,size(f,3)) ! don't overstep bounds - end if - - if (fy == 0) then - iy1 = iy0 - else - iy1 = min(iy0+1,size(f,2)) ! don't overstep bounds - end if - - if (fz == 0) then - iz1 = iz0 - else - iz1 = min(iz0+1,size(f,1)) ! don't overstep bounds - end if - - c000 = f(iz0,iy0,ix0) - c001 = f(iz1,iy0,ix0) - c010 = f(iz0,iy1,ix0) - c011 = f(iz1,iy1,ix0) - c100 = f(iz0,iy0,ix1) - c101 = f(iz1,iy0,ix1) - c110 = f(iz0,iy1,ix1) - c111 = f(iz1,iy1,ix1) - - c00 = c000*(1.0-fx) + c100*fx - c01 = c001*(1.0-fx) + c101*fx - c10 = c010*(1.0-fx) + c110*fx - c11 = c011*(1.0-fx) + c111*fx - - c0 = c00 *(1.0-fy) + c10 *fy - c1 = c01 *(1.0-fy) + c11 *fy - - c = c0 *(1.0-fz) + c1 *fz - - END SUBROUTINE - - SUBROUTINE calculate2Dinterpolation(f, ix0, iy0, fx, fy, c) - REAL(DbKi), INTENT (IN ) :: f(:,:) ! data array - INTEGER(IntKi), INTENT (IN ) :: ix0, iy0 ! indices for interpolation - REAL(DbKi), INTENT (IN ) :: fx, fy ! interpolation fractions - REAL(DbKi), INTENT ( OUT) :: c ! the output value - - INTEGER(IntKi) :: ix1, iy1 ! second indices - REAL(DbKi) :: c00, c01, c10, c11, c0, c1 - - ! handle end case conditions - IF (fx == 0) THEN - ix1 = ix0 - ELSE - ix1 = min(ix0+1,size(f,2)) ! don't overstep bounds - END IF - IF (fy == 0) THEN - iy1 = iy0 - ELSE - iy1 = min(iy0+1,size(f,1)) ! don't overstep bounds - END IF - c00 = f(iy0, ix0) - c01 = f(iy1, ix0) - c10 = f(iy0, ix1) - c11 = f(iy1, ix1) - c0 = c00 *(1.0-fx) + c10 *fx - c1 = c01 *(1.0-fx) + c11 *fx - c = c0 *(1.0-fy) + c1 *fy - END SUBROUTINE calculate2Dinterpolation - - ! ============ below are some math convenience functions =============== - ! should add error checking if I keep these, but hopefully there are existing NWTCLib functions to replace them - - - ! return unit vector (u) and in direction from r1 to r2 and distance between points - !----------------------------------------------------------------------- - SUBROUTINE UnitVector( r1, r2, u, Length ) ! note: order of parameters chagned in this function - - REAL(DbKi), INTENT(IN ) :: r1(:) - REAL(DbKi), INTENT(IN ) :: r2(:) - REAL(DbKi), INTENT( OUT) :: u(:) - REAL(DbKi), INTENT( OUT) :: length - - u = r2 - r1 - length = TwoNorm(u) - - if ( .NOT. EqualRealNos(length, 0.0_DbKi ) ) THEN - u = u / Length - END IF - - END SUBROUTINE UnitVector - !----------------------------------------------------------------------- - - ! scale vector to desired length - !----------------------------------------------------------------------- - SUBROUTINE ScaleVector( u_in, newlength, u_out ) - REAL(DbKi), INTENT(IN ) :: u_in(3) ! input vector - REAL(DbKi), INTENT(IN ) :: newlength ! desired length of output vector - REAL(DbKi), INTENT(INOUT) :: u_out(3) ! output vector (hopefully can be the same as u_in without issue) - - REAL(DbKi) :: length_squared - REAL(DbKi) :: scaler - INTEGER(IntKi) :: J - - length_squared = 0.0; - DO J=1,3 - length_squared = length_squared + u_in(J)*u_in(J) - END DO - - if (length_squared > 0) then - scaler = newlength/sqrt(length_squared) - else ! if original vector is zero, return zero - scaler = 0_DbKi - end if - - DO J=1,3 - u_out(J) = u_in(J) * scaler - END DO - - END SUBROUTINE ScaleVector - !----------------------------------------------------------------------- - - - ! calculate orientation angles of a cylindrical object - !----------------------------------------------------------------------- - subroutine GetOrientationAngles(p1, p2, phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat) - real(DbKi), intent(in ) :: p1(3),p2(3) - real(DbKi), intent( out) :: phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat(3) - - real(DbKi) :: vec(3), vecLen, vecLen2D - - ! calculate isntantaneous incline angle and heading, and related trig values - ! the first and last NodeIndx values point to the corresponding Joint nodes idices which are at the start of the Mesh - vec = p2 - p1 - vecLen = SQRT(Dot_Product(vec,vec)) - vecLen2D = SQRT(vec(1)**2+vec(2)**2) - if ( vecLen < 0.000001 ) then - print *, "ERROR in GetOrientationAngles in MoorDyn" !call SeterrStat(ErrID_Fatal, 'An element of the Morison structure has co-located endpoints! This should never occur. Please review your model.', errStat, errMsg, 'Morison_CalcOutput' ) - print *, p1 - print *, p2 - k_hat = NaN ! 1.0/0.0 - else - k_hat = vec / vecLen - phi = atan2(vecLen2D, vec(3)) ! incline angle - end if - if ( phi < 0.000001) then - beta = 0.0_ReKi - else - beta = atan2(vec(2), vec(1)) ! heading of incline - endif - sinPhi = sin(phi) - cosPhi = cos(phi) - tanPhi = tan(phi) - sinBeta = sin(beta) - cosBeta = cos(beta) - - end subroutine GetOrientationAngles - !----------------------------------------------------------------------- - - - ! calculate position and velocity of point based on its position relative to moving 6DOF body - !----------------------------------------------------------------------- - SUBROUTINE TransformKinematics(rRelBody, r_in, TransMat, rd_in, r_out, rd_out) - REAL(DbKi), INTENT(IN ) :: rRelBody(:) ! coordinate of end A - REAL(DbKi), INTENT(IN ) :: r_in(3) ! Rod unit vector - REAL(DbKi), INTENT(IN ) :: TransMat(3,3)! - REAL(DbKi), INTENT(IN ) :: rd_in(6) ! 6DOF velecity vector about Rod end A, in global orientation frame - REAL(DbKi), INTENT( OUT) :: r_out(3) ! coordinates of end B - REAL(DbKi), INTENT( OUT) :: rd_out(3) ! velocity of end B - - REAL(DbKi) :: rRel(3) - - ! rd_in should be in global orientation frame - ! note: it's okay if r_out and rd_out are 6-size. Only the first 3 will be written, and 4-6 will - ! already be correct or can be assigned seperately from r_in and rd_in (assuming orientation frames are identical) - - - ! locations (unrotated reference frame) about platform reference point (2021-01-05: just transposed TransMat, it was incorrect before) - rRel(1) = TransMat(1,1)*rRelBody(1) + TransMat(1,2)*rRelBody(2) + TransMat(1,3)*rRelBody(3) ! x - rRel(2) = TransMat(2,1)*rRelBody(1) + TransMat(2,2)*rRelBody(2) + TransMat(2,3)*rRelBody(3) ! y - rRel(3) = TransMat(3,1)*rRelBody(1) + TransMat(3,2)*rRelBody(2) + TransMat(3,3)*rRelBody(3) ! z - - ! absolute locations - r_out = rRel + r_in - - ! absolute velocities - rd_out(1) = - rd_in(6)*rRel(2) + rd_in(5)*rRel(3) + rd_in(1) ! x - rd_out(2) = rd_in(6)*rRel(1) - rd_in(4)*rRel(3) + rd_in(2) ! y - rd_out(3) = -rd_in(5)*rRel(1) + rd_in(4)*rRel(2) + rd_in(3) ! z - - ! absolute accelerations - rd_out(1) = - rd_in(6)*rRel(2) + rd_in(5)*rRel(3) + rd_in(1) ! x - rd_out(2) = rd_in(6)*rRel(1) - rd_in(4)*rRel(3) + rd_in(2) ! y - rd_out(3) = -rd_in(5)*rRel(1) + rd_in(4)*rRel(2) + rd_in(3) ! z - - - - !rRel = MATMUL(TransMat, rRelBody) - !H = getH(rRel) - !! absolute locations - !r_out = rRel + r_in - !! absolute velocities - !rd_out = MATMUL( H, rd_in(4:6)) + rd_in(1:3) - - - END SUBROUTINE TransformKinematics - !----------------------------------------------------------------------- - - - - ! calculate position, velocity, and acceleration of point based on its position relative to moving 6DOF body - !----------------------------------------------------------------------- - SUBROUTINE TransformKinematicsA(rRelBody, r_in, TransMat, v_in, a_in, r_out, v_out, a_out) - REAL(DbKi), INTENT(IN ) :: rRelBody(:) ! relative location of point about reference point, in local/reference coordinate system - REAL(DbKi), INTENT(IN ) :: r_in(3) ! translation applied to reference point - REAL(DbKi), INTENT(IN ) :: TransMat(3,3)! rotation matrix describing rotation about reference point - REAL(DbKi), INTENT(IN ) :: v_in(6) ! 6DOF velecity vector about ref point in global orientation frame - REAL(DbKi), INTENT(IN ) :: a_in(6) ! 6DOF acceleration vector - REAL(DbKi), INTENT( OUT) :: r_out(3) ! coordinates of point of interest - REAL(DbKi), INTENT( OUT) :: v_out(3) ! velocity of point - REAL(DbKi), INTENT( OUT) :: a_out(3) ! acceleration of point - - REAL(DbKi) :: rRel(3) - REAL(DbKi) :: rRel2(3) - - REAL(DbKi) :: r_out2(3) - REAL(DbKi) :: rd_out2(3) - REAL(DbKi) :: H(3,3) - - ! rd_in should be in global orientation frame - ! note: it's okay if r_out and rd_out are 6-size. Only the first 3 will be written, and 4-6 will - ! already be correct or can be assigned seperately from r_in and rd_in (assuming orientation frames are identical) - - - ! locations about ref point in *unrotated* reference frame - !rRel2(1) = TransMat(1,1)*rRelBody(1) + TransMat(2,1)*rRelBody(2) + TransMat(3,1)*rRelBody(3) ! x - !rRel2(2) = TransMat(1,2)*rRelBody(1) + TransMat(2,2)*rRelBody(2) + TransMat(3,2)*rRelBody(3) ! y - !rRel2(3) = TransMat(1,3)*rRelBody(1) + TransMat(2,3)*rRelBody(2) + TransMat(3,3)*rRelBody(3) ! z - - rRel = MATMUL(TransMat, rRelBody) - - H = getH(rRel) - - ! absolute locations - r_out = rRel + r_in - - ! absolute velocities - !rd_out2(1) = - v_in(6)*rRel(2) + v_in(5)*rRel(3) + v_in(1) ! x - !rd_out2(2) = v_in(6)*rRel(1) - v_in(4)*rRel(3) + v_in(2) ! y - !rd_out2(3) = -v_in(5)*rRel(1) + v_in(4)*rRel(2) + v_in(3) ! z - - v_out = MATMUL( H, v_in(4:6)) + v_in(1:3) - - ! absolute accelerations - a_out = MATMUL( H, a_in(4:6)) + a_in(1:3) ! << should add second order terms! - - - END SUBROUTINE TransformKinematicsA - !----------------------------------------------------------------------- - - ! calculate position and velocity of point along rod (distance L along direction u) - !----------------------------------------------------------------------- - SUBROUTINE TransformKinematicsAtoB(rA, u, L, rd_in, r_out, rd_out) - REAL(DbKi), INTENT(IN ) :: rA(3) ! coordinate of end A - REAL(DbKi), INTENT(IN ) :: u(3) ! Rod unit vector - REAL(DbKi), INTENT(IN ) :: L ! Rod length from end A to B - REAL(DbKi), INTENT(IN ) :: rd_in(6) ! 6DOF velecity vector about Rod end A, in global orientation frame - REAL(DbKi), INTENT( OUT) :: r_out(3) ! coordinates of end B - REAL(DbKi), INTENT( OUT) :: rd_out(3) ! velocity of end B - - REAL(DbKi) :: rRel(3) - - - ! locations (unrotated reference frame) - rRel = L*u ! relative location of point B from point A - r_out = rRel + rA ! absolute location of point B - - ! absolute velocities - rd_out(1) = - rd_in(6)*rRel(2) + rd_in(5)*rRel(3) + rd_in(1) ! x - rd_out(2) = rd_in(6)*rRel(1) - rd_in(4)*rRel(3) + rd_in(2) ! y - rd_out(3) = -rd_in(5)*rRel(1) + rd_in(4)*rRel(2) + rd_in(3) ! z - - - END SUBROUTINE TransformKinematicsAtoB - !----------------------------------------------------------------------- - - ! - !----------------------------------------------------------------------- - SUBROUTINE TranslateForce3to6DOF(dx, F, Fout) - REAL(DbKi), INTENT(IN ) :: dx(3) ! displacement vector from ref point to point of force (F) application - REAL(DbKi), INTENT(IN ) :: F(3) ! applied force - REAL(DbKi), INTENT( OUT) :: Fout(6) ! resultant applied force and moment about ref point - - Fout(1:3) = F - - Fout(4:6) = CROSS_PRODUCT(dx, F) - - END SUBROUTINE TranslateForce3to6DOF - !----------------------------------------------------------------------- - - - ! - !----------------------------------------------------------------------- - SUBROUTINE TranslateMass3to6DOF(dx, Min, Mout) - REAL(DbKi), INTENT(IN ) :: dx(3) ! displacement vector from ref point to point of mass matrix (Min) - REAL(DbKi), INTENT(IN ) :: Min( 3,3) ! original mass matrix (assumed at center of mass, or a point mass) - REAL(DbKi), INTENT( OUT) :: Mout(6,6) ! resultant mass and inertia matrix about ref point - - REAL(DbKi) :: H( 3,3) ! "anti-symmetric tensor components" from Sadeghi and Incecik - REAL(DbKi) :: tempM( 3,3) - REAL(DbKi) :: tempM2(3,3) - REAL(DbKi) :: Htrans(3,3) - Integer(IntKi) :: I,J - - ! sub-matrix definitions are accordint to | m J | - ! | J^T I | - - H = getH(dx); - - ! mass matrix [m'] = [m] - Mout(1:3,1:3) = Min - - ! product of inertia matrix [J'] = [m][H] + [J] - Mout(1:3,4:6) = MATMUL(Min, H) - Mout(4:6,1:3) = TRANSPOSE(Mout(1:3,4:6)) - - !moment of inertia matrix [I'] = [H][m][H]^T + [J]^T [H] + [H]^T [J] + [I] - Mout(4:6,4:6) = MATMUL(MATMUL(H, Min), TRANSPOSE(H)) - - END SUBROUTINE TranslateMass3to6DOF - !----------------------------------------------------------------------- - - ! - !----------------------------------------------------------------------- - SUBROUTINE TranslateMass6to6DOF(dx, Min, Mout) - REAL(DbKi), INTENT(IN ) :: dx(3) ! displacement vector from ref point to point of mass matrix (Min) - REAL(DbKi), INTENT(IN ) :: Min( 6,6) ! original mass matrix - REAL(DbKi), INTENT( OUT) :: Mout(6,6) ! resultant mass and inertia matrix about ref point - - REAL(DbKi) :: H( 3,3) ! "anti-symmetric tensor components" from Sadeghi and Incecik - - H = getH(dx); - - ! mass matrix [m'] = [m] - Mout(1:3,1:3) = Min(1:3,1:3) - - ! product of inertia matrix [J'] = [m][H] + [J] - Mout(1:3,4:6) = MATMUL(Min(1:3,1:3), H) + Min(1:3,4:6) - Mout(4:6,1:3) = TRANSPOSE(Mout(1:3,4:6)) - - !moment of inertia matrix [I'] = [H][m][H]^T + [J]^T [H] + [H]^T [J] + [I] - Mout(4:6,4:6) = MATMUL(MATMUL(H, Min(1:3,1:3)), TRANSPOSE(H)) + MATMUL(Min(4:6,1:3),H) + MATMUL(TRANSPOSE(H),Min(1:3,4:6)) + Min(4:6,4:6) - - END SUBROUTINE TranslateMass6to6DOF - !----------------------------------------------------------------------- - - ! produce alternator matrix - !----------------------------------------------------------------------- - FUNCTION GetH(r) - Real(DbKi), INTENT(IN) :: r(3) ! inputted vector - Real(DbKi) :: GetH(3,3) ! outputted matrix - - GetH(2,1) = -r(3) - GetH(1,2) = r(3) - GetH(3,1) = r(2) - GetH(1,3) = -r(2) - GetH(3,2) = -r(1) - GetH(2,3) = r(1) - - GetH(1,1) = 0.0_DbKi - GetH(2,2) = 0.0_DbKi - GetH(3,3) = 0.0_DbKi - - END FUNCTION GetH - !----------------------------------------------------------------------- - - - - ! apply a rotation to a 6-by-6 mass/inertia tensor (see Sadeghi and Incecik 2005 for theory) - !----------------------------------------------------------------------- - FUNCTION RotateM6(Min, rotMat) result(outMat) - - Real(DbKi), INTENT(IN) :: Min(6,6) ! inputted matrix to be rotated - Real(DbKi), INTENT(IN) :: rotMat(3,3) ! rotation matrix (DCM) - Real(DbKi) :: outMat(6,6) ! rotated matrix - - ! the process for each of the following is to - ! 1. copy out the relevant 3x3 matrix section, - ! 2. rotate it, and - ! 3. paste it into the output 6x6 matrix - - ! mass matrix - outMat(1:3,1:3) = rotateM3(Min(1:3,1:3), rotMat) - - ! product of inertia matrix - outMat(1:3,4:6) = rotateM3(Min(1:3,4:6), rotMat) - outMat(4:6,1:3) = TRANSPOSE(outMat(1:3,4:6)) - - ! moment of inertia matrix - outMat(4:6,4:6) = rotateM3(Min(4:6,4:6), rotMat) - - END FUNCTION RotateM6 - - - ! apply a rotation to a 3-by-3 mass matrix or any other second order tensor - !----------------------------------------------------------------------- - FUNCTION RotateM3(Min, rotMat) result(outMat) - - Real(DbKi), INTENT(IN) :: Min(3,3) ! inputted matrix to be rotated - Real(DbKi), INTENT(IN) :: rotMat(3,3) ! rotation matrix (DCM) - Real(DbKi) :: outMat(3,3) ! rotated matrix - - ! overall operation is [m'] = [a]*[m]*[a]^T - - outMat = MATMUL( MATMUL(rotMat, Min), TRANSPOSE(rotMat) ) - - END FUNCTION RotateM3 - - - - - - ! calculates rotation matrix R to rotate from global axes to a member's local axes - !----------------------------------------------------------------------- - FUNCTION CalcOrientation(phi, beta, gamma) result(R) - - REAL(DbKi), INTENT ( IN ) :: phi ! member incline angle - REAL(DbKi), INTENT ( IN ) :: beta ! member incline heading - REAL(DbKi), INTENT ( IN ) :: gamma ! member twist angle - REAL(DbKi) :: R(3,3) ! rotation matrix - - INTEGER(IntKi) :: errStat - CHARACTER(100) :: errMsg - - REAL(DbKi) :: s1, c1, s2, c2, s3, c3 - - - ! trig terms for Euler angles rotation based on beta, phi, and gamma - s1 = sin(beta) - c1 = cos(beta) - s2 = sin(phi) - c2 = cos(phi) - s3 = sin(gamma) - c3 = cos(gamma) - - ! calculate rotation matrix based on Z1Y2Z3 Euler rotation sequence from https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix - - R(1,1) = c1*c2*c3-s1*s3 - R(1,2) = -c3*s1-c1*c2*s3 - R(1,3) = c1*s2 - R(2,1) = c1*s3+c2*c3*s1 - R(2,2) = c1*c3-c2*s1*s3 - R(2,3) = s1*s2 - R(3,1) = -c3*s2 - R(3,2) = s2*s3 - R(3,3) = c2 - - ! could also calculate unit normals p1 and p2 for rectangular cross sections - !p1 = matmul( R, [1,0,0] ) ! unit vector that is perpendicular to the 'beta' plane if gamma is zero - !p2 = cross( q, p1 ) ! unit vector orthogonal to both p1 and q - - END FUNCTION CalcOrientation - - - !compute the inverse of a 3-by-3 matrix m - !----------------------------------------------------------------------- - SUBROUTINE Inverse3by3( Minv, M ) - Real(DbKi), INTENT(OUT) :: Minv(3,3) ! returned inverse matrix - Real(DbKi), INTENT(IN) :: M(3,3) ! inputted matrix - - Real(DbKi) :: det ! the determinant - Real(DbKi) :: invdet ! inverse of the determinant - - det = M(1, 1) * (M(2, 2) * M(3, 3) - M(3, 2) * M(2, 3)) - & - M(1, 2) * (M(2, 1) * M(3, 3) - M(2, 3) * M(3, 1)) + & - M(1, 3) * (M(2, 1) * M(3, 2) - M(2, 2) * M(3, 1)); - - invdet = 1.0 / det ! because multiplying is faster than dividing - - Minv(1, 1) = (M(2, 2) * M(3, 3) - M(3, 2) * M(2, 3)) * invdet - Minv(1, 2) = (M(1, 3) * M(3, 2) - M(1, 2) * M(3, 3)) * invdet - Minv(1, 3) = (M(1, 2) * M(2, 3) - M(1, 3) * M(2, 2)) * invdet - Minv(2, 1) = (M(2, 3) * M(3, 1) - M(2, 1) * M(3, 3)) * invdet - Minv(2, 2) = (M(1, 1) * M(3, 3) - M(1, 3) * M(3, 1)) * invdet - Minv(2, 3) = (M(2, 1) * M(1, 3) - M(1, 1) * M(2, 3)) * invdet - Minv(3, 1) = (M(2, 1) * M(3, 2) - M(3, 1) * M(2, 2)) * invdet - Minv(3, 2) = (M(3, 1) * M(1, 2) - M(1, 1) * M(3, 2)) * invdet - Minv(3, 3) = (M(1, 1) * M(2, 2) - M(2, 1) * M(1, 2)) * invdet - - END SUBROUTINE Inverse3by3 - !----------------------------------------------------------------------- - - - ! One-function implementation of Crout LU Decomposition. Solves Ax=b for x - SUBROUTINE LUsolve(n, A, LU, b, y, x) - - INTEGER(intKi), INTENT(IN ) :: n ! size of matrices and vectors - Real(DbKi), INTENT(IN ) :: A( n,n) ! LHS matrix (e.g. mass matrix) - Real(DbKi), INTENT(INOUT) :: LU(n,n) ! stores LU matrix data - Real(DbKi), INTENT(IN ) :: b(n) ! RHS vector - Real(DbKi), INTENT(INOUT) :: y(n) ! temporary vector - Real(DbKi), INTENT( OUT) :: x(n) ! LHS vector to solve for - - INTEGER(intKi) :: i,j,k,p - Real(DbKi) :: sum - - DO k = 1,n - DO i = k,n - - sum = 0.0_DbKi - - DO p=1,k-1 !for(int p=0; p=0; --i) - - sum = 0.0_DbKi - - DO k=i+1, n - sum = sum + LU(i,k)*x(k) - END DO - - x(i) = (y(i)-sum) - - END DO !j (actually decrementing i) - - END SUBROUTINE LUsolve - - !==================================================================================================== !++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ !> This routine initializes the array that maps rows/columns of the Jacobian to specific mesh fields. diff --git a/modules/moordyn/src/MoorDyn_Driver.f90 b/modules/moordyn/src/MoorDyn_Driver.f90 index a365415f8e..4b5cfb3f8b 100644 --- a/modules/moordyn/src/MoorDyn_Driver.f90 +++ b/modules/moordyn/src/MoorDyn_Driver.f90 @@ -146,6 +146,7 @@ PROGRAM MoorDyn_Driver ! set the input file name and other environment terms !MD_InitInp%NStepWave = 1 ! an arbitrary number > 0 (to set the size of the wave data, which currently contains all zero values) + MD_InitInp%Tmax = drvrInitInp%TMax MD_InitInp%g = drvrInitInp%Gravity MD_InitInp%rhoW = drvrInitInp%rhoW MD_InitInp%WtrDepth = drvrInitInp%WtrDepth @@ -330,8 +331,14 @@ PROGRAM MoorDyn_Driver ! get interpolated position of coupling point r_in(i, J) = PtfmMotIn(iIn, J+1) + frac*(PtfmMotIn(iIn+1, J+1) - PtfmMotIn(iIn, J+1)) - ! use forward different to estimate velocity of coupling point - rd_in(i, J) = (PtfmMotIn(iIn+1, J+1) - PtfmMotIn(iIn, J+1)) / (PtfmMotIn(iIn+1, 1) - PtfmMotIn(iIn, 1)) + if (iIn==1) then + ! use forward different to estimate velocity of coupling point + rd_in(i, J) = (PtfmMotIn(iIn+1, J+1) - PtfmMotIn(iIn, J+1)) / (PtfmMotIn(iIn+1, 1) - PtfmMotIn(iIn, 1)) + else + ! use central different to estimate velocity of coupling point + rd_in(i, J) = (PtfmMotIn(iIn+1, J+1) - PtfmMotIn(iIn-1, J+1)) / (PtfmMotIn(iIn+1, 1) - PtfmMotIn(iIn-1, 1)) + + end if END DO EXIT ! break out of the loop for this time step once we've done its interpolation diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index 4215872fc7..9d5d0f22d7 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -99,7 +99,7 @@ MODULE MoorDyn_IO ! PUBLIC :: MDIO_ReadInput - PUBLIC :: getBathymetry + PUBLIC :: setupBathymetry PUBLIC :: getCoefficientOrCurve PUBLIC :: SplitByBars PUBLIC :: DecomposeString @@ -112,7 +112,7 @@ MODULE MoorDyn_IO CONTAINS - SUBROUTINE getBathymetry(inputString, defaultDepth, BathGrid, BathGrid_Xs, BathGrid_Ys, ErrStat3, ErrMsg3) + SUBROUTINE setupBathymetry(inputString, defaultDepth, BathGrid, BathGrid_Xs, BathGrid_Ys, ErrStat3, ErrMsg3) ! SUBROUTINE getBathymetry(inputString, BathGrid, BathGrid_Xs, BathGrid_Ys, BathGrid_npoints, ErrStat3, ErrMsg3) CHARACTER(40), INTENT(IN ) :: inputString ! string describing water depth or bathymetry filename @@ -200,7 +200,7 @@ SUBROUTINE getBathymetry(inputString, defaultDepth, BathGrid, BathGrid_Xs, BathG END IF - END SUBROUTINE getBathymetry + END SUBROUTINE setupBathymetry ! read in stiffness/damping coefficient or load nonlinear data file if applicable diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 3793803dde..172e306422 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -26,6 +26,7 @@ typedef ^ ^ ReKi WtrDepth - -99 typedef ^ ^ ReKi PtfmInit {:}{:} - - "initial position of platform(s) shape: 6, nTurbines" - typedef ^ ^ IntKi FarmSize - 0 - "Indicates normal FAST module mode if 0, FAST.Farm coupled mode and =nTurbines if >0" - typedef ^ ^ ReKi TurbineRefPos {:}{:} - - "reference position of turbines in farm, shape: 3, nTurbines" - +typedef ^ ^ ReKi Tmax - - - "simulation duration" "[s]" typedef ^ ^ CHARACTER(1024) FileName - "" - "MoorDyn input file" typedef ^ ^ CHARACTER(1024) RootName - - - "RootName for writing output files" - typedef ^ ^ LOGICAL UsePrimaryInputFile - .TRUE. - "Read input file instead of passed data" - @@ -300,15 +301,12 @@ typedef ^ ^ IntKi DerivOrder_x {:} ## ============================== Define Continuous states here: ===================================================================================================================================== typedef ^ ContinuousStateType DbKi states {:} "" - "state vector of mooring system, e.g. node coordinates and velocities" "" - ## ============================== Define Discrete states here: ===================================================================================================================================== typedef ^ DiscreteStateType SiKi dummy - - - "Remove this variable if you have discrete states" - - ## ============================== Define constraint states here: ===================================================================================================================================== typedef ^ ConstraintStateType SiKi dummy - - - "Remove this variable if you have constraint states" - - ## ============================== Define Other states here: ===================================================================================================================================== typedef ^ OtherStateType SiKi dummy - - - "Remove this variable if you have other states" - @@ -368,9 +366,10 @@ typedef ^ ^ IntKi nCpldRods {:} typedef ^ ^ IntKi nCpldCons {:} - - "number of coupled points (for FAST.Farm, size>1 with an entry for each turbine)" "" typedef ^ ^ IntKi NConns - 0 - "number of Connect type Connections - not to be confused with NConnects" "" typedef ^ ^ IntKi NAnchs - 0 - "number of Anchor type Connections" "" +typedef ^ ^ DbKi Tmax - - - "simulation duration" "[s]" typedef ^ ^ DbKi g - 9.81 - "gravitational constant (positive)" "[m/s^2]" typedef ^ ^ DbKi rhoW - 1025 - "density of seawater" "[kg/m^3]" -#typedef ^ ^ DbKi WtrDpth - - - "water depth" "[m]" +typedef ^ ^ DbKi WtrDpth - - - "water depth" "[m]" typedef ^ ^ DbKi kBot - - - "bottom stiffness" "[Pa/m]" typedef ^ ^ DbKi cBot - - - "bottom damping" "[Pa-s/m]" typedef ^ ^ DbKi dtM0 - - - "desired mooring model time step" "[s]" @@ -381,22 +380,31 @@ typedef ^ ^ CHARACTER(1024) RootName - typedef ^ ^ MD_OutParmType OutParam {:} - - "Names and units (and other characteristics) of all requested output parameters" - typedef ^ ^ CHARACTER(1) Delim - - - "Column delimiter for output text files" - typedef ^ ^ IntKi MDUnOut - - - "Unit number of main output file" -typedef ^ ^ IntKi WaterKin - - - "Flag for whether or how to consider water kinematics" +typedef ^ ^ IntKi WaveKin - - - "Flag for whether or how to consider water kinematics" +typedef ^ ^ IntKi Current - - - "Flag for whether or how to consider water kinematics" typedef ^ ^ IntKi nTurbines - - - "Number of turbines if MoorDyn is performing an array-level simulation with FAST.Farm, otherwise 0" typedef ^ ^ ReKi TurbineRefPos {:}{:} - - "reference position of turbines in farm, shape: 3, nTurbines" - - -typedef ^ ^ DbKi ux {:}{:}{:}{:} - - "water velocities time series at each grid point" - -typedef ^ ^ DbKi uy {:}{:}{:}{:} - - "water velocities time series at each grid point" - -typedef ^ ^ DbKi uz {:}{:}{:}{:} - - "water velocities time series at each grid point" - -typedef ^ ^ DbKi ax {:}{:}{:}{:} - - "water accelerations time series at each grid point" - -typedef ^ ^ DbKi ay {:}{:}{:}{:} - - "water accelerations time series at each grid point" - -typedef ^ ^ DbKi az {:}{:}{:}{:} - - "water accelerations time series at each grid point" - -typedef ^ ^ DbKi PDyn {:}{:}{:}{:} - - "water dynamic pressure time series at each grid point" - -typedef ^ ^ DbKi zeta {:}{:}{:} - - "water surface elevations time series at each surface grid point" - -typedef ^ ^ DbKi px {:} - - "" - -typedef ^ ^ DbKi py {:} - - "" - -typedef ^ ^ DbKi pz {:} - - "" - -typedef ^ ^ DbKi tWave {:} - - "wave data time step array" - +# --- parameters for wave and current --- +typedef ^ ^ IntKi nxWave - - - "number of x wave grid points" - +typedef ^ ^ IntKi nyWave - - - "number of y wave grid points" - +typedef ^ ^ IntKi nzWave - - - "number of z wave grid points" - +typedef ^ ^ IntKi ntWave - - - "number of wave time steps" - +typedef ^ ^ ReKi pxWave {:} - - "x location of wave grid points" - +typedef ^ ^ ReKi pyWave {:} - - "y location of wave grid points" - +typedef ^ ^ ReKi pzWave {:} - - "z location of wave grid points" - +typedef ^ ^ ReKi dtWave - - - "wave data time step" - +typedef ^ ^ ReKi uxWave {:}{:}{:}{:} - - "wave velocities time series at each grid point" - +typedef ^ ^ ReKi uyWave {:}{:}{:}{:} - - "wave velocities time series at each grid point" - +typedef ^ ^ ReKi uzWave {:}{:}{:}{:} - - "wave velocities time series at each grid point" - +typedef ^ ^ ReKi axWave {:}{:}{:}{:} - - "wave accelerations time series at each grid point" - +typedef ^ ^ ReKi ayWave {:}{:}{:}{:} - - "wave accelerations time series at each grid point" - +typedef ^ ^ ReKi azWave {:}{:}{:}{:} - - "wave accelerations time series at each grid point" - +typedef ^ ^ ReKi PDyn {:}{:}{:}{:} - - "wave dynamic pressure time series at each grid point" - +typedef ^ ^ ReKi zeta {:}{:}{:} - - "wave surface elevations time series at each surface grid point" - +typedef ^ ^ IntKi nzCurrent - - - "number of z current grid points" - +typedef ^ ^ ReKi pzCurrent {:} - - "z location of current grid points" - +typedef ^ ^ ReKi uxCurrent {:} - - "current velocities time series at each grid point" - +typedef ^ ^ ReKi uyCurrent {:} - - "current velocities time series at each grid point" - # --- Parameters for linearization --- typedef ^ ^ Integer Nx0 - - - "copy of initial size of system state vector, for linearization routines" - typedef ^ ^ Integer Jac_u_indx {:}{:} - - "matrix to help fill/pack the u vector in computing the jacobian" - diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index e7c6f4dc14..0191b8d0b8 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -49,6 +49,7 @@ MODULE MoorDyn_Types REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: PtfmInit !< initial position of platform(s) shape: 6, nTurbines [-] INTEGER(IntKi) :: FarmSize = 0 !< Indicates normal FAST module mode if 0, FAST.Farm coupled mode and =nTurbines if >0 [-] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: TurbineRefPos !< reference position of turbines in farm, shape: 3, nTurbines [-] + REAL(ReKi) :: Tmax !< simulation duration [[s]] CHARACTER(1024) :: FileName !< MoorDyn input file [-] CHARACTER(1024) :: RootName !< RootName for writing output files [-] LOGICAL :: UsePrimaryInputFile = .TRUE. !< Read input file instead of passed data [-] @@ -266,7 +267,7 @@ MODULE MoorDyn_Types REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: ld !< segment unstretched length rate of change (used in active tensioning) [[m]] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: lstr !< segment stretched length [[m]] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: lstrd !< segment change in stretched length [[m/s]] - REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: dl_S !< segment stretch attributed to static stiffness portion [[m]] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: dl_1 !< segment stretch attributed to static stiffness portion [[m]] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: V !< segment volume [[m^3]] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: U !< water velocity at node [[m/s]] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Ud !< water acceleration at node [[m/s^2]] @@ -397,8 +398,10 @@ MODULE MoorDyn_Types INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: nCpldCons !< number of coupled points (for FAST.Farm, size>1 with an entry for each turbine) [] INTEGER(IntKi) :: NConns = 0 !< number of Connect type Connections - not to be confused with NConnects [] INTEGER(IntKi) :: NAnchs = 0 !< number of Anchor type Connections [] + REAL(DbKi) :: Tmax !< simulation duration [[s]] REAL(DbKi) :: g = 9.81 !< gravitational constant (positive) [[m/s^2]] REAL(DbKi) :: rhoW = 1025 !< density of seawater [[kg/m^3]] + REAL(DbKi) :: WtrDpth !< water depth [[m]] REAL(DbKi) :: kBot !< bottom stiffness [[Pa/m]] REAL(DbKi) :: cBot !< bottom damping [[Pa-s/m]] REAL(DbKi) :: dtM0 !< desired mooring model time step [[s]] @@ -409,21 +412,30 @@ MODULE MoorDyn_Types TYPE(MD_OutParmType) , DIMENSION(:), ALLOCATABLE :: OutParam !< Names and units (and other characteristics) of all requested output parameters [-] CHARACTER(1) :: Delim !< Column delimiter for output text files [-] INTEGER(IntKi) :: MDUnOut !< Unit number of main output file [-] - INTEGER(IntKi) :: WaterKin !< Flag for whether or how to consider water kinematics [-] + INTEGER(IntKi) :: WaveKin !< Flag for whether or how to consider water kinematics [-] + INTEGER(IntKi) :: Current !< Flag for whether or how to consider water kinematics [-] INTEGER(IntKi) :: nTurbines !< Number of turbines if MoorDyn is performing an array-level simulation with FAST.Farm, otherwise 0 [-] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: TurbineRefPos !< reference position of turbines in farm, shape: 3, nTurbines [-] - REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: ux !< water velocities time series at each grid point [-] - REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: uy !< water velocities time series at each grid point [-] - REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: uz !< water velocities time series at each grid point [-] - REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: ax !< water accelerations time series at each grid point [-] - REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: ay !< water accelerations time series at each grid point [-] - REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: az !< water accelerations time series at each grid point [-] - REAL(DbKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: PDyn !< water dynamic pressure time series at each grid point [-] - REAL(DbKi) , DIMENSION(:,:,:), ALLOCATABLE :: zeta !< water surface elevations time series at each surface grid point [-] - REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: px !< [-] - REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: py !< [-] - REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: pz !< [-] - REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: tWave !< wave data time step array [-] + INTEGER(IntKi) :: nxWave !< number of x wave grid points [-] + INTEGER(IntKi) :: nyWave !< number of y wave grid points [-] + INTEGER(IntKi) :: nzWave !< number of z wave grid points [-] + INTEGER(IntKi) :: ntWave !< number of wave time steps [-] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: pxWave !< x location of wave grid points [-] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: pyWave !< y location of wave grid points [-] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: pzWave !< z location of wave grid points [-] + REAL(ReKi) :: dtWave !< wave data time step [-] + REAL(ReKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: uxWave !< wave velocities time series at each grid point [-] + REAL(ReKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: uyWave !< wave velocities time series at each grid point [-] + REAL(ReKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: uzWave !< wave velocities time series at each grid point [-] + REAL(ReKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: axWave !< wave accelerations time series at each grid point [-] + REAL(ReKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: ayWave !< wave accelerations time series at each grid point [-] + REAL(ReKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: azWave !< wave accelerations time series at each grid point [-] + REAL(ReKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: PDyn !< wave dynamic pressure time series at each grid point [-] + REAL(ReKi) , DIMENSION(:,:,:), ALLOCATABLE :: zeta !< wave surface elevations time series at each surface grid point [-] + INTEGER(IntKi) :: nzCurrent !< number of z current grid points [-] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: pzCurrent !< z location of current grid points [-] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: uxCurrent !< current velocities time series at each grid point [-] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: uyCurrent !< current velocities time series at each grid point [-] INTEGER(IntKi) :: Nx0 !< copy of initial size of system state vector, for linearization routines [-] INTEGER(IntKi) , DIMENSION(:,:), ALLOCATABLE :: Jac_u_indx !< matrix to help fill/pack the u vector in computing the jacobian [-] REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: du !< vector that determines size of perturbation for u (inputs) [-] @@ -646,6 +658,7 @@ SUBROUTINE MD_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, ErrSt END IF DstInitInputData%TurbineRefPos = SrcInitInputData%TurbineRefPos ENDIF + DstInitInputData%Tmax = SrcInitInputData%Tmax DstInitInputData%FileName = SrcInitInputData%FileName DstInitInputData%RootName = SrcInitInputData%RootName DstInitInputData%UsePrimaryInputFile = SrcInitInputData%UsePrimaryInputFile @@ -825,6 +838,7 @@ SUBROUTINE MD_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg Int_BufSz = Int_BufSz + 2*2 ! TurbineRefPos upper/lower bounds for each dimension Re_BufSz = Re_BufSz + SIZE(InData%TurbineRefPos) ! TurbineRefPos END IF + Re_BufSz = Re_BufSz + 1 ! Tmax Int_BufSz = Int_BufSz + 1*LEN(InData%FileName) ! FileName Int_BufSz = Int_BufSz + 1*LEN(InData%RootName) ! RootName Int_BufSz = Int_BufSz + 1 ! UsePrimaryInputFile @@ -953,6 +967,8 @@ SUBROUTINE MD_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg END DO END DO END IF + ReKiBuf(Re_Xferred) = InData%Tmax + Re_Xferred = Re_Xferred + 1 DO I = 1, LEN(InData%FileName) IntKiBuf(Int_Xferred) = ICHAR(InData%FileName(I:I), IntKi) Int_Xferred = Int_Xferred + 1 @@ -1202,6 +1218,8 @@ SUBROUTINE MD_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Err END DO END DO END IF + OutData%Tmax = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 DO I = 1, LEN(OutData%FileName) OutData%FileName(I:I) = CHAR(IntKiBuf(Int_Xferred)) Int_Xferred = Int_Xferred + 1 @@ -4474,17 +4492,17 @@ SUBROUTINE MD_CopyLine( SrcLineData, DstLineData, CtrlCode, ErrStat, ErrMsg ) END IF DstLineData%lstrd = SrcLineData%lstrd ENDIF -IF (ALLOCATED(SrcLineData%dl_S)) THEN - i1_l = LBOUND(SrcLineData%dl_S,1) - i1_u = UBOUND(SrcLineData%dl_S,1) - IF (.NOT. ALLOCATED(DstLineData%dl_S)) THEN - ALLOCATE(DstLineData%dl_S(i1_l:i1_u),STAT=ErrStat2) +IF (ALLOCATED(SrcLineData%dl_1)) THEN + i1_l = LBOUND(SrcLineData%dl_1,1) + i1_u = UBOUND(SrcLineData%dl_1,1) + IF (.NOT. ALLOCATED(DstLineData%dl_1)) THEN + ALLOCATE(DstLineData%dl_1(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%dl_S.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%dl_1.', ErrStat, ErrMsg,RoutineName) RETURN END IF END IF - DstLineData%dl_S = SrcLineData%dl_S + DstLineData%dl_1 = SrcLineData%dl_1 ENDIF IF (ALLOCATED(SrcLineData%V)) THEN i1_l = LBOUND(SrcLineData%V,1) @@ -4758,8 +4776,8 @@ SUBROUTINE MD_DestroyLine( LineData, ErrStat, ErrMsg ) IF (ALLOCATED(LineData%lstrd)) THEN DEALLOCATE(LineData%lstrd) ENDIF -IF (ALLOCATED(LineData%dl_S)) THEN - DEALLOCATE(LineData%dl_S) +IF (ALLOCATED(LineData%dl_1)) THEN + DEALLOCATE(LineData%dl_1) ENDIF IF (ALLOCATED(LineData%V)) THEN DEALLOCATE(LineData%V) @@ -4921,10 +4939,10 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_BufSz = Int_BufSz + 2*1 ! lstrd upper/lower bounds for each dimension Db_BufSz = Db_BufSz + SIZE(InData%lstrd) ! lstrd END IF - Int_BufSz = Int_BufSz + 1 ! dl_S allocated yes/no - IF ( ALLOCATED(InData%dl_S) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! dl_S upper/lower bounds for each dimension - Db_BufSz = Db_BufSz + SIZE(InData%dl_S) ! dl_S + Int_BufSz = Int_BufSz + 1 ! dl_1 allocated yes/no + IF ( ALLOCATED(InData%dl_1) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! dl_1 upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%dl_1) ! dl_1 END IF Int_BufSz = Int_BufSz + 1 ! V allocated yes/no IF ( ALLOCATED(InData%V) ) THEN @@ -5259,18 +5277,18 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Db_Xferred = Db_Xferred + 1 END DO END IF - IF ( .NOT. ALLOCATED(InData%dl_S) ) THEN + IF ( .NOT. ALLOCATED(InData%dl_1) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%dl_S,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%dl_S,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%dl_1,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%dl_1,1) Int_Xferred = Int_Xferred + 2 - DO i1 = LBOUND(InData%dl_S,1), UBOUND(InData%dl_S,1) - DbKiBuf(Db_Xferred) = InData%dl_S(i1) + DO i1 = LBOUND(InData%dl_1,1), UBOUND(InData%dl_1,1) + DbKiBuf(Db_Xferred) = InData%dl_1(i1) Db_Xferred = Db_Xferred + 1 END DO END IF @@ -5901,21 +5919,21 @@ SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) Db_Xferred = Db_Xferred + 1 END DO END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! dl_S not allocated + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! dl_1 not allocated Int_Xferred = Int_Xferred + 1 ELSE Int_Xferred = Int_Xferred + 1 i1_l = IntKiBuf( Int_Xferred ) i1_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%dl_S)) DEALLOCATE(OutData%dl_S) - ALLOCATE(OutData%dl_S(i1_l:i1_u),STAT=ErrStat2) + IF (ALLOCATED(OutData%dl_1)) DEALLOCATE(OutData%dl_1) + ALLOCATE(OutData%dl_1(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%dl_S.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%dl_1.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i1 = LBOUND(OutData%dl_S,1), UBOUND(OutData%dl_S,1) - OutData%dl_S(i1) = DbKiBuf(Db_Xferred) + DO i1 = LBOUND(OutData%dl_1,1), UBOUND(OutData%dl_1,1) + OutData%dl_1(i1) = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 END DO END IF @@ -10517,8 +10535,10 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) ENDIF DstParamData%NConns = SrcParamData%NConns DstParamData%NAnchs = SrcParamData%NAnchs + DstParamData%Tmax = SrcParamData%Tmax DstParamData%g = SrcParamData%g DstParamData%rhoW = SrcParamData%rhoW + DstParamData%WtrDpth = SrcParamData%WtrDpth DstParamData%kBot = SrcParamData%kBot DstParamData%cBot = SrcParamData%cBot DstParamData%dtM0 = SrcParamData%dtM0 @@ -10544,7 +10564,8 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) ENDIF DstParamData%Delim = SrcParamData%Delim DstParamData%MDUnOut = SrcParamData%MDUnOut - DstParamData%WaterKin = SrcParamData%WaterKin + DstParamData%WaveKin = SrcParamData%WaveKin + DstParamData%Current = SrcParamData%Current DstParamData%nTurbines = SrcParamData%nTurbines IF (ALLOCATED(SrcParamData%TurbineRefPos)) THEN i1_l = LBOUND(SrcParamData%TurbineRefPos,1) @@ -10560,113 +10581,154 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) END IF DstParamData%TurbineRefPos = SrcParamData%TurbineRefPos ENDIF -IF (ALLOCATED(SrcParamData%ux)) THEN - i1_l = LBOUND(SrcParamData%ux,1) - i1_u = UBOUND(SrcParamData%ux,1) - i2_l = LBOUND(SrcParamData%ux,2) - i2_u = UBOUND(SrcParamData%ux,2) - i3_l = LBOUND(SrcParamData%ux,3) - i3_u = UBOUND(SrcParamData%ux,3) - i4_l = LBOUND(SrcParamData%ux,4) - i4_u = UBOUND(SrcParamData%ux,4) - IF (.NOT. ALLOCATED(DstParamData%ux)) THEN - ALLOCATE(DstParamData%ux(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%ux.', ErrStat, ErrMsg,RoutineName) + DstParamData%nxWave = SrcParamData%nxWave + DstParamData%nyWave = SrcParamData%nyWave + DstParamData%nzWave = SrcParamData%nzWave + DstParamData%ntWave = SrcParamData%ntWave +IF (ALLOCATED(SrcParamData%pxWave)) THEN + i1_l = LBOUND(SrcParamData%pxWave,1) + i1_u = UBOUND(SrcParamData%pxWave,1) + IF (.NOT. ALLOCATED(DstParamData%pxWave)) THEN + ALLOCATE(DstParamData%pxWave(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%pxWave.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%pxWave = SrcParamData%pxWave +ENDIF +IF (ALLOCATED(SrcParamData%pyWave)) THEN + i1_l = LBOUND(SrcParamData%pyWave,1) + i1_u = UBOUND(SrcParamData%pyWave,1) + IF (.NOT. ALLOCATED(DstParamData%pyWave)) THEN + ALLOCATE(DstParamData%pyWave(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%pyWave.', ErrStat, ErrMsg,RoutineName) RETURN END IF END IF - DstParamData%ux = SrcParamData%ux + DstParamData%pyWave = SrcParamData%pyWave ENDIF -IF (ALLOCATED(SrcParamData%uy)) THEN - i1_l = LBOUND(SrcParamData%uy,1) - i1_u = UBOUND(SrcParamData%uy,1) - i2_l = LBOUND(SrcParamData%uy,2) - i2_u = UBOUND(SrcParamData%uy,2) - i3_l = LBOUND(SrcParamData%uy,3) - i3_u = UBOUND(SrcParamData%uy,3) - i4_l = LBOUND(SrcParamData%uy,4) - i4_u = UBOUND(SrcParamData%uy,4) - IF (.NOT. ALLOCATED(DstParamData%uy)) THEN - ALLOCATE(DstParamData%uy(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) +IF (ALLOCATED(SrcParamData%pzWave)) THEN + i1_l = LBOUND(SrcParamData%pzWave,1) + i1_u = UBOUND(SrcParamData%pzWave,1) + IF (.NOT. ALLOCATED(DstParamData%pzWave)) THEN + ALLOCATE(DstParamData%pzWave(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%uy.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%pzWave.', ErrStat, ErrMsg,RoutineName) RETURN END IF END IF - DstParamData%uy = SrcParamData%uy + DstParamData%pzWave = SrcParamData%pzWave ENDIF -IF (ALLOCATED(SrcParamData%uz)) THEN - i1_l = LBOUND(SrcParamData%uz,1) - i1_u = UBOUND(SrcParamData%uz,1) - i2_l = LBOUND(SrcParamData%uz,2) - i2_u = UBOUND(SrcParamData%uz,2) - i3_l = LBOUND(SrcParamData%uz,3) - i3_u = UBOUND(SrcParamData%uz,3) - i4_l = LBOUND(SrcParamData%uz,4) - i4_u = UBOUND(SrcParamData%uz,4) - IF (.NOT. ALLOCATED(DstParamData%uz)) THEN - ALLOCATE(DstParamData%uz(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + DstParamData%dtWave = SrcParamData%dtWave +IF (ALLOCATED(SrcParamData%uxWave)) THEN + i1_l = LBOUND(SrcParamData%uxWave,1) + i1_u = UBOUND(SrcParamData%uxWave,1) + i2_l = LBOUND(SrcParamData%uxWave,2) + i2_u = UBOUND(SrcParamData%uxWave,2) + i3_l = LBOUND(SrcParamData%uxWave,3) + i3_u = UBOUND(SrcParamData%uxWave,3) + i4_l = LBOUND(SrcParamData%uxWave,4) + i4_u = UBOUND(SrcParamData%uxWave,4) + IF (.NOT. ALLOCATED(DstParamData%uxWave)) THEN + ALLOCATE(DstParamData%uxWave(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%uz.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%uxWave.', ErrStat, ErrMsg,RoutineName) RETURN END IF END IF - DstParamData%uz = SrcParamData%uz + DstParamData%uxWave = SrcParamData%uxWave ENDIF -IF (ALLOCATED(SrcParamData%ax)) THEN - i1_l = LBOUND(SrcParamData%ax,1) - i1_u = UBOUND(SrcParamData%ax,1) - i2_l = LBOUND(SrcParamData%ax,2) - i2_u = UBOUND(SrcParamData%ax,2) - i3_l = LBOUND(SrcParamData%ax,3) - i3_u = UBOUND(SrcParamData%ax,3) - i4_l = LBOUND(SrcParamData%ax,4) - i4_u = UBOUND(SrcParamData%ax,4) - IF (.NOT. ALLOCATED(DstParamData%ax)) THEN - ALLOCATE(DstParamData%ax(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) +IF (ALLOCATED(SrcParamData%uyWave)) THEN + i1_l = LBOUND(SrcParamData%uyWave,1) + i1_u = UBOUND(SrcParamData%uyWave,1) + i2_l = LBOUND(SrcParamData%uyWave,2) + i2_u = UBOUND(SrcParamData%uyWave,2) + i3_l = LBOUND(SrcParamData%uyWave,3) + i3_u = UBOUND(SrcParamData%uyWave,3) + i4_l = LBOUND(SrcParamData%uyWave,4) + i4_u = UBOUND(SrcParamData%uyWave,4) + IF (.NOT. ALLOCATED(DstParamData%uyWave)) THEN + ALLOCATE(DstParamData%uyWave(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%ax.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%uyWave.', ErrStat, ErrMsg,RoutineName) RETURN END IF END IF - DstParamData%ax = SrcParamData%ax + DstParamData%uyWave = SrcParamData%uyWave ENDIF -IF (ALLOCATED(SrcParamData%ay)) THEN - i1_l = LBOUND(SrcParamData%ay,1) - i1_u = UBOUND(SrcParamData%ay,1) - i2_l = LBOUND(SrcParamData%ay,2) - i2_u = UBOUND(SrcParamData%ay,2) - i3_l = LBOUND(SrcParamData%ay,3) - i3_u = UBOUND(SrcParamData%ay,3) - i4_l = LBOUND(SrcParamData%ay,4) - i4_u = UBOUND(SrcParamData%ay,4) - IF (.NOT. ALLOCATED(DstParamData%ay)) THEN - ALLOCATE(DstParamData%ay(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) +IF (ALLOCATED(SrcParamData%uzWave)) THEN + i1_l = LBOUND(SrcParamData%uzWave,1) + i1_u = UBOUND(SrcParamData%uzWave,1) + i2_l = LBOUND(SrcParamData%uzWave,2) + i2_u = UBOUND(SrcParamData%uzWave,2) + i3_l = LBOUND(SrcParamData%uzWave,3) + i3_u = UBOUND(SrcParamData%uzWave,3) + i4_l = LBOUND(SrcParamData%uzWave,4) + i4_u = UBOUND(SrcParamData%uzWave,4) + IF (.NOT. ALLOCATED(DstParamData%uzWave)) THEN + ALLOCATE(DstParamData%uzWave(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%ay.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%uzWave.', ErrStat, ErrMsg,RoutineName) RETURN END IF END IF - DstParamData%ay = SrcParamData%ay + DstParamData%uzWave = SrcParamData%uzWave ENDIF -IF (ALLOCATED(SrcParamData%az)) THEN - i1_l = LBOUND(SrcParamData%az,1) - i1_u = UBOUND(SrcParamData%az,1) - i2_l = LBOUND(SrcParamData%az,2) - i2_u = UBOUND(SrcParamData%az,2) - i3_l = LBOUND(SrcParamData%az,3) - i3_u = UBOUND(SrcParamData%az,3) - i4_l = LBOUND(SrcParamData%az,4) - i4_u = UBOUND(SrcParamData%az,4) - IF (.NOT. ALLOCATED(DstParamData%az)) THEN - ALLOCATE(DstParamData%az(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) +IF (ALLOCATED(SrcParamData%axWave)) THEN + i1_l = LBOUND(SrcParamData%axWave,1) + i1_u = UBOUND(SrcParamData%axWave,1) + i2_l = LBOUND(SrcParamData%axWave,2) + i2_u = UBOUND(SrcParamData%axWave,2) + i3_l = LBOUND(SrcParamData%axWave,3) + i3_u = UBOUND(SrcParamData%axWave,3) + i4_l = LBOUND(SrcParamData%axWave,4) + i4_u = UBOUND(SrcParamData%axWave,4) + IF (.NOT. ALLOCATED(DstParamData%axWave)) THEN + ALLOCATE(DstParamData%axWave(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%az.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%axWave.', ErrStat, ErrMsg,RoutineName) RETURN END IF END IF - DstParamData%az = SrcParamData%az + DstParamData%axWave = SrcParamData%axWave +ENDIF +IF (ALLOCATED(SrcParamData%ayWave)) THEN + i1_l = LBOUND(SrcParamData%ayWave,1) + i1_u = UBOUND(SrcParamData%ayWave,1) + i2_l = LBOUND(SrcParamData%ayWave,2) + i2_u = UBOUND(SrcParamData%ayWave,2) + i3_l = LBOUND(SrcParamData%ayWave,3) + i3_u = UBOUND(SrcParamData%ayWave,3) + i4_l = LBOUND(SrcParamData%ayWave,4) + i4_u = UBOUND(SrcParamData%ayWave,4) + IF (.NOT. ALLOCATED(DstParamData%ayWave)) THEN + ALLOCATE(DstParamData%ayWave(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%ayWave.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%ayWave = SrcParamData%ayWave +ENDIF +IF (ALLOCATED(SrcParamData%azWave)) THEN + i1_l = LBOUND(SrcParamData%azWave,1) + i1_u = UBOUND(SrcParamData%azWave,1) + i2_l = LBOUND(SrcParamData%azWave,2) + i2_u = UBOUND(SrcParamData%azWave,2) + i3_l = LBOUND(SrcParamData%azWave,3) + i3_u = UBOUND(SrcParamData%azWave,3) + i4_l = LBOUND(SrcParamData%azWave,4) + i4_u = UBOUND(SrcParamData%azWave,4) + IF (.NOT. ALLOCATED(DstParamData%azWave)) THEN + ALLOCATE(DstParamData%azWave(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%azWave.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%azWave = SrcParamData%azWave ENDIF IF (ALLOCATED(SrcParamData%PDyn)) THEN i1_l = LBOUND(SrcParamData%PDyn,1) @@ -10702,53 +10764,42 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) END IF DstParamData%zeta = SrcParamData%zeta ENDIF -IF (ALLOCATED(SrcParamData%px)) THEN - i1_l = LBOUND(SrcParamData%px,1) - i1_u = UBOUND(SrcParamData%px,1) - IF (.NOT. ALLOCATED(DstParamData%px)) THEN - ALLOCATE(DstParamData%px(i1_l:i1_u),STAT=ErrStat2) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%px.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF - END IF - DstParamData%px = SrcParamData%px -ENDIF -IF (ALLOCATED(SrcParamData%py)) THEN - i1_l = LBOUND(SrcParamData%py,1) - i1_u = UBOUND(SrcParamData%py,1) - IF (.NOT. ALLOCATED(DstParamData%py)) THEN - ALLOCATE(DstParamData%py(i1_l:i1_u),STAT=ErrStat2) + DstParamData%nzCurrent = SrcParamData%nzCurrent +IF (ALLOCATED(SrcParamData%pzCurrent)) THEN + i1_l = LBOUND(SrcParamData%pzCurrent,1) + i1_u = UBOUND(SrcParamData%pzCurrent,1) + IF (.NOT. ALLOCATED(DstParamData%pzCurrent)) THEN + ALLOCATE(DstParamData%pzCurrent(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%py.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%pzCurrent.', ErrStat, ErrMsg,RoutineName) RETURN END IF END IF - DstParamData%py = SrcParamData%py + DstParamData%pzCurrent = SrcParamData%pzCurrent ENDIF -IF (ALLOCATED(SrcParamData%pz)) THEN - i1_l = LBOUND(SrcParamData%pz,1) - i1_u = UBOUND(SrcParamData%pz,1) - IF (.NOT. ALLOCATED(DstParamData%pz)) THEN - ALLOCATE(DstParamData%pz(i1_l:i1_u),STAT=ErrStat2) +IF (ALLOCATED(SrcParamData%uxCurrent)) THEN + i1_l = LBOUND(SrcParamData%uxCurrent,1) + i1_u = UBOUND(SrcParamData%uxCurrent,1) + IF (.NOT. ALLOCATED(DstParamData%uxCurrent)) THEN + ALLOCATE(DstParamData%uxCurrent(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%pz.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%uxCurrent.', ErrStat, ErrMsg,RoutineName) RETURN END IF END IF - DstParamData%pz = SrcParamData%pz + DstParamData%uxCurrent = SrcParamData%uxCurrent ENDIF -IF (ALLOCATED(SrcParamData%tWave)) THEN - i1_l = LBOUND(SrcParamData%tWave,1) - i1_u = UBOUND(SrcParamData%tWave,1) - IF (.NOT. ALLOCATED(DstParamData%tWave)) THEN - ALLOCATE(DstParamData%tWave(i1_l:i1_u),STAT=ErrStat2) +IF (ALLOCATED(SrcParamData%uyCurrent)) THEN + i1_l = LBOUND(SrcParamData%uyCurrent,1) + i1_u = UBOUND(SrcParamData%uyCurrent,1) + IF (.NOT. ALLOCATED(DstParamData%uyCurrent)) THEN + ALLOCATE(DstParamData%uyCurrent(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%tWave.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%uyCurrent.', ErrStat, ErrMsg,RoutineName) RETURN END IF END IF - DstParamData%tWave = SrcParamData%tWave + DstParamData%uyCurrent = SrcParamData%uyCurrent ENDIF DstParamData%Nx0 = SrcParamData%Nx0 IF (ALLOCATED(SrcParamData%Jac_u_indx)) THEN @@ -10820,23 +10871,32 @@ SUBROUTINE MD_DestroyParam( ParamData, ErrStat, ErrMsg ) IF (ALLOCATED(ParamData%TurbineRefPos)) THEN DEALLOCATE(ParamData%TurbineRefPos) ENDIF -IF (ALLOCATED(ParamData%ux)) THEN - DEALLOCATE(ParamData%ux) +IF (ALLOCATED(ParamData%pxWave)) THEN + DEALLOCATE(ParamData%pxWave) +ENDIF +IF (ALLOCATED(ParamData%pyWave)) THEN + DEALLOCATE(ParamData%pyWave) +ENDIF +IF (ALLOCATED(ParamData%pzWave)) THEN + DEALLOCATE(ParamData%pzWave) ENDIF -IF (ALLOCATED(ParamData%uy)) THEN - DEALLOCATE(ParamData%uy) +IF (ALLOCATED(ParamData%uxWave)) THEN + DEALLOCATE(ParamData%uxWave) ENDIF -IF (ALLOCATED(ParamData%uz)) THEN - DEALLOCATE(ParamData%uz) +IF (ALLOCATED(ParamData%uyWave)) THEN + DEALLOCATE(ParamData%uyWave) ENDIF -IF (ALLOCATED(ParamData%ax)) THEN - DEALLOCATE(ParamData%ax) +IF (ALLOCATED(ParamData%uzWave)) THEN + DEALLOCATE(ParamData%uzWave) ENDIF -IF (ALLOCATED(ParamData%ay)) THEN - DEALLOCATE(ParamData%ay) +IF (ALLOCATED(ParamData%axWave)) THEN + DEALLOCATE(ParamData%axWave) ENDIF -IF (ALLOCATED(ParamData%az)) THEN - DEALLOCATE(ParamData%az) +IF (ALLOCATED(ParamData%ayWave)) THEN + DEALLOCATE(ParamData%ayWave) +ENDIF +IF (ALLOCATED(ParamData%azWave)) THEN + DEALLOCATE(ParamData%azWave) ENDIF IF (ALLOCATED(ParamData%PDyn)) THEN DEALLOCATE(ParamData%PDyn) @@ -10844,17 +10904,14 @@ SUBROUTINE MD_DestroyParam( ParamData, ErrStat, ErrMsg ) IF (ALLOCATED(ParamData%zeta)) THEN DEALLOCATE(ParamData%zeta) ENDIF -IF (ALLOCATED(ParamData%px)) THEN - DEALLOCATE(ParamData%px) -ENDIF -IF (ALLOCATED(ParamData%py)) THEN - DEALLOCATE(ParamData%py) +IF (ALLOCATED(ParamData%pzCurrent)) THEN + DEALLOCATE(ParamData%pzCurrent) ENDIF -IF (ALLOCATED(ParamData%pz)) THEN - DEALLOCATE(ParamData%pz) +IF (ALLOCATED(ParamData%uxCurrent)) THEN + DEALLOCATE(ParamData%uxCurrent) ENDIF -IF (ALLOCATED(ParamData%tWave)) THEN - DEALLOCATE(ParamData%tWave) +IF (ALLOCATED(ParamData%uyCurrent)) THEN + DEALLOCATE(ParamData%uyCurrent) ENDIF IF (ALLOCATED(ParamData%Jac_u_indx)) THEN DEALLOCATE(ParamData%Jac_u_indx) @@ -10931,8 +10988,10 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si END IF Int_BufSz = Int_BufSz + 1 ! NConns Int_BufSz = Int_BufSz + 1 ! NAnchs + Db_BufSz = Db_BufSz + 1 ! Tmax Db_BufSz = Db_BufSz + 1 ! g Db_BufSz = Db_BufSz + 1 ! rhoW + Db_BufSz = Db_BufSz + 1 ! WtrDpth Db_BufSz = Db_BufSz + 1 ! kBot Db_BufSz = Db_BufSz + 1 ! cBot Db_BufSz = Db_BufSz + 1 ! dtM0 @@ -10966,72 +11025,89 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si END IF Int_BufSz = Int_BufSz + 1*LEN(InData%Delim) ! Delim Int_BufSz = Int_BufSz + 1 ! MDUnOut - Int_BufSz = Int_BufSz + 1 ! WaterKin + Int_BufSz = Int_BufSz + 1 ! WaveKin + Int_BufSz = Int_BufSz + 1 ! Current Int_BufSz = Int_BufSz + 1 ! nTurbines Int_BufSz = Int_BufSz + 1 ! TurbineRefPos allocated yes/no IF ( ALLOCATED(InData%TurbineRefPos) ) THEN Int_BufSz = Int_BufSz + 2*2 ! TurbineRefPos upper/lower bounds for each dimension Re_BufSz = Re_BufSz + SIZE(InData%TurbineRefPos) ! TurbineRefPos END IF - Int_BufSz = Int_BufSz + 1 ! ux allocated yes/no - IF ( ALLOCATED(InData%ux) ) THEN - Int_BufSz = Int_BufSz + 2*4 ! ux upper/lower bounds for each dimension - Db_BufSz = Db_BufSz + SIZE(InData%ux) ! ux - END IF - Int_BufSz = Int_BufSz + 1 ! uy allocated yes/no - IF ( ALLOCATED(InData%uy) ) THEN - Int_BufSz = Int_BufSz + 2*4 ! uy upper/lower bounds for each dimension - Db_BufSz = Db_BufSz + SIZE(InData%uy) ! uy - END IF - Int_BufSz = Int_BufSz + 1 ! uz allocated yes/no - IF ( ALLOCATED(InData%uz) ) THEN - Int_BufSz = Int_BufSz + 2*4 ! uz upper/lower bounds for each dimension - Db_BufSz = Db_BufSz + SIZE(InData%uz) ! uz - END IF - Int_BufSz = Int_BufSz + 1 ! ax allocated yes/no - IF ( ALLOCATED(InData%ax) ) THEN - Int_BufSz = Int_BufSz + 2*4 ! ax upper/lower bounds for each dimension - Db_BufSz = Db_BufSz + SIZE(InData%ax) ! ax - END IF - Int_BufSz = Int_BufSz + 1 ! ay allocated yes/no - IF ( ALLOCATED(InData%ay) ) THEN - Int_BufSz = Int_BufSz + 2*4 ! ay upper/lower bounds for each dimension - Db_BufSz = Db_BufSz + SIZE(InData%ay) ! ay - END IF - Int_BufSz = Int_BufSz + 1 ! az allocated yes/no - IF ( ALLOCATED(InData%az) ) THEN - Int_BufSz = Int_BufSz + 2*4 ! az upper/lower bounds for each dimension - Db_BufSz = Db_BufSz + SIZE(InData%az) ! az + Int_BufSz = Int_BufSz + 1 ! nxWave + Int_BufSz = Int_BufSz + 1 ! nyWave + Int_BufSz = Int_BufSz + 1 ! nzWave + Int_BufSz = Int_BufSz + 1 ! ntWave + Int_BufSz = Int_BufSz + 1 ! pxWave allocated yes/no + IF ( ALLOCATED(InData%pxWave) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! pxWave upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%pxWave) ! pxWave + END IF + Int_BufSz = Int_BufSz + 1 ! pyWave allocated yes/no + IF ( ALLOCATED(InData%pyWave) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! pyWave upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%pyWave) ! pyWave + END IF + Int_BufSz = Int_BufSz + 1 ! pzWave allocated yes/no + IF ( ALLOCATED(InData%pzWave) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! pzWave upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%pzWave) ! pzWave + END IF + Re_BufSz = Re_BufSz + 1 ! dtWave + Int_BufSz = Int_BufSz + 1 ! uxWave allocated yes/no + IF ( ALLOCATED(InData%uxWave) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! uxWave upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%uxWave) ! uxWave + END IF + Int_BufSz = Int_BufSz + 1 ! uyWave allocated yes/no + IF ( ALLOCATED(InData%uyWave) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! uyWave upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%uyWave) ! uyWave + END IF + Int_BufSz = Int_BufSz + 1 ! uzWave allocated yes/no + IF ( ALLOCATED(InData%uzWave) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! uzWave upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%uzWave) ! uzWave + END IF + Int_BufSz = Int_BufSz + 1 ! axWave allocated yes/no + IF ( ALLOCATED(InData%axWave) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! axWave upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%axWave) ! axWave + END IF + Int_BufSz = Int_BufSz + 1 ! ayWave allocated yes/no + IF ( ALLOCATED(InData%ayWave) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! ayWave upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%ayWave) ! ayWave + END IF + Int_BufSz = Int_BufSz + 1 ! azWave allocated yes/no + IF ( ALLOCATED(InData%azWave) ) THEN + Int_BufSz = Int_BufSz + 2*4 ! azWave upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%azWave) ! azWave END IF Int_BufSz = Int_BufSz + 1 ! PDyn allocated yes/no IF ( ALLOCATED(InData%PDyn) ) THEN Int_BufSz = Int_BufSz + 2*4 ! PDyn upper/lower bounds for each dimension - Db_BufSz = Db_BufSz + SIZE(InData%PDyn) ! PDyn + Re_BufSz = Re_BufSz + SIZE(InData%PDyn) ! PDyn END IF Int_BufSz = Int_BufSz + 1 ! zeta allocated yes/no IF ( ALLOCATED(InData%zeta) ) THEN Int_BufSz = Int_BufSz + 2*3 ! zeta upper/lower bounds for each dimension - Db_BufSz = Db_BufSz + SIZE(InData%zeta) ! zeta - END IF - Int_BufSz = Int_BufSz + 1 ! px allocated yes/no - IF ( ALLOCATED(InData%px) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! px upper/lower bounds for each dimension - Db_BufSz = Db_BufSz + SIZE(InData%px) ! px + Re_BufSz = Re_BufSz + SIZE(InData%zeta) ! zeta END IF - Int_BufSz = Int_BufSz + 1 ! py allocated yes/no - IF ( ALLOCATED(InData%py) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! py upper/lower bounds for each dimension - Db_BufSz = Db_BufSz + SIZE(InData%py) ! py + Int_BufSz = Int_BufSz + 1 ! nzCurrent + Int_BufSz = Int_BufSz + 1 ! pzCurrent allocated yes/no + IF ( ALLOCATED(InData%pzCurrent) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! pzCurrent upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%pzCurrent) ! pzCurrent END IF - Int_BufSz = Int_BufSz + 1 ! pz allocated yes/no - IF ( ALLOCATED(InData%pz) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! pz upper/lower bounds for each dimension - Db_BufSz = Db_BufSz + SIZE(InData%pz) ! pz + Int_BufSz = Int_BufSz + 1 ! uxCurrent allocated yes/no + IF ( ALLOCATED(InData%uxCurrent) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! uxCurrent upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%uxCurrent) ! uxCurrent END IF - Int_BufSz = Int_BufSz + 1 ! tWave allocated yes/no - IF ( ALLOCATED(InData%tWave) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! tWave upper/lower bounds for each dimension - Db_BufSz = Db_BufSz + SIZE(InData%tWave) ! tWave + Int_BufSz = Int_BufSz + 1 ! uyCurrent allocated yes/no + IF ( ALLOCATED(InData%uyCurrent) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! uyCurrent upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%uyCurrent) ! uyCurrent END IF Int_BufSz = Int_BufSz + 1 ! Nx0 Int_BufSz = Int_BufSz + 1 ! Jac_u_indx allocated yes/no @@ -11151,10 +11227,14 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%NAnchs Int_Xferred = Int_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%Tmax + Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%g Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%rhoW Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%WtrDpth + Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%kBot Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%cBot @@ -11218,7 +11298,9 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si END DO ! I IntKiBuf(Int_Xferred) = InData%MDUnOut Int_Xferred = Int_Xferred + 1 - IntKiBuf(Int_Xferred) = InData%WaterKin + IntKiBuf(Int_Xferred) = InData%WaveKin + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%Current Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%nTurbines Int_Xferred = Int_Xferred + 1 @@ -11242,181 +11324,236 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si END DO END DO END IF - IF ( .NOT. ALLOCATED(InData%ux) ) THEN + IntKiBuf(Int_Xferred) = InData%nxWave + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nyWave + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nzWave + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%ntWave + Int_Xferred = Int_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%pxWave) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%pxWave,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%pxWave,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%pxWave,1), UBOUND(InData%pxWave,1) + ReKiBuf(Re_Xferred) = InData%pxWave(i1) + Re_Xferred = Re_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%pyWave) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%pyWave,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%pyWave,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%pyWave,1), UBOUND(InData%pyWave,1) + ReKiBuf(Re_Xferred) = InData%pyWave(i1) + Re_Xferred = Re_Xferred + 1 + END DO + END IF + IF ( .NOT. ALLOCATED(InData%pzWave) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%ux,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ux,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%pzWave,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%pzWave,1) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%ux,2) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ux,2) + + DO i1 = LBOUND(InData%pzWave,1), UBOUND(InData%pzWave,1) + ReKiBuf(Re_Xferred) = InData%pzWave(i1) + Re_Xferred = Re_Xferred + 1 + END DO + END IF + ReKiBuf(Re_Xferred) = InData%dtWave + Re_Xferred = Re_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%uxWave) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uxWave,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uxWave,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%uxWave,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uxWave,2) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%ux,3) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ux,3) + IntKiBuf( Int_Xferred ) = LBOUND(InData%uxWave,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uxWave,3) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%ux,4) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ux,4) + IntKiBuf( Int_Xferred ) = LBOUND(InData%uxWave,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uxWave,4) Int_Xferred = Int_Xferred + 2 - DO i4 = LBOUND(InData%ux,4), UBOUND(InData%ux,4) - DO i3 = LBOUND(InData%ux,3), UBOUND(InData%ux,3) - DO i2 = LBOUND(InData%ux,2), UBOUND(InData%ux,2) - DO i1 = LBOUND(InData%ux,1), UBOUND(InData%ux,1) - DbKiBuf(Db_Xferred) = InData%ux(i1,i2,i3,i4) - Db_Xferred = Db_Xferred + 1 + DO i4 = LBOUND(InData%uxWave,4), UBOUND(InData%uxWave,4) + DO i3 = LBOUND(InData%uxWave,3), UBOUND(InData%uxWave,3) + DO i2 = LBOUND(InData%uxWave,2), UBOUND(InData%uxWave,2) + DO i1 = LBOUND(InData%uxWave,1), UBOUND(InData%uxWave,1) + ReKiBuf(Re_Xferred) = InData%uxWave(i1,i2,i3,i4) + Re_Xferred = Re_Xferred + 1 END DO END DO END DO END DO END IF - IF ( .NOT. ALLOCATED(InData%uy) ) THEN + IF ( .NOT. ALLOCATED(InData%uyWave) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%uy,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uy,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%uyWave,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uyWave,1) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%uy,2) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uy,2) + IntKiBuf( Int_Xferred ) = LBOUND(InData%uyWave,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uyWave,2) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%uy,3) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uy,3) + IntKiBuf( Int_Xferred ) = LBOUND(InData%uyWave,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uyWave,3) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%uy,4) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uy,4) + IntKiBuf( Int_Xferred ) = LBOUND(InData%uyWave,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uyWave,4) Int_Xferred = Int_Xferred + 2 - DO i4 = LBOUND(InData%uy,4), UBOUND(InData%uy,4) - DO i3 = LBOUND(InData%uy,3), UBOUND(InData%uy,3) - DO i2 = LBOUND(InData%uy,2), UBOUND(InData%uy,2) - DO i1 = LBOUND(InData%uy,1), UBOUND(InData%uy,1) - DbKiBuf(Db_Xferred) = InData%uy(i1,i2,i3,i4) - Db_Xferred = Db_Xferred + 1 + DO i4 = LBOUND(InData%uyWave,4), UBOUND(InData%uyWave,4) + DO i3 = LBOUND(InData%uyWave,3), UBOUND(InData%uyWave,3) + DO i2 = LBOUND(InData%uyWave,2), UBOUND(InData%uyWave,2) + DO i1 = LBOUND(InData%uyWave,1), UBOUND(InData%uyWave,1) + ReKiBuf(Re_Xferred) = InData%uyWave(i1,i2,i3,i4) + Re_Xferred = Re_Xferred + 1 END DO END DO END DO END DO END IF - IF ( .NOT. ALLOCATED(InData%uz) ) THEN + IF ( .NOT. ALLOCATED(InData%uzWave) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%uz,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uz,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%uzWave,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uzWave,1) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%uz,2) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uz,2) + IntKiBuf( Int_Xferred ) = LBOUND(InData%uzWave,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uzWave,2) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%uz,3) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uz,3) + IntKiBuf( Int_Xferred ) = LBOUND(InData%uzWave,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uzWave,3) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%uz,4) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uz,4) + IntKiBuf( Int_Xferred ) = LBOUND(InData%uzWave,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uzWave,4) Int_Xferred = Int_Xferred + 2 - DO i4 = LBOUND(InData%uz,4), UBOUND(InData%uz,4) - DO i3 = LBOUND(InData%uz,3), UBOUND(InData%uz,3) - DO i2 = LBOUND(InData%uz,2), UBOUND(InData%uz,2) - DO i1 = LBOUND(InData%uz,1), UBOUND(InData%uz,1) - DbKiBuf(Db_Xferred) = InData%uz(i1,i2,i3,i4) - Db_Xferred = Db_Xferred + 1 + DO i4 = LBOUND(InData%uzWave,4), UBOUND(InData%uzWave,4) + DO i3 = LBOUND(InData%uzWave,3), UBOUND(InData%uzWave,3) + DO i2 = LBOUND(InData%uzWave,2), UBOUND(InData%uzWave,2) + DO i1 = LBOUND(InData%uzWave,1), UBOUND(InData%uzWave,1) + ReKiBuf(Re_Xferred) = InData%uzWave(i1,i2,i3,i4) + Re_Xferred = Re_Xferred + 1 END DO END DO END DO END DO END IF - IF ( .NOT. ALLOCATED(InData%ax) ) THEN + IF ( .NOT. ALLOCATED(InData%axWave) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%ax,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ax,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%axWave,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%axWave,1) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%ax,2) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ax,2) + IntKiBuf( Int_Xferred ) = LBOUND(InData%axWave,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%axWave,2) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%ax,3) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ax,3) + IntKiBuf( Int_Xferred ) = LBOUND(InData%axWave,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%axWave,3) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%ax,4) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ax,4) + IntKiBuf( Int_Xferred ) = LBOUND(InData%axWave,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%axWave,4) Int_Xferred = Int_Xferred + 2 - DO i4 = LBOUND(InData%ax,4), UBOUND(InData%ax,4) - DO i3 = LBOUND(InData%ax,3), UBOUND(InData%ax,3) - DO i2 = LBOUND(InData%ax,2), UBOUND(InData%ax,2) - DO i1 = LBOUND(InData%ax,1), UBOUND(InData%ax,1) - DbKiBuf(Db_Xferred) = InData%ax(i1,i2,i3,i4) - Db_Xferred = Db_Xferred + 1 + DO i4 = LBOUND(InData%axWave,4), UBOUND(InData%axWave,4) + DO i3 = LBOUND(InData%axWave,3), UBOUND(InData%axWave,3) + DO i2 = LBOUND(InData%axWave,2), UBOUND(InData%axWave,2) + DO i1 = LBOUND(InData%axWave,1), UBOUND(InData%axWave,1) + ReKiBuf(Re_Xferred) = InData%axWave(i1,i2,i3,i4) + Re_Xferred = Re_Xferred + 1 END DO END DO END DO END DO END IF - IF ( .NOT. ALLOCATED(InData%ay) ) THEN + IF ( .NOT. ALLOCATED(InData%ayWave) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%ay,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ay,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%ayWave,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ayWave,1) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%ay,2) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ay,2) + IntKiBuf( Int_Xferred ) = LBOUND(InData%ayWave,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ayWave,2) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%ay,3) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ay,3) + IntKiBuf( Int_Xferred ) = LBOUND(InData%ayWave,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ayWave,3) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%ay,4) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ay,4) + IntKiBuf( Int_Xferred ) = LBOUND(InData%ayWave,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%ayWave,4) Int_Xferred = Int_Xferred + 2 - DO i4 = LBOUND(InData%ay,4), UBOUND(InData%ay,4) - DO i3 = LBOUND(InData%ay,3), UBOUND(InData%ay,3) - DO i2 = LBOUND(InData%ay,2), UBOUND(InData%ay,2) - DO i1 = LBOUND(InData%ay,1), UBOUND(InData%ay,1) - DbKiBuf(Db_Xferred) = InData%ay(i1,i2,i3,i4) - Db_Xferred = Db_Xferred + 1 + DO i4 = LBOUND(InData%ayWave,4), UBOUND(InData%ayWave,4) + DO i3 = LBOUND(InData%ayWave,3), UBOUND(InData%ayWave,3) + DO i2 = LBOUND(InData%ayWave,2), UBOUND(InData%ayWave,2) + DO i1 = LBOUND(InData%ayWave,1), UBOUND(InData%ayWave,1) + ReKiBuf(Re_Xferred) = InData%ayWave(i1,i2,i3,i4) + Re_Xferred = Re_Xferred + 1 END DO END DO END DO END DO END IF - IF ( .NOT. ALLOCATED(InData%az) ) THEN + IF ( .NOT. ALLOCATED(InData%azWave) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%az,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%az,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%azWave,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%azWave,1) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%az,2) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%az,2) + IntKiBuf( Int_Xferred ) = LBOUND(InData%azWave,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%azWave,2) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%az,3) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%az,3) + IntKiBuf( Int_Xferred ) = LBOUND(InData%azWave,3) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%azWave,3) Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%az,4) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%az,4) + IntKiBuf( Int_Xferred ) = LBOUND(InData%azWave,4) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%azWave,4) Int_Xferred = Int_Xferred + 2 - DO i4 = LBOUND(InData%az,4), UBOUND(InData%az,4) - DO i3 = LBOUND(InData%az,3), UBOUND(InData%az,3) - DO i2 = LBOUND(InData%az,2), UBOUND(InData%az,2) - DO i1 = LBOUND(InData%az,1), UBOUND(InData%az,1) - DbKiBuf(Db_Xferred) = InData%az(i1,i2,i3,i4) - Db_Xferred = Db_Xferred + 1 + DO i4 = LBOUND(InData%azWave,4), UBOUND(InData%azWave,4) + DO i3 = LBOUND(InData%azWave,3), UBOUND(InData%azWave,3) + DO i2 = LBOUND(InData%azWave,2), UBOUND(InData%azWave,2) + DO i1 = LBOUND(InData%azWave,1), UBOUND(InData%azWave,1) + ReKiBuf(Re_Xferred) = InData%azWave(i1,i2,i3,i4) + Re_Xferred = Re_Xferred + 1 END DO END DO END DO @@ -11445,8 +11582,8 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si DO i3 = LBOUND(InData%PDyn,3), UBOUND(InData%PDyn,3) DO i2 = LBOUND(InData%PDyn,2), UBOUND(InData%PDyn,2) DO i1 = LBOUND(InData%PDyn,1), UBOUND(InData%PDyn,1) - DbKiBuf(Db_Xferred) = InData%PDyn(i1,i2,i3,i4) - Db_Xferred = Db_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%PDyn(i1,i2,i3,i4) + Re_Xferred = Re_Xferred + 1 END DO END DO END DO @@ -11471,70 +11608,57 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si DO i3 = LBOUND(InData%zeta,3), UBOUND(InData%zeta,3) DO i2 = LBOUND(InData%zeta,2), UBOUND(InData%zeta,2) DO i1 = LBOUND(InData%zeta,1), UBOUND(InData%zeta,1) - DbKiBuf(Db_Xferred) = InData%zeta(i1,i2,i3) - Db_Xferred = Db_Xferred + 1 + ReKiBuf(Re_Xferred) = InData%zeta(i1,i2,i3) + Re_Xferred = Re_Xferred + 1 END DO END DO END DO END IF - IF ( .NOT. ALLOCATED(InData%px) ) THEN - IntKiBuf( Int_Xferred ) = 0 - Int_Xferred = Int_Xferred + 1 - ELSE - IntKiBuf( Int_Xferred ) = 1 + IntKiBuf(Int_Xferred) = InData%nzCurrent Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%px,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%px,1) - Int_Xferred = Int_Xferred + 2 - - DO i1 = LBOUND(InData%px,1), UBOUND(InData%px,1) - DbKiBuf(Db_Xferred) = InData%px(i1) - Db_Xferred = Db_Xferred + 1 - END DO - END IF - IF ( .NOT. ALLOCATED(InData%py) ) THEN + IF ( .NOT. ALLOCATED(InData%pzCurrent) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%py,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%py,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%pzCurrent,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%pzCurrent,1) Int_Xferred = Int_Xferred + 2 - DO i1 = LBOUND(InData%py,1), UBOUND(InData%py,1) - DbKiBuf(Db_Xferred) = InData%py(i1) - Db_Xferred = Db_Xferred + 1 + DO i1 = LBOUND(InData%pzCurrent,1), UBOUND(InData%pzCurrent,1) + ReKiBuf(Re_Xferred) = InData%pzCurrent(i1) + Re_Xferred = Re_Xferred + 1 END DO END IF - IF ( .NOT. ALLOCATED(InData%pz) ) THEN + IF ( .NOT. ALLOCATED(InData%uxCurrent) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%pz,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%pz,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%uxCurrent,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uxCurrent,1) Int_Xferred = Int_Xferred + 2 - DO i1 = LBOUND(InData%pz,1), UBOUND(InData%pz,1) - DbKiBuf(Db_Xferred) = InData%pz(i1) - Db_Xferred = Db_Xferred + 1 + DO i1 = LBOUND(InData%uxCurrent,1), UBOUND(InData%uxCurrent,1) + ReKiBuf(Re_Xferred) = InData%uxCurrent(i1) + Re_Xferred = Re_Xferred + 1 END DO END IF - IF ( .NOT. ALLOCATED(InData%tWave) ) THEN + IF ( .NOT. ALLOCATED(InData%uyCurrent) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 ELSE IntKiBuf( Int_Xferred ) = 1 Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%tWave,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%tWave,1) + IntKiBuf( Int_Xferred ) = LBOUND(InData%uyCurrent,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%uyCurrent,1) Int_Xferred = Int_Xferred + 2 - DO i1 = LBOUND(InData%tWave,1), UBOUND(InData%tWave,1) - DbKiBuf(Db_Xferred) = InData%tWave(i1) - Db_Xferred = Db_Xferred + 1 + DO i1 = LBOUND(InData%uyCurrent,1), UBOUND(InData%uyCurrent,1) + ReKiBuf(Re_Xferred) = InData%uyCurrent(i1) + Re_Xferred = Re_Xferred + 1 END DO END IF IntKiBuf(Int_Xferred) = InData%Nx0 @@ -11707,10 +11831,14 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg Int_Xferred = Int_Xferred + 1 OutData%NAnchs = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 + OutData%Tmax = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 OutData%g = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 OutData%rhoW = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 + OutData%WtrDpth = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 OutData%kBot = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 OutData%cBot = DbKiBuf(Db_Xferred) @@ -11789,7 +11917,9 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg END DO ! I OutData%MDUnOut = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 - OutData%WaterKin = IntKiBuf(Int_Xferred) + OutData%WaveKin = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%Current = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 OutData%nTurbines = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 @@ -11816,7 +11946,71 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg END DO END DO END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ux not allocated + OutData%nxWave = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nyWave = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nzWave = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%ntWave = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! pxWave not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%pxWave)) DEALLOCATE(OutData%pxWave) + ALLOCATE(OutData%pxWave(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%pxWave.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%pxWave,1), UBOUND(OutData%pxWave,1) + OutData%pxWave(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! pyWave not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%pyWave)) DEALLOCATE(OutData%pyWave) + ALLOCATE(OutData%pyWave(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%pyWave.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%pyWave,1), UBOUND(OutData%pyWave,1) + OutData%pyWave(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! pzWave not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%pzWave)) DEALLOCATE(OutData%pzWave) + ALLOCATE(OutData%pzWave(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%pzWave.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%pzWave,1), UBOUND(OutData%pzWave,1) + OutData%pzWave(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END IF + OutData%dtWave = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! uxWave not allocated Int_Xferred = Int_Xferred + 1 ELSE Int_Xferred = Int_Xferred + 1 @@ -11832,24 +12026,24 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg i4_l = IntKiBuf( Int_Xferred ) i4_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%ux)) DEALLOCATE(OutData%ux) - ALLOCATE(OutData%ux(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ALLOCATED(OutData%uxWave)) DEALLOCATE(OutData%uxWave) + ALLOCATE(OutData%uxWave(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ux.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%uxWave.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i4 = LBOUND(OutData%ux,4), UBOUND(OutData%ux,4) - DO i3 = LBOUND(OutData%ux,3), UBOUND(OutData%ux,3) - DO i2 = LBOUND(OutData%ux,2), UBOUND(OutData%ux,2) - DO i1 = LBOUND(OutData%ux,1), UBOUND(OutData%ux,1) - OutData%ux(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 + DO i4 = LBOUND(OutData%uxWave,4), UBOUND(OutData%uxWave,4) + DO i3 = LBOUND(OutData%uxWave,3), UBOUND(OutData%uxWave,3) + DO i2 = LBOUND(OutData%uxWave,2), UBOUND(OutData%uxWave,2) + DO i1 = LBOUND(OutData%uxWave,1), UBOUND(OutData%uxWave,1) + OutData%uxWave(i1,i2,i3,i4) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 END DO END DO END DO END DO END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! uy not allocated + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! uyWave not allocated Int_Xferred = Int_Xferred + 1 ELSE Int_Xferred = Int_Xferred + 1 @@ -11865,24 +12059,24 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg i4_l = IntKiBuf( Int_Xferred ) i4_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%uy)) DEALLOCATE(OutData%uy) - ALLOCATE(OutData%uy(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ALLOCATED(OutData%uyWave)) DEALLOCATE(OutData%uyWave) + ALLOCATE(OutData%uyWave(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%uy.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%uyWave.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i4 = LBOUND(OutData%uy,4), UBOUND(OutData%uy,4) - DO i3 = LBOUND(OutData%uy,3), UBOUND(OutData%uy,3) - DO i2 = LBOUND(OutData%uy,2), UBOUND(OutData%uy,2) - DO i1 = LBOUND(OutData%uy,1), UBOUND(OutData%uy,1) - OutData%uy(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 + DO i4 = LBOUND(OutData%uyWave,4), UBOUND(OutData%uyWave,4) + DO i3 = LBOUND(OutData%uyWave,3), UBOUND(OutData%uyWave,3) + DO i2 = LBOUND(OutData%uyWave,2), UBOUND(OutData%uyWave,2) + DO i1 = LBOUND(OutData%uyWave,1), UBOUND(OutData%uyWave,1) + OutData%uyWave(i1,i2,i3,i4) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 END DO END DO END DO END DO END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! uz not allocated + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! uzWave not allocated Int_Xferred = Int_Xferred + 1 ELSE Int_Xferred = Int_Xferred + 1 @@ -11898,24 +12092,24 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg i4_l = IntKiBuf( Int_Xferred ) i4_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%uz)) DEALLOCATE(OutData%uz) - ALLOCATE(OutData%uz(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ALLOCATED(OutData%uzWave)) DEALLOCATE(OutData%uzWave) + ALLOCATE(OutData%uzWave(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%uz.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%uzWave.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i4 = LBOUND(OutData%uz,4), UBOUND(OutData%uz,4) - DO i3 = LBOUND(OutData%uz,3), UBOUND(OutData%uz,3) - DO i2 = LBOUND(OutData%uz,2), UBOUND(OutData%uz,2) - DO i1 = LBOUND(OutData%uz,1), UBOUND(OutData%uz,1) - OutData%uz(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 + DO i4 = LBOUND(OutData%uzWave,4), UBOUND(OutData%uzWave,4) + DO i3 = LBOUND(OutData%uzWave,3), UBOUND(OutData%uzWave,3) + DO i2 = LBOUND(OutData%uzWave,2), UBOUND(OutData%uzWave,2) + DO i1 = LBOUND(OutData%uzWave,1), UBOUND(OutData%uzWave,1) + OutData%uzWave(i1,i2,i3,i4) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 END DO END DO END DO END DO END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ax not allocated + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! axWave not allocated Int_Xferred = Int_Xferred + 1 ELSE Int_Xferred = Int_Xferred + 1 @@ -11931,24 +12125,24 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg i4_l = IntKiBuf( Int_Xferred ) i4_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%ax)) DEALLOCATE(OutData%ax) - ALLOCATE(OutData%ax(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ALLOCATED(OutData%axWave)) DEALLOCATE(OutData%axWave) + ALLOCATE(OutData%axWave(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ax.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%axWave.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i4 = LBOUND(OutData%ax,4), UBOUND(OutData%ax,4) - DO i3 = LBOUND(OutData%ax,3), UBOUND(OutData%ax,3) - DO i2 = LBOUND(OutData%ax,2), UBOUND(OutData%ax,2) - DO i1 = LBOUND(OutData%ax,1), UBOUND(OutData%ax,1) - OutData%ax(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 + DO i4 = LBOUND(OutData%axWave,4), UBOUND(OutData%axWave,4) + DO i3 = LBOUND(OutData%axWave,3), UBOUND(OutData%axWave,3) + DO i2 = LBOUND(OutData%axWave,2), UBOUND(OutData%axWave,2) + DO i1 = LBOUND(OutData%axWave,1), UBOUND(OutData%axWave,1) + OutData%axWave(i1,i2,i3,i4) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 END DO END DO END DO END DO END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ay not allocated + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! ayWave not allocated Int_Xferred = Int_Xferred + 1 ELSE Int_Xferred = Int_Xferred + 1 @@ -11964,24 +12158,24 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg i4_l = IntKiBuf( Int_Xferred ) i4_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%ay)) DEALLOCATE(OutData%ay) - ALLOCATE(OutData%ay(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ALLOCATED(OutData%ayWave)) DEALLOCATE(OutData%ayWave) + ALLOCATE(OutData%ayWave(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ay.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%ayWave.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i4 = LBOUND(OutData%ay,4), UBOUND(OutData%ay,4) - DO i3 = LBOUND(OutData%ay,3), UBOUND(OutData%ay,3) - DO i2 = LBOUND(OutData%ay,2), UBOUND(OutData%ay,2) - DO i1 = LBOUND(OutData%ay,1), UBOUND(OutData%ay,1) - OutData%ay(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 + DO i4 = LBOUND(OutData%ayWave,4), UBOUND(OutData%ayWave,4) + DO i3 = LBOUND(OutData%ayWave,3), UBOUND(OutData%ayWave,3) + DO i2 = LBOUND(OutData%ayWave,2), UBOUND(OutData%ayWave,2) + DO i1 = LBOUND(OutData%ayWave,1), UBOUND(OutData%ayWave,1) + OutData%ayWave(i1,i2,i3,i4) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 END DO END DO END DO END DO END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! az not allocated + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! azWave not allocated Int_Xferred = Int_Xferred + 1 ELSE Int_Xferred = Int_Xferred + 1 @@ -11997,18 +12191,18 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg i4_l = IntKiBuf( Int_Xferred ) i4_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%az)) DEALLOCATE(OutData%az) - ALLOCATE(OutData%az(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) + IF (ALLOCATED(OutData%azWave)) DEALLOCATE(OutData%azWave) + ALLOCATE(OutData%azWave(i1_l:i1_u,i2_l:i2_u,i3_l:i3_u,i4_l:i4_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%az.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%azWave.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i4 = LBOUND(OutData%az,4), UBOUND(OutData%az,4) - DO i3 = LBOUND(OutData%az,3), UBOUND(OutData%az,3) - DO i2 = LBOUND(OutData%az,2), UBOUND(OutData%az,2) - DO i1 = LBOUND(OutData%az,1), UBOUND(OutData%az,1) - OutData%az(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 + DO i4 = LBOUND(OutData%azWave,4), UBOUND(OutData%azWave,4) + DO i3 = LBOUND(OutData%azWave,3), UBOUND(OutData%azWave,3) + DO i2 = LBOUND(OutData%azWave,2), UBOUND(OutData%azWave,2) + DO i1 = LBOUND(OutData%azWave,1), UBOUND(OutData%azWave,1) + OutData%azWave(i1,i2,i3,i4) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 END DO END DO END DO @@ -12040,8 +12234,8 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg DO i3 = LBOUND(OutData%PDyn,3), UBOUND(OutData%PDyn,3) DO i2 = LBOUND(OutData%PDyn,2), UBOUND(OutData%PDyn,2) DO i1 = LBOUND(OutData%PDyn,1), UBOUND(OutData%PDyn,1) - OutData%PDyn(i1,i2,i3,i4) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 + OutData%PDyn(i1,i2,i3,i4) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 END DO END DO END DO @@ -12069,82 +12263,66 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg DO i3 = LBOUND(OutData%zeta,3), UBOUND(OutData%zeta,3) DO i2 = LBOUND(OutData%zeta,2), UBOUND(OutData%zeta,2) DO i1 = LBOUND(OutData%zeta,1), UBOUND(OutData%zeta,1) - OutData%zeta(i1,i2,i3) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 + OutData%zeta(i1,i2,i3) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 END DO END DO END DO END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! px not allocated + OutData%nzCurrent = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 - ELSE - Int_Xferred = Int_Xferred + 1 - i1_l = IntKiBuf( Int_Xferred ) - i1_u = IntKiBuf( Int_Xferred + 1) - Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%px)) DEALLOCATE(OutData%px) - ALLOCATE(OutData%px(i1_l:i1_u),STAT=ErrStat2) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%px.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF - DO i1 = LBOUND(OutData%px,1), UBOUND(OutData%px,1) - OutData%px(i1) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 - END DO - END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! py not allocated + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! pzCurrent not allocated Int_Xferred = Int_Xferred + 1 ELSE Int_Xferred = Int_Xferred + 1 i1_l = IntKiBuf( Int_Xferred ) i1_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%py)) DEALLOCATE(OutData%py) - ALLOCATE(OutData%py(i1_l:i1_u),STAT=ErrStat2) + IF (ALLOCATED(OutData%pzCurrent)) DEALLOCATE(OutData%pzCurrent) + ALLOCATE(OutData%pzCurrent(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%py.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%pzCurrent.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i1 = LBOUND(OutData%py,1), UBOUND(OutData%py,1) - OutData%py(i1) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 + DO i1 = LBOUND(OutData%pzCurrent,1), UBOUND(OutData%pzCurrent,1) + OutData%pzCurrent(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 END DO END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! pz not allocated + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! uxCurrent not allocated Int_Xferred = Int_Xferred + 1 ELSE Int_Xferred = Int_Xferred + 1 i1_l = IntKiBuf( Int_Xferred ) i1_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%pz)) DEALLOCATE(OutData%pz) - ALLOCATE(OutData%pz(i1_l:i1_u),STAT=ErrStat2) + IF (ALLOCATED(OutData%uxCurrent)) DEALLOCATE(OutData%uxCurrent) + ALLOCATE(OutData%uxCurrent(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%pz.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%uxCurrent.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i1 = LBOUND(OutData%pz,1), UBOUND(OutData%pz,1) - OutData%pz(i1) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 + DO i1 = LBOUND(OutData%uxCurrent,1), UBOUND(OutData%uxCurrent,1) + OutData%uxCurrent(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 END DO END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! tWave not allocated + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! uyCurrent not allocated Int_Xferred = Int_Xferred + 1 ELSE Int_Xferred = Int_Xferred + 1 i1_l = IntKiBuf( Int_Xferred ) i1_u = IntKiBuf( Int_Xferred + 1) Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%tWave)) DEALLOCATE(OutData%tWave) - ALLOCATE(OutData%tWave(i1_l:i1_u),STAT=ErrStat2) + IF (ALLOCATED(OutData%uyCurrent)) DEALLOCATE(OutData%uyCurrent) + ALLOCATE(OutData%uyCurrent(i1_l:i1_u),STAT=ErrStat2) IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%tWave.', ErrStat, ErrMsg,RoutineName) + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%uyCurrent.', ErrStat, ErrMsg,RoutineName) RETURN END IF - DO i1 = LBOUND(OutData%tWave,1), UBOUND(OutData%tWave,1) - OutData%tWave(i1) = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 + DO i1 = LBOUND(OutData%uyCurrent,1), UBOUND(OutData%uyCurrent,1) + OutData%uyCurrent(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 END DO END IF OutData%Nx0 = IntKiBuf(Int_Xferred) From fcb63e77d893cebe2c6d13cb3d8ffb5417a5cc2e Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Tue, 19 Oct 2021 14:52:39 -0600 Subject: [PATCH 069/242] Add MD_InitInp Tmax in FAST_Subs --- modules/openfast-library/src/FAST_Subs.f90 | 1 + 1 file changed, 1 insertion(+) diff --git a/modules/openfast-library/src/FAST_Subs.f90 b/modules/openfast-library/src/FAST_Subs.f90 index 4f3a43b775..682025edd6 100644 --- a/modules/openfast-library/src/FAST_Subs.f90 +++ b/modules/openfast-library/src/FAST_Subs.f90 @@ -952,6 +952,7 @@ SUBROUTINE FAST_InitializeAll( t_initial, p_FAST, y_FAST, m_FAST, ED, BD, SrvD, Init%InData_MD%g = Init%OutData_ED%Gravity ! This need to be according to g used in ElastoDyn Init%InData_MD%rhoW = Init%OutData_HD%WtrDens ! This needs to be set according to seawater density in HydroDyn Init%InData_MD%WtrDepth = Init%OutData_HD%WtrDpth ! This need to be set according to the water depth in HydroDyn + Init%InData_MD%Tmax = p_FAST%TMax ! expected simulation duration (used by MoorDyn for wave kinematics preprocesing) Init%InData_MD%Linearize = p_FAST%Linearize From f1e6f3ed861d9ce96502f1641764ed53ce3cbd08 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Wed, 20 Oct 2021 15:16:52 -0600 Subject: [PATCH 070/242] Adding the new MoorDyn sources files I forgot to add in commit 3a1173b2469cde453b938ab8b5977ee189ccc2ee --- modules/moordyn/src/MoorDyn_Body.f90 | 543 +++++++ modules/moordyn/src/MoorDyn_Line.f90 | 1447 ++++++++++++++++++ modules/moordyn/src/MoorDyn_Misc.f90 | 1972 +++++++++++++++++++++++++ modules/moordyn/src/MoorDyn_Point.f90 | 419 ++++++ modules/moordyn/src/MoorDyn_Rod.f90 | 1140 ++++++++++++++ 5 files changed, 5521 insertions(+) create mode 100644 modules/moordyn/src/MoorDyn_Body.f90 create mode 100644 modules/moordyn/src/MoorDyn_Line.f90 create mode 100644 modules/moordyn/src/MoorDyn_Misc.f90 create mode 100644 modules/moordyn/src/MoorDyn_Point.f90 create mode 100644 modules/moordyn/src/MoorDyn_Rod.f90 diff --git a/modules/moordyn/src/MoorDyn_Body.f90 b/modules/moordyn/src/MoorDyn_Body.f90 new file mode 100644 index 0000000000..9850d9120c --- /dev/null +++ b/modules/moordyn/src/MoorDyn_Body.f90 @@ -0,0 +1,543 @@ +!********************************************************************************************************************************** +! LICENSING +! Copyright (C) 2020-2021 Alliance for Sustainable Energy, LLC +! Copyright (C) 2015-2019 Matthew Hall +! +! This file is part of MoorDyn. +! +! Licensed under the Apache License, Version 2.0 (the "License"); +! you may not use this file except in compliance with the License. +! You may obtain a copy of the License at +! +! http://www.apache.org/licenses/LICENSE-2.0 +! +! Unless required by applicable law or agreed to in writing, software +! distributed under the License is distributed on an "AS IS" BASIS, +! WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +! See the License for the specific language governing permissions and +! limitations under the License. +! +!********************************************************************************************************************************** +MODULE MoorDyn_Body + + USE MoorDyn_Types + USE MoorDyn_IO + USE NWTC_Library + USE MoorDyn_Misc + !USE MoorDyn_Line, only : Line_SetEndKinematics, Line_GetEndStuff + USE MoorDyn_Point, only : Connect_SetKinematics, Connect_GetNetForceAndMass + USE MoorDyn_Rod, only : Rod_Initialize, Rod_SetKinematics, Rod_GetNetForceAndMass + + IMPLICIT NONE + + PRIVATE + + INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output + + PUBLIC :: Body_Setup + PUBLIC :: Body_Initialize + PUBLIC :: Body_InitializeUnfree + PUBLIC :: Body_SetKinematics + PUBLIC :: Body_SetState + PUBLIC :: Body_SetDependentKin + PUBLIC :: Body_GetStateDeriv + PUBLIC :: Body_DoRHS + PUBLIC :: Body_GetCoupledForce + PUBLIC :: Body_AddConnect + PUBLIC :: Body_AddRod + + + +CONTAINS + + + SUBROUTINE Body_Setup( Body, tempArray, rhoW, ErrStat, ErrMsg) + + TYPE(MD_Body), INTENT(INOUT) :: Body ! the single body object of interest + REAL(DbKi), INTENT(IN) :: tempArray(6) ! initial pose of body + REAL(DbKi), INTENT(IN) :: rhoW + INTEGER, INTENT(INOUT ) :: ErrStat ! returns a non-zero value when an error occurs + CHARACTER(*), INTENT(INOUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None + + INTEGER(4) :: J ! Generic index + INTEGER(4) :: K ! Generic index + INTEGER(IntKi) :: N + + REAL(DbKi) :: Mtemp(6,6) + + ! set initial velocity to zero + Body%v6 = 0.0_DbKi + + !also set number of attached rods and points to zero initially + Body%nAttachedC = 0 + Body%nAttachedR = 0 + + ! for now take one entry and apply to all three DOFs just using a single entry for all axes <<<<< + DO J=2,3 + Body%BodyI(J) = Body%BodyI(1) + Body%BodyCdA(J) = Body%BodyCdA(1) + Body%BodyCa(J) = Body%BodyCa(1) + END DO + + ! set up body initial mass matrix (excluding any rods or attachements) + DO J=1,3 + Mtemp(J,J) = Body%bodyM ! fill in mass + Mtemp(3+J,3+J) = Body%bodyI(J) ! fill in inertia + END DO + + CALL TranslateMass6to6DOF(Body%rCG, Mtemp, Body%M0) ! account for potential CG offset <<< is the direction right? <<< + + DO J=1,6 + Body%M0(J,J) = Body%M0(J,J) + Body%bodyV*Body%bodyCa(1) ! add added mass in each direction about ref point (so only diagonals) <<< eventually expand to multi D + END DO + + ! --------------- if this is an independent body (not coupled) ---------- + ! set initial position and orientation of body from input file + Body%r6 = tempArray + + ! calculate orientation matrix based on latest angles + !RotMat(r6[3], r6[4], r6[5], OrMat); + Body%OrMat = TRANSPOSE( EulerConstruct( Body%r6(4:6) ) ) ! full Euler angle approach <<<< need to check order + + IF (wordy > 0) print *, "Set up Body ",Body%IdNum, ", type ", Body%typeNum + + ! need to add cleanup sub <<< + + END SUBROUTINE Body_Setup + +! ! used to initialize bodies that aren't free i.e. don't have states +! !-------------------------------------------------------------- +! SUBROUTINE Body_InitializeUnfree(Body, r6_in, mesh, mesh_index, m) +! +! Type(MD_Body), INTENT(INOUT) :: Body ! the Body object +! Real(DbKi), INTENT(IN ) :: r6_in(6) ! state vector section for this line +! TYPE(MeshType), INTENT(INOUT) :: mesh ! +! Integer(IntKi), INTENT(IN ) :: mesh_index ! index of the node in the mesh for the current object being initialized +! TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects +! +! INTEGER(IntKi) :: l ! index of segments or nodes along line +! REAL(DbKi) :: rRef(3) ! reference position of mesh node +! REAL(DbKi) :: OrMat(3,3) ! DCM for body orientation based on r6_in +! REAL(DbKi) :: dummyStates(12) +! +! +! rRef = 0.0_DbKi ! <<< maybe this should be the offsets of the local platform origins from the global origins in future? And that's what's specificed by the Body input coordinates? +! +! CALL MeshPositionNode(mesh, mesh+index, rRef,ErrStat2,ErrMsg2)! "assign the coordinates (u%PtFairleadDisplacement%Position) of each node in the global coordinate space" +! +! CALL CheckError( ErrStat2, ErrMsg2 ) +! IF (ErrStat >= AbortErrLev) RETURN +! +! ! Apply offsets due to initial platform rotations and translations (fixed Jun 19, 2015) +! CALL SmllRotTrans('body rotation matrix', r6_in(4),r6_in(5),r6_in(6), OrMat, '', ErrStat2, ErrMsg2) +! mesh%TranslationDisp(1, mesh_index) = r6_in(1) + OrMat(1,1)*rRef(1) + OrMat(2,1)*rRef(2) + OrMat(3,1)*rRef(3) - rRef(1) +! mesh%TranslationDisp(2, mesh_index) = r6_in(2) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2) +! mesh%TranslationDisp(3, mesh_index) = r6_in(3) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) +! +! ! what about node point orientation ??? +! +! ! If any Rod is fixed to the body (not pinned), initialize it now because otherwise it won't be initialized +! DO l=1, Body%nAttachedR +! if (m%RodList(Body%attachedR(l))%typeNum == 2) CALL Rod_Initialize(m%RodList(Body%attachedR(l)), dummyStates, m%LineList) +! END DO +! +! ! Note: Connections don't need any initialization +! +! END SUBROUTINE Body_InitializeUnfree +! !-------------------------------------------------------------- + + + ! used to initialize bodies that are free + !-------------------------------------------------------------- + SUBROUTINE Body_Initialize(Body, states, m) + + Type(MD_Body), INTENT(INOUT) :: Body ! the Body object + Real(DbKi), INTENT(INOUT) :: states(:) ! state vector section for this Body + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + + INTEGER(IntKi) :: l ! index of segments or nodes along line + REAL(DbKi) :: dummyStates(12) ! dummy vector to mimic states when initializing a rigidly attached rod + + + ! assign initial body kinematics to state vector + states(7:12) = Body%r6 + states(1:6 ) = Body%v6 + + + ! set positions of any dependent connections and rods now (before they are initialized) + CALL Body_SetDependentKin(Body, 0.0_DbKi, m) + + ! If any Rod is fixed to the body (not pinned), initialize it now because otherwise it won't be initialized + DO l=1, Body%nAttachedR + if (m%RodList(Body%attachedR(l))%typeNum == 2) CALL Rod_Initialize(m%RodList(Body%attachedR(l)), dummyStates, m) + END DO + + ! Note: Connections don't need any initialization + + END SUBROUTINE Body_Initialize + !-------------------------------------------------------------- + + ! used to initialize bodies that are coupled or fixed + !-------------------------------------------------------------- + SUBROUTINE Body_InitializeUnfree(Body, m) + + Type(MD_Body), INTENT(INOUT) :: Body ! the Body object + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + + INTEGER(IntKi) :: l ! index of segments or nodes along line + REAL(DbKi) :: dummyStates(12) ! dummy vector to mimic states when initializing a rigidly attached rod + + + ! set positions of any dependent connections and rods now (before they are initialized) + CALL Body_SetDependentKin(Body, 0.0_DbKi, m) + + ! If any Rod is fixed to the body (not pinned), initialize it now because otherwise it won't be initialized + DO l=1, Body%nAttachedR + if (m%RodList(Body%attachedR(l))%typeNum == 2) CALL Rod_Initialize(m%RodList(Body%attachedR(l)), dummyStates, m) + END DO + + ! Note: Connections don't need any initialization + + END SUBROUTINE Body_InitializeUnfree + !-------------------------------------------------------------- + + + + + ! set kinematics for Bodies if they are coupled (or ground) + !-------------------------------------------------------------- + SUBROUTINE Body_SetKinematics(Body, r_in, v_in, a_in, t, m) + + Type(MD_Body), INTENT(INOUT) :: Body ! the Body object + Real(DbKi), INTENT(IN ) :: r_in(6) ! 6-DOF position + Real(DbKi), INTENT(IN ) :: v_in(6) ! 6-DOF velocity + Real(DbKi), INTENT(IN ) :: a_in(6) ! 6-DOF acceleration (only used for coupled rods) + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects (for simplicity, since Bodies deal with Rods and Connections) + + + INTEGER(IntKi) :: l + + ! store current time + Body%time = t + + ! if (abs(Body%typeNum) == 2) then ! body coupled in 6 DOF, or ground + Body%r6 = r_in + Body%v6 = v_in + Body%a6 = a_in + + ! since this body has no states and all DOFs have been set, pass its kinematics to dependent attachments + CALL Body_SetDependentKin(Body, t, m) + + ! else if (abs(Body%typeNum) == 1) then ! body pinned at reference point + ! + ! ! set Body *end A only* kinematics based on BCs (linear model for now) + ! Body%r6(1:3) = r_in(1:3) + ! Body%v6(1:3) = v_in(1:3) + ! + ! ! Body is pinned so only ref point posiiton is specified, rotations are left alone and will be + ! ! handled, along with passing kinematics to attached objects, by separate call to setState + ! + ! else + ! print *, "Error: Body_SetKinematics called for a free Body." ! <<< + ! end if + + END SUBROUTINE Body_SetKinematics + !-------------------------------------------------------------- + + + !-------------------------------------------------------------- + SUBROUTINE Body_SetState(Body, X, t, m) + + Type(MD_Body), INTENT(INOUT) :: Body ! the Body object + Real(DbKi), INTENT(IN ) :: X(:) ! state vector section for this line + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + + INTEGER(IntKi) :: l ! index of segments or nodes along line + INTEGER(IntKi) :: J ! index + + ! store current time + Body%time = t + + + + Body%r6 = X(7:12) ! get positions + Body%v6 = X(1:6) ! get velocities + + + ! set positions of any dependent connections and rods + CALL Body_SetDependentKin(Body, t, m) + + END SUBROUTINE Body_SetState + !-------------------------------------------------------------- + + + ! set the states (positions and velocities) of any connects or rods that are part of this body + ! also computes the orientation matrix (never skip this sub!) + !-------------------------------------------------------------- + SUBROUTINE Body_SetDependentKin(Body, t, m) + + Type(MD_Body), INTENT(INOUT) :: Body ! the Bodyion object + REAL(DbKi), INTENT(IN ) :: t + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects (for simplicity, since Bodies deal with Rods and Connections) + + INTEGER(IntKi) :: l ! index of attached objects + + Real(DbKi) :: rConnect(3) + Real(DbKi) :: rdConnect(3) + Real(DbKi) :: rRod(6) + Real(DbKi) :: vRod(6) + Real(DbKi) :: aRod(6) + + + + ! calculate orientation matrix based on latest angles + !CALL SmllRotTrans('', Body%r6(4), Body%r6(5), Body%r6(6), Body%TransMat, '', ErrStat2, ErrMsg2) + Body%OrMat = TRANSPOSE( EulerConstruct( Body%r6(4:6) ) ) ! full Euler angle approach <<<< need to check order + + ! set kinematics of any dependent connections + do l = 1,Body%nAttachedC + + CALL transformKinematics(Body%rConnectRel(:,l), Body%r6, Body%OrMat, Body%v6, rConnect, rdConnect) !<<< should double check this function + + ! >>> need to add acceleration terms here too? <<< + + ! pass above to the connection and get it to calculate the forces + CALL Connect_SetKinematics( m%ConnectList(Body%attachedC(l)), rConnect, rdConnect, m%zeros6(1:3), t, m) + end do + + ! set kinematics of any dependent Rods + do l=1,Body%nAttachedR + + ! calculate displaced coordinates/orientation and velocities of each rod <<<<<<<<<<<<< + ! do 3d details of Rod ref point + CALL TransformKinematicsA( Body%r6RodRel(1:3,l), Body%r6(1:3), Body%OrMat, Body%v6, Body%a6, rRod(1:3), vRod(1:3), aRod(1:3)) ! set first three entires (end A translation) of rRod and rdRod + ! does the above function need to take in all 6 elements of r6RodRel?? + + ! do rotational stuff + rRod(4:6) = MATMUL(Body%OrMat, Body%r6RodRel(4:6,l)) !<<<<<< correct? <<<<< rotateVector3(r6RodRel[i]+3, OrMat, rRod+3); ! rotate rod relative unit vector by OrMat to get unit vec in reference coords + vRod(4:6) = Body%v6(4:6) ! transformed rotational velocity. <<< is this okay as is? <<<< + aRod(4:6) = Body%a6(4:6) + + ! pass above to the rod and get it to calculate the forces + CALL Rod_SetKinematics(m%RodList(Body%attachedR(l)), rRod, vRod, aRod, t, m) + end do + + END SUBROUTINE Body_SetDependentKin + !-------------------------------------------------------------- + + + !-------------------------------------------------------------- + SUBROUTINE Body_GetStateDeriv(Body, Xd, m, p) + + Type(MD_Body), INTENT(INOUT) :: Body ! the Bodyion object + Real(DbKi), INTENT(INOUT) :: Xd(:) ! state derivative vector section for this line + + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + INTEGER(IntKi) :: J ! index + + Real(DbKi) :: acc(6) ! 6DOF acceleration vector + + Real(DbKi) :: y_temp (6) ! temporary vector for LU decomposition + Real(DbKi) :: LU_temp(6,6) ! temporary matrix for LU decomposition + + + ! Initialize temp variables + y_temp = 0.0_DbKi +! FIXME: should LU_temp be set to M_out before calling LUsolve????? + LU_temp = 0.0_DbKi + + CALL Body_DoRHS(Body, m, p) + + ! solve for accelerations in [M]{a}={f} using LU decomposition + CALL LUsolve(6, Body%M, LU_temp, Body%F6net, y_temp, acc) + + ! fill in state derivatives + Xd(7:12) = Body%v6 ! dxdt = V (velocities) + Xd(1:6) = acc ! dVdt = a (accelerations) + + ! store accelerations in case they're useful as output + Body%a6 = acc + + ! check for NaNs (should check all state derivatives, not just first 6) + DO J = 1, 6 + IF (Is_NaN(Xd(J))) THEN + print *, "NaN detected at time ", Body%time, " in Body ",Body%IdNum, "in MoorDyn," + IF (wordy > 0) print *, "state derivatives:" + IF (wordy > 0) print *, Xd + EXIT + END IF + END DO + + + END SUBROUTINE Body_GetStateDeriv + !-------------------------------------------------------------- + + !-------------------------------------------------------------- + SUBROUTINE Body_DoRHS(Body, m, p) + + Type(MD_Body), INTENT(INOUT) :: Body ! the Bodyion object + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables + + INTEGER(IntKi) :: l ! index of attached lines + INTEGER(IntKi) :: I ! index + INTEGER(IntKi) :: J ! index + INTEGER(IntKi) :: K ! index + + Real(DbKi) :: Fgrav(3) ! body weight force + Real(DbKi) :: body_rCGrotated(3) ! instantaneous vector from body ref point to CG + Real(DbKi) :: U(3) ! water velocity - zero for now + Real(DbKi) :: Ud(3) ! water acceleration- zero for now + Real(DbKi) :: vi(6) ! relative water velocity (last 3 terms are rotatonal and will be set to zero + Real(DbKi) :: F6_i(6) ! net force and moments from an attached object + Real(DbKi) :: M6_i(6,6) ! mass and inertia from an attached object + + ! Initialize variables + U = 0.0_DbKi ! Set to zero for now + + ! First, the body's own mass matrix must be adjusted based on its orientation so that + ! we have a mass matrix in the global orientation frame + Body%M = RotateM6(Body%M0, Body%OrMat) + + !gravity on core body + Fgrav(1) = 0.0_DbKi + Fgrav(2) = 0.0_DbKi + Fgrav(3) = Body%bodyV * p%rhow * p%g - Body%bodyM * p%g ! weight+buoyancy vector + + body_rCGrotated = MATMUL(Body%OrMat, Body%rCG) ! rotateVector3(body_rCG, OrMat, body_rCGrotated); ! relative vector to body CG in inertial orientation + CALL translateForce3to6DOF(body_rCGrotated, Fgrav, Body%F6net) ! gravity forces and moments about body ref point given CG location + + + ! --------------------------------- apply wave kinematics ------------------------------------ + !env->waves->getU(r6, t, U); ! call generic function to get water velocities <<<<<<<<< all needs updating + + ! for (int J=0; J<3; J++) + ! Ud[J] = 0.0; ! set water accelerations as zero for now + ! ------------------------------------------------------------------------------------------ + + ! viscous drag calculation (on core body) + vi(1:3) = U - Body%v6(1:3) ! relative flow velocity over body ref point + vi(4:6) = - Body%v6(4:6) ! for rotation, this is just the negative of the body's rotation for now (not allowing flow rotation) + + Body%F6net = Body%F6net + 0.5*p%rhoW * vi * abs(vi) * Body%bodyCdA + ! <<< NOTE, for body this should be fixed to account for orientation!! <<< what about drag in rotational DOFs??? <<<<<<<<<<<<<< + + + + ! Get contributions from any dependent connections + do l = 1,Body%nAttachedC + + ! get net force and mass from Connection on body ref point (global orientation) + CALL Connect_GetNetForceAndMass( m%ConnectList(Body%attachedC(l)), Body%r6(1:3), F6_i, M6_i, m, p) + + ! sum quantitites + Body%F6net = Body%F6net + F6_i + Body%M = Body%M + M6_i + end do + + ! Get contributions from any dependent Rods + do l=1,Body%nAttachedR + + ! get net force and mass from Rod on body ref point (global orientation) + CALL Rod_GetNetForceAndMass(m%RodList(Body%attachedR(l)), Body%r6(1:3), F6_i, M6_i, m, p) + + ! sum quantitites + Body%F6net = Body%F6net + F6_i + Body%M = Body%M + M6_i + end do + + + END SUBROUTINE Body_DoRHS + !===================================================================== + + + ! calculate the aggregate 3/6DOF rigid-body loads of a coupled rod including inertial loads + !-------------------------------------------------------------- + SUBROUTINE Body_GetCoupledForce(Body, Fnet_out, m, p) + + Type(MD_Body), INTENT(INOUT) :: Body ! the Body object + Real(DbKi), INTENT( OUT) :: Fnet_out(6) ! force and moment vector + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + Real(DbKi) :: F6_iner(6) ! inertial reaction force + + ! do calculations of forces and masses on the body + CALL Body_DoRHS(Body, m, p) + + ! add inertial loads as appropriate + if (Body%typeNum == -1) then + + F6_iner = 0.0_DbKi !-MATMUL(Body%M, Body%a6) <<<<<<<< why does including F6_iner cause instability??? + Fnet_out = Body%F6net + F6_iner ! add inertial loads + + else + print *, "ERROR, Body_GetCoupledForce called for wrong (non-coupled) body type in MoorDyn!" + end if + + END SUBROUTINE Body_GetCoupledForce + !-------------------------------------------------------------- + + + + ! this function handles assigning a connection to a body + !-------------------------------------------------------------- + SUBROUTINE Body_AddConnect(Body, connectID, coords) + + Type(MD_Body), INTENT(INOUT) :: Body ! the Connection object + Integer(IntKi), INTENT(IN ) :: connectID + REAL(DbKi), INTENT(IN ) :: coords(3) + + + IF (wordy > 0) Print*, "C", connectID, "->B", Body%IdNum + + IF(Body%nAttachedC < 30) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. + Body%nAttachedC = Body%nAttachedC + 1 ! increment the number connected + Body%AttachedC(Body%nAttachedC) = connectID + Body%rConnectRel(:,Body%nAttachedC) = coords ! store relative position of connect on body + ELSE + Print*, "too many Points attached to Body ", Body%IdNum, " in MoorDyn!" + END IF + + END SUBROUTINE Body_AddConnect + + + ! this function handles assigning a rod to a body + !-------------------------------------------------------------- + SUBROUTINE Body_AddRod(Body, rodID, coords) + + Type(MD_Body), INTENT(INOUT) :: Body ! the Connection object + Integer(IntKi), INTENT(IN ) :: rodID + REAL(DbKi), INTENT(IN ) :: coords(6) ! positions of rod ends A and B relative to body + + REAL(DbKi) :: tempUnitVec(3) + REAL(DbKi) :: dummyLength + + IF (wordy > 0) Print*, "R", rodID, "->B", Body%IdNum + + IF(Body%nAttachedR < 30) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. + Body%nAttachedR = Body%nAttachedR + 1 ! increment the number connected + + ! store rod ID + Body%AttachedR(Body%nAttachedR) = rodID + + ! store Rod end A relative position and unit vector from end A to B + CALL UnitVector(coords(1:3), coords(4:6), tempUnitVec, dummyLength) + Body%r6RodRel(1:3, Body%nAttachedR) = coords(1:3) + Body%r6RodRel(4:6, Body%nAttachedR) = tempUnitVec + + ELSE + Print*, "too many rods attached to Body ", Body%IdNum, " in MoorDyn" + END IF + + END SUBROUTINE Body_AddRod + + + +END MODULE MoorDyn_Body diff --git a/modules/moordyn/src/MoorDyn_Line.f90 b/modules/moordyn/src/MoorDyn_Line.f90 new file mode 100644 index 0000000000..af63cff3b6 --- /dev/null +++ b/modules/moordyn/src/MoorDyn_Line.f90 @@ -0,0 +1,1447 @@ +!********************************************************************************************************************************** +! LICENSING +! Copyright (C) 2020-2021 Alliance for Sustainable Energy, LLC +! Copyright (C) 2015-2019 Matthew Hall +! +! This file is part of MoorDyn. +! +! Licensed under the Apache License, Version 2.0 (the "License"); +! you may not use this file except in compliance with the License. +! You may obtain a copy of the License at +! +! http://www.apache.org/licenses/LICENSE-2.0 +! +! Unless required by applicable law or agreed to in writing, software +! distributed under the License is distributed on an "AS IS" BASIS, +! WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +! See the License for the specific language governing permissions and +! limitations under the License. +! +!********************************************************************************************************************************** +MODULE MoorDyn_Line + + USE MoorDyn_Types + USE MoorDyn_IO + USE NWTC_Library + USE MoorDyn_Misc + + IMPLICIT NONE + + PRIVATE + + INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output + + PUBLIC :: SetupLine + PUBLIC :: Line_Initialize + PUBLIC :: Line_SetState + PUBLIC :: Line_GetStateDeriv + PUBLIC :: Line_SetEndKinematics + PUBLIC :: Line_GetEndStuff + PUBLIC :: Line_GetEndSegmentInfo + PUBLIC :: Line_SetEndOrientation + + + +CONTAINS + + + !----------------------------------------------------------------------- + ! >>>>>>>>>>>>>> rename/reorganize this subroutine >>>>>>>>>>>>> + SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) + ! allocate arrays in line object + + TYPE(MD_Line), INTENT(INOUT) :: Line ! the single line object of interest + TYPE(MD_LineProp), INTENT(INOUT) :: LineProp ! the single line property set for the line of interest + REAL(DbKi), INTENT(IN) :: rhoW + INTEGER, INTENT( INOUT ) :: ErrStat ! returns a non-zero value when an error occurs + CHARACTER(*), INTENT( INOUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None + + INTEGER(4) :: I, J, K ! Generic index + INTEGER(IntKi) :: N + REAL(DbKi) :: temp + + + N = Line%N ! number of segments in this line (for code readability) + + ! -------------- save some section properties to the line object itself ----------------- + + Line%d = LineProp%d + Line%rho = LineProp%w/(Pi/4.0 * Line%d * Line%d) + + Line%EA = LineProp%EA + ! note: Line%BA is set later + Line%EA_D = LineProp%EA_D + Line%BA_D = LineProp%BA_D + !Line%EI = LineProp%EI <<< for bending stiffness + + Line%Can = LineProp%Can + Line%Cat = LineProp%Cat + Line%Cdn = LineProp%Cdn + Line%Cdt = LineProp%Cdt + + ! copy over elasticity data + Line%ElasticMod = LineProp%ElasticMod + + Line%nEApoints = LineProp%nEApoints + DO I = 1,Line%nEApoints + Line%stiffXs(I) = LineProp%stiffXs(I) + Line%stiffYs(I) = LineProp%stiffYs(I) ! note: this does not convert to E (not EA) like done in C version + END DO + + Line%nBApoints = LineProp%nBApoints + DO I = 1,Line%nBApoints + Line%dampXs(I) = LineProp%dampXs(I) + Line%dampYs(I) = LineProp%dampYs(I) + END DO + + Line%nEIpoints = LineProp%nEIpoints + DO I = 1,Line%nEIpoints + Line%bstiffXs(I) = LineProp%bstiffXs(I) + Line%bstiffYs(I) = LineProp%bstiffYs(I) ! copy over + END DO + + + ! Specify specific internal damping coefficient (BA) for this line. + ! Will be equal to inputted BA of LineType if input value is positive. + ! If input value is negative, it is considered to be desired damping ratio (zeta) + ! from which the line's BA can be calculated based on the segment natural frequency. + IF (LineProp%BA < 0) THEN + ! - we assume desired damping coefficient is zeta = -LineProp%BA + ! - highest axial vibration mode of a segment is wn = sqrt(k/m) = 2N/UnstrLen*sqrt(EA/w) + Line%BA = -LineProp%BA * Line%UnstrLen / Line%N * SQRT(LineProp%EA * LineProp%w) + IF (wordy > 1) print *, 'Based on zeta, BA set to ', Line%BA + + IF (wordy > 1) print *, 'Negative BA input detected, treating as -zeta. For zeta = ', -LineProp%BA, ', setting BA to ', Line%BA + + ELSE + Line%BA = LineProp%BA + IF (wordy > 1) temp = Line%N * Line%BA / Line%UnstrLen * SQRT(1.0/(LineProp%EA * LineProp%w)) + IF (wordy > 1) print *, 'BA set as input to ', Line%BA, '. Corresponding zeta is ', temp + END IF + + !temp = 2*Line%N / Line%UnstrLen * sqrt( LineProp%EA / LineProp%w) / TwoPi + !print *, 'Segment natural frequency is ', temp, ' Hz' + + + + ! allocate node positions and velocities (NOTE: these arrays start at ZERO) + ALLOCATE ( Line%r(3, 0:N), Line%rd(3, 0:N), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) THEN + ErrMsg = ' Error allocating r and rd arrays.' + !CALL CleanUp() + RETURN + END IF + + ! if using viscoelastic model, allocate additional state quantities + if (Line%ElasticMod == 2) then + ALLOCATE ( Line%dl_1(N), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) THEN + ErrMsg = ' Error allocating dl_1 array.' + !CALL CleanUp() + RETURN + END IF + ! initialize to zero + Line%dl_1 = 0.0_DbKi + end if + + ! allocate node and segment tangent vectors + ALLOCATE ( Line%q(3, 0:N), Line%qs(3, N), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) THEN + ErrMsg = ' Error allocating q or qs array.' + !CALL CleanUp() + RETURN + END IF + + ! allocate segment scalar quantities + ALLOCATE ( Line%l(N), Line%ld(N), Line%lstr(N), Line%lstrd(N), Line%V(N), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) THEN + ErrMsg = ' Error allocating segment scalar quantity arrays.' + !CALL CleanUp() + RETURN + END IF + + ! assign values for l, ld, and V + DO J=1,N + Line%l(J) = Line%UnstrLen/REAL(N, DbKi) + Line%ld(J)= 0.0_DbKi + Line%V(J) = Line%l(J)*0.25*Pi*LineProp%d*LineProp%d + END DO + + ! allocate water related vectors + ALLOCATE ( Line%U(3, 0:N), Line%Ud(3, 0:N), Line%zeta(0:N), Line%PDyn(0:N), STAT = ErrStat ) + ! set to zero initially (important of wave kinematics are not being used) + Line%U = 0.0_DbKi + Line%Ud = 0.0_DbKi + Line%zeta = 0.0_DbKi + Line%PDyn = 0.0_DbKi + + ! allocate segment tension and internal damping force vectors + ALLOCATE ( Line%T(3, N), Line%Td(3, N), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) THEN + ErrMsg = ' Error allocating T and Td arrays.' + !CALL CleanUp() + RETURN + END IF + + ! allocate node force vectors + ALLOCATE ( Line%W(3, 0:N), Line%Dp(3, 0:N), Line%Dq(3, 0:N), Line%Ap(3, 0:N), & + Line%Aq(3, 0:N), Line%B(3, 0:N), Line%Fnet(3, 0:N), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) THEN + ErrMsg = ' Error allocating node force arrays.' + !CALL CleanUp() + RETURN + END IF + + ! set gravity and bottom contact forces to zero initially (because the horizontal components should remain at zero) + DO J = 0,N + DO K = 1,3 + Line%W(K,J) = 0.0_DbKi + Line%B(K,J) = 0.0_DbKi + END DO + END DO + + ! allocate mass and inverse mass matrices for each node (including ends) + ALLOCATE ( Line%S(3, 3, 0:N), Line%M(3, 3, 0:N), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) THEN + ErrMsg = ' Error allocating T and Td arrays.' + !CALL CleanUp() + RETURN + END IF + + + + ! need to add cleanup sub <<< + + + END SUBROUTINE SetupLine + !-------------------------------------------------------------- + + + + + + !----------------------------------------------------------------------------------------======= + SUBROUTINE Line_Initialize (Line, LineProp, rhoW, ErrStat, ErrMsg) + ! calculate initial profile of the line using quasi-static model + + TYPE(MD_Line), INTENT(INOUT) :: Line ! the single line object of interest + TYPE(MD_LineProp), INTENT(INOUT) :: LineProp ! the single line property set for the line of interest + REAL(DbKi), INTENT(IN) :: rhoW + INTEGER, INTENT( INOUT ) :: ErrStat ! returns a non-zero value when an error occurs + CHARACTER(*), INTENT( INOUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None + + REAL(DbKi) :: COSPhi ! Cosine of the angle between the xi-axis of the inertia frame and the X-axis of the local coordinate system of the current mooring line (-) + REAL(DbKi) :: SINPhi ! Sine of the angle between the xi-axis of the inertia frame and the X-axis of the local coordinate system of the current mooring line (-) + REAL(DbKi) :: XF ! Horizontal distance between anchor and fairlead of the current mooring line (meters) + REAL(DbKi) :: ZF ! Vertical distance between anchor and fairlead of the current mooring line (meters) + INTEGER(4) :: I ! Generic index + INTEGER(4) :: J ! Generic index + + + INTEGER(IntKi) :: ErrStat2 ! Error status of the operation + CHARACTER(ErrMsgLen) :: ErrMsg2 ! Error message if ErrStat2 /= ErrID_None + REAL(DbKi) :: WetWeight + REAL(DbKi) :: SeabedCD = 0.0_DbKi + REAL(DbKi) :: TenTol = 0.0001_DbKi + REAL(DbKi), ALLOCATABLE :: LSNodes(:) + REAL(DbKi), ALLOCATABLE :: LNodesX(:) + REAL(DbKi), ALLOCATABLE :: LNodesZ(:) + INTEGER(IntKi) :: N + + + N = Line%N ! for convenience + + ! try to calculate initial line profile using catenary routine (from FAST v.7) + ! note: much of this function is adapted from the FAST source code + + ! Transform the fairlead location from the inertial frame coordinate system + ! to the local coordinate system of the current line (this coordinate + ! system lies at the current anchor, Z being vertical, and X directed from + ! current anchor to the current fairlead). Also, compute the orientation + ! of this local coordinate system: + + XF = SQRT( ( Line%r(1,N) - Line%r(1,0) )**2.0 + ( Line%r(2,N) - Line%r(2,0) )**2.0 ) + ZF = Line%r(3,N) - Line%r(3,0) + + IF ( XF == 0.0 ) THEN ! .TRUE. if the current mooring line is exactly vertical; thus, the solution below is ill-conditioned because the orientation is undefined; so set it such that the tensions and nodal positions are only vertical + COSPhi = 0.0_DbKi + SINPhi = 0.0_DbKi + ELSE ! The current mooring line must not be vertical; use simple trigonometry + COSPhi = ( Line%r(1,N) - Line%r(1,0) )/XF + SINPhi = ( Line%r(2,N) - Line%r(2,0) )/XF + ENDIF + + WetWeight = LineProp%w - 0.25*Pi*LineProp%d*LineProp%d*rhoW + + !LineNodes = Line%N + 1 ! number of nodes in line for catenary model to worry about + + ! allocate temporary arrays for catenary routine + ALLOCATE ( LSNodes(N+1), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) THEN + ErrMsg = ' Error allocating LSNodes array.' + CALL CleanUp() + RETURN + END IF + + ALLOCATE ( LNodesX(N+1), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) THEN + ErrMsg = ' Error allocating LNodesX array.' + CALL CleanUp() + RETURN + END IF + + ALLOCATE ( LNodesZ(N+1), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) THEN + ErrMsg = ' Error allocating LNodesZ array.' + CALL CleanUp() + RETURN + END IF + + ! Assign node arc length locations + LSNodes(1) = 0.0_DbKi + DO I=2,N + LSNodes(I) = LSNodes(I-1) + Line%l(I-1) ! note: l index is because line segment indices start at 1 + END DO + LSNodes(N+1) = Line%UnstrLen ! ensure the last node length isn't longer than the line due to numerical error + + ! Solve the analytical, static equilibrium equations for a catenary (or + ! taut) mooring line with seabed interaction in order to find the + ! horizontal and vertical tensions at the fairlead in the local coordinate + ! system of the current line: + ! NOTE: The values for the horizontal and vertical tensions at the fairlead + ! from the previous time step are used as the initial guess values at + ! at this time step (because the LAnchHTe(:) and LAnchVTe(:) arrays + ! are stored in a module and thus their values are saved from CALL to + ! CALL). + + + CALL Catenary ( XF , ZF , Line%UnstrLen, LineProp%EA , & + WetWeight , SeabedCD, TenTol, (N+1) , & + LSNodes, LNodesX, LNodesZ , ErrStat2, ErrMsg2) + + IF (ErrStat2 == ErrID_None) THEN ! if it worked, use it + ! Transform the positions of each node on the current line from the local + ! coordinate system of the current line to the inertial frame coordinate + ! system: + + DO J = 0,N ! Loop through all nodes per line where the line position and tension can be output + Line%r(1,J) = Line%r(1,0) + LNodesX(J+1)*COSPhi + Line%r(2,J) = Line%r(2,0) + LNodesX(J+1)*SINPhi + Line%r(3,J) = Line%r(3,0) + LNodesZ(J+1) + ENDDO ! J - All nodes per line where the line position and tension can be output + + + ELSE ! if there is a problem with the catenary approach, just stretch the nodes linearly between fairlead and anchor + + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'Line_Initialize') + +! print *, "Node positions: " + + DO J = 0,N ! Loop through all nodes per line where the line position and tension can be output + Line%r(1,J) = Line%r(1,0) + (Line%r(1,N) - Line%r(1,0))*REAL(J, DbKi)/REAL(N, DbKi) + Line%r(2,J) = Line%r(2,0) + (Line%r(2,N) - Line%r(2,0))*REAL(J, DbKi)/REAL(N, DbKi) + Line%r(3,J) = Line%r(3,0) + (Line%r(3,N) - Line%r(3,0))*REAL(J, DbKi)/REAL(N, DbKi) + +! print*, Line%r(:,J) + ENDDO + +! print*,"FYI line end A and B node coords are" +! print*, Line%r(:,0) +! print*, Line%r(:,N) + ENDIF + + + + CALL CleanUp() ! deallocate temporary arrays + + + + CONTAINS + + + !----------------------------------------------------------------------- + SUBROUTINE CleanUp() + ! deallocate temporary arrays + + IF (ALLOCATED(LSNodes)) DEALLOCATE(LSNodes) + IF (ALLOCATED(LNodesX)) DEALLOCATE(LNodesX) + IF (ALLOCATED(LNodesZ)) DEALLOCATE(LNodesZ) + + END SUBROUTINE CleanUp + !----------------------------------------------------------------------- + + + !----------------------------------------------------------------------- + SUBROUTINE Catenary ( XF_In, ZF_In, L_In , EA_In, & + W_In , CB_In, Tol_In, N , & + s_In , X_In , Z_In , ErrStat, ErrMsg ) + + ! This subroutine is copied from FAST v7 with minor modifications + + ! This routine solves the analytical, static equilibrium equations + ! for a catenary (or taut) mooring line with seabed interaction. + ! Stretching of the line is accounted for, but bending stiffness + ! is not. Given the mooring line properties and the fairlead + ! position relative to the anchor, this routine finds the line + ! configuration and tensions. Since the analytical solution + ! involves two nonlinear equations (XF and ZF) in two unknowns + ! (HF and VF), a Newton-Raphson iteration scheme is implemented in + ! order to solve for the solution. The values of HF and VF that + ! are passed into this routine are used as the initial guess in + ! the iteration. The Newton-Raphson iteration is only accurate in + ! double precision, so all of the input/output arguments are + ! converteds to/from double precision from/to default precision. + + ! >>>> TO DO: streamline this function, if it's still to be used at all <<<< + + ! USE Precision + + + IMPLICIT NONE + + + ! Passed Variables: + + INTEGER(4), INTENT(IN ) :: N ! Number of nodes where the line position and tension can be output (-) + + REAL(DbKi), INTENT(IN ) :: CB_In ! Coefficient of seabed static friction drag (a negative value indicates no seabed) (-) + REAL(DbKi), INTENT(IN ) :: EA_In ! Extensional stiffness of line (N) + ! REAL(DbKi), INTENT( OUT) :: HA_In ! Effective horizontal tension in line at the anchor (N) + ! REAL(DbKi), INTENT(INOUT) :: HF_In ! Effective horizontal tension in line at the fairlead (N) + REAL(DbKi), INTENT(IN ) :: L_In ! Unstretched length of line (meters) + REAL(DbKi), INTENT(IN ) :: s_In (N) ! Unstretched arc distance along line from anchor to each node where the line position and tension can be output (meters) + ! REAL(DbKi), INTENT( OUT) :: Te_In (N) ! Effective line tensions at each node (N) + REAL(DbKi), INTENT(IN ) :: Tol_In ! Convergence tolerance within Newton-Raphson iteration specified as a fraction of tension (-) + ! REAL(DbKi), INTENT( OUT) :: VA_In ! Effective vertical tension in line at the anchor (N) + ! REAL(DbKi), INTENT(INOUT) :: VF_In ! Effective vertical tension in line at the fairlead (N) + REAL(DbKi), INTENT(IN ) :: W_In ! Weight of line in fluid per unit length (N/m) + REAL(DbKi), INTENT( OUT) :: X_In (N) ! Horizontal locations of each line node relative to the anchor (meters) + REAL(DbKi), INTENT(IN ) :: XF_In ! Horizontal distance between anchor and fairlead (meters) + REAL(DbKi), INTENT( OUT) :: Z_In (N) ! Vertical locations of each line node relative to the anchor (meters) + REAL(DbKi), INTENT(IN ) :: ZF_In ! Vertical distance between anchor and fairlead (meters) + INTEGER, INTENT( OUT ) :: ErrStat ! returns a non-zero value when an error occurs + CHARACTER(*), INTENT( OUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None + + + ! Local Variables: + + REAL(DbKi) :: CB ! Coefficient of seabed static friction (a negative value indicates no seabed) (-) + REAL(DbKi) :: CBOvrEA ! = CB/EA + REAL(DbKi) :: DET ! Determinant of the Jacobian matrix (m^2/N^2) + REAL(DbKi) :: dHF ! Increment in HF predicted by Newton-Raphson (N) + REAL(DbKi) :: dVF ! Increment in VF predicted by Newton-Raphson (N) + REAL(DbKi) :: dXFdHF ! Partial derivative of the calculated horizontal distance with respect to the horizontal fairlead tension (m/N): dXF(HF,VF)/dHF + REAL(DbKi) :: dXFdVF ! Partial derivative of the calculated horizontal distance with respect to the vertical fairlead tension (m/N): dXF(HF,VF)/dVF + REAL(DbKi) :: dZFdHF ! Partial derivative of the calculated vertical distance with respect to the horizontal fairlead tension (m/N): dZF(HF,VF)/dHF + REAL(DbKi) :: dZFdVF ! Partial derivative of the calculated vertical distance with respect to the vertical fairlead tension (m/N): dZF(HF,VF)/dVF + REAL(DbKi) :: EA ! Extensional stiffness of line (N) + REAL(DbKi) :: EXF ! Error function between calculated and known horizontal distance (meters): XF(HF,VF) - XF + REAL(DbKi) :: EZF ! Error function between calculated and known vertical distance (meters): ZF(HF,VF) - ZF + REAL(DbKi) :: HA ! Effective horizontal tension in line at the anchor (N) + REAL(DbKi) :: HF ! Effective horizontal tension in line at the fairlead (N) + REAL(DbKi) :: HFOvrW ! = HF/W + REAL(DbKi) :: HFOvrWEA ! = HF/WEA + REAL(DbKi) :: L ! Unstretched length of line (meters) + REAL(DbKi) :: Lamda0 ! Catenary parameter used to generate the initial guesses of the horizontal and vertical tensions at the fairlead for the Newton-Raphson iteration (-) + REAL(DbKi) :: LMax ! Maximum stretched length of the line with seabed interaction beyond which the line would have to double-back on itself; here the line forms an "L" between the anchor and fairlead (i.e. it is horizontal along the seabed from the anchor, then vertical to the fairlead) (meters) + REAL(DbKi) :: LMinVFOvrW ! = L - VF/W + REAL(DbKi) :: LOvrEA ! = L/EA + REAL(DbKi) :: s (N) ! Unstretched arc distance along line from anchor to each node where the line position and tension can be output (meters) + REAL(DbKi) :: sOvrEA ! = s(I)/EA + REAL(DbKi) :: SQRT1VFOvrHF2 ! = SQRT( 1.0_DbKi + VFOvrHF2 ) + REAL(DbKi) :: SQRT1VFMinWLOvrHF2 ! = SQRT( 1.0_DbKi + VFMinWLOvrHF2 ) + REAL(DbKi) :: SQRT1VFMinWLsOvrHF2 ! = SQRT( 1.0_DbKi + VFMinWLsOvrHF*VFMinWLsOvrHF ) + REAL(DbKi) :: Te (N) ! Effective line tensions at each node (N) + REAL(DbKi) :: Tol ! Convergence tolerance within Newton-Raphson iteration specified as a fraction of tension (-) + REAL(DbKi) :: VA ! Effective vertical tension in line at the anchor (N) + REAL(DbKi) :: VF ! Effective vertical tension in line at the fairlead (N) + REAL(DbKi) :: VFMinWL ! = VF - WL + REAL(DbKi) :: VFMinWLOvrHF ! = VFMinWL/HF + REAL(DbKi) :: VFMinWLOvrHF2 ! = VFMinWLOvrHF*VFMinWLOvrHF + REAL(DbKi) :: VFMinWLs ! = VFMinWL + Ws + REAL(DbKi) :: VFMinWLsOvrHF ! = VFMinWLs/HF + REAL(DbKi) :: VFOvrHF ! = VF/HF + REAL(DbKi) :: VFOvrHF2 ! = VFOvrHF*VFOvrHF + REAL(DbKi) :: VFOvrWEA ! = VF/WEA + REAL(DbKi) :: W ! Weight of line in fluid per unit length (N/m) + REAL(DbKi) :: WEA ! = W*EA + REAL(DbKi) :: WL ! Total weight of line in fluid (N): W*L + REAL(DbKi) :: Ws ! = W*s(I) + REAL(DbKi) :: X (N) ! Horizontal locations of each line node relative to the anchor (meters) + REAL(DbKi) :: XF ! Horizontal distance between anchor and fairlead (meters) + REAL(DbKi) :: XF2 ! = XF*XF + REAL(DbKi) :: Z (N) ! Vertical locations of each line node relative to the anchor (meters) + REAL(DbKi) :: ZF ! Vertical distance between anchor and fairlead (meters) + REAL(DbKi) :: ZF2 ! = ZF*ZF + + INTEGER(4) :: I ! Index for counting iterations or looping through line nodes (-) + INTEGER(4) :: MaxIter ! Maximum number of Newton-Raphson iterations possible before giving up (-) + + LOGICAL :: FirstIter ! Flag to determine whether or not this is the first time through the Newton-Raphson interation (flag) + + + ErrStat = ERrId_None + + + ! The Newton-Raphson iteration is only accurate in double precision, so + ! convert the input arguments into double precision: + + CB = REAL( CB_In , DbKi ) + EA = REAL( EA_In , DbKi ) + HF = 0.0_DbKi ! = REAL( HF_In , DbKi ) + L = REAL( L_In , DbKi ) + s (:) = REAL( s_In (:), DbKi ) + Tol = REAL( Tol_In , DbKi ) + VF = 0.0_DbKi ! keeping this for some error catching functionality? (at first glance) ! VF = REAL( VF_In , DbKi ) + W = REAL( W_In , DbKi ) + XF = REAL( XF_In , DbKi ) + ZF = REAL( ZF_In , DbKi ) + + + + ! HF and VF cannot be initialized to zero when a portion of the line rests on the seabed and the anchor tension is nonzero + + ! Generate the initial guess values for the horizontal and vertical tensions + ! at the fairlead in the Newton-Raphson iteration for the catenary mooring + ! line solution. Use starting values documented in: Peyrot, Alain H. and + ! Goulois, A. M., "Analysis Of Cable Structures," Computers & Structures, + ! Vol. 10, 1979, pp. 805-813: + XF2 = XF*XF + ZF2 = ZF*ZF + + IF ( XF == 0.0_DbKi ) THEN ! .TRUE. if the current mooring line is exactly vertical + Lamda0 = 1.0D+06 + ELSEIF ( L <= SQRT( XF2 + ZF2 ) ) THEN ! .TRUE. if the current mooring line is taut + Lamda0 = 0.2_DbKi + ELSE ! The current mooring line must be slack and not vertical + Lamda0 = SQRT( 3.0_DbKi*( ( L**2 - ZF2 )/XF2 - 1.0_DbKi ) ) + ENDIF + + HF = ABS( 0.5_DbKi*W* XF/ Lamda0 ) + VF = 0.5_DbKi*W*( ZF/TANH(Lamda0) + L ) + + + ! Abort when there is no solution or when the only possible solution is + ! illogical: + + IF ( Tol <= EPSILON(TOL) ) THEN ! .TRUE. when the convergence tolerance is specified incorrectly + ErrStat = ErrID_Warn + ErrMsg = ' Convergence tolerance must be greater than zero in routine Catenary().' + return + ELSEIF ( XF < 0.0_DbKi ) THEN ! .TRUE. only when the local coordinate system is not computed correctly + ErrStat = ErrID_Warn + ErrMsg = ' The horizontal distance between an anchor and its'// & + ' fairlead must not be less than zero in routine Catenary().' + return + + ELSEIF ( ZF < 0.0_DbKi ) THEN ! .TRUE. if the fairlead has passed below its anchor + ErrStat = ErrID_Warn + ErrMsg = " A line's fairlead is defined as below its anchor. You may need to swap a line's fairlead and anchor end nodes." + return + + ELSEIF ( L <= 0.0_DbKi ) THEN ! .TRUE. when the unstretched line length is specified incorrectly + ErrStat = ErrID_Warn + ErrMsg = ' Unstretched length of line must be greater than zero in routine Catenary().' + return + + ELSEIF ( EA <= 0.0_DbKi ) THEN ! .TRUE. when the unstretched line length is specified incorrectly + ErrStat = ErrID_Warn + ErrMsg = ' Extensional stiffness of line must be greater than zero in routine Catenary().' + return + + ELSEIF ( W == 0.0_DbKi ) THEN ! .TRUE. when the weight of the line in fluid is zero so that catenary solution is ill-conditioned + ErrStat = ErrID_Warn + ErrMsg = ' The weight of the line in fluid must not be zero. '// & + ' Routine Catenary() cannot solve quasi-static mooring line solution.' + return + + + ELSEIF ( W > 0.0_DbKi ) THEN ! .TRUE. when the line will sink in fluid + + LMax = XF - EA/W + SQRT( (EA/W)*(EA/W) + 2.0_DbKi*ZF*EA/W ) ! Compute the maximum stretched length of the line with seabed interaction beyond which the line would have to double-back on itself; here the line forms an "L" between the anchor and fairlead (i.e. it is horizontal along the seabed from the anchor, then vertical to the fairlead) + + IF ( ( L >= LMax ) .AND. ( CB >= 0.0_DbKi ) ) then ! .TRUE. if the line is as long or longer than its maximum possible value with seabed interaction + ErrStat = ErrID_Warn + ErrMsg = ' Unstretched mooring line length too large. '// & + ' Routine Catenary() cannot solve quasi-static mooring line solution.' + return + END IF + + ENDIF + + + ! Initialize some commonly used terms that don't depend on the iteration: + + WL = W *L + WEA = W *EA + LOvrEA = L /EA + CBOvrEA = CB /EA + MaxIter = INT(1.0_DbKi/Tol) ! Smaller tolerances may take more iterations, so choose a maximum inversely proportional to the tolerance + + + + ! To avoid an ill-conditioned situation, ensure that the initial guess for + ! HF is not less than or equal to zero. Similarly, avoid the problems + ! associated with having exactly vertical (so that HF is zero) or exactly + ! horizontal (so that VF is zero) lines by setting the minimum values + ! equal to the tolerance. This prevents us from needing to implement + ! the known limiting solutions for vertical or horizontal lines (and thus + ! complicating this routine): + + HF = MAX( HF, Tol ) + XF = MAX( XF, Tol ) + ZF = MAX( ZF, TOl ) + + + + ! Solve the analytical, static equilibrium equations for a catenary (or + ! taut) mooring line with seabed interaction: + + ! Begin Newton-Raphson iteration: + + I = 1 ! Initialize iteration counter + FirstIter = .TRUE. ! Initialize iteration flag + + DO + + + ! Initialize some commonly used terms that depend on HF and VF: + + VFMinWL = VF - WL + LMinVFOvrW = L - VF/W + HFOvrW = HF/W + HFOvrWEA = HF/WEA + VFOvrWEA = VF/WEA + VFOvrHF = VF/HF + VFMinWLOvrHF = VFMinWL/HF + VFOvrHF2 = VFOvrHF *VFOvrHF + VFMinWLOvrHF2 = VFMinWLOvrHF*VFMinWLOvrHF + SQRT1VFOvrHF2 = SQRT( 1.0_DbKi + VFOvrHF2 ) + SQRT1VFMinWLOvrHF2 = SQRT( 1.0_DbKi + VFMinWLOvrHF2 ) + + + ! Compute the error functions (to be zeroed) and the Jacobian matrix + ! (these depend on the anticipated configuration of the mooring line): + + IF ( ( CB < 0.0_DbKi ) .OR. ( W < 0.0_DbKi ) .OR. ( VFMinWL > 0.0_DbKi ) ) THEN ! .TRUE. when no portion of the line rests on the seabed + + EXF = ( LOG( VFOvrHF + SQRT1VFOvrHF2 ) & + - LOG( VFMinWLOvrHF + SQRT1VFMinWLOvrHF2 ) )*HFOvrW & + + LOvrEA* HF - XF + EZF = ( SQRT1VFOvrHF2 & + - SQRT1VFMinWLOvrHF2 )*HFOvrW & + + LOvrEA*( VF - 0.5_DbKi*WL ) - ZF + + dXFdHF = ( LOG( VFOvrHF + SQRT1VFOvrHF2 ) & + - LOG( VFMinWLOvrHF + SQRT1VFMinWLOvrHF2 ) )/ W & + - ( ( VFOvrHF + VFOvrHF2 /SQRT1VFOvrHF2 )/( VFOvrHF + SQRT1VFOvrHF2 ) & + - ( VFMinWLOvrHF + VFMinWLOvrHF2/SQRT1VFMinWLOvrHF2 )/( VFMinWLOvrHF + SQRT1VFMinWLOvrHF2 ) )/ W & + + LOvrEA + dXFdVF = ( ( 1.0_DbKi + VFOvrHF /SQRT1VFOvrHF2 )/( VFOvrHF + SQRT1VFOvrHF2 ) & + - ( 1.0_DbKi + VFMinWLOvrHF /SQRT1VFMinWLOvrHF2 )/( VFMinWLOvrHF + SQRT1VFMinWLOvrHF2 ) )/ W + dZFdHF = ( SQRT1VFOvrHF2 & + - SQRT1VFMinWLOvrHF2 )/ W & + - ( VFOvrHF2 /SQRT1VFOvrHF2 & + - VFMinWLOvrHF2/SQRT1VFMinWLOvrHF2 )/ W + dZFdVF = ( VFOvrHF /SQRT1VFOvrHF2 & + - VFMinWLOvrHF /SQRT1VFMinWLOvrHF2 )/ W & + + LOvrEA + + + ELSEIF ( -CB*VFMinWL < HF ) THEN ! .TRUE. when a portion of the line rests on the seabed and the anchor tension is nonzero + + EXF = LOG( VFOvrHF + SQRT1VFOvrHF2 ) *HFOvrW & + - 0.5_DbKi*CBOvrEA*W* LMinVFOvrW*LMinVFOvrW & + + LOvrEA* HF + LMinVFOvrW - XF + EZF = ( SQRT1VFOvrHF2 - 1.0_DbKi )*HFOvrW & + + 0.5_DbKi*VF*VFOvrWEA - ZF + + dXFdHF = LOG( VFOvrHF + SQRT1VFOvrHF2 ) / W & + - ( ( VFOvrHF + VFOvrHF2 /SQRT1VFOvrHF2 )/( VFOvrHF + SQRT1VFOvrHF2 ) )/ W & + + LOvrEA + dXFdVF = ( ( 1.0_DbKi + VFOvrHF /SQRT1VFOvrHF2 )/( VFOvrHF + SQRT1VFOvrHF2 ) )/ W & + + CBOvrEA*LMinVFOvrW - 1.0_DbKi/W + dZFdHF = ( SQRT1VFOvrHF2 - 1.0_DbKi & + - VFOvrHF2 /SQRT1VFOvrHF2 )/ W + dZFdVF = ( VFOvrHF /SQRT1VFOvrHF2 )/ W & + + VFOvrWEA + + + ELSE ! 0.0_DbKi < HF <= -CB*VFMinWL ! A portion of the line must rest on the seabed and the anchor tension is zero + + EXF = LOG( VFOvrHF + SQRT1VFOvrHF2 ) *HFOvrW & + - 0.5_DbKi*CBOvrEA*W*( LMinVFOvrW*LMinVFOvrW - ( LMinVFOvrW - HFOvrW/CB )*( LMinVFOvrW - HFOvrW/CB ) ) & + + LOvrEA* HF + LMinVFOvrW - XF + EZF = ( SQRT1VFOvrHF2 - 1.0_DbKi )*HFOvrW & + + 0.5_DbKi*VF*VFOvrWEA - ZF + + dXFdHF = LOG( VFOvrHF + SQRT1VFOvrHF2 ) / W & + - ( ( VFOvrHF + VFOvrHF2 /SQRT1VFOvrHF2 )/( VFOvrHF + SQRT1VFOvrHF2 ) )/ W & + + LOvrEA - ( LMinVFOvrW - HFOvrW/CB )/EA + dXFdVF = ( ( 1.0_DbKi + VFOvrHF /SQRT1VFOvrHF2 )/( VFOvrHF + SQRT1VFOvrHF2 ) )/ W & + + HFOvrWEA - 1.0_DbKi/W + dZFdHF = ( SQRT1VFOvrHF2 - 1.0_DbKi & + - VFOvrHF2 /SQRT1VFOvrHF2 )/ W + dZFdVF = ( VFOvrHF /SQRT1VFOvrHF2 )/ W & + + VFOvrWEA + + + ENDIF + + + ! Compute the determinant of the Jacobian matrix and the incremental + ! tensions predicted by Newton-Raphson: + + + DET = dXFdHF*dZFdVF - dXFdVF*dZFdHF + + if ( EqualRealNos( DET, 0.0_DbKi ) ) then +!bjj: there is a serious problem with the debugger here when DET = 0 + ErrStat = ErrID_Warn + ErrMsg = ' Iteration not convergent (DET is 0). '// & + ' Routine Catenary() cannot solve quasi-static mooring line solution.' + return + endif + + + dHF = ( -dZFdVF*EXF + dXFdVF*EZF )/DET ! This is the incremental change in horizontal tension at the fairlead as predicted by Newton-Raphson + dVF = ( dZFdHF*EXF - dXFdHF*EZF )/DET ! This is the incremental change in vertical tension at the fairlead as predicted by Newton-Raphson + + dHF = dHF*( 1.0_DbKi - Tol*I ) ! Reduce dHF by factor (between 1 at I = 1 and 0 at I = MaxIter) that reduces linearly with iteration count to ensure that we converge on a solution even in the case were we obtain a nonconvergent cycle about the correct solution (this happens, for example, if we jump to quickly between a taut and slack catenary) + dVF = dVF*( 1.0_DbKi - Tol*I ) ! Reduce dHF by factor (between 1 at I = 1 and 0 at I = MaxIter) that reduces linearly with iteration count to ensure that we converge on a solution even in the case were we obtain a nonconvergent cycle about the correct solution (this happens, for example, if we jump to quickly between a taut and slack catenary) + + dHF = MAX( dHF, ( Tol - 1.0_DbKi )*HF ) ! To avoid an ill-conditioned situation, make sure HF does not go less than or equal to zero by having a lower limit of Tol*HF [NOTE: the value of dHF = ( Tol - 1.0_DbKi )*HF comes from: HF = HF + dHF = Tol*HF when dHF = ( Tol - 1.0_DbKi )*HF] + + ! Check if we have converged on a solution, or restart the iteration, or + ! Abort if we cannot find a solution: + + IF ( ( ABS(dHF) <= ABS(Tol*HF) ) .AND. ( ABS(dVF) <= ABS(Tol*VF) ) ) THEN ! .TRUE. if we have converged; stop iterating! [The converge tolerance, Tol, is a fraction of tension] + + EXIT + + + ELSEIF ( ( I == MaxIter ) .AND. ( FirstIter ) ) THEN ! .TRUE. if we've iterated MaxIter-times for the first time; + + ! Perhaps we failed to converge because our initial guess was too far off. + ! (This could happen, for example, while linearizing a model via large + ! pertubations in the DOFs.) Instead, use starting values documented in: + ! Peyrot, Alain H. and Goulois, A. M., "Analysis Of Cable Structures," + ! Computers & Structures, Vol. 10, 1979, pp. 805-813: + ! NOTE: We don't need to check if the current mooring line is exactly + ! vertical (i.e., we don't need to check if XF == 0.0), because XF is + ! limited by the tolerance above. + + XF2 = XF*XF + ZF2 = ZF*ZF + + IF ( L <= SQRT( XF2 + ZF2 ) ) THEN ! .TRUE. if the current mooring line is taut + Lamda0 = 0.2_DbKi + ELSE ! The current mooring line must be slack and not vertical + Lamda0 = SQRT( 3.0_DbKi*( ( L*L - ZF2 )/XF2 - 1.0_DbKi ) ) + ENDIF + + HF = MAX( ABS( 0.5_DbKi*W* XF/ Lamda0 ), Tol ) ! As above, set the lower limit of the guess value of HF to the tolerance + VF = 0.5_DbKi*W*( ZF/TANH(Lamda0) + L ) + + + ! Restart Newton-Raphson iteration: + + I = 0 + FirstIter = .FALSE. + dHF = 0.0_DbKi + dVF = 0.0_DbKi + + + ELSEIF ( ( I == MaxIter ) .AND. ( .NOT. FirstIter ) ) THEN ! .TRUE. if we've iterated as much as we can take without finding a solution; Abort + ErrStat = ErrID_Warn + ErrMsg = ' Iteration not convergent. '// & + ' Routine Catenary() cannot solve quasi-static mooring line solution.' + RETURN + + + ENDIF + + + ! Increment fairlead tensions and iteration counter so we can try again: + + HF = HF + dHF + VF = VF + dVF + + I = I + 1 + + + ENDDO + + + + ! We have found a solution for the tensions at the fairlead! + + ! Now compute the tensions at the anchor and the line position and tension + ! at each node (again, these depend on the configuration of the mooring + ! line): + + IF ( ( CB < 0.0_DbKi ) .OR. ( W < 0.0_DbKi ) .OR. ( VFMinWL > 0.0_DbKi ) ) THEN ! .TRUE. when no portion of the line rests on the seabed + + ! Anchor tensions: + + HA = HF + VA = VFMinWL + + + ! Line position and tension at each node: + + DO I = 1,N ! Loop through all nodes where the line position and tension are to be computed + + IF ( ( s(I) < 0.0_DbKi ) .OR. ( s(I) > L ) ) THEN + ErrStat = ErrID_Warn + ErrMsg = ' All line nodes must be located between the anchor ' & + //'and fairlead (inclusive) in routine Catenary().' + RETURN + END IF + + Ws = W *s(I) ! Initialize + VFMinWLs = VFMinWL + Ws ! some commonly + VFMinWLsOvrHF = VFMinWLs/HF ! used terms + sOvrEA = s(I) /EA ! that depend + SQRT1VFMinWLsOvrHF2 = SQRT( 1.0_DbKi + VFMinWLsOvrHF*VFMinWLsOvrHF ) ! on s(I) + + X (I) = ( LOG( VFMinWLsOvrHF + SQRT1VFMinWLsOvrHF2 ) & + - LOG( VFMinWLOvrHF + SQRT1VFMinWLOvrHF2 ) )*HFOvrW & + + sOvrEA* HF + Z (I) = ( SQRT1VFMinWLsOvrHF2 & + - SQRT1VFMinWLOvrHF2 )*HFOvrW & + + sOvrEA*( VFMinWL + 0.5_DbKi*Ws ) + Te(I) = SQRT( HF*HF + VFMinWLs*VFMinWLs ) + + ENDDO ! I - All nodes where the line position and tension are to be computed + + + ELSEIF ( -CB*VFMinWL < HF ) THEN ! .TRUE. when a portion of the line rests on the seabed and the anchor tension is nonzero + + ! Anchor tensions: + + HA = HF + CB*VFMinWL + VA = 0.0_DbKi + + + ! Line position and tension at each node: + + DO I = 1,N ! Loop through all nodes where the line position and tension are to be computed + + IF ( ( s(I) < 0.0_DbKi ) .OR. ( s(I) > L ) ) THEN + ErrStat = ErrID_Warn + ErrMsg = ' All line nodes must be located between the anchor ' & + //'and fairlead (inclusive) in routine Catenary().' + RETURN + END IF + + Ws = W *s(I) ! Initialize + VFMinWLs = VFMinWL + Ws ! some commonly + VFMinWLsOvrHF = VFMinWLs/HF ! used terms + sOvrEA = s(I) /EA ! that depend + SQRT1VFMinWLsOvrHF2 = SQRT( 1.0_DbKi + VFMinWLsOvrHF*VFMinWLsOvrHF ) ! on s(I) + + IF ( s(I) <= LMinVFOvrW ) THEN ! .TRUE. if this node rests on the seabed and the tension is nonzero + + X (I) = s(I) & + + sOvrEA*( HF + CB*VFMinWL + 0.5_DbKi*Ws*CB ) + Z (I) = 0.0_DbKi + Te(I) = HF + CB*VFMinWLs + + ELSE ! LMinVFOvrW < s <= L ! This node must be above the seabed + + X (I) = LOG( VFMinWLsOvrHF + SQRT1VFMinWLsOvrHF2 ) *HFOvrW & + + sOvrEA* HF + LMinVFOvrW - 0.5_DbKi*CB*VFMinWL*VFMinWL/WEA + Z (I) = ( - 1.0_DbKi + SQRT1VFMinWLsOvrHF2 )*HFOvrW & + + sOvrEA*( VFMinWL + 0.5_DbKi*Ws ) + 0.5_DbKi* VFMinWL*VFMinWL/WEA + Te(I) = SQRT( HF*HF + VFMinWLs*VFMinWLs ) + + ENDIF + + ENDDO ! I - All nodes where the line position and tension are to be computed + + + ELSE ! 0.0_DbKi < HF <= -CB*VFMinWL ! A portion of the line must rest on the seabed and the anchor tension is zero + + ! Anchor tensions: + + HA = 0.0_DbKi + VA = 0.0_DbKi + + + ! Line position and tension at each node: + + DO I = 1,N ! Loop through all nodes where the line position and tension are to be computed + + IF ( ( s(I) < 0.0_DbKi ) .OR. ( s(I) > L ) ) THEN + ErrStat = ErrID_Warn + ErrMsg = ' All line nodes must be located between the anchor ' & + //'and fairlead (inclusive) in routine Catenary().' + RETURN + END IF + + Ws = W *s(I) ! Initialize + VFMinWLs = VFMinWL + Ws ! some commonly + VFMinWLsOvrHF = VFMinWLs/HF ! used terms + sOvrEA = s(I) /EA ! that depend + SQRT1VFMinWLsOvrHF2 = SQRT( 1.0_DbKi + VFMinWLsOvrHF*VFMinWLsOvrHF ) ! on s(I) + + IF ( s(I) <= LMinVFOvrW - HFOvrW/CB ) THEN ! .TRUE. if this node rests on the seabed and the tension is zero + + X (I) = s(I) + Z (I) = 0.0_DbKi + Te(I) = 0.0_DbKi + + ELSEIF ( s(I) <= LMinVFOvrW ) THEN ! .TRUE. if this node rests on the seabed and the tension is nonzero + + X (I) = s(I) - ( LMinVFOvrW - 0.5_DbKi*HFOvrW/CB )*HF/EA & + + sOvrEA*( HF + CB*VFMinWL + 0.5_DbKi*Ws*CB ) + 0.5_DbKi*CB*VFMinWL*VFMinWL/WEA + Z (I) = 0.0_DbKi + Te(I) = HF + CB*VFMinWLs + + ELSE ! LMinVFOvrW < s <= L ! This node must be above the seabed + + X (I) = LOG( VFMinWLsOvrHF + SQRT1VFMinWLsOvrHF2 ) *HFOvrW & + + sOvrEA* HF + LMinVFOvrW - ( LMinVFOvrW - 0.5_DbKi*HFOvrW/CB )*HF/EA + Z (I) = ( - 1.0_DbKi + SQRT1VFMinWLsOvrHF2 )*HFOvrW & + + sOvrEA*( VFMinWL + 0.5_DbKi*Ws ) + 0.5_DbKi* VFMinWL*VFMinWL/WEA + Te(I) = SQRT( HF*HF + VFMinWLs*VFMinWLs ) + + ENDIF + + ENDDO ! I - All nodes where the line position and tension are to be computed + + + ENDIF + + + + ! The Newton-Raphson iteration is only accurate in double precision, so + ! convert the output arguments back into the default precision for real + ! numbers: + + !HA_In = REAL( HA , DbKi ) !mth: for this I only care about returning node positions + !HF_In = REAL( HF , DbKi ) + !Te_In(:) = REAL( Te(:), DbKi ) + !VA_In = REAL( VA , DbKi ) + !VF_In = REAL( VF , DbKi ) + X_In (:) = REAL( X (:), DbKi ) + Z_In (:) = REAL( Z (:), DbKi ) + + END SUBROUTINE Catenary + !----------------------------------------------------------------------- + + + END SUBROUTINE Line_Initialize + !-------------------------------------------------------------- + + + !-------------------------------------------------------------- + SUBROUTINE Line_SetState(Line, X, t) + + TYPE(MD_Line), INTENT(INOUT) :: Line ! the current Line object + Real(DbKi), INTENT(IN ) :: X(:) ! state vector section for this line + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + + INTEGER(IntKi) :: i ! index of segments or nodes along line + INTEGER(IntKi) :: J ! index + + + ! store current time + Line%time = t + + ! set interior node positions and velocities based on state vector + DO I=1,Line%N-1 + DO J=1,3 + + Line%r( J,I) = X( 3*Line%N-3 + 3*I-3 + J) ! get positions + Line%rd(J,I) = X( 3*I-3 + J) ! get velocities + + END DO + END DO + + ! if using viscoelastic model, also set the static stiffness stretch + if (Line%ElasticMod == 2) then + do I=1,Line%N + Line%dl_1(I) = X( 6*Line%N-6 + I) ! these will be the last N entries in the state vector + end do + end if + + END SUBROUTINE Line_SetState + !-------------------------------------------------------------- + + !-------------------------------------------------------------- + SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, AnchMtot) + + TYPE(MD_Line), INTENT(INOUT) :: Line ! the current Line object + Real(DbKi), INTENT(INOUT) :: Xd(:) ! state derivative vector section for this line + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + TYPE(MD_ParameterType), INTENT(IN ) :: p ! Parameters + + ! Real(DbKi), INTENT( IN ) :: X(:) ! state vector, provided + ! Real(DbKi), INTENT( INOUT ) :: Xd(:) ! derivative of state vector, returned ! cahnged to INOUT + ! Real(DbKi), INTENT (IN) :: t ! instantaneous time + ! TYPE(MD_Line), INTENT (INOUT) :: Line ! label for the current line, for convenience + ! TYPE(MD_LineProp), INTENT(IN) :: LineProp ! the single line property set for the line of interest + ! Real(DbKi), INTENT(INOUT) :: FairFtot(:) ! total force on Connect top of line is attached to + ! Real(DbKi), INTENT(INOUT) :: FairMtot(:,:) ! total mass of Connect top of line is attached to + ! Real(DbKi), INTENT(INOUT) :: AnchFtot(:) ! total force on Connect bottom of line is attached to + ! Real(DbKi), INTENT(INOUT) :: AnchMtot(:,:) ! total mass of Connect bottom of line is attached to + + + INTEGER(IntKi) :: i ! index of segments or nodes along line + INTEGER(IntKi) :: J ! index + INTEGER(IntKi) :: K ! index + INTEGER(IntKi) :: N ! number of segments in line + Real(DbKi) :: d ! line diameter + Real(DbKi) :: rho ! line material density [kg/m^3] + Real(DbKi) :: Sum1 ! for summing squares + Real(DbKi) :: dummyLength ! + Real(DbKi) :: m_i ! node mass + Real(DbKi) :: v_i ! node submerged volume + Real(DbKi) :: Vi(3) ! relative water velocity at a given node + Real(DbKi) :: Vp(3) ! transverse relative water velocity component at a given node + Real(DbKi) :: Vq(3) ! tangential relative water velocity component at a given node + Real(DbKi) :: SumSqVp ! + Real(DbKi) :: SumSqVq ! + Real(DbKi) :: MagVp ! + Real(DbKi) :: MagVq ! + Real(DbKi) :: MagT ! tension stiffness force magnitude + Real(DbKi) :: MagTd ! tension damping force magnitude + Real(DbKi) :: Xi ! used in interpolating from lookup table + Real(DbKi) :: Yi ! used in interpolating from lookup table + Real(DbKi) :: dl ! stretch of a segment [m] + Real(DbKi) :: ld_1 ! rate of change of static stiffness portion of segment [m/s] + Real(DbKi) :: EA_1 ! stiffness of 'static stiffness' portion of segment, combines with dynamic stiffness to give static stiffnes [m/s] + + Real(DbKi) :: depth ! local water depth interpolated from bathymetry grid + + + N = Line%N ! for convenience + d = Line%d + rho = Line%rho + + ! note that end node kinematics should have already been set by attached objects + + ! ! set end node positions and velocities from connect objects' states + ! DO J = 1, 3 + ! Line%r( J,N) = m%ConnectList(Line%FairConnect)%r(J) + ! Line%r( J,0) = m%ConnectList(Line%AnchConnect)%r(J) + ! Line%rd(J,N) = m%ConnectList(Line%FairConnect)%rd(J) + ! Line%rd(J,0) = m%ConnectList(Line%AnchConnect)%rd(J) + ! END DO + + + + ! -------------------- calculate various kinematic quantities --------------------------- + DO I = 1, N + + + ! calculate current (Stretched) segment lengths and unit tangent vectors (qs) for each segment (this is used for bending calculations) + CALL UnitVector(Line%r(:,I-1), Line%r(:,I), Line%qs(:,I), Line%lstr(I)) + + ! should add catch here for if lstr is ever zero + + Sum1 = 0.0_DbKi + DO J = 1, 3 + Sum1 = Sum1 + (Line%r(J,I) - Line%r(J,I-1))*(Line%rd(J,I) - Line%rd(J,I-1)) + END DO + Line%lstrd(I) = Sum1/Line%lstr(I) ! segment stretched length rate of change + + ! Line%V(I) = Pi/4.0 * d*d*Line%l(I) !volume attributed to segment + END DO + + !calculate unit tangent vectors (q) for each node (including ends) note: I think these are pointing toward 0 rather than N! + CALL UnitVector(Line%r(:,0), Line%r(:,1), Line%q(:,0), dummyLength) ! compute unit vector q + DO I = 1, N-1 + CALL UnitVector(Line%r(:,I-1), Line%r(:,I+1), Line%q(:,I), dummyLength) ! compute unit vector q ... using adjacent two nodes! + END DO + CALL UnitVector(Line%r(:,N-1), Line%r(:,N), Line%q(:,N), dummyLength) ! compute unit vector q + + + ! --------------------------------- apply wave kinematics ------------------------------------ + + ! IF (p%WaterKin > 0) THEN ! wave kinematics interpolated from global grid in Waves object + ! DO i=0,N + ! CALL getWaveKin(p, Line%r(1,i), Line%r(2,i), Line%r(3,i), Line%time, m%WaveTi, Line%U(:,i), Line%Ud(:,i), Line%zeta(i), Line%PDyn(i)) + ! END DO + ! END IF + + + ! --------------- calculate mass (including added mass) matrix for each node ----------------- + DO I = 0, N + IF (I==0) THEN + m_i = Pi/8.0 *d*d*Line%l(1)*rho + v_i = 0.5 *Line%V(1) + ELSE IF (I==N) THEN + m_i = pi/8.0 *d*d*Line%l(N)*rho; + v_i = 0.5*Line%V(N) + ELSE + m_i = pi/8.0 * d*d*rho*(Line%l(I) + Line%l(I+1)) + v_i = 0.5 *(Line%V(I) + Line%V(I+1)) + END IF + + DO J=1,3 + DO K=1,3 + IF (J==K) THEN + Line%M(K,J,I) = m_i + p%rhoW*v_i*( Line%Can*(1 - Line%q(J,I)*Line%q(K,I)) + Line%Cat*Line%q(J,I)*Line%q(K,I) ) + ELSE + Line%M(K,J,I) = p%rhoW*v_i*( Line%Can*(-Line%q(J,I)*Line%q(K,I)) + Line%Cat*Line%q(J,I)*Line%q(K,I) ) + END IF + END DO + END DO + + CALL Inverse3by3(Line%S(:,:,I), Line%M(:,:,I)) ! invert mass matrix + END DO + + + ! ------------------ CALCULATE FORCES ON EACH NODE ---------------------------- + + ! loop through the segments + DO I = 1, N + + ! handle nonlinear stiffness if needed + if (Line%nEApoints > 0) then + + Xi = Line%l(I)/Line%lstr(I) - 1.0 ! strain rate based on inputs + Yi = 0.0_DbKi + + ! find stress based on strain + if (Xi < 0.0) then ! if negative strain (compression), zero stress + Yi = 0.0_DbKi + else if (Xi < Line%stiffXs(1)) then ! if strain below first data point, interpolate from zero + Yi = Xi * Line%stiffYs(1)/Line%stiffXs(1) + else if (Xi >= Line%stiffXs(Line%nEApoints)) then ! if strain exceeds last data point, use last data point + Yi = Line%stiffYs(Line%nEApoints) + else ! otherwise we're in range of the table so interpolate! + do J=1, Line%nEApoints-1 ! go through lookup table until next entry exceeds inputted strain rate + if (Line%stiffXs(J+1) > Xi) then + Yi = Line%stiffYs(J) + (Xi-Line%stiffXs(J)) * (Line%stiffYs(J+1)-Line%stiffYs(J))/(Line%stiffXs(J+1)-Line%stiffXs(J)) + exit + end if + end do + end if + + ! calculate a young's modulus equivalent value based on stress/strain + Line%EA = Yi/Xi + end if + + + ! >>>> could do similar as above for nonlinear damping or bending stiffness <<<< + if (Line%nBApoints > 0) print *, 'Nonlinear elastic damping not yet implemented' + if (Line%nEIpoints > 0) print *, 'Nonlinear bending stiffness not yet implemented' + + + ! basic elasticity model + if (Line%ElasticMod == 1) then + ! line tension, inherently including possibility of dynamic length changes in l term + if (Line%lstr(I)/Line%l(I) > 1.0) then + MagT = Line%EA *( Line%lstr(I)/Line%l(I) - 1.0 ) + else + MagT = 0.0_DbKi ! cable can't "push" + end if + ! line internal damping force based on line-specific BA value, including possibility of dynamic length changes in l and ld terms + MagTd = Line%BA* ( Line%lstrd(I) - Line%lstr(I)*Line%ld(I)/Line%l(I) )/Line%l(I) + + ! viscoelastic model + else if (Line%ElasticMod == 2) then + + EA_1 = Line%EA_D*Line%EA/(Line%EA_D - Line%EA)! calculated EA_1 which is the stiffness in series with EA_D that will result in the desired static stiffness of EA_S + + dl = Line%lstr(I) - Line%l(I) ! delta l of this segment + + ld_1 = (Line%EA_D*dl - (Line%EA_D + EA_1)*Line%dl_1(I) + Line%BA_D*Line%lstrd(I)) /( Line%BA_D + Line%BA) ! rate of change of static stiffness portion [m/s] + + !MagT = (Line%EA*Line%dl_S(I) + Line%BA*ld_S)/ Line%l(I) ! compute tension based on static portion (dynamic portion would give same) + MagT = EA_1*Line%dl_1(I)/ Line%l(I) + MagTd = Line%BA*ld_1 / Line%l(I) + + ! update state derivative for static stiffness stretch (last N entries in the state vector) + Xd( 6*N-6 + I) = ld_1 + + end if + + + do J = 1, 3 + !Line%T(J,I) = Line%EA *( 1.0/Line%l(I) - 1.0/Line%lstr(I) ) * (Line%r(J,I)-Line%r(J,I-1)) + Line%T(J,I) = MagT * Line%qs(J,I) + !Line%Td(J,I) = Line%BA* ( Line%lstrd(I) / Line%l(I) ) * (Line%r(J,I)-Line%r(J,I-1)) / Line%lstr(I) ! note new form of damping coefficient, BA rather than Cint + Line%Td(J,I) = MagTd * Line%qs(J,I) + end do + end do + + + ! loop through the nodes + DO I = 0, N + + !submerged weight (including buoyancy) + IF (I==0) THEN + Line%W(3,I) = Pi/8.0*d*d* Line%l(1)*(rho - p%rhoW) *(-p%g) ! assuming g is positive + ELSE IF (i==N) THEN + Line%W(3,I) = pi/8.0*d*d* Line%l(N)*(rho - p%rhoW) *(-p%g) + ELSE + Line%W(3,I) = pi/8.0*d*d* (Line%l(I)*(rho - p%rhoW) + Line%l(I+1)*(rho - p%rhoW) )*(-p%g) ! left in this form for future free surface handling + END IF + + !relative flow velocities + DO J = 1, 3 + Vi(J) = 0.0 - Line%rd(J,I) ! relative flow velocity over node -- this is where wave velicites would be added + END DO + + ! decomponse relative flow into components + SumSqVp = 0.0_DbKi ! start sums of squares at zero + SumSqVq = 0.0_DbKi + DO J = 1, 3 + Vq(J) = DOT_PRODUCT( Vi , Line%q(:,I) ) * Line%q(J,I); ! tangential relative flow component + Vp(J) = Vi(J) - Vq(J) ! transverse relative flow component + SumSqVq = SumSqVq + Vq(J)*Vq(J) + SumSqVp = SumSqVp + Vp(J)*Vp(J) + END DO + MagVp = sqrt(SumSqVp) ! get magnitudes of flow components + MagVq = sqrt(SumSqVq) + + ! transverse and tangenential drag + IF (I==0) THEN + Line%Dp(:,I) = 0.25*p%rhoW*Line%Cdn* d*Line%l(1) * MagVp * Vp + Line%Dq(:,I) = 0.25*p%rhoW*Line%Cdt* Pi*d*Line%l(1) * MagVq * Vq + ELSE IF (I==N) THEN + Line%Dp(:,I) = 0.25*p%rhoW*Line%Cdn* d*Line%l(N) * MagVp * Vp + Line%Dq(:,I) = 0.25*p%rhoW*Line%Cdt* Pi*d*Line%l(N) * MagVq * Vq + ELSE + Line%Dp(:,I) = 0.25*p%rhoW*Line%Cdn* d*(Line%l(I) + Line%l(I+1)) * MagVp * vp + Line%Dq(:,I) = 0.25*p%rhoW*Line%Cdt* Pi*d*(Line%l(I) + Line%l(I+1)) * MagVq * vq + END IF + + ! F-K force from fluid acceleration not implemented yet + + ! bottom contact (stiffness and damping, vertical-only for now) - updated Nov 24 for general case where anchor and fairlead ends may deal with bottom contact forces + + ! interpolate the local depth from the bathymetry grid + CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, Line%r(1,I), Line%r(2,I), depth) + + IF (Line%r(3,I) < -depth) THEN + IF (I==0) THEN + Line%B(3,I) = ( (-depth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*( Line%l(I+1) ) + ELSE IF (I==N) THEN + Line%B(3,I) = ( (-depth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) ) + ELSE + Line%B(3,I) = ( (-depth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) + Line%l(I+1) ) + ! IF (Line%r(3,I) < -p%WtrDpth) THEN + ! IF (I==0) THEN + ! Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*( Line%l(I+1) ) + ! ELSE IF (I==N) THEN + ! Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) ) + ! ELSE + ! Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) + Line%l(I+1) ) + + + + END IF + ELSE + Line%B(3,I) = 0.0_DbKi + END IF + + ! total forces + IF (I==0) THEN + Line%Fnet(:,I) = Line%T(:,1) + Line%Td(:,1) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) + ELSE IF (I==N) THEN + Line%Fnet(:,I) = -Line%T(:,N) - Line%Td(:,N) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) + ELSE + Line%Fnet(:,I) = Line%T(:,I+1) - Line%T(:,I) + Line%Td(:,I+1) - Line%Td(:,I) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) + END IF + + END DO ! I - done looping through nodes + + ! loop through internal nodes and update their states <<< should/could convert to matrix operations instead of all these loops + DO I=1, N-1 + DO J=1,3 + + ! calculate RHS constant (premultiplying force vector by inverse of mass matrix ... i.e. rhs = S*Forces) + Sum1 = 0.0_DbKi ! reset temporary accumulator + DO K = 1, 3 + Sum1 = Sum1 + Line%S(K,J,I) * Line%Fnet(K,I) ! matrix-vector multiplication [S i]{Forces i} << double check indices + END DO ! K + + ! update states + Xd(3*N-3 + 3*I-3 + J) = Line%rd(J,I); ! dxdt = V (velocities) + Xd( 3*I-3 + J) = Sum1 ! dVdt = RHS * A (accelerations) + + END DO ! J + END DO ! I + + + ! check for NaNs + DO J = 1, 6*(N-1) + IF (Is_NaN(Xd(J))) THEN + print *, "NaN detected at time ", Line%time, " in Line ", Line%IdNum, " in MoorDyn." + IF (wordy > 1) THEN + print *, "state derivatives:" + print *, Xd + + + + print *, "m_i p%rhoW v_i Line%Can Line%Cat" + print *, m_i + print *, p%rhoW + print *, v_i + print *, Line%Can + print *, Line%Cat + + print *, "Line%q" + print *, Line%q + + print *, "Line%r" + print *, Line%r + + + print *, "Here is the mass matrix set" + print *, Line%M + + print *, "Here is the inverted mass matrix set" + print *, Line%S + + print *, "Here is the net force set" + print *, Line%Fnet + END IF + + EXIT + END IF + END DO + + + ! ! add force and mass of end nodes to the Connects they correspond to <<<<<<<<<<<< do this from Connection instead now! + ! DO J = 1,3 + ! FairFtot(J) = FairFtot(J) + Line%F(J,N) + ! AnchFtot(J) = AnchFtot(J) + Line%F(J,0) + ! DO K = 1,3 + ! FairMtot(K,J) = FairMtot(K,J) + Line%M(K,J,N) + ! AnchMtot(K,J) = AnchMtot(K,J) + Line%M(K,J,0) + ! END DO + ! END DO + + END SUBROUTINE Line_GetStateDeriv + !===================================================================== + + + !-------------------------------------------------------------- + SUBROUTINE Line_SetEndKinematics(Line, r_in, rd_in, t, topOfLine) + + TYPE(MD_Line), INTENT(INOUT) :: Line ! the current Line object + Real(DbKi), INTENT(IN ) :: r_in( 3) ! state vector section for this line + Real(DbKi), INTENT(IN ) :: rd_in(3) ! state vector section for this line + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + INTEGER(IntKi), INTENT(IN ) :: topOfLine ! 0 for end A (Node 0), 1 for end B (node N) + + Integer(IntKi) :: I,J + INTEGER(IntKi) :: inode + + IF (topOfLine==1) THEN + inode = Line%N + Line%endTypeB = 0 ! set as ball rather than rigid connection (unless changed later by SetEndOrientation) + ELSE + inode = 0 + Line%endTypeA = 0 ! set as ball rather than rigid connection (unless changed later by SetEndOrientation) + END IF + + !Line%r( :,inode) = r_in + !Line%rd(:,inode) = rd_in + + DO J = 1,3 + Line%r( :,inode) = r_in + Line%rd(:,inode) = rd_in + END DO + + ! print *, "SetEndKinematics of line ", Line%idNum, " top?:", topOfLine + ! print *, r_in + ! print *, Line%r( :,inode), " - confirming, node ", inode + ! print *, rd_in + + Line%time = t + + END SUBROUTINE Line_SetEndKinematics + !-------------------------------------------------------------- + + + ! get force, moment, and mass of line at line end node + !-------------------------------------------------------------- + SUBROUTINE Line_GetEndStuff(Line, Fnet_out, Moment_out, M_out, topOfLine) + + TYPE(MD_Line), INTENT(INOUT) :: Line ! label for the current line, for convenience + REAL(DbKi), INTENT( OUT) :: Fnet_out(3) ! net force on end node + REAL(DbKi), INTENT( OUT) :: Moment_out(3) ! moment on end node (future capability) + REAL(DbKi), INTENT( OUT) :: M_out(3,3) ! mass matrix of end node + INTEGER(IntKi), INTENT(IN ) :: topOfLine ! 0 for end A (Node 0), 1 for end B (node N) + + Integer(IntKi) :: I,J + INTEGER(IntKi) :: inode + + IF (topOfLine==1) THEN ! end B of line + Fnet_out = Line%Fnet(:, Line%N) + Moment_out = Line%endMomentB + M_out = Line%M(:,:, Line%N) + ELSE ! end A of line + Fnet_out = Line%Fnet(:, 0) + Moment_out = Line%endMomentA + M_out = Line%M(:,:, 0) + END IF + + END SUBROUTINE Line_GetEndStuff + !-------------------------------------------------------------- + + + ! set end kinematics of a line that's attached to a Rod, and this includes rotational information + !-------------------------------------------------------------- + SUBROUTINE Line_GetEndSegmentInfo(Line, qEnd, EIout, dlOut, topOfLine) + + TYPE(MD_Line), INTENT(INOUT) :: Line ! label for the current line, for convenience + REAL(DbKi), INTENT( OUT) :: qEnd(3) + REAL(DbKi), INTENT( OUT) :: EIout + REAL(DbKi), INTENT( OUT) :: dlOut + INTEGER(IntKi), INTENT(IN ) :: topOfLine ! 0 for end A (Node 0), 1 for end B (node N) + + EIout = Line%EI; + + if (topOfLine==1) then + CALL UnitVector(Line%r(:,Line%N-1), Line%r(:,Line%N), qEnd, dlOut) ! unit vector of last line segment + else + CALL UnitVector(Line%r(:,0 ), Line%r(:,1 ), qEnd, dlOut) ! unit vector of first line segment + END IF + + END SUBROUTINE Line_GetEndSegmentInfo + !-------------------------------------------------------------- + + + ! set end node unit vector of a line (this is called by an attached to a Rod, only applicable for bending stiffness) + !-------------------------------------------------------------- + SUBROUTINE Line_SetEndOrientation(Line, qin, topOfLine, rodEndB) + + TYPE(MD_Line), INTENT(INOUT) :: Line ! label for the current line, for convenience + REAL(DbKi), INTENT(IN ) :: qin(3) ! the rod's axis unit vector + INTEGER(IntKi), INTENT(IN ) :: topOfLine ! 0 for end A (Node 0), 1 for end B (node N) + INTEGER(IntKi), INTENT(IN ) :: rodEndB ! =0 means the line is attached to Rod end A, =1 means attached to Rod end B (implication for unit vector sign) + + if (topOfLine==1) then + + Line%endTypeB = 1 ! indicate attached to Rod (at every time step, just in case line get detached) + + if (rodEndB==1) then + Line%q(:,Line%N) = -qin ! -----line----->[B<==ROD==A] + else + Line%q(:,Line%N) = qin ! -----line----->[A==ROD==>B] + end if + else + + Line%endTypeA = 1 ! indicate attached to Rod (at every time step, just in case line get detached) ! indicate attached to Rod + + if (rodEndB==1) then + Line%q(:,0 ) = qin ! [A==ROD==>B]-----line-----> + else + Line%q(:,0 ) = -qin ! [B<==ROD==A]-----line-----> + end if + end if + + END SUBROUTINE Line_SetEndOrientation + !-------------------------------------------------------------- + + +END MODULE MoorDyn_Line diff --git a/modules/moordyn/src/MoorDyn_Misc.f90 b/modules/moordyn/src/MoorDyn_Misc.f90 new file mode 100644 index 0000000000..1e198a7dad --- /dev/null +++ b/modules/moordyn/src/MoorDyn_Misc.f90 @@ -0,0 +1,1972 @@ +!********************************************************************************************************************************** +! LICENSING +! Copyright (C) 2020-2021 Alliance for Sustainable Energy, LLC +! Copyright (C) 2015-2019 Matthew Hall +! +! This file is part of MoorDyn. +! +! Licensed under the Apache License, Version 2.0 (the "License"); +! you may not use this file except in compliance with the License. +! You may obtain a copy of the License at +! +! http:!www.apache.org/licenses/LICENSE-2.0 +! +! Unless required by applicable law or agreed to in writing, software +! distributed under the License is distributed on an "AS IS" BASIS, +! WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +! See the License for the specific language governing permissions and +! limitations under the License. +! +!********************************************************************************************************************************** +MODULE MoorDyn_Misc + + USE MoorDyn_Types + USE NWTC_Library + USE NWTC_FFTPACK + + IMPLICIT NONE + + PRIVATE + + INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output + + PUBLIC :: UnitVector + PUBLIC :: ScaleVector + PUBLIC :: GetOrientationAngles + PUBLIC :: TransformKinematics + PUBLIC :: TransformKinematicsA + PUBLIC :: TransformKinematicsAtoB + PUBLIC :: TranslateForce3to6DOF + PUBLIC :: TranslateMass3to6DOF + PUBLIC :: TranslateMass6to6DOF + PUBLIC :: GetH + PUBLIC :: RotateM6 + PUBLIC :: RotateM3 + PUBLIC :: CalcOrientation + PUBLIC :: Inverse3by3 + PUBLIC :: LUsolve + + PUBLIC :: getInterpNums + PUBLIC :: calculate4Dinterpolation + PUBLIC :: calculate3Dinterpolation + PUBLIC :: calculate2Dinterpolation + + PUBLIC :: getDepthFromBathymetry + + PUBLIC :: getWaterKin + PUBLIC :: setupWaterKin + +CONTAINS + + + ! ::::::::::::::::::::::::::::::::: math convenience functions :::::::::::::::::::::::::::::::::: + ! should add error checking if I keep these, but hopefully there are existing NWTCLib functions to replace them + + ! return unit vector (u) and in direction from r1 to r2 and distance between points + !----------------------------------------------------------------------- + SUBROUTINE UnitVector( r1, r2, u, Length ) ! note: order of parameters chagned in this function + + REAL(DbKi), INTENT(IN ) :: r1(:) + REAL(DbKi), INTENT(IN ) :: r2(:) + REAL(DbKi), INTENT( OUT) :: u(:) + REAL(DbKi), INTENT( OUT) :: length + + u = r2 - r1 + length = TwoNorm(u) + + if ( .NOT. EqualRealNos(length, 0.0_DbKi ) ) THEN + u = u / Length + END IF + + END SUBROUTINE UnitVector + !----------------------------------------------------------------------- + + ! scale vector to desired length + !----------------------------------------------------------------------- + SUBROUTINE ScaleVector( u_in, newlength, u_out ) + REAL(DbKi), INTENT(IN ) :: u_in(3) ! input vector + REAL(DbKi), INTENT(IN ) :: newlength ! desired length of output vector + REAL(DbKi), INTENT(INOUT) :: u_out(3) ! output vector (hopefully can be the same as u_in without issue) + + REAL(DbKi) :: length_squared + REAL(DbKi) :: scaler + INTEGER(IntKi) :: J + + length_squared = 0.0; + DO J=1,3 + length_squared = length_squared + u_in(J)*u_in(J) + END DO + + if (length_squared > 0) then + scaler = newlength/sqrt(length_squared) + else ! if original vector is zero, return zero + scaler = 0_DbKi + end if + + DO J=1,3 + u_out(J) = u_in(J) * scaler + END DO + + END SUBROUTINE ScaleVector + !----------------------------------------------------------------------- + + + ! calculate orientation angles of a cylindrical object + !----------------------------------------------------------------------- + subroutine GetOrientationAngles(p1, p2, phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat) + real(DbKi), intent(in ) :: p1(3),p2(3) + real(DbKi), intent( out) :: phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat(3) + + real(DbKi) :: vec(3), vecLen, vecLen2D + + ! calculate isntantaneous incline angle and heading, and related trig values + ! the first and last NodeIndx values point to the corresponding Joint nodes idices which are at the start of the Mesh + vec = p2 - p1 + vecLen = SQRT(Dot_Product(vec,vec)) + vecLen2D = SQRT(vec(1)**2+vec(2)**2) + if ( vecLen < 0.000001 ) then + print *, "ERROR in GetOrientationAngles in MoorDyn" !call SeterrStat(ErrID_Fatal, 'An element of the Morison structure has co-located endpoints! This should never occur. Please review your model.', errStat, errMsg, 'Morison_CalcOutput' ) + print *, p1 + print *, p2 + k_hat = NaN ! 1.0/0.0 + else + k_hat = vec / vecLen + phi = atan2(vecLen2D, vec(3)) ! incline angle + end if + if ( phi < 0.000001) then + beta = 0.0_ReKi + else + beta = atan2(vec(2), vec(1)) ! heading of incline + endif + sinPhi = sin(phi) + cosPhi = cos(phi) + tanPhi = tan(phi) + sinBeta = sin(beta) + cosBeta = cos(beta) + + end subroutine GetOrientationAngles + !----------------------------------------------------------------------- + + + ! calculate position and velocity of point based on its position relative to moving 6DOF body + !----------------------------------------------------------------------- + SUBROUTINE TransformKinematics(rRelBody, r_in, TransMat, rd_in, r_out, rd_out) + REAL(DbKi), INTENT(IN ) :: rRelBody(:) ! coordinate of end A + REAL(DbKi), INTENT(IN ) :: r_in(3) ! Rod unit vector + REAL(DbKi), INTENT(IN ) :: TransMat(3,3)! + REAL(DbKi), INTENT(IN ) :: rd_in(6) ! 6DOF velecity vector about Rod end A, in global orientation frame + REAL(DbKi), INTENT( OUT) :: r_out(3) ! coordinates of end B + REAL(DbKi), INTENT( OUT) :: rd_out(3) ! velocity of end B + + REAL(DbKi) :: rRel(3) + + ! rd_in should be in global orientation frame + ! note: it's okay if r_out and rd_out are 6-size. Only the first 3 will be written, and 4-6 will + ! already be correct or can be assigned seperately from r_in and rd_in (assuming orientation frames are identical) + + + ! locations (unrotated reference frame) about platform reference point (2021-01-05: just transposed TransMat, it was incorrect before) + rRel(1) = TransMat(1,1)*rRelBody(1) + TransMat(1,2)*rRelBody(2) + TransMat(1,3)*rRelBody(3) ! x + rRel(2) = TransMat(2,1)*rRelBody(1) + TransMat(2,2)*rRelBody(2) + TransMat(2,3)*rRelBody(3) ! y + rRel(3) = TransMat(3,1)*rRelBody(1) + TransMat(3,2)*rRelBody(2) + TransMat(3,3)*rRelBody(3) ! z + + ! absolute locations + r_out = rRel + r_in + + ! absolute velocities + rd_out(1) = - rd_in(6)*rRel(2) + rd_in(5)*rRel(3) + rd_in(1) ! x + rd_out(2) = rd_in(6)*rRel(1) - rd_in(4)*rRel(3) + rd_in(2) ! y + rd_out(3) = -rd_in(5)*rRel(1) + rd_in(4)*rRel(2) + rd_in(3) ! z + + ! absolute accelerations + rd_out(1) = - rd_in(6)*rRel(2) + rd_in(5)*rRel(3) + rd_in(1) ! x + rd_out(2) = rd_in(6)*rRel(1) - rd_in(4)*rRel(3) + rd_in(2) ! y + rd_out(3) = -rd_in(5)*rRel(1) + rd_in(4)*rRel(2) + rd_in(3) ! z + + + + !rRel = MATMUL(TransMat, rRelBody) + !H = getH(rRel) + !! absolute locations + !r_out = rRel + r_in + !! absolute velocities + !rd_out = MATMUL( H, rd_in(4:6)) + rd_in(1:3) + + + END SUBROUTINE TransformKinematics + !----------------------------------------------------------------------- + + + + ! calculate position, velocity, and acceleration of point based on its position relative to moving 6DOF body + !----------------------------------------------------------------------- + SUBROUTINE TransformKinematicsA(rRelBody, r_in, TransMat, v_in, a_in, r_out, v_out, a_out) + REAL(DbKi), INTENT(IN ) :: rRelBody(:) ! relative location of point about reference point, in local/reference coordinate system + REAL(DbKi), INTENT(IN ) :: r_in(3) ! translation applied to reference point + REAL(DbKi), INTENT(IN ) :: TransMat(3,3)! rotation matrix describing rotation about reference point + REAL(DbKi), INTENT(IN ) :: v_in(6) ! 6DOF velecity vector about ref point in global orientation frame + REAL(DbKi), INTENT(IN ) :: a_in(6) ! 6DOF acceleration vector + REAL(DbKi), INTENT( OUT) :: r_out(3) ! coordinates of point of interest + REAL(DbKi), INTENT( OUT) :: v_out(3) ! velocity of point + REAL(DbKi), INTENT( OUT) :: a_out(3) ! acceleration of point + + REAL(DbKi) :: rRel(3) + REAL(DbKi) :: rRel2(3) + + REAL(DbKi) :: r_out2(3) + REAL(DbKi) :: rd_out2(3) + REAL(DbKi) :: H(3,3) + + ! rd_in should be in global orientation frame + ! note: it's okay if r_out and rd_out are 6-size. Only the first 3 will be written, and 4-6 will + ! already be correct or can be assigned seperately from r_in and rd_in (assuming orientation frames are identical) + + + ! locations about ref point in *unrotated* reference frame + !rRel2(1) = TransMat(1,1)*rRelBody(1) + TransMat(2,1)*rRelBody(2) + TransMat(3,1)*rRelBody(3) ! x + !rRel2(2) = TransMat(1,2)*rRelBody(1) + TransMat(2,2)*rRelBody(2) + TransMat(3,2)*rRelBody(3) ! y + !rRel2(3) = TransMat(1,3)*rRelBody(1) + TransMat(2,3)*rRelBody(2) + TransMat(3,3)*rRelBody(3) ! z + + rRel = MATMUL(TransMat, rRelBody) + + H = getH(rRel) + + ! absolute locations + r_out = rRel + r_in + + ! absolute velocities + !rd_out2(1) = - v_in(6)*rRel(2) + v_in(5)*rRel(3) + v_in(1) ! x + !rd_out2(2) = v_in(6)*rRel(1) - v_in(4)*rRel(3) + v_in(2) ! y + !rd_out2(3) = -v_in(5)*rRel(1) + v_in(4)*rRel(2) + v_in(3) ! z + + v_out = MATMUL( H, v_in(4:6)) + v_in(1:3) + + ! absolute accelerations + a_out = MATMUL( H, a_in(4:6)) + a_in(1:3) ! << should add second order terms! + + + END SUBROUTINE TransformKinematicsA + !----------------------------------------------------------------------- + + ! calculate position and velocity of point along rod (distance L along direction u) + !----------------------------------------------------------------------- + SUBROUTINE TransformKinematicsAtoB(rA, u, L, rd_in, r_out, rd_out) + REAL(DbKi), INTENT(IN ) :: rA(3) ! coordinate of end A + REAL(DbKi), INTENT(IN ) :: u(3) ! Rod unit vector + REAL(DbKi), INTENT(IN ) :: L ! Rod length from end A to B + REAL(DbKi), INTENT(IN ) :: rd_in(6) ! 6DOF velecity vector about Rod end A, in global orientation frame + REAL(DbKi), INTENT( OUT) :: r_out(3) ! coordinates of end B + REAL(DbKi), INTENT( OUT) :: rd_out(3) ! velocity of end B + + REAL(DbKi) :: rRel(3) + + + ! locations (unrotated reference frame) + rRel = L*u ! relative location of point B from point A + r_out = rRel + rA ! absolute location of point B + + ! absolute velocities + rd_out(1) = - rd_in(6)*rRel(2) + rd_in(5)*rRel(3) + rd_in(1) ! x + rd_out(2) = rd_in(6)*rRel(1) - rd_in(4)*rRel(3) + rd_in(2) ! y + rd_out(3) = -rd_in(5)*rRel(1) + rd_in(4)*rRel(2) + rd_in(3) ! z + + + END SUBROUTINE TransformKinematicsAtoB + !----------------------------------------------------------------------- + + ! + !----------------------------------------------------------------------- + SUBROUTINE TranslateForce3to6DOF(dx, F, Fout) + REAL(DbKi), INTENT(IN ) :: dx(3) ! displacement vector from ref point to point of force (F) application + REAL(DbKi), INTENT(IN ) :: F(3) ! applied force + REAL(DbKi), INTENT( OUT) :: Fout(6) ! resultant applied force and moment about ref point + + Fout(1:3) = F + + Fout(4:6) = CROSS_PRODUCT(dx, F) + + END SUBROUTINE TranslateForce3to6DOF + !----------------------------------------------------------------------- + + + ! + !----------------------------------------------------------------------- + SUBROUTINE TranslateMass3to6DOF(dx, Min, Mout) + REAL(DbKi), INTENT(IN ) :: dx(3) ! displacement vector from ref point to point of mass matrix (Min) + REAL(DbKi), INTENT(IN ) :: Min( 3,3) ! original mass matrix (assumed at center of mass, or a point mass) + REAL(DbKi), INTENT( OUT) :: Mout(6,6) ! resultant mass and inertia matrix about ref point + + REAL(DbKi) :: H( 3,3) ! "anti-symmetric tensor components" from Sadeghi and Incecik + REAL(DbKi) :: tempM( 3,3) + REAL(DbKi) :: tempM2(3,3) + REAL(DbKi) :: Htrans(3,3) + Integer(IntKi) :: I,J + + ! sub-matrix definitions are accordint to | m J | + ! | J^T I | + + H = getH(dx); + + ! mass matrix [m'] = [m] + Mout(1:3,1:3) = Min + + ! product of inertia matrix [J'] = [m][H] + [J] + Mout(1:3,4:6) = MATMUL(Min, H) + Mout(4:6,1:3) = TRANSPOSE(Mout(1:3,4:6)) + + !moment of inertia matrix [I'] = [H][m][H]^T + [J]^T [H] + [H]^T [J] + [I] + Mout(4:6,4:6) = MATMUL(MATMUL(H, Min), TRANSPOSE(H)) + + END SUBROUTINE TranslateMass3to6DOF + !----------------------------------------------------------------------- + + ! + !----------------------------------------------------------------------- + SUBROUTINE TranslateMass6to6DOF(dx, Min, Mout) + REAL(DbKi), INTENT(IN ) :: dx(3) ! displacement vector from ref point to point of mass matrix (Min) + REAL(DbKi), INTENT(IN ) :: Min( 6,6) ! original mass matrix + REAL(DbKi), INTENT( OUT) :: Mout(6,6) ! resultant mass and inertia matrix about ref point + + REAL(DbKi) :: H( 3,3) ! "anti-symmetric tensor components" from Sadeghi and Incecik + + H = getH(dx); + + ! mass matrix [m'] = [m] + Mout(1:3,1:3) = Min(1:3,1:3) + + ! product of inertia matrix [J'] = [m][H] + [J] + Mout(1:3,4:6) = MATMUL(Min(1:3,1:3), H) + Min(1:3,4:6) + Mout(4:6,1:3) = TRANSPOSE(Mout(1:3,4:6)) + + !moment of inertia matrix [I'] = [H][m][H]^T + [J]^T [H] + [H]^T [J] + [I] + Mout(4:6,4:6) = MATMUL(MATMUL(H, Min(1:3,1:3)), TRANSPOSE(H)) + MATMUL(Min(4:6,1:3),H) + MATMUL(TRANSPOSE(H),Min(1:3,4:6)) + Min(4:6,4:6) + + END SUBROUTINE TranslateMass6to6DOF + !----------------------------------------------------------------------- + + ! produce alternator matrix + !----------------------------------------------------------------------- + FUNCTION GetH(r) + Real(DbKi), INTENT(IN) :: r(3) ! inputted vector + Real(DbKi) :: GetH(3,3) ! outputted matrix + + GetH(2,1) = -r(3) + GetH(1,2) = r(3) + GetH(3,1) = r(2) + GetH(1,3) = -r(2) + GetH(3,2) = -r(1) + GetH(2,3) = r(1) + + GetH(1,1) = 0.0_DbKi + GetH(2,2) = 0.0_DbKi + GetH(3,3) = 0.0_DbKi + + END FUNCTION GetH + !----------------------------------------------------------------------- + + + + ! apply a rotation to a 6-by-6 mass/inertia tensor (see Sadeghi and Incecik 2005 for theory) + !----------------------------------------------------------------------- + FUNCTION RotateM6(Min, rotMat) result(outMat) + + Real(DbKi), INTENT(IN) :: Min(6,6) ! inputted matrix to be rotated + Real(DbKi), INTENT(IN) :: rotMat(3,3) ! rotation matrix (DCM) + Real(DbKi) :: outMat(6,6) ! rotated matrix + + ! the process for each of the following is to + ! 1. copy out the relevant 3x3 matrix section, + ! 2. rotate it, and + ! 3. paste it into the output 6x6 matrix + + ! mass matrix + outMat(1:3,1:3) = rotateM3(Min(1:3,1:3), rotMat) + + ! product of inertia matrix + outMat(1:3,4:6) = rotateM3(Min(1:3,4:6), rotMat) + outMat(4:6,1:3) = TRANSPOSE(outMat(1:3,4:6)) + + ! moment of inertia matrix + outMat(4:6,4:6) = rotateM3(Min(4:6,4:6), rotMat) + + END FUNCTION RotateM6 + + + ! apply a rotation to a 3-by-3 mass matrix or any other second order tensor + !----------------------------------------------------------------------- + FUNCTION RotateM3(Min, rotMat) result(outMat) + + Real(DbKi), INTENT(IN) :: Min(3,3) ! inputted matrix to be rotated + Real(DbKi), INTENT(IN) :: rotMat(3,3) ! rotation matrix (DCM) + Real(DbKi) :: outMat(3,3) ! rotated matrix + + ! overall operation is [m'] = [a]*[m]*[a]^T + + outMat = MATMUL( MATMUL(rotMat, Min), TRANSPOSE(rotMat) ) + + END FUNCTION RotateM3 + + + + + + ! calculates rotation matrix R to rotate from global axes to a member's local axes + !----------------------------------------------------------------------- + FUNCTION CalcOrientation(phi, beta, gamma) result(R) + + REAL(DbKi), INTENT ( IN ) :: phi ! member incline angle + REAL(DbKi), INTENT ( IN ) :: beta ! member incline heading + REAL(DbKi), INTENT ( IN ) :: gamma ! member twist angle + REAL(DbKi) :: R(3,3) ! rotation matrix + + INTEGER(IntKi) :: errStat + CHARACTER(100) :: errMsg + + REAL(DbKi) :: s1, c1, s2, c2, s3, c3 + + + ! trig terms for Euler angles rotation based on beta, phi, and gamma + s1 = sin(beta) + c1 = cos(beta) + s2 = sin(phi) + c2 = cos(phi) + s3 = sin(gamma) + c3 = cos(gamma) + + ! calculate rotation matrix based on Z1Y2Z3 Euler rotation sequence from https:!en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + + R(1,1) = c1*c2*c3-s1*s3 + R(1,2) = -c3*s1-c1*c2*s3 + R(1,3) = c1*s2 + R(2,1) = c1*s3+c2*c3*s1 + R(2,2) = c1*c3-c2*s1*s3 + R(2,3) = s1*s2 + R(3,1) = -c3*s2 + R(3,2) = s2*s3 + R(3,3) = c2 + + ! could also calculate unit normals p1 and p2 for rectangular cross sections + !p1 = matmul( R, [1,0,0] ) ! unit vector that is perpendicular to the 'beta' plane if gamma is zero + !p2 = cross( q, p1 ) ! unit vector orthogonal to both p1 and q + + END FUNCTION CalcOrientation + + + !compute the inverse of a 3-by-3 matrix m + !----------------------------------------------------------------------- + SUBROUTINE Inverse3by3( Minv, M ) + Real(DbKi), INTENT(OUT) :: Minv(3,3) ! returned inverse matrix + Real(DbKi), INTENT(IN) :: M(3,3) ! inputted matrix + + Real(DbKi) :: det ! the determinant + Real(DbKi) :: invdet ! inverse of the determinant + + det = M(1, 1) * (M(2, 2) * M(3, 3) - M(3, 2) * M(2, 3)) - & + M(1, 2) * (M(2, 1) * M(3, 3) - M(2, 3) * M(3, 1)) + & + M(1, 3) * (M(2, 1) * M(3, 2) - M(2, 2) * M(3, 1)); + + invdet = 1.0 / det ! because multiplying is faster than dividing + + Minv(1, 1) = (M(2, 2) * M(3, 3) - M(3, 2) * M(2, 3)) * invdet + Minv(1, 2) = (M(1, 3) * M(3, 2) - M(1, 2) * M(3, 3)) * invdet + Minv(1, 3) = (M(1, 2) * M(2, 3) - M(1, 3) * M(2, 2)) * invdet + Minv(2, 1) = (M(2, 3) * M(3, 1) - M(2, 1) * M(3, 3)) * invdet + Minv(2, 2) = (M(1, 1) * M(3, 3) - M(1, 3) * M(3, 1)) * invdet + Minv(2, 3) = (M(2, 1) * M(1, 3) - M(1, 1) * M(2, 3)) * invdet + Minv(3, 1) = (M(2, 1) * M(3, 2) - M(3, 1) * M(2, 2)) * invdet + Minv(3, 2) = (M(3, 1) * M(1, 2) - M(1, 1) * M(3, 2)) * invdet + Minv(3, 3) = (M(1, 1) * M(2, 2) - M(2, 1) * M(1, 2)) * invdet + + END SUBROUTINE Inverse3by3 + !----------------------------------------------------------------------- + + + ! One-function implementation of Crout LU Decomposition. Solves Ax=b for x + SUBROUTINE LUsolve(n, A, LU, b, y, x) + + INTEGER(intKi), INTENT(IN ) :: n ! size of matrices and vectors + Real(DbKi), INTENT(IN ) :: A( n,n) ! LHS matrix (e.g. mass matrix) + Real(DbKi), INTENT(INOUT) :: LU(n,n) ! stores LU matrix data + Real(DbKi), INTENT(IN ) :: b(n) ! RHS vector + Real(DbKi), INTENT(INOUT) :: y(n) ! temporary vector + Real(DbKi), INTENT( OUT) :: x(n) ! LHS vector to solve for + + INTEGER(intKi) :: i,j,k,p + Real(DbKi) :: sum + + DO k = 1,n + DO i = k,n + + sum = 0.0_DbKi + + DO p=1,k-1 !for(int p=0; p=0; --i) + + sum = 0.0_DbKi + + DO k=i+1, n + sum = sum + LU(i,k)*x(k) + END DO + + x(i) = (y(i)-sum) + + END DO !j (actually decrementing i) + + END SUBROUTINE LUsolve + + + + ! :::::::::::::::::::::::::: interpolation subroutines ::::::::::::::::::::::::::::::: + + + SUBROUTINE getInterpNums(xlist, xin, istart, i, fout) + + Real(DbKi), INTENT (IN ) :: xlist(:) ! list of x values + Real(DbKi), INTENT (IN ) :: xin ! x value to be interpolated + Integer(IntKi),INTENT (IN ) :: istart ! first lower index to try + Integer(IntKi),INTENT ( OUT) :: i ! lower index to interpolate from + Real(DbKi), INTENT ( OUT) :: fout ! fraction to return such that y* = y[i] + fout*(y[i+1]-y[i]) + + Integer(IntKi) :: i1 + Integer(IntKi) :: nx + + i1 = 1 ! Setting in declaration causes an implied save, which would never allow this routine to find anything at the start of the array. + + nx = SIZE(xlist) + + if (xin <= xlist(1)) THEN ! below lowest data point + i = 1_IntKi + fout = 0.0_DbKi + + else if (xlist(nx) <= xin) THEN ! above highest data point + i = nx + fout = 0.0_DbKi + + else ! within the data range + + IF (xlist(min(istart,nx)) < xin) i1 = istart ! if istart is below the actual value, start with it instead of starting at 1 to save time, but make sure it doesn't overstep the array + + DO i = i1, nx-1 + IF (xlist(i+1) > xin) THEN + fout = (xin - xlist(i) )/( xlist(i+1) - xlist(i) ) + exit + END IF + END DO + END IF + + END SUBROUTINE getInterpNums + + + SUBROUTINE getInterpNumsSiKi(xlist, xin, istart, i, fout) + + Real(SiKi), INTENT (IN ) :: xlist(:) ! list of x values + Real(SiKi), INTENT (IN ) :: xin ! x value to be interpolated + Integer(IntKi),INTENT (IN ) :: istart ! first lower index to try + Integer(IntKi),INTENT ( OUT) :: i ! lower index to interpolate from + Real(SiKi), INTENT ( OUT) :: fout ! fraction to return such that y* = y[i] + fout*(y[i+1]-y[i]) + + Integer(IntKi) :: i1 + Integer(IntKi) :: nx + + i1 = 1 ! Setting in declaration causes an implied save, which would never allow this routine to find anything at the start of the array. + + nx = SIZE(xlist) + + if (xin <= xlist(1)) THEN ! below lowest data point + i = 1_IntKi + fout = 0.0_SiKi + + else if (xlist(nx) <= xin) THEN ! above highest data point + i = nx + fout = 0.0_SiKi + + else ! within the data range + + IF (xlist(min(istart,nx)) < xin) i1 = istart ! if istart is below the actual value, start with it instead of starting at 1 to save time, but make sure it doesn't overstep the array + + DO i = i1, nx-1 + IF (xlist(i+1) > xin) THEN + fout = (xin - xlist(i) )/( xlist(i+1) - xlist(i) ) + exit + END IF + END DO + END IF + + END SUBROUTINE getInterpNumsSiKi + + SUBROUTINE calculate4Dinterpolation(f, ix0, iy0, iz0, it0, fx, fy, fz, ft, c) + + Real(SiKi), INTENT (IN ) :: f(:,:,:,:) ! data array + INTEGER(IntKi), INTENT (IN ) :: ix0, iy0, iz0, it0 ! indices for interpolation + Real(SiKi), INTENT (IN ) :: fx, fy, fz, ft ! interpolation fractions + Real(DbKi), INTENT ( OUT) :: c ! the output value + + INTEGER(IntKi) :: ix1, iy1, iz1, it1 ! second indices + REAL(SiKi) :: c000, c001, c010, c011, c100, c101, c110, c111 + REAL(SiKi) :: c00, c01, c10, c11, c0, c1 + + ! handle end case conditions + if (fx == 0) then + ix1 = ix0 + else + ix1 = min(ix0+1,size(f,4)) ! don't overstep bounds + end if + + if (fy == 0) then + iy1 = iy0 + else + iy1 = min(iy0+1,size(f,3)) ! don't overstep bounds + end if + + if (fz == 0) then + iz1 = iz0 + else + iz1 = min(iz0+1,size(f,2)) ! don't overstep bounds + end if + + if (ft == 0) then + it1 = it0 + else + it1 = min(it0+1,size(f,1)) ! don't overstep bounds + end if + + c000 = f(it0,iz0,iy0,ix0)*(1.0-ft) + f(it1,iz0,iy0,ix0)*ft + c001 = f(it0,iz1,iy0,ix0)*(1.0-ft) + f(it1,iz1,iy0,ix0)*ft + c010 = f(it0,iz0,iy1,ix0)*(1.0-ft) + f(it1,iz0,iy1,ix0)*ft + c011 = f(it0,iz1,iy1,ix0)*(1.0-ft) + f(it1,iz1,iy1,ix0)*ft + c100 = f(it0,iz0,iy0,ix1)*(1.0-ft) + f(it1,iz0,iy0,ix1)*ft + c101 = f(it0,iz1,iy0,ix1)*(1.0-ft) + f(it1,iz1,iy0,ix1)*ft + c110 = f(it0,iz0,iy1,ix1)*(1.0-ft) + f(it1,iz0,iy1,ix1)*ft + c111 = f(it0,iz1,iy1,ix1)*(1.0-ft) + f(it1,iz1,iy1,ix1)*ft + + c00 = c000*(1.0-fx) + c100*fx + c01 = c001*(1.0-fx) + c101*fx + c10 = c010*(1.0-fx) + c110*fx + c11 = c011*(1.0-fx) + c111*fx + + c0 = c00 *(1.0-fy) + c10 *fy + c1 = c01 *(1.0-fy) + c11 *fy + + c = c0 *(1.0-fz) + c1 *fz + + END SUBROUTINE + + + SUBROUTINE calculate3Dinterpolation(f, ix0, iy0, iz0, fx, fy, fz, c) + + Real(SiKi), INTENT (IN ) :: f(:,:,:) ! data array + INTEGER(IntKi), INTENT (IN ) :: ix0, iy0, iz0 ! indices for interpolation + Real(SiKi), INTENT (IN ) :: fx, fy, fz ! interpolation fractions + Real(DbKi), INTENT ( OUT) :: c ! the output value + + INTEGER(IntKi) :: ix1, iy1, iz1 ! second indices + REAL(SiKi) :: c000, c001, c010, c011, c100, c101, c110, c111 + REAL(SiKi) :: c00, c01, c10, c11, c0, c1 + + ! note that "z" could also be "t" - dimension names are arbitrary + + ! handle end case conditions + if (fx == 0) then + ix1 = ix0 + else + ix1 = min(ix0+1,size(f,3)) ! don't overstep bounds + end if + + if (fy == 0) then + iy1 = iy0 + else + iy1 = min(iy0+1,size(f,2)) ! don't overstep bounds + end if + + if (fz == 0) then + iz1 = iz0 + else + iz1 = min(iz0+1,size(f,1)) ! don't overstep bounds + end if + + c000 = f(iz0,iy0,ix0) + c001 = f(iz1,iy0,ix0) + c010 = f(iz0,iy1,ix0) + c011 = f(iz1,iy1,ix0) + c100 = f(iz0,iy0,ix1) + c101 = f(iz1,iy0,ix1) + c110 = f(iz0,iy1,ix1) + c111 = f(iz1,iy1,ix1) + + c00 = c000*(1.0-fx) + c100*fx + c01 = c001*(1.0-fx) + c101*fx + c10 = c010*(1.0-fx) + c110*fx + c11 = c011*(1.0-fx) + c111*fx + + c0 = c00 *(1.0-fy) + c10 *fy + c1 = c01 *(1.0-fy) + c11 *fy + + c = c0 *(1.0-fz) + c1 *fz + + END SUBROUTINE + + SUBROUTINE calculate2Dinterpolation(f, ix0, iy0, fx, fy, c) + REAL(DbKi), INTENT (IN ) :: f(:,:) ! data array + INTEGER(IntKi), INTENT (IN ) :: ix0, iy0 ! indices for interpolation + REAL(DbKi), INTENT (IN ) :: fx, fy ! interpolation fractions + REAL(DbKi), INTENT ( OUT) :: c ! the output value + + INTEGER(IntKi) :: ix1, iy1 ! second indices + REAL(DbKi) :: c00, c01, c10, c11, c0, c1 + + ! handle end case conditions + IF (fx == 0) THEN + ix1 = ix0 + ELSE + ix1 = min(ix0+1,size(f,2)) ! don't overstep bounds + END IF + IF (fy == 0) THEN + iy1 = iy0 + ELSE + iy1 = min(iy0+1,size(f,1)) ! don't overstep bounds + END IF + c00 = f(iy0, ix0) + c01 = f(iy1, ix0) + c10 = f(iy0, ix1) + c11 = f(iy1, ix1) + c0 = c00 *(1.0-fx) + c10 *fx + c1 = c01 *(1.0-fx) + c11 *fx + c = c0 *(1.0-fy) + c1 *fy + END SUBROUTINE calculate2Dinterpolation + + + + + + ! :::::::::::::::::::::::::: bathymetry subroutines ::::::::::::::::::::::::::::::: + + + SUBROUTINE getDepthFromBathymetry(BathymetryGrid, BathGrid_Xs, BathGrid_Ys, LineX, LineY, depth) + + REAL(DbKi), INTENT(IN ) :: BathymetryGrid(:,:) ! need colons or some sort of dimension setting + REAL(DbKi), INTENT(IN ) :: BathGrid_Xs(:) + REAL(DbKi), INTENT(IN ) :: BathGrid_Ys(:) + REAL(DbKi), INTENT(IN ) :: LineX + REAL(DbKi), INTENT(IN ) :: LineY + REAL(DbKi), INTENT( OUT) :: depth + + INTEGER(IntKi) :: ix, iy ! indeces for interpolation + Real(DbKi) :: fx, fy ! interpolation fractions + + CALL getInterpNums(BathGrid_Xs, LineX, 1, ix, fx) + CALL getInterpNums(BathGrid_Ys, LineY, 1, iy, fy) + + CALL calculate2Dinterpolation(BathymetryGrid, ix, iy, fx, fy, depth) + + END SUBROUTINE getDepthFromBathymetry + + + ! :::::::::::::::::::::::::: wave and current subroutines ::::::::::::::::::::::::::::::: + + + ! master function to get wave/water kinematics at a given point -- called by each object from grid-based data + SUBROUTINE getWaterKin(p, x, y, z, t, tindex, U, Ud, zeta, PDyn) + + ! note, this whole approach assuems that px, py, and pz are in increasing order + + TYPE(MD_ParameterType),INTENT (IN ) :: p ! MoorDyn parameters (contains the wave info for now) + Real(DbKi), INTENT (IN ) :: x + Real(DbKi), INTENT (IN ) :: y + Real(DbKi), INTENT (IN ) :: z + Real(DbKi), INTENT (IN ) :: t + INTEGER(IntKi), INTENT (INOUT) :: tindex ! pass time index to try starting from, returns identified time index + Real(DbKi), INTENT (INOUT) :: U(3) + Real(DbKi), INTENT (INOUT) :: Ud(3) + Real(DbKi), INTENT (INOUT) :: zeta + Real(DbKi), INTENT (INOUT) :: PDyn + + + INTEGER(IntKi) :: ix, iy, iz, it ! indeces for interpolation + INTEGER(IntKi) :: N ! number of rod elements for convenience + Real(ReKi) :: fx, fy, fz, ft ! interpolation fractions + !Real(DbKi) :: qt ! used in time step interpolation + + + ! if wave kinematics enabled, get interpolated values from grid + if (p%WaveKin > 0) then + + CALL getInterpNumsSiKi(p%pxWave , REAL(x), 1, ix, fx) + CALL getInterpNumsSiKi(p%pyWave , REAL(y), 1, iy, fy) + CALL getInterpNumsSiKi(p%pzWave , REAL(z), 1, iz, fz) + !CALL getInterpNums(p%tWave, t, tindex, it, ft) + it = floor(t/ p%dtWave) + ft = (t - it*p%dtWave)/p%dtWave + tindex = it + + CALL calculate3Dinterpolation(p%zeta, ix, iy, it, fx, fy, ft, zeta) + + CALL calculate4Dinterpolation(p%PDyn, ix, iy, iz, it, fx, fy, fz, ft, PDyn) + + CALL calculate4Dinterpolation(p%uxWave, ix, iy, iz, it, fx, fy, fz, ft, U(1) ) + CALL calculate4Dinterpolation(p%uyWave, ix, iy, iz, it, fx, fy, fz, ft, U(2) ) + CALL calculate4Dinterpolation(p%uzWave, ix, iy, iz, it, fx, fy, fz, ft, U(3) ) + CALL calculate4Dinterpolation(p%axWave, ix, iy, iz, it, fx, fy, fz, ft, Ud(1) ) + CALL calculate4Dinterpolation(p%ayWave, ix, iy, iz, it, fx, fy, fz, ft, Ud(2) ) + CALL calculate4Dinterpolation(p%azWave, ix, iy, iz, it, fx, fy, fz, ft, Ud(3) ) + else + U = 0.0_DbKi + Ud = 0.0_DbKi + end if + + + ! if current kinematics enabled, add interpolated current values from profile + if (p%Current > 0) then + + CALL getInterpNumsSiKi(p%pzCurrent, REAL(z), 1, iz, fz) + + U(1) = U(1) + p%uxCurrent(iz) + fz*(p%uxCurrent(iz+1)-p%uxCurrent(iz))/(p%pzCurrent(iz+1)-p%pzCurrent(iz)) + U(2) = U(2) + p%uyCurrent(iz) + fz*(p%uyCurrent(iz+1)-p%uyCurrent(iz))/(p%pzCurrent(iz+1)-p%pzCurrent(iz)) + end if + + END SUBROUTINE getWaterKin + + + ! unused routine with old code for taking wave kinematic grid inputs from HydroDyn + SUBROUTINE CopyWaterKinFromHydroDyn(p, InitInp) + + TYPE(MD_InitInputType), INTENT(IN ) :: InitInp ! INTENT(INOUT) : Input data for initialization routine + TYPE(MD_ParameterType), INTENT( OUT) :: p ! INTENT( OUT) : Parameters + + INTEGER(IntKi) :: I, J, K, Itemp + + + ! ----------------------------- Arrays for wave kinematics ----------------------------- + + + ! :::::::::::::: BELOW WILL BE USED EVENTUALLY WHEN WAVE INFO IS AN INPUT :::::::::::::::::: + ! ! The rAll array contains all nodes or reference points in the system + ! ! (x,y,z global coordinates for each) in the order of bodies, rods, points, internal line nodes. + ! + ! ! count the number of nodes to use for passing wave kinematics + ! J=0 + ! ! Body reference point coordinates + ! J = J + p%nBodies + ! ! Rod node coordinates (including ends) + ! DO l = 1, p%nRods + ! J = J + (m%RodList(l)%N + 1) + ! END DO + ! ! Point reference point coordinates + ! J = J + p%nConnects + ! ! Line internal node coordinates + ! DO l = 1, p%nLines + ! J = J + (m%LineList(l)%N - 1) + ! END DO + ! + ! ! allocate all relevant arrays + ! ! allocate state vector and temporary state vectors based on size just calculated + ! ALLOCATE ( y%rAll(3,J), u%U(3,J), u%Ud(3,J), u%zeta(J), u%PDyn(J), STAT = ErrStat ) + ! IF ( ErrStat /= ErrID_None ) THEN + ! ErrMsg = ' Error allocating wave kinematics vectors.' + ! RETURN + ! END IF + ! + ! + ! ! go through the nodes and fill in the data (this should maybe be turned into a global function) + ! J=0 + ! ! Body reference point coordinates + ! DO I = 1, p%nBodies + ! J = J + 1 + ! y%rAll(:,J) = m%BodyList(I)%r6(1:3) + ! END DO + ! ! Rod node coordinates + ! DO I = 1, p%nRods + ! DO K = 0,m%RodList(I)%N + ! J = J + 1 + ! y%rAll(:,J) = m%RodList(I)%r(:,K) + ! END DO + ! END DO + ! ! Point reference point coordinates + ! DO I = 1, p%nConnects + ! J = J + 1 + ! y%rAll(:,J) = m%ConnectList(I)%r + ! END DO + ! ! Line internal node coordinates + ! DO I = 1, p%nLines + ! DO K = 1,m%LineList(I)%N-1 + ! J = J + 1 + ! y%rAll(:,J) = m%LineList(I)%r(:,K) + ! END DO + ! END DO + ! :::::::::::::::: the above might be used eventually. For now, let's store wave info grids within this module ::::::::::::::::: + + + ! ----- copy wave grid data over from HydroDyn (as was done in USFLOWT branch) ----- + + ! get grid and time info (currently this is hard-coded to match what's in HydroDyn_Input + ! DO I=1,p%nzWave + ! p%pz(I) = 1.0 - 2.0**(p%nzWave-I) ! -127, -63, -31, -15, -7, -3, -1, 0 + ! END DO + ! DO J = 1,p%nyWave + ! p%py(J) = WaveGrid_y0 + WaveGrid_dy*(J-1) + ! END DO + ! DO K = 1,p%nxWave + ! p%px(K) = WaveGrid_x0 + WaveGrid_dx*(K-1) + ! END DO + ! + ! p%tWave = InitInp%WaveTime + + DO I=1,p%nzWave + DO J = 1,p%nyWave + DO K = 1,p%nxWave + Itemp = (I-1)*p%nxWave*p%nyWave + (J-1)*p%nxWave + K ! index of actual node on 3D grid + + p%uxWave (:,I,J,K) = InitInp%WaveVel( :,Itemp,1) ! note: indices are t, z, y, x + p%uyWave (:,I,J,K) = InitInp%WaveVel( :,Itemp,2) + p%uzWave (:,I,J,K) = InitInp%WaveVel( :,Itemp,3) + p%axWave (:,I,J,K) = InitInp%WaveAcc( :,Itemp,1) + p%ayWave (:,I,J,K) = InitInp%WaveAcc( :,Itemp,2) + p%azWave (:,I,J,K) = InitInp%WaveAcc( :,Itemp,3) + p%PDyn(:,I,J,K) = InitInp%WavePDyn(:,Itemp) + END DO + END DO + END DO + + DO J = 1,p%nyWave + DO K = 1,p%nxWave + Itemp = (J-1)*p%nxWave + K ! index of actual node on surface 2D grid + p%zeta(:,J,K) = InitInp%WaveElev(:,Itemp) + END DO + END DO + + END SUBROUTINE CopyWaterKinFromHydroDyn + + + ! ----- write wave grid spacing to output file ----- + SUBROUTINE WriteWaveGrid(p, ErrStat, ErrMsg) + + TYPE(MD_ParameterType), INTENT(INOUT) :: p ! Parameters + + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + + INTEGER(IntKi) :: ErrStat2 + CHARACTER(120) :: ErrMsg2 + + CHARACTER(120) :: Frmt + INTEGER(IntKi) :: UnOut ! for outputing wave kinematics data + INTEGER(IntKi) :: I + + + CALL GetNewUnit( UnOut) + + CALL OpenFOutFile ( UnOut, "waves.txt", ErrStat, ErrMsg ) + IF ( ErrStat > ErrID_None ) THEN + ErrMsg = ' Error opening wave grid file: '//TRIM(ErrMsg) + ErrStat = ErrID_Fatal + RETURN + END IF + + WRITE(UnOut, *, IOSTAT=ErrStat2) TRIM( 'MoorDyn v2 wave/current kinematics grid file' ) + WRITE(UnOut, *, IOSTAT=ErrStat2) TRIM( '---------------------------------------------' ) + WRITE(UnOut, *, IOSTAT=ErrStat2) TRIM( 'The following 6 lines (4-9) specify the input type then the inputs for x, then, y, then z coordinates.' ) + + WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - X input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) + Frmt = '('//TRIM(Int2LStr(5))//'(A1,e10.4))' + WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%pxWave(I))), I=1,p%nxWave ) + + WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - Y input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) + Frmt = '('//TRIM(Int2LStr(5))//'(A1,e10.4))' + WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%pyWave(I))), I=1,p%nyWave ) + + WRITE(UnOut,*, IOSTAT=ErrStat2) TRIM( '1 - Z input type (0: not used; 1: list values in ascending order; 2: uniform specified by -xlim, xlim, num)' ) + Frmt = '('//TRIM(Int2LStr(8))//'(A1,e10.4))' + WRITE(UnOut,*, IOSTAT=ErrStat2) ( " ", TRIM(Num2LStr(p%pzWave(I))), I=1,p%nzWave ) + + CLOSE(UnOut, IOSTAT = ErrStat ) + IF ( ErrStat /= 0 ) THEN + ErrMsg = 'Error closing wave grid file' + END IF + + END SUBROUTINE WriteWaveGrid + + + ! ----- write wave kinematics grid data to output file ----- + SUBROUTINE WriteWaveData(p, ErrStat, ErrMsg) + + TYPE(MD_ParameterType), INTENT(INOUT) :: p ! Parameters + + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + + INTEGER(IntKi) :: ErrStat2 + CHARACTER(120) :: ErrMsg2 + + INTEGER(IntKi) :: UnOut ! for outputing wave kinematics data + INTEGER(IntKi) :: I,J,K, l, Itemp + + CALL GetNewUnit( UnOut) + + CALL OpenFOutFile ( UnOut, "wave data.txt", ErrStat, ErrMsg ) + IF ( ErrStat > ErrID_None ) THEN + ErrMsg = ' Error opening wave grid file: '//TRIM(ErrMsg) + ErrStat = ErrID_Fatal + RETURN + END IF + + ! write channel labels + + + ! time + WRITE(UnOut,"(A10)", IOSTAT=ErrStat2, advance="no") "Time" + + DO J = 1,p%nyWave !y + DO K = 1,p%nxWave !x + WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ze0", Num2Lstr(J+10*K) + END DO + END DO + DO I=1,p%nzWave !z + DO J = 1,p%nyWave !y + DO K = 1,p%nxWave !x + WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ux", Num2Lstr(I+10*J+100*K) + WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " uy", Num2Lstr(I+10*J+100*K) + WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " uz", Num2Lstr(I+10*J+100*K) + END DO + END DO + END DO + DO I=1,p%nzWave !z + DO J = 1,p%nyWave !y + DO K = 1,p%nxWave !x + WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ax", Num2Lstr(I+10*J+100*K) + WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " ay", Num2Lstr(I+10*J+100*K) + WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " az", Num2Lstr(I+10*J+100*K) + END DO + END DO + END DO + DO I=1,p%nzWave !z + DO J = 1,p%nyWave !y + DO K = 1,p%nxWave !x + WRITE(UnOut,"(A3,A8)", IOSTAT=ErrStat2, advance="no") " pd", Num2Lstr(I+10*J+100*K) + END DO + END DO + END DO + + ! end the line + WRITE(UnOut, "(A1)", IOSTAT=ErrStat2) " " + + + + DO l=1, p%ntWave ! loop through all time steps + + ! time + WRITE(UnOut,"(F10.4)", IOSTAT=ErrStat2, advance="no") p%dtWave*(l-1) + !WRITE(UnOut,"(F10.4)", IOSTAT=ErrStat2, advance="no") InitInp%WaveTime(l) + + ! wave elevation (all slices for now, to check) + DO J = 1,p%nyWave !y + DO K = 1,p%nxWave !x + Itemp = (J-1)*p%nxWave + K ! index of actual node + + WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%zeta(l,J,K) + END DO + END DO + + ! wave velocities + DO I=1,p%nzWave !z + DO J = 1,p%nyWave !y + DO K = 1,p%nxWave !x + Itemp = (I-1)*p%nxWave*p%nyWave + (J-1)*p%nxWave + K ! index of actual node + + WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%uxWave(l,I,J,K) + WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%uyWave(l,I,J,K) + WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%uzWave(l,I,J,K) + END DO + END DO + END DO + + ! wave accelerations + DO I=1,p%nzWave !z + DO J = 1,p%nyWave !y + DO K = 1,p%nxWave !x + Itemp = (I-1)*p%nxWave*p%nyWave + (J-1)*p%nxWave + K ! index of actual node + + WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%axWave(l,I,J,K) + WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%ayWave(l,I,J,K) + WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%azWave(l,I,J,K) + END DO + END DO + END DO + + ! dynamic pressure + DO I=1,p%nzWave !z + DO J = 1,p%nyWave !y + DO K = 1,p%nxWave !x + Itemp = (I-1)*p%nxWave*p%nyWave + (J-1)*p%nxWave + K ! index of actual node + + WRITE(UnOut,"(A1,e10.3)", IOSTAT=ErrStat2, advance="no") " ", p%PDyn(l,I,J,K) + END DO + END DO + END DO + + ! end the line + WRITE(UnOut, "(A1)", IOSTAT=ErrStat2) " " + + + END DO + + + CLOSE(UnOut, IOSTAT = ErrStat ) + IF ( ErrStat /= 0 ) THEN + ErrMsg = 'Error closing wave grid file' + END IF + + END SUBROUTINE WriteWaveData + + + SUBROUTINE SetupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) + + CHARACTER(40), INTENT(IN ) :: WaterKinString ! string describing water kinematics filename + TYPE(MD_ParameterType), INTENT(INOUT) :: p ! Parameters + REAL(ReKi), INTENT(IN ) :: Tmax + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + + INTEGER(IntKi) :: I, iIn, ix, iy, iz + INTEGER(IntKi) :: ntIn ! number of time series inputs from file + INTEGER(IntKi) :: UnIn ! unit number for coefficient input file + INTEGER(IntKi) :: UnEcho + REAL(SiKi) :: pzCurrentTemp(100) + REAL(SiKi) :: uxCurrentTemp(100) + REAL(SiKi) :: uyCurrentTemp(100) + + CHARACTER(120) :: WaveKinFile + INTEGER(IntKi) :: UnElev ! unit number for coefficient input file + REAL(SiKi), ALLOCATABLE :: WaveTimeIn(:) ! temporarily holds wave input time series + REAL(SiKi), ALLOCATABLE :: WaveElevIn(:) + REAL(SiKi), ALLOCATABLE :: WaveElev0(:) ! interpolated reference wave elevation time series + REAL(SiKi) :: WaveDir + REAL(SiKi) :: t, Frac + CHARACTER(1024) :: FileName ! Name of MoorDyn input file + CHARACTER(120) :: Line + CHARACTER(120) :: Line2 + CHARACTER(120) :: entries2 + INTEGER(IntKi) :: coordtype + + INTEGER(IntKi) :: NStepWave ! + INTEGER(IntKi) :: NStepWave2 ! + REAL(SiKi) :: WaveTMax ! max wave elevation time series duration after optimizing lenght for FFT + REAL(SiKi) :: WaveDOmega + REAL(SiKi) :: SinWaveDir ! SIN( WaveDirArr(I) ) -- Each wave frequency has a unique wave direction. + REAL(SiKi) :: CosWaveDir ! COS( WaveDirArr(I) ) -- Each wave frequency has a unique wave direction. + + REAL(SiKi), ALLOCATABLE :: TmpFFTWaveElev(:) ! Data for the FFT calculation + TYPE(FFT_DataType) :: FFT_Data ! the instance of the FFT module we're using + + + COMPLEX(SiKi),ALLOCATABLE :: tmpComplex(:) ! A temporary array (0:NStepWave2-1) for FFT use. + + REAL(SiKi) :: Omega ! Wave frequency (rad/s) + COMPLEX(SiKi), PARAMETER :: ImagNmbr = (0.0,1.0) ! The imaginary number, SQRT(-1.0) + COMPLEX(SiKi) :: ImagOmega ! = ImagNmbr*Omega (rad/s) + REAL(DbKi), ALLOCATABLE :: WaveNmbr(:) ! wave number for frequency array + REAL(SiKi), ALLOCATABLE :: WaveElevC0(:,:) ! Discrete Fourier transform of the instantaneous elevation of incident waves at the ref point (meters) + COMPLEX(SiKi), ALLOCATABLE :: WaveElevC( :) ! Discrete Fourier transform of the instantaneous elevation of incident waves at the ref point (meters) + COMPLEX(SiKi), ALLOCATABLE :: WaveAccCHx(:) ! Discrete Fourier transform of the instantaneous horizontal acceleration in x-direction of incident waves before applying stretching at the zi-coordinates for points (m/s^2) + COMPLEX(SiKi), ALLOCATABLE :: WaveAccCHy(:) ! Discrete Fourier transform of the instantaneous horizontal acceleration in y-direction of incident waves before applying stretching at the zi-coordinates for points (m/s^2) + COMPLEX(SiKi), ALLOCATABLE :: WaveAccCV( :) ! Discrete Fourier transform of the instantaneous vertical acceleration of incident waves before applying stretching at the zi-coordinates for points (m/s^2) + COMPLEX(SiKi), ALLOCATABLE :: WaveDynPC( :) ! Discrete Fourier transform of the instantaneous dynamic pressure of incident waves before applying stretching at the zi-coordinates for points (N/m^2) + COMPLEX(SiKi), ALLOCATABLE :: WaveVelCHx(:) ! Discrete Fourier transform of the instantaneous horizontal velocity of incident waves before applying stretching at the zi-coordinates for points (m/s) + COMPLEX(SiKi), ALLOCATABLE :: WaveVelCHy(:) ! Discrete Fourier transform of the instantaneous horizontal velocity in x-direction of incident waves before applying stretching at the zi-coordinates for points (m/s) + COMPLEX(SiKi), ALLOCATABLE :: WaveVelCV( :) ! Discrete Fourier transform of the instantaneous vertical velocity in y-direction of incident waves before applying stretching at the zi-coordinates for points (m/s) + COMPLEX(SiKi) :: WGNC ! Discrete Fourier transform of the realization of a White Gaussian Noise (WGN) time series process with unit variance for the current frequency component (-) + + INTEGER(IntKi) :: ErrStatTmp + INTEGER(IntKi) :: ErrStat2 + CHARACTER(120) :: ErrMsg2 + CHARACTER(120) :: RoutineName = 'SetupWaveKin' + + IF (LEN_TRIM(WaterKinString) == 0) THEN + ! If the input is empty (not provided), there are no water kinematics to be included + p%WaveKin = 0 + p%Current = 0 + return + + ELSE IF (SCAN(WaterKinString, "abcdfghijklmnopqrstuvwxyzABCDFGHIJKLMNOPQRSTUVWXYZ") == 0) THEN + ! If the input has no letters, let's assume it's a number + print *, "ERROR WaveKin option does not currently support numeric entries. It must be a filename." + p%WaveKin = 0 + p%Current = 0 + return + END IF + + + ! otherwise interpret the input as a file name to load the bathymetry lookup data from + PRINT *, "found a letter in the depth value so will try to load a water kinematics input file" + + + ! -------- load water kinematics input file ------------- + + FileName = TRIM(WaterKinString) + UnEcho=-1 + CALL GetNewUnit( UnIn ) + CALL OpenFInpFile( UnIn, FileName, ErrStat2, ErrMsg2); if(Failed()) return + + + CALL ReadCom( UnIn, FileName, 'MoorDyn water kinematics input file header', ErrStat2, ErrMsg2, UnEcho); if(Failed()) return + CALL ReadCom( UnIn, FileName, 'MoorDyn water kinematics input file header', ErrStat2, ErrMsg2, UnEcho); if(Failed()) return + ! ----- waves ----- + CALL ReadCom( UnIn, FileName, 'waves header', ErrStat2, ErrMsg2, UnEcho); if(Failed()) return + CALL ReadVar( UnIn, FileName, p%WaveKin , 'WaveKinMod' , 'WaveKinMod' , ErrStat2, ErrMsg2, UnEcho); if(Failed()) return + CALL ReadVar( UnIn, FileName, WaveKinFile, 'WaveKinFile', 'WaveKinFile' , ErrStat2, ErrMsg2, UnEcho); if(Failed()) return + CALL ReadVar( UnIn, FileName, p%dtWave , 'dtWave', 'time step for waves', ErrStat2, ErrMsg2, UnEcho); if(Failed()) return + CALL ReadVar( UnIn, FileName, WaveDir , 'WaveDir' , 'wave direction', ErrStat2, ErrMsg2, UnEcho); if(Failed()) return + ! X grid points + READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type + READ(UnIn,*, IOSTAT=ErrStat2) entries2 ! get entries as string to be processed + CALL gridAxisCoords(coordtype, entries2, p%pxWave, p%nxWave, ErrStat2, ErrMsg2) + ! Y grid points + READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type + READ(UnIn,*, IOSTAT=ErrStat2) entries2 ! get entries as string to be processed + CALL gridAxisCoords(coordtype, entries2, p%pyWave, p%nyWave, ErrStat2, ErrMsg2) + ! Z grid points + READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type + READ(UnIn,*, IOSTAT=ErrStat2) entries2 ! get entries as string to be processed + CALL gridAxisCoords(coordtype, entries2, p%pzWave, p%nzWave, ErrStat2, ErrMsg2) + ! ----- current ----- + CALL ReadCom( UnIn, FileName, 'current header', ErrStat2, ErrMsg2, UnEcho); if(Failed()) return + CALL ReadVar( UnIn, FileName, p%Current, 'CurrentMod', 'CurrentMod', ErrStat2, ErrMsg2, UnEcho); if(Failed()) return + CALL ReadCom( UnIn, FileName, 'current profile header', ErrStat2, ErrMsg2, UnEcho); if(Failed()) return + CALL ReadCom( UnIn, FileName, 'current profile header', ErrStat2, ErrMsg2, UnEcho); if(Failed()) return + ! current profile table... (read through to end of file or ---) + DO I=1,100 + READ(UnIn, *, IOSTAT=ErrStat2) pzCurrentTemp(i), uxCurrentTemp(i), uyCurrentTemp(i) ! read into a line + if (ErrStat2 /= 0) then + p%nzCurrent = i-1 ! save number of valid current depth points in profile + EXIT ! break out of the loop if it couldn't read the line (i.e. if at end of file) + end if + if (i == 100) then + print*,"WARNING: MD can handle a maximum of 100 current profile points" + exit + end if + END DO + + + CLOSE(UnIn) + + + ! ------------------- start with wave kinematics ----------------------- + + ! WaveKin options: 0 - none or set externally during the sim (Waves object not needed unless there's current) [default] + ! 1 - set externally for each node in each object (Waves object not needed unless there's current) (TBD) + ! 2 - set from inputted wave elevation FFT, grid approach* (TBD) + ! 3 - set from inputted wave elevation time series, grid approach* [supported] + ! 4 - set from inputted wave elevation FFT, node approach (TBD) + ! 5 - set from inputted wave elevation time series, node approach (TBD) + ! 6 - set from inputted velocity, acceleration, and wave elevation grid data (TBD)** + + ! Current options: 0 - no currents or set externally (as part of WaveKin =0 or 1 approach) [default] + ! 1 - read in steady current profile, grid approach (current_profile.txt)** [supported] + ! 2 - read in dynamic current profile, grid approach (current_profile_dynamic.txt)** (TBD) + ! 3 - read in steady current profile, node approach (current_profile.txt) (TBD) + ! 4 - read in dynamic current profile, node approach (current_profile_dynamic.txt) (TBD) + + ! * the first call to any of these will attempt to load water_grid.txt to define the grid to put things on + ! ** if a grid has already been set, these will interpolate onto it, otherwise they'll make a new grid based on their provided coordinates + + ! NOTE: lots of partial code is available from MD-C for supporting various wave kinematics input options + + ! WaveKin and Current compatibility check could go here in future + + + ! --------------------- set from inputted wave elevation time series, grid approach ------------------- + if (p%WaveKin == 3) then + + + IF ( LEN_TRIM( WaveKinFile ) == 0 ) THEN + CALL SetErrStat( ErrID_Fatal,'WaveKinFile must not be an empty string.',ErrStat, ErrMsg, RoutineName); return + RETURN + END IF + + !IF ( PathIsRelative( WaveKinFile ) ) THEN ! properly handle relative path <<< + ! CALL GetPath( TRIM(InitInp%InputFile), TmpPath ) + ! WaveKinFile = TRIM(TmpPath)//TRIM(WaveKinFile) + !END IF + + ! note: following is adapted from MoorDyn_Driver + + CALL GetNewUnit( UnElev ) + + CALL OpenFInpFile ( UnElev, WaveKinFile, ErrStat2, ErrMsg2 ); if(Failed()) return + + print *, 'Reading wave elevation data from ', trim(WaveKinFile) + + ! Read through length of file to find its length + i = 1 ! start counter + DO + READ(UnElev,'(A)',IOSTAT=ErrStat2) Line !read into a line + IF (ErrStat2 /= 0) EXIT ! break out of the loop if it couldn't read the line (i.e. if at end of file) + i = i+1 + END DO + + ! rewind to start of input file to re-read things now that we know how long it is + REWIND(UnElev) + + ntIn = i-3 ! save number of lines of file + + + ! allocate space for input wave elevation array (including time column) + CALL AllocAry(WaveTimeIn, ntIn, 'WaveTimeIn', ErrStat2, ErrMsg2 ); if(Failed()) return + CALL AllocAry(WaveElevIn, ntIn, 'WaveElevIn', ErrStat2, ErrMsg2 ); if(Failed()) return + + ! read the data in from the file + READ(UnElev,'(A)',IOSTAT=ErrStat2) Line ! skip the first two lines as headers + READ(UnElev,'(A)',IOSTAT=ErrStat2) Line ! + + DO i = 1, ntIn + READ (UnElev, *, IOSTAT=ErrStat2) WaveTimeIn(i), WaveElevIn(i) + + IF ( ErrStat2 /= 0 ) THEN + CALL SetErrStat( ErrID_Fatal,'Error reading WaveElev input file.',ErrStat, ErrMsg, RoutineName); return + END IF + END DO + + ! Close the inputs file + CLOSE ( UnElev ) + + print *, "Read ", ntIn, " time steps from input file." + + ! if (WaveTimeIn(ntIn) < TMax) then <<<< need to handle if time series is too short? + + ! specify stepping details + p%ntWave = CEILING(Tmax/p%dtWave) ! number of wave time steps + + + ! allocate space for processed reference wave elevation time series + CALL AllocAry(WaveElev0, p%ntWave, 'WaveElev0', ErrStat2, ErrMsg2 ); if(Failed()) return + + ! go through and interpolate (should replace with standard function) + DO i = 1, p%ntWave + t = p%dtWave*(i-1) + + ! interpolation routine + DO iIn = 1,ntIn-1 + IF (WaveTimeIn(iIn+1) > t) THEN ! find the right two points to interpolate between (remember that the first column of PtfmMotIn is time) + frac = (t - WaveTimeIn(iIn) )/( WaveTimeIn(iIn+1) - WaveTimeIn(iIn) ) ! interpolation fraction (0-1) between two interpolation points + WaveElev0(i) = WaveElevIn(iIn) + frac*(WaveElevIn(iIn+1) - WaveElevIn(iIn)) ! get interpolated wave elevation + EXIT ! break out of the loop for this time step once we've done its interpolation + END IF + END DO + END DO + + ! note: following is adapted from UserWaves.v90 UserWaveElevations_Init + + + + ! Set new value for NStepWave so that the FFT algorithms are efficient. We will use the values passed in rather than what is read from the file + + IF ( MOD(p%ntWave,2) == 1 ) p%ntWave = p%ntWave + 1 ! Set NStepWave to an even integer + NStepWave2 = MAX( p%ntWave/2, 1 ) ! Make sure that NStepWave is an even product of small factors (PSF) that is + NStepWave = 2*PSF ( NStepWave2, 9 ) ! greater or equal to WaveTMax/WaveDT to ensure that the FFT is efficient. + NStepWave2 = NStepWave/2 ! Update the value of NStepWave2 based on the value needed for NStepWave. + WaveTMax = NStepWave*p%dtWave ! Update the value of WaveTMax based on the value needed for NStepWave. + WaveDOmega = TwoPi/TMax ! Compute the frequency step for incident wave calculations. + p%ntWave = NStepWave + + + + + ! Allocate array to hold the wave elevations for calculation of FFT. + ALLOCATE ( TmpFFTWaveElev( 0:NStepWave-1 ), STAT=ErrStatTmp ) + IF (ErrStatTmp /= 0) CALL SetErrStat(ErrID_Fatal,'Cannot allocate array TmpFFTWaveElev.',ErrStat,ErrMsg,RoutineName) + + ! Allocate frequency array for the wave elevation information in frequency space + ALLOCATE ( WaveElevC0(2, 0:NStepWave2 ), STAT=ErrStatTmp ) + IF (ErrStatTmp /= 0) CALL SetErrStat(ErrID_Fatal,'Cannot allocate array WaveElevC0.',ErrStat,ErrMsg,RoutineName) + + + ! Now check if all the allocations worked properly + IF ( ErrStat >= AbortErrLev ) THEN + CALL CleanUp() + RETURN + END IF + + ! Set the values + TmpFFTWaveElev = 0.0_DbKi + WaveElevC0(:,:) = 0.0_DbKi + + + ! Copy values over + DO I=0,MIN(p%ntWave, NStepWave-1) + TmpFFTWaveElev(I) = WaveElev0(I) + ENDDO + + ! Initialize the FFT + CALL InitFFT ( NStepWave, FFT_Data, .FALSE., ErrStatTmp ) + CALL SetErrStat(ErrStatTmp,'Error occured while initializing the FFT.',ErrStat,ErrMsg,RoutineName); if(Failed()) return + + ! Apply the forward FFT to get the real and imaginary parts of the frequency information. + CALL ApplyFFT_f ( TmpFFTWaveElev(:), FFT_Data, ErrStatTmp ) ! Note that the TmpFFTWaveElev now contains the real and imaginary bits. + CALL SetErrStat(ErrStatTmp,'Error occured while applying the forwards FFT to TmpFFTWaveElev array.',ErrStat,ErrMsg,RoutineName); if(Failed()) return + + ! Copy the resulting TmpFFTWaveElev(:) data over to the WaveElevC0 array + DO I=1,NStepWave2-1 + WaveElevC0 (1,I) = TmpFFTWaveElev(2*I-1) + WaveElevC0 (2,I) = TmpFFTWaveElev(2*I) + ENDDO + WaveElevC0(:,NStepWave2) = 0.0_SiKi + + CALL ExitFFT(FFT_Data, ErrStatTmp) + CALL SetErrStat(ErrStatTmp,'Error occured while cleaning up after the FFTs.', ErrStat,ErrMsg,RoutineName); if(Failed()) return + + + IF (ALLOCATED( WaveElev0 )) DEALLOCATE( WaveElev0 , STAT=ErrStatTmp) + IF (ALLOCATED( TmpFFTWaveElev )) DEALLOCATE( TmpFFTWaveElev, STAT=ErrStatTmp) + + + + ! note: following is a very streamlined adaptation from from Waves.v90 VariousWaves_Init + + ! allocate all the wave kinematics FFT arrays + ALLOCATE( WaveNmbr (0:NStepWave2), STAT=ErrStatTmp); CALL SetErrStat(ErrStatTmp,'Cannot allocate WaveNmbr. ',ErrStat,ErrMsg,RoutineName) + ALLOCATE( tmpComplex(0:NStepWave2), STAT=ErrStatTmp); CALL SetErrStat(ErrStatTmp,'Cannot allocate tmpComplex.',ErrStat,ErrMsg,RoutineName) + ALLOCATE( WaveElevC (0:NStepWave2), STAT=ErrStatTmp); CALL SetErrStat(ErrStatTmp,'Cannot allocate WaveElevC .',ErrStat,ErrMsg,RoutineName) + ALLOCATE( WaveDynPC (0:NStepWave2), STAT=ErrStatTmp); CALL SetErrStat(ErrStatTmp,'Cannot allocate WaveDynPC .',ErrStat,ErrMsg,RoutineName) + ALLOCATE( WaveVelCHx(0:NStepWave2), STAT=ErrStatTmp); CALL SetErrStat(ErrStatTmp,'Cannot allocate WaveVelCHx.',ErrStat,ErrMsg,RoutineName) + ALLOCATE( WaveVelCHy(0:NStepWave2), STAT=ErrStatTmp); CALL SetErrStat(ErrStatTmp,'Cannot allocate WaveVelCHy.',ErrStat,ErrMsg,RoutineName) + ALLOCATE( WaveVelCV (0:NStepWave2), STAT=ErrStatTmp); CALL SetErrStat(ErrStatTmp,'Cannot allocate WaveVelCV .',ErrStat,ErrMsg,RoutineName) + ALLOCATE( WaveAccCHx(0:NStepWave2), STAT=ErrStatTmp); CALL SetErrStat(ErrStatTmp,'Cannot allocate WaveAccCHx.',ErrStat,ErrMsg,RoutineName) + ALLOCATE( WaveAccCHy(0:NStepWave2), STAT=ErrStatTmp); CALL SetErrStat(ErrStatTmp,'Cannot allocate WaveAccCHy.',ErrStat,ErrMsg,RoutineName) + ALLOCATE( WaveAccCV (0:NStepWave2), STAT=ErrStatTmp); CALL SetErrStat(ErrStatTmp,'Cannot allocate WaveAccCV .',ErrStat,ErrMsg,RoutineName) + + ! allocate time series grid data arrays (now that we know the number of time steps coming from the IFFTs) + CALL allocateKinematicsArrays() + + + ! Set the CosWaveDir and SinWaveDir values + CosWaveDir=COS(D2R*WaveDir) + SinWaveDir=SIN(D2R*WaveDir) + + ! get wave number array once + DO I = 0, NStepWave2 + WaveNmbr(i) = WaveNumber ( dble(I*WaveDOmega), p%g, p%WtrDpth ) + tmpComplex(I) = CMPLX(WaveElevC0(1,I), WaveElevC0(2,I)) + END DO + + ! set up FFTer for doing IFFTs + CALL InitFFT ( NStepWave, FFT_Data, .TRUE., ErrStatTmp ) + CALL SetErrStat(ErrStatTmp,'Error occured while initializing the FFT.', ErrStat, ErrMsg, routineName); if(Failed()) return + + ! Loop through all points where the incident wave kinematics will be computed + do ix = 1,p%nxWave + do iy = 1,p%nyWave + do iz = 1,p%nzWave + + ! Compute the discrete Fourier transform of the incident wave kinematics + do I = 0, NStepWave2 ! Loop through the positive frequency components (including zero) of the discrete Fourier transforms + + Omega = I*WaveDOmega + ImagOmega = ImagNmbr*Omega + + WaveElevC (i) = EXP( -ImagNmbr*WaveNmbr(i)*( p%pxWave(ix)*CosWaveDir + p%pyWave(iy)*SinWaveDir )) + WaveDynPC (i) = p%rhoW*p%g* tmpComplex(i)* WaveElevC(i) * COSHNumOvrCOSHDen( WaveNmbr(i), p%WtrDpth, DBLE(p%pzWave(iz)) ) + WaveVelCHx(i) = CosWaveDir*Omega*tmpComplex(i)* WaveElevC(i) * COSHNumOvrSINHDen( WaveNmbr(i), p%WtrDpth, DBLE(p%pzWave(iz)) ) + WaveVelCHy(i) = SinWaveDir*Omega*tmpComplex(i)* WaveElevC(i) * COSHNumOvrSINHDen( WaveNmbr(i), p%WtrDpth, DBLE(p%pzWave(iz)) ) + WaveVelCV (i) = ImagOmega*tmpComplex(i)* WaveElevC(i) * SINHNumOvrSINHDen( WaveNmbr(i), p%WtrDpth, DBLE(p%pzWave(iz)) ) + WaveAccCHx(i) = ImagOmega*WaveVelCHx(I) + WaveAccCHy(i) = ImagOmega*WaveVelCHy(I) + WaveAccCV (i) = ImagOmega*WaveVelCV (I) + end do ! I, frequencies + + ! now IFFT all the wave kinematics except surface elevation and save it into the grid of data + CALL ApplyFFT_cx( p%PDyn (:,iz,iy,ix), WaveDynPC , FFT_Data, ErrStatTmp ); CALL SetErrStat(ErrStatTmp,'Error IFFTing WaveDynP.', ErrStat,ErrMsg,RoutineName) + CALL ApplyFFT_cx( p%uxWave(:,iz,iy,ix), WaveVelCHx, FFT_Data, ErrStatTmp ); CALL SetErrStat(ErrStatTmp,'Error IFFTing WaveVelHx.',ErrStat,ErrMsg,RoutineName) + CALL ApplyFFT_cx( p%uyWave(:,iz,iy,ix), WaveVelCHy, FFT_Data, ErrStatTmp ); CALL SetErrStat(ErrStatTmp,'Error IFFTing WaveVelHy.',ErrStat,ErrMsg,RoutineName) + CALL ApplyFFT_cx( p%uzWave(:,iz,iy,ix), WaveVelCV , FFT_Data, ErrStatTmp ); CALL SetErrStat(ErrStatTmp,'Error IFFTing WaveVelV.', ErrStat,ErrMsg,RoutineName) + CALL ApplyFFT_cx( p%axWave(:,iz,iy,ix), WaveAccCHx, FFT_Data, ErrStatTmp ); CALL SetErrStat(ErrStatTmp,'Error IFFTing WaveAccHx.',ErrStat,ErrMsg,RoutineName) + CALL ApplyFFT_cx( p%ayWave(:,iz,iy,ix), WaveAccCHy, FFT_Data, ErrStatTmp ); CALL SetErrStat(ErrStatTmp,'Error IFFTing WaveAccHy.',ErrStat,ErrMsg,RoutineName) + CALL ApplyFFT_cx( p%azWave(:,iz,iy,ix), WaveAccCV , FFT_Data, ErrStatTmp ); CALL SetErrStat(ErrStatTmp,'Error IFFTing WaveAccV.', ErrStat,ErrMsg,RoutineName) + + end do ! iz + + ! IFFT wave elevation here because it's only at the surface + CALL ApplyFFT_cx( p%zeta(:,iy,ix) , WaveElevC , FFT_Data, ErrStatTmp ); CALL SetErrStat(ErrStatTmp,'Error IFFTing WaveElev.', ErrStat,ErrMsg,RoutineName) + + end do ! iy + end do ! ix + + ! could also reproduce the wave elevation at 0,0,0 on a separate channel for verification... + + CALL ExitFFT(FFT_Data, ErrStatTmp) + CALL SetErrStat(ErrStatTmp,'Error occured while cleaning up after the IFFTs.', ErrStat,ErrMsg,RoutineName); if(Failed()) return + + end if ! p%WaveKin == 3 + + + ! --------------------------------- now do currents -------------------------------- + if (p%Current == 1) then + + ! allocate current profile arrays to correct size + CALL AllocAry( p%pzCurrent, p%nzCurrent, 'pzCurrent', ErrStat2, ErrMsg2 ); if(Failed()) return + CALL AllocAry( p%uxCurrent, p%nzCurrent, 'uxCurrent', ErrStat2, ErrMsg2 ); if(Failed()) return + CALL AllocAry( p%uyCurrent, p%nzCurrent, 'uyCurrent', ErrStat2, ErrMsg2 ); if(Failed()) return + + ! copy over data + do i = 1,p%nzCurrent + p%pzCurrent(i) = pzCurrentTemp(i) + p%uxCurrent(i) = uxCurrentTemp(i) + p%uyCurrent(i) = uyCurrentTemp(i) + end do + + end if ! p%Current == 1 + + + ! ------------------------------ clean up and finished --------------------------- + CALL cleanup() + + + CONTAINS + + + ! get grid axis coordinates, initialize/record in array, and return size + SUBROUTINE gridAxisCoords(coordtype, entries, coordarray, n, ErrStat, ErrMsg) + + INTEGER(IntKi), INTENT(IN ) :: coordtype + CHARACTER(*), INTENT(INOUT) :: entries + REAL(ReKi), ALLOCATABLE, INTENT(INOUT) :: coordarray(:) + INTEGER(IntKi), INTENT( OUT) :: n + + + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! Error status of the operation + CHARACTER(*), INTENT( OUT) :: ErrMsg ! Error message if ErrStat /= ErrID_None + + REAL(ReKi) :: tempArray (100) + REAL(ReKi) :: dx + INTEGER(IntKi) :: nEntries, I + + ! get array of coordinate entries + CALL stringToArray(entries, nEntries, tempArray) + + ! set number of coordinates + if ( coordtype==0) then ! 0: not used - make one grid point at zero + n = 1; + else if (coordtype==1) then ! 1: list values in ascending order + n = nEntries + else if (coordtype==2) then ! 2: uniform specified by -xlim, xlim, num + n = int(tempArray(3)) + else + print *, "Error: invalid coordinate type specified to gridAxisCoords" + end if + + ! allocate coordinate array + CALL AllocAry(coordarray, n, 'x,y, or z grid points' , ErrStat, ErrMsg) + !ALLOCATE ( coordarray(n), STAT=ErrStat) + + ! fill in coordinates + if ( coordtype==0) then + coordarray(1) = 0.0_ReKi + + else if (coordtype==1) then + coordarray(1:n) = tempArray(1:n) + + else if (coordtype==2) then + coordarray(1) = tempArray(1) + coordarray(n) = tempArray(2) + dx = (coordarray(n)-coordarray(0))/REAL(n-1) + do i=2,n-1 + coordarray(i) = coordarray(1) + REAL(i)*dx + end do + + else + print *, "Error: invalid coordinate type specified to gridAxisCoords" + end if + + print *, "Set water grid coordinates to :" + DO i=1,n + print *, " ", coordarray(i) + end do + + END SUBROUTINE gridAxisCoords + + + ! Extract an array of numbers out of a string with comma-separated numbers (this could go in a more general location) + SUBROUTINE stringToArray(instring, n, outarray) + + CHARACTER(*), INTENT(INOUT) :: instring + INTEGER(IntKi), INTENT( OUT) :: n + REAL(ReKi), INTENT( OUT) :: outarray(100) ! array of output numbers (100 maximum) + + CHARACTER(40) :: tempstring + INTEGER :: pos1, pos2, i + + outarray = 0.0_ReKi + + n = 0 + pos1=1 + + DO + pos2 = INDEX(instring(pos1:), ",") ! find index of next comma + IF (pos2 == 0) THEN ! if there isn't another comma, read the last entry and call it done (this could be the only entry if no commas) + n = n + 1 + READ(instring(pos1:), *) outarray(n) + EXIT + END IF + n = n + 1 + if (n > 100) then + print *, "ERROR - stringToArray cannot do more than 100 entries" + end if + READ(instring(pos1:pos1+pos2-2), *) outarray(n) + + pos1 = pos2+pos1 + END DO + + END SUBROUTINE stringToArray + + + ! allocate water kinematics arrays + SUBROUTINE allocateKinematicsArrays() + ! error check print *, "Error in Waves::makeGrid, a time or space array is size zero." << endl; + + ALLOCATE ( p%uxWave( p%ntWave,p%nzWave,p%nyWave,p%nxWave), STAT=ErrStatTmp) + ALLOCATE ( p%uyWave( p%ntWave,p%nzWave,p%nyWave,p%nxWave), STAT=ErrStatTmp) + ALLOCATE ( p%uzWave( p%ntWave,p%nzWave,p%nyWave,p%nxWave), STAT=ErrStatTmp) + ALLOCATE ( p%axWave( p%ntWave,p%nzWave,p%nyWave,p%nxWave), STAT=ErrStatTmp) + ALLOCATE ( p%ayWave( p%ntWave,p%nzWave,p%nyWave,p%nxWave), STAT=ErrStatTmp) + ALLOCATE ( p%azWave( p%ntWave,p%nzWave,p%nyWave,p%nxWave), STAT=ErrStatTmp) + ALLOCATE ( p%PDyn ( p%ntWave,p%nzWave,p%nyWave,p%nxWave), STAT=ErrStatTmp) + ALLOCATE ( p%zeta ( p%ntWave,p%nyWave,p%nxWave), STAT = ErrStatTmp ) ! 2D grid over x and y only + + END SUBROUTINE allocateKinematicsArrays + + + ! compact way to set the right error status and check if an abort is needed (and do cleanup if so) + LOGICAL FUNCTION Failed() + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'SetupWaterKin') + Failed = ErrStat >= AbortErrLev + if (Failed) call CleanUp() + END FUNCTION Failed + + + SUBROUTINE CleanUp + + !IF (ALLOCATED( WaveElev )) DEALLOCATE( WaveElev, STAT=ErrStatTmp) + !IF (ALLOCATED( WaveTime )) DEALLOCATE( WaveTime, STAT=ErrStatTmp) + IF (ALLOCATED( TmpFFTWaveElev )) DEALLOCATE( TmpFFTWaveElev, STAT=ErrStatTmp) + IF (ALLOCATED( WaveElevC0 )) DEALLOCATE( WaveElevC0, STAT=ErrStatTmp) + + ! >>> missing some things <<< + + IF (ALLOCATED( WaveNmbr )) DEALLOCATE( WaveNmbr , STAT=ErrStatTmp) + IF (ALLOCATED( tmpComplex )) DEALLOCATE( tmpComplex , STAT=ErrStatTmp) + IF (ALLOCATED( WaveElevC )) DEALLOCATE( WaveElevC , STAT=ErrStatTmp) + IF (ALLOCATED( WaveDynPC )) DEALLOCATE( WaveDynPC , STAT=ErrStatTmp) + IF (ALLOCATED( WaveVelCHx )) DEALLOCATE( WaveVelCHx , STAT=ErrStatTmp) + IF (ALLOCATED( WaveVelCHy )) DEALLOCATE( WaveVelCHy , STAT=ErrStatTmp) + IF (ALLOCATED( WaveVelCV )) DEALLOCATE( WaveVelCV , STAT=ErrStatTmp) + IF (ALLOCATED( WaveAccCHx )) DEALLOCATE( WaveAccCHx , STAT=ErrStatTmp) + IF (ALLOCATED( WaveAccCHy )) DEALLOCATE( WaveAccCHy , STAT=ErrStatTmp) + IF (ALLOCATED( WaveAccCV )) DEALLOCATE( WaveAccCV , STAT=ErrStatTmp) + + END SUBROUTINE CleanUp + + + !======================================================================= + FUNCTION WaveNumber ( Omega, g, h ) + + + ! This FUNCTION solves the finite depth dispersion relationship: + ! + ! k*tanh(k*h)=(Omega^2)/g + ! + ! for k, the wavenumber (WaveNumber) given the frequency, Omega, + ! gravitational constant, g, and water depth, h, as inputs. A + ! high order initial guess is used in conjunction with a quadratic + ! Newton's method for the solution with seven significant digits + ! accuracy using only one iteration pass. The method is due to + ! Professor J.N. Newman of M.I.T. as found in routine EIGVAL of + ! the SWIM-MOTION-LINES (SML) software package in source file + ! Solve.f of the SWIM module. + + + + IMPLICIT NONE + + + ! Passed Variables: + + REAL(DbKi), INTENT(IN ) :: g ! Gravitational acceleration (m/s^2) + REAL(DbKi), INTENT(IN ) :: h ! Water depth (meters) + REAL(DbKi), INTENT(IN ) :: Omega ! Wave frequency (rad/s) + REAL(DbKi) :: WaveNumber ! This function = wavenumber, k (1/m) + + + ! Local Variables: + + REAL(DbKi) :: A ! A temporary variable used in the solution. + REAL(DbKi) :: B ! A temporary variable used in the solution. + REAL(DbKi) :: C ! A temporary variable used in the solution. + REAL(DbKi) :: C2 ! A temporary variable used in the solution. + REAL(DbKi) :: CC ! A temporary variable used in the solution. + REAL(DbKi) :: E2 ! A temporary variable used in the solution. + REAL(DbKi) :: X0 ! A temporary variable used in the solution. + + + + ! Compute the wavenumber, unless Omega is zero, in which case, return + ! zero: + + IF ( Omega == 0.0 ) THEN ! When .TRUE., the formulation below is ill-conditioned; thus, the known value of zero is returned. + + + WaveNumber = 0.0 + + + ELSE ! Omega > 0.0; solve for the wavenumber as usual. + + + C = Omega*Omega*h/REAL(g,DbKi) + CC = C*C + + + ! Find X0: + + IF ( C <= 2.0 ) THEN + + X0 = SQRT(C)*( 1.0 + C*( 0.169 + (0.031*C) ) ) + + ELSE + + E2 = EXP(-2.0*C) + + X0 = C*( 1.0 + ( E2*( 2.0 - (12.0*E2) ) ) ) + + END IF + + + ! Find the WaveNumber: + + IF ( C <= 4.8 ) THEN + + C2 = CC - X0*X0 + A = 1.0/( C - C2 ) + B = A*( ( 0.5*LOG( ( X0 + C )/( X0 - C ) ) ) - X0 ) + + WaveNumber = ( X0 - ( B*C2*( 1.0 + (A*B*C*X0) ) ) )/h + + ELSE + + WaveNumber = X0/h + + END IF + + + END IF + + + + RETURN + END FUNCTION WaveNumber + + !======================================================================= + FUNCTION COSHNumOvrCOSHDen ( k, h, z ) + + + ! This FUNCTION computes the shallow water hyperbolic numerator + ! over denominator term in the wave kinematics expressions: + ! + ! COSH( k*( z + h ) )/COSH( k*h ) + ! + ! given the wave number, k, water depth, h, and elevation z, as + ! inputs. + + IMPLICIT NONE + + + ! Passed Variables: + + REAL(SiKi) :: COSHNumOvrCOSHDen ! This function = COSH( k*( z + h ) )/COSH( k*h ) (-) + REAL(DbKi), INTENT(IN ) :: h ! Water depth ( h > 0 ) (meters) + REAL(DbKi), INTENT(IN ) :: k ! Wave number ( k >= 0 ) (1/m) + REAL(DbKi), INTENT(IN ) :: z ! Elevation (-h <= z <= 0 ) (meters) + + + + ! Compute the hyperbolic numerator over denominator: + + IF ( k*h > 89.4_DbKi ) THEN ! When .TRUE., the shallow water formulation will trigger a floating point overflow error; however, COSH( k*( z + h ) )/COSH( k*h ) = EXP( k*z ) + EXP( -k*( z + 2*h ) ) for large k*h. This equals the deep water formulation, EXP( k*z ), except near z = -h, because h > 14.23*wavelength (since k = 2*Pi/wavelength) in this case. + + COSHNumOvrCOSHDen = REAL(EXP( k*z ) + EXP( -k*( z + 2.0_DbKi*h ) )) + + ELSE ! 0 < k*h <= 89.4; use the shallow water formulation. + + COSHNumOvrCOSHDen =REAL( COSH( k*( z + h ) ),R8Ki)/COSH( k*h ) + + END IF + + + + RETURN + END FUNCTION COSHNumOvrCOSHDen +!======================================================================= + FUNCTION COSHNumOvrSINHDen ( k, h, z ) + + + ! This FUNCTION computes the shallow water hyperbolic numerator + ! over denominator term in the wave kinematics expressions: + ! + ! COSH( k*( z + h ) )/SINH( k*h ) + ! + ! given the wave number, k, water depth, h, and elevation z, as + ! inputs. + + + + IMPLICIT NONE + + + ! Passed Variables: + + REAL(SiKi) :: COSHNumOvrSINHDen ! This function = COSH( k*( z + h ) )/SINH( k*h ) (-) + REAL(DbKi), INTENT(IN ) :: h ! Water depth ( h > 0 ) (meters) + REAL(DbKi), INTENT(IN ) :: k ! Wave number ( k >= 0 ) (1/m) + REAL(DbKi), INTENT(IN ) :: z ! Elevation (-h <= z <= 0 ) (meters) + + + + ! Compute the hyperbolic numerator over denominator: + + + IF ( k < EPSILON(0.0_DbKi) ) THEN ! When .TRUE., the shallow water formulation is ill-conditioned; thus, HUGE(k) is returned to approximate the known value of infinity. + + COSHNumOvrSINHDen = HUGE( k ) + + ELSEIF ( k*h > 89.4_DbKi ) THEN ! When .TRUE., the shallow water formulation will trigger a floating point overflow error; however, COSH( k*( z + h ) )/SINH( k*h ) = EXP( k*z ) + EXP( -k*( z + 2*h ) ) for large k*h. This equals the deep water formulation, EXP( k*z ), except near z = -h, because h > 14.23*wavelength (since k = 2*Pi/wavelength) in this case. + + COSHNumOvrSINHDen = EXP( k*z ) + EXP( -k*( z + 2*h ) ) + + ELSE ! 0 < k*h <= 89.4; use the shallow water formulation. + + COSHNumOvrSINHDen = COSH( k*( z + h ) )/SINH( k*h ) + + END IF + + + + RETURN + END FUNCTION COSHNumOvrSINHDen +!======================================================================= + FUNCTION COTH ( X ) + + + ! This FUNCTION computes the hyperbolic cotangent, + ! COSH(X)/SINH(X). + + + USE Precision + + + IMPLICIT NONE + + + ! Passed Variables: + + REAL(DbKi) :: COTH ! This function = COSH( X )/SINH( X ) (-) + REAL(DbKi), INTENT(IN ) :: X ! The argument (-) + + + + ! Compute the hyperbolic cotangent: + + IF ( X == 0.0_DbKi ) THEN ! When .TRUE., the formulation below is ill-conditioned; thus, HUGE(X) is returned to approximate the known value of infinity. + + COTH = HUGE( X ) + + ELSE ! X /= 0.0; use the numerically-stable computation of COTH(X) by means of TANH(X). + + COTH = 1.0_DbKi/TANH( X ) ! = COSH( X )/SINH( X ) + + END IF + + + + RETURN + END FUNCTION COTH + + !======================================================================= + FUNCTION SINHNumOvrSINHDen ( k, h, z ) + + + ! This FUNCTION computes the shallow water hyperbolic numerator + ! over denominator term in the wave kinematics expressions: + ! + ! SINH( k*( z + h ) )/SINH( k*h ) + ! + ! given the wave number, k, water depth, h, and elevation z, as + ! inputs. + + + IMPLICIT NONE + + + ! Passed Variables: + + REAL(SiKi) :: SINHNumOvrSINHDen ! This function = SINH( k*( z + h ) )/SINH( k*h ) (-) + REAL(DbKi), INTENT(IN ) :: h ! Water depth ( h > 0 ) (meters) + REAL(DbKi), INTENT(IN ) :: k ! Wave number ( k >= 0 ) (1/m) + REAL(DbKi), INTENT(IN ) :: z ! Elevation (-h <= z <= 0 ) (meters) + + + + ! Compute the hyperbolic numerator over denominator: + + IF ( k == 0.0_DbKi ) THEN ! When .TRUE., the shallow water formulation is ill-conditioned; thus, the known value of unity is returned. + + SINHNumOvrSINHDen = 1.0 + + ELSEIF ( k*h > 89.4_DbKi ) THEN ! When .TRUE., the shallow water formulation will trigger a floating point overflow error; however, SINH( k*( z + h ) )/SINH( k*h ) = EXP( k*z ) - EXP( -k*( z + 2*h ) ) for large k*h. This equals the deep water formulation, EXP( k*z ), except near z = -h, because h > 14.23*wavelength (since k = 2*Pi/wavelength) in this case. + + SINHNumOvrSINHDen = EXP( k*z ) - EXP( -k*( z + 2.0_DbKi*h ) ) + + ELSE ! 0 < k*h <= 89.4; use the shallow water formulation. + + SINHNumOvrSINHDen = SINH( k*( z + h ) )/SINH( k*h ) + + END IF + + + + RETURN + END FUNCTION SINHNumOvrSINHDen + + END SUBROUTINE SetupWaterKin + + + + + +END MODULE MoorDyn_Misc diff --git a/modules/moordyn/src/MoorDyn_Point.f90 b/modules/moordyn/src/MoorDyn_Point.f90 new file mode 100644 index 0000000000..976af8539b --- /dev/null +++ b/modules/moordyn/src/MoorDyn_Point.f90 @@ -0,0 +1,419 @@ +!********************************************************************************************************************************** +! LICENSING +! Copyright (C) 2020-2021 Alliance for Sustainable Energy, LLC +! Copyright (C) 2015-2019 Matthew Hall +! +! This file is part of MoorDyn. +! +! Licensed under the Apache License, Version 2.0 (the "License"); +! you may not use this file except in compliance with the License. +! You may obtain a copy of the License at +! +! http://www.apache.org/licenses/LICENSE-2.0 +! +! Unless required by applicable law or agreed to in writing, software +! distributed under the License is distributed on an "AS IS" BASIS, +! WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +! See the License for the specific language governing permissions and +! limitations under the License. +! +!********************************************************************************************************************************** +MODULE MoorDyn_Point + + USE MoorDyn_Types + USE MoorDyn_IO + USE NWTC_Library + USE MoorDyn_Misc + USE MoorDyn_Line, only : Line_SetEndKinematics, Line_GetEndStuff + + IMPLICIT NONE + + PRIVATE + + INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output + + PUBLIC :: Connect_Initialize + PUBLIC :: Connect_SetKinematics + PUBLIC :: Connect_SetState + PUBLIC :: Connect_GetStateDeriv + PUBLIC :: Connect_DoRHS + PUBLIC :: Connect_GetCoupledForce + PUBLIC :: Connect_GetNetForceAndMass + PUBLIC :: Connect_AddLine + PUBLIC :: Connect_RemoveLine + + +CONTAINS + + + !-------------------------------------------------------------- + SUBROUTINE Connect_Initialize(Connect, states, m) + + Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object + Real(DbKi), INTENT(INOUT) :: states(6) ! state vector section for this Connection + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + + INTEGER(IntKi) :: l + + + if (Connect%typeNum == 0) then ! error check + + ! pass kinematics to any attached lines so they have initial positions at this initialization stage + DO l=1,Connect%nAttached + IF (wordy > 1) print *, "Connect ", Connect%IdNum, " setting end kinematics of line ", Connect%attached(l), " to ", Connect%r + CALL Line_SetEndKinematics(m%LineList(Connect%attached(l)), Connect%r, Connect%rd, 0.0_DbKi, Connect%Top(l)) + END DO + + + ! assign initial node kinematics to state vector + states(4:6) = Connect%r + states(1:3) = Connect%rd + + + IF (wordy > 0) print *, "Initialized Connection ", Connect%IdNum + + else + print *," Error: wrong Point type given to Connect_Initialize for number ", Connect%idNum + end if + + END SUBROUTINE Connect_Initialize + !-------------------------------------------------------------- + + + !-------------------------------------------------------------- + SUBROUTINE Connect_SetKinematics(Connect, r_in, rd_in, a_in, t, m) + + Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object + Real(DbKi), INTENT(IN ) :: r_in( 3) ! position + Real(DbKi), INTENT(IN ) :: rd_in(3) ! velocity + Real(DbKi), INTENT(IN ) :: a_in(3) ! acceleration (only used for coupled connects) + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + + + INTEGER(IntKi) :: l + + ! store current time + Connect%time = t + + + ! if (Connect%typeNum==0) THEN ! anchor ( <<< to be changed/expanded) ... in MoorDyn F also used for coupled connections + + ! set position and velocity + Connect%r = r_in + Connect%rd = rd_in + Connect%a = a_in + + ! pass latest kinematics to any attached lines + DO l=1,Connect%nAttached + CALL Line_SetEndKinematics(m%LineList(Connect%attached(l)), Connect%r, Connect%rd, t, Connect%Top(l)) + END DO + + ! else + ! + ! PRINT*,"Error: setKinematics called for wrong Connection type. Connection ", Connect%IdNum, " type ", Connect%typeNum + + ! END IF + + + END SUBROUTINE Connect_SetKinematics + !-------------------------------------------------------------- + + !-------------------------------------------------------------- + SUBROUTINE Connect_SetState(Connect, X, t, m) + + Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object + Real(DbKi), INTENT(IN ) :: X(:) ! state vector section for this line + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + + INTEGER(IntKi) :: l ! index of segments or nodes along line + INTEGER(IntKi) :: J ! index + + + ! store current time + Connect%time = t + + ! from state values, get r and rdot values + DO J=1,3 + Connect%r( J) = X(3 + J) ! get positions + Connect%rd(J) = X( J) ! get velocities + END DO + + ! pass latest kinematics to any attached lines + DO l=1,Connect%nAttached + CALL Line_SetEndKinematics(m%LineList(Connect%attached(l)), Connect%r, Connect%rd, t, Connect%Top(l)) + END DO + + END SUBROUTINE Connect_SetState + !-------------------------------------------------------------- + + !-------------------------------------------------------------- + SUBROUTINE Connect_GetStateDeriv(Connect, Xd, m, p) + + Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object + Real(DbKi), INTENT(INOUT) :: Xd(:) ! state derivative vector section for this line + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables + + !INTEGER(IntKi) :: l ! index of attached lines + INTEGER(IntKi) :: J ! index + INTEGER(IntKi) :: K ! index + Real(DbKi) :: Sum1 ! for adding things + + Real(DbKi) :: S(3,3) ! inverse mass matrix + + + CALL Connect_DoRHS(Connect, m, p) + +! // solve for accelerations in [M]{a}={f} using LU decomposition +! double M_tot[9]; // serialize total mass matrix for easy processing +! for (int I=0; I<3; I++) for (int J=0; J<3; J++) M_tot[3*I+J]=M[I][J]; +! double LU[9]; // serialized matrix that will hold LU matrices combined +! Crout(3, M_tot, LU); // perform LU decomposition on mass matrix +! double acc[3]; // acceleration vector to solve for +! solveCrout(3, LU, Fnet, acc); // solve for acceleration vector + + ! solve for accelerations in [M]{a}={f} using LU decomposition +! CALL LUsolve(6, M_out, LU_temp, Fnet_out, y_temp, acc) + + + ! invert node mass matrix + CALL Inverse3by3(S, Connect%M) + + ! accelerations + Connect%a = MATMUL(S, Connect%Fnet) + + ! fill in state derivatives + Xd(4:6) = Connect%rd ! dxdt = V (velocities) + Xd(1:3) = Connect%a ! dVdt = RHS * A (accelerations) + + + ! check for NaNs + DO J = 1, 6 + IF (Is_NaN(Xd(J))) THEN + print *, "NaN detected at time ", Connect%time, " in Point ",Connect%IdNum, " in MoorDyn." + IF (wordy > 1) print *, "state derivatives:" + IF (wordy > 1) print *, Xd + EXIT + END IF + END DO + + END SUBROUTINE Connect_GetStateDeriv + !-------------------------------------------------------------- + + !-------------------------------------------------------------- + SUBROUTINE Connect_DoRHS(Connect, m, p) + + Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connection object + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables + + INTEGER(IntKi) :: l ! index of attached lines + INTEGER(IntKi) :: I ! index + INTEGER(IntKi) :: J ! index + INTEGER(IntKi) :: K ! index + + Real(DbKi) :: Fnet_i(3) ! force from an attached line + Real(DbKi) :: Moment_dummy(3) ! dummy vector to hold unused line end moments + Real(DbKi) :: M_i(3,3) ! mass from an attached line + + + ! start with the Connection's own forces including buoyancy and weight, and its own mass + Connect%Fnet(1) = Connect%conFX + Connect%Fnet(2) = Connect%conFY + Connect%Fnet(3) = Connect%conFZ + Connect%conV*p%rhoW*p%g - Connect%conM*p%g + + Connect%M = 0.0_DbKi ! clear (zero) the connect mass matrix + + DO J = 1,3 + Connect%M (J,J) = Connect%conM ! set the diagonals to the self-mass (to start with) + END DO + + + ! print *, "connection number", Connect%IdNum + ! print *, "attached lines: ", Connect%attached + ! print *, "size of line list" , size(m%LineList) + + ! loop through attached lines, adding force and mass contributions + DO l=1,Connect%nAttached + + ! print *, " l", l + ! print *, Connect%attached(l) + ! print *, m%LineList(Connect%attached(l))%Fnet + ! + ! + ! print *, " attached line ID", m%LineList(Connect%attached(l))%IdNum + + CALL Line_GetEndStuff(m%LineList(Connect%attached(l)), Fnet_i, Moment_dummy, M_i, Connect%Top(l)) + + ! sum quantitites + Connect%Fnet = Connect%Fnet + Fnet_i + Connect%M = Connect%M + M_i + + END DO + + + ! XXXWhen this sub is called, any self weight, buoyancy, or external forcing should have already been + ! added by the calling subroutine. The only thing left is any added mass or drag forces from the connection (e.g. float) + ! itself, which will be added below.XXX + + + ! IF (EqualRealNos(t, 0.0_DbKi)) THEN ! this is old: with current IC gen approach, we skip the first call to the line objects, because they're set AFTER the call to the connects + ! + ! DO J = 1,3 + ! Xd(3+J) = X(J) ! velocities - these are unused in integration + ! Xd(J) = 0.0_DbKi ! accelerations - these are unused in integration + ! END DO + ! ELSE + ! ! from state values, get r and rdot values + ! DO J = 1,3 + ! Connect%r(J) = X(3 + J) ! get positions + ! Connect%rd(J) = X(J) ! get velocities + ! END DO + ! END IF + + + ! add any added mass and drag forces from the Connect body itself + DO J = 1,3 + Connect%Fnet(J) = Connect%Fnet(J) - 0.5 * p%rhoW * Connect%rd(J) * abs(Connect%rd(J)) * Connect%conCdA; ! add drag forces - corrected Nov 24 + Connect%M (J,J) = Connect%M (J,J) + Connect%conV*p%rhoW*Connect%conCa; ! add added mass + + END DO + + ! would this sub ever need to include the m*a inertial term? Is it ever called for coupled connects? <<< + + END SUBROUTINE Connect_DoRHS + !===================================================================== + + + ! calculate the force including inertial loads on connect that is coupled + !-------------------------------------------------------------- + SUBROUTINE Connect_GetCoupledForce(Connect, Fnet_out, m, p) + + Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connect object + Real(DbKi), INTENT( OUT) :: Fnet_out(3) ! force and moment vector about rRef + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + Real(DbKi) :: F_iner(3) ! inertial force + + IF (Connect%typeNum == -1) then + ! calculate forces and masses of connect + CALL Connect_DoRHS(Connect, m, p) + + ! add inertial loads as appropriate + F_iner = -MATMUL(Connect%M, Connect%a) ! inertial loads + Fnet_out = Connect%Fnet + F_iner ! add inertial loads + + ELSE + print *, "Connect_GetCoupledForce called for wrong (uncoupled) Point type in MoorDyn!" + END IF + + END SUBROUTINE Connect_GetCoupledForce + + + ! calculate the force and mass contributions of the connect on the parent body (only for type 3 connects?) + !-------------------------------------------------------------- + SUBROUTINE Connect_GetNetForceAndMass(Connect, rRef, Fnet_out, M_out, m, p) + + Type(MD_Connect), INTENT(INOUT) :: Connect ! the Connect object + Real(DbKi), INTENT(IN ) :: rRef(3) ! global coordinates of reference point (i.e. the parent body) + Real(DbKi), INTENT( OUT) :: Fnet_out(6) ! force and moment vector about rRef + Real(DbKi), INTENT( OUT) :: M_out(6,6) ! mass and inertia matrix about rRef + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + Real(DbKi) :: rRel( 3) ! position of connection relative to the body reference point (global orientation frame) + + + CALL Connect_DoRHS(Connect, m, p) + + rRel = Connect%r - rRef ! vector from body reference point to node + + ! convert net force into 6dof force about body ref point + CALL translateForce3to6DOF(rRel, Connect%Fnet, Fnet_out) + + ! convert mass matrix to 6by6 mass matrix about body ref point + CALL translateMass3to6DOF(rRel, Connect%M, M_out) + + END SUBROUTINE Connect_GetNetForceAndMass + + + + + ! this function handles assigning a line to a connection node + !-------------------------------------------------------------- + SUBROUTINE Connect_AddLine(Connect, lineID, TopOfLine) + + Type(MD_Connect), INTENT (INOUT) :: Connect ! the Connection object + Integer(IntKi), INTENT( IN ) :: lineID + Integer(IntKi), INTENT( IN ) :: TopOfLine + + IF (wordy > 0) Print*, "L", lineID, "->C", Connect%IdNum + + IF (Connect%nAttached <10) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. + Connect%nAttached = Connect%nAttached + 1 ! add the line to the number connected + Connect%Attached(Connect%nAttached) = lineID + Connect%Top(Connect%nAttached) = TopOfLine ! attached to line ... 1 = top/fairlead(end B), 0 = bottom/anchor(end A) + ELSE + Print*, "Too many lines connected to Point ", Connect%IdNum, " in MoorDyn!" + END IF + + END SUBROUTINE Connect_AddLine + + + ! this function handles removing a line from a connection node + !-------------------------------------------------------------- + SUBROUTINE Connect_RemoveLine(Connect, lineID, TopOfLine, rEnd, rdEnd) + + Type(MD_Connect), INTENT (INOUT) :: Connect ! the Connection object + Integer(IntKi), INTENT( IN ) :: lineID + Integer(IntKi), INTENT( OUT) :: TopOfLine + REAL(DbKi), INTENT(INOUT) :: rEnd(3) + REAL(DbKi), INTENT(INOUT) :: rdEnd(3) + + Integer(IntKi) :: l,m,J + + DO l = 1,Connect%nAttached ! look through attached lines + + IF (Connect%Attached(l) == lineID) THEN ! if this is the line's entry in the attachment list + + TopOfLine = Connect%Top(l); ! record which end of the line was attached + + DO m = l,Connect%nAttached-1 + + Connect%Attached(m) = Connect%Attached(m+1) ! move subsequent line links forward one spot in the list to eliminate this line link + Connect%Top( m) = Connect%Top(m+1) + + Connect%nAttached = Connect%nAttached - 1 ! reduce attached line counter by 1 + + ! also pass back the kinematics at the end + DO J = 1,3 + rEnd( J) = Connect%r( J) + rdEnd(J) = Connect%rd(J) + END DO + + print*, "Detached line ", lineID, " from Connection ", Connect%IdNum + + EXIT + END DO + + IF (l == Connect%nAttached) THEN ! detect if line not found + print *, "Error: failed to find line to remove during removeLineFromConnect call to connection ", Connect%IdNum, ". Line ", lineID + END IF + + END IF + + END DO + + END SUBROUTINE Connect_RemoveLine + + + +END MODULE MoorDyn_Point diff --git a/modules/moordyn/src/MoorDyn_Rod.f90 b/modules/moordyn/src/MoorDyn_Rod.f90 new file mode 100644 index 0000000000..6bee471e54 --- /dev/null +++ b/modules/moordyn/src/MoorDyn_Rod.f90 @@ -0,0 +1,1140 @@ +!********************************************************************************************************************************** +! LICENSING +! Copyright (C) 2020-2021 Alliance for Sustainable Energy, LLC +! Copyright (C) 2015-2019 Matthew Hall +! +! This file is part of MoorDyn. +! +! Licensed under the Apache License, Version 2.0 (the "License"); +! you may not use this file except in compliance with the License. +! You may obtain a copy of the License at +! +! http://www.apache.org/licenses/LICENSE-2.0 +! +! Unless required by applicable law or agreed to in writing, software +! distributed under the License is distributed on an "AS IS" BASIS, +! WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +! See the License for the specific language governing permissions and +! limitations under the License. +! +!********************************************************************************************************************************** +MODULE MoorDyn_Rod + + USE MoorDyn_Types + USE MoorDyn_IO + USE NWTC_Library + USE MoorDyn_Misc + USE MoorDyn_Line, only : Line_SetEndKinematics, Line_GetEndStuff, Line_SetEndOrientation, Line_GetEndSegmentInfo + + IMPLICIT NONE + + PRIVATE + + INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output + + PUBLIC :: Rod_Setup + PUBLIC :: Rod_Initialize + PUBLIC :: Rod_SetKinematics + PUBLIC :: Rod_SetState + PUBLIC :: Rod_GetStateDeriv + PUBLIC :: Rod_DoRHS + PUBLIC :: Rod_GetCoupledForce + PUBLIC :: Rod_GetNetForceAndMass + PUBLIC :: Rod_AddLine + PUBLIC :: Rod_RemoveLine + + + +CONTAINS + + + !----------------------------------------------------------------------- + SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) + + TYPE(MD_Rod), INTENT(INOUT) :: Rod ! the single rod object of interest + TYPE(MD_RodProp), INTENT(INOUT) :: RodProp ! the single rod property set for the line of interest + REAL(DbKi), INTENT(IN) :: endCoords(6) + REAL(DbKi), INTENT(IN) :: rhoW + INTEGER, INTENT( INOUT ) :: ErrStat ! returns a non-zero value when an error occurs + CHARACTER(*), INTENT( INOUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None + + INTEGER(4) :: J ! Generic index + INTEGER(4) :: K ! Generic index + INTEGER(IntKi) :: N + + N = Rod%N ! number of segments in this line (for code readability) + + ! -------------- save some section properties to the line object itself ----------------- + + Rod%d = RodProp%d + Rod%rho = RodProp%w/(Pi/4.0 * Rod%d * Rod%d) + + Rod%Can = RodProp%Can + Rod%Cat = RodProp%Cat + Rod%Cdn = RodProp%Cdn + Rod%Cdt = RodProp%Cdt + Rod%CaEnd = RodProp%CaEnd + Rod%CdEnd = RodProp%CdEnd + + + ! allocate node positions and velocities (NOTE: these arrays start at ZERO) + ALLOCATE ( Rod%r(3, 0:N), Rod%rd(3, 0:N), STAT = ErrStat ) ! <<<<<< add error checks here + IF ( ErrStat /= ErrID_None ) print *, "Alloc error 1 in MoorDyn" + + ! allocate segment scalar quantities + ALLOCATE ( Rod%l(N), Rod%V(N), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) print *, "Alloc error 2 in MoorDyn" + + ! allocate water related vectors + ALLOCATE ( Rod%U(3, 0:N), Rod%Ud(3, 0:N), Rod%zeta(0:N), Rod%PDyn(0:N), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) print *, "Alloc error 3 in MoorDyn" + ! set to zero initially (important of wave kinematics are not being used) + Rod%U = 0.0_DbKi + Rod%Ud = 0.0_DbKi + Rod%zeta = 0.0_DbKi + Rod%PDyn = 0.0_DbKi + + ! allocate node force vectors + ALLOCATE ( Rod%W(3, 0:N), Rod%Bo(3, 0:N), Rod%Dp(3, 0:N), Rod%Dq(3, 0:N), Rod%Ap(3, 0:N), & + Rod%Aq(3, 0:N), Rod%Pd(3, 0:N), Rod%B(3, 0:N), Rod%Fnet(3, 0:N), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) print *, "Alloc error 4 in MoorDyn" + + ! allocate mass and inverse mass matrices for each node (including ends) + ALLOCATE ( Rod%M(3, 3, 0:N), STAT = ErrStat ) + IF ( ErrStat /= ErrID_None ) print *, "Alloc error 5 in MoorDyn" + + + + ! ------------------------- set some geometric properties and the starting kinematics ------------------------- + + CALL UnitVector(endCoords(1:3), endCoords(4:6), Rod%q, Rod%UnstrLen) ! get Rod axis direction vector and Rod length + + ! set Rod positions if applicable + if (Rod%typeNum==0) then ! for an independent rod, set the position right off the bat + + Rod%r6(1:3) = endCoords(1:3) ! (end A coordinates) + Rod%v6(1:3) = 0.0_DbKi ! (end A velocity, unrotated axes) + + Rod%r6(4:6) = Rod%q ! (Rod direction unit vector) + Rod%v6(4:6) = 0.0_DbKi ! (rotational velocities about unrotated axes) + + + else if (abs(Rod%typeNum)==1) then ! for a pinned rod, just set the orientation (position will be set later by parent object) + + Rod%r6(4:6) = Rod%q ! (Rod direction unit vector) + Rod%v6(4:6) = 0.0_DbKi ! (rotational velocities about unrotated axes) + + end if + ! otherwise (for a fixed rod) the positions will be set by the parent body or via coupling + + + + ! save mass for future calculations >>>> should calculate I_l and I_r here in future <<<< + Rod%mass = Rod%UnstrLen*RodProp%w + + + ! assign values for l and V + DO J=1,N + Rod%l(J) = Rod%UnstrLen/REAL(N, DbKi) + Rod%V(J) = Rod%l(J)*0.25*Pi*RodProp%d*RodProp%d + END DO + + + + ! set gravity and bottom contact forces to zero initially (because the horizontal components should remain at zero) + DO J = 0,N + DO K = 1,3 + Rod%W(K,J) = 0.0_DbKi + Rod%B(K,J) = 0.0_DbKi + END DO + END DO + + ! >>> why are the above assignments making l V W and B appear as "undefined pointer/array"s??? <<< + + IF (wordy > 0) print *, "Set up Rod ",Rod%IdNum, ", type ", Rod%typeNum + + ! need to add cleanup sub <<< + + END SUBROUTINE Rod_Setup + !-------------------------------------------------------------- + + + + + ! Make output file for Rod and set end kinematics of any attached lines. + ! For free Rods, fill in the initial states into the state vector. + ! Notes: r6 and v6 must already be set. + ! ground- or body-pinned rods have already had setKinematics called to set first 3 elements of r6, v6. + !-------------------------------------------------------------- + SUBROUTINE Rod_Initialize(Rod, states, m) + + TYPE(MD_Rod), INTENT(INOUT) :: Rod ! the rod object + Real(DbKi), INTENT(INOUT) :: states(:) ! state vector section for this line + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + + + INTEGER(IntKi) :: l ! index of segments or nodes along line + REAL(DbKi) :: rRef(3) ! reference position of mesh node + REAL(DbKi) :: OrMat(3,3) ! DCM for body orientation based on r6_in + + IF (wordy > 0) print *, "initializing Rod ", Rod%idNum + + ! the r6 and v6 vectors should have already been set + ! r and rd of ends have already been set by setup function or by parent object <<<<< right? <<<<< + + + ! Pass kinematics to any attached lines (this is just like what a Connection does, except for both ends) + ! so that they have the correct initial positions at this initialization stage. + + if (Rod%typeNum >- 2) CALL Rod_SetDependentKin(Rod, 0.0_DbKi, m) ! don't call this for type -2 coupled Rods as it's already been called + + + ! assign the resulting kinematics to its part of the state vector (only matters if it's an independent Rod) + + if (Rod%typeNum == 0) then ! free Rod type + + states(1:6) = 0.0_DbKi ! zero velocities for initialization + states(7:9) = Rod%r(:,0) ! end A position + states(10:12) = Rod%q ! rod direction unit vector + + else if (abs(Rod%typeNum) ==1 ) then ! pinned rod type (coupled or attached to something previously via setPinKin) + + states(1:3) = 0.0_DbKi ! zero velocities for initialization + states(4:6) = Rod%q ! rod direction unit vector + + end if + + ! note: this may also be called by a coupled rod (type = -1) in which case states will be empty + + + END SUBROUTINE Rod_Initialize + !-------------------------------------------------------------- + + + + + ! set kinematics for Rods ONLY if they are attached to a body (including a coupled body) or coupled (otherwise shouldn't be called) + !-------------------------------------------------------------- + SUBROUTINE Rod_SetKinematics(Rod, r6_in, v6_in, a6_in, t, m) + + Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object + Real(DbKi), INTENT(IN ) :: r6_in(6) ! 6-DOF position + Real(DbKi), INTENT(IN ) :: v6_in(6) ! 6-DOF velocity + Real(DbKi), INTENT(IN ) :: a6_in(6) ! 6-DOF acceleration (only used for coupled rods) + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + + INTEGER(IntKi) :: l + + Rod%time = t ! store current time + + + if (abs(Rod%typeNum) == 2) then ! rod rigidly coupled to a body, or ground, or coupling point + Rod%r6 = r6_in + Rod%v6 = v6_in + Rod%a6 = a6_in + + call ScaleVector(Rod%r6(4:6), 1.0_DbKi, Rod%r6(4:6)); ! enforce direction vector to be a unit vector + + ! since this rod has no states and all DOFs have been set, pass its kinematics to dependent Lines + CALL Rod_SetDependentKin(Rod, t, m) + + else if (abs(Rod%typeNum) == 1) then ! rod end A pinned to a body, or ground, or coupling point + + ! set Rod *end A only* kinematics based on BCs (linear model for now) + Rod%r6(1:3) = r6_in(1:3) + Rod%v6(1:3) = v6_in(1:3) + Rod%a6(1:3) = a6_in(1:3) + + + ! Rod is pinned so only end A is specified, rotations are left alone and will be + ! handled, along with passing kinematics to dependent lines, by separate call to setState + + else + print *, "Error: Rod_SetKinematics called for a free Rod in MoorDyn." ! <<< + end if + + + ! update Rod direction unit vector (simply equal to last three entries of r6, presumably these were set elsewhere for pinned Rods) + Rod%q = Rod%r6(4:6) + + + + END SUBROUTINE Rod_SetKinematics + !-------------------------------------------------------------- + + ! pass the latest states to the rod if it has any DOFs/states (then update rod end kinematics including attached lines) + !-------------------------------------------------------------- + SUBROUTINE Rod_SetState(Rod, X, t, m) + + Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object + Real(DbKi), INTENT(IN ) :: X(:) ! state vector section for this line + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + + INTEGER(IntKi) :: J ! index + + + ! for a free Rod, there are 12 states: + ! [ x, y, z velocity of end A, then rate of change of u/v/w coordinates of unit vector pointing toward end B, + ! then x, y, z coordinate of end A, u/v/w coordinates of unit vector pointing toward end B] + + ! for a pinned Rod, there are 6 states (rotational only): + ! [ rate of change of u/v/w coordinates of unit vector pointing toward end B, + ! then u/v/w coordinates of unit vector pointing toward end B] + + + ! store current time + Rod%time = t + + + ! copy over state values for potential use during derivative calculations + if (Rod%typeNum == 0) then ! free Rod type + + ! CALL ScaleVector(X(10:12), 1.0, X(10:12)) ! enforce direction vector to be a unit vector <<<< can't do this with FAST frameowrk, could be a problem!! + + ! TODO: add "controller" adjusting state derivatives of X(10:12) to artificially force X(10:12) to remain a unit vector <<<<<<<<<<< + + + Rod%r6(1:3) = X(7:9) ! (end A coordinates) + Rod%v6(1:3) = X(1:3) ! (end A velocity, unrotated axes) + CALL ScaleVector(X(10:12), 1.0_DbKi, Rod%r6(4:6)) !Rod%r6(4:6) = X(10:12) ! (Rod direction unit vector) + Rod%v6(4:6) = X(4:6) ! (rotational velocities about unrotated axes) + + + CALL Rod_SetDependentKin(Rod, t, m) + + else if (abs(Rod%typeNum) == 1) then ! pinned rod type (coupled or attached to something)t previously via setPinKin) + + !CALL ScaleVector(X(4:6), 1.0, X(4:6)) ! enforce direction vector to be a unit vector + + + CALL ScaleVector(X(4:6), 1.0_DbKi, Rod%r6(4:6)) !Rod%r6(3+J) = X(3+J) ! (Rod direction unit vector) + Rod%v6(4:6) = X(1:3) ! (rotational velocities about unrotated axes) + + + CALL Rod_SetDependentKin(Rod, t, m) + + else + print *, "Error: Rod::setState called for a non-free rod type in MoorDyn" ! <<< + end if + + ! update Rod direction unit vector (simply equal to last three entries of r6) + Rod%q = Rod%r6(4:6) + + END SUBROUTINE Rod_SetState + !-------------------------------------------------------------- + + + ! Set the Rod end kinematics then set the kinematics of dependent objects (any attached lines). + ! This also determines the orientation of zero-length rods. + !-------------------------------------------------------------- + SUBROUTINE Rod_SetDependentKin(Rod, t, m) + + Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object + Real(DbKi), INTENT(IN ) :: t ! instantaneous time + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects (for simplicity, since Bodies deal with Rods and Connections) + + INTEGER(IntKi) :: l ! index of segments or nodes along line + INTEGER(IntKi) :: J ! index + INTEGER(IntKi) :: N ! number of segments + + REAL(DbKi) :: qEnd(3) ! unit vector of attached line end segment, following same direction convention as Rod's q vector + REAL(DbKi) :: EIend ! bending stiffness of attached line end segment + REAL(DbKi) :: dlEnd ! stretched length of attached line end segment + REAL(DbKi) :: qMomentSum(3) ! summation of qEnd*EI/dl_stretched (with correct sign) for each attached line + + + ! Initialize variables + qMomentSum = 0.0_DbKi + + ! in future pass accelerations here too? <<<< + + N = Rod%N + + ! from state values, set positions of end nodes + ! end A + Rod%r(:,0) = Rod%r6(1:3) ! positions + Rod%rd(:,0) = Rod%v6(1:3) ! velocities + + !print *, Rod%r6(1:3) + !print *, Rod%r(:,0) + + if (Rod%N > 0) then ! set end B nodes only if the rod isn't zero length + CALL transformKinematicsAtoB(Rod%r6(1:3), Rod%r6(4:6), Rod%UnstrLen, Rod%v6, Rod%r(:,N), Rod%rd(:,N)) ! end B + end if + + ! pass end node kinematics to any attached lines (this is just like what a Connection does, except for both ends) + DO l=1,Rod%nAttachedA + CALL Line_SetEndKinematics(m%LineList(Rod%attachedA(l)), Rod%r(:,0), Rod%rd(:,0), t, Rod%TopA(l)) + END DO + DO l=1,Rod%nAttachedB + CALL Line_SetEndKinematics(m%LineList(Rod%attachedB(l)), Rod%r(:,N), Rod%rd(:,N), t, Rod%TopB(l)) + END DO + + + ! if this is a zero-length Rod, get bending moment-related information from attached lines and compute Rod's equilibrium orientation + if (N==0) then + + DO l=1,Rod%nAttachedA + + CALL Line_GetEndSegmentInfo(m%LineList(Rod%attachedA(l)), qEnd, EIend, dlEnd, Rod%TopA(l)) + + qMomentSum = qMomentSum + qEnd*EIend/dlEnd ! add each component to the summation vector + + END DO + + DO l=1,Rod%nAttachedB + + CALL Line_GetEndSegmentInfo(m%LineList(Rod%attachedB(l)), qEnd, EIend, dlEnd, Rod%TopB(l)) + + qMomentSum = qMomentSum + qEnd*EIend/dlEnd ! add each component to the summation vector + + END DO + + ! solve for line unit vector that balances all moments (unit vector of summation of qEnd*EI/dl_stretched over each line) + CALL ScaleVector(qMomentSum, 1.0_DbKi, Rod%q) + END IF + + ! pass Rod orientation to any attached lines (this is just like what a Connection does, except for both ends) + DO l=1,Rod%nAttachedA + CALL Line_SetEndOrientation(m%LineList(Rod%attachedA(l)), Rod%q, Rod%TopA(l), 0) + END DO + DO l=1,Rod%nAttachedB + CALL Line_SetEndOrientation(m%LineList(Rod%attachedB(l)), Rod%q, Rod%TopB(l), 1) + END DO + + END SUBROUTINE Rod_SetDependentKin + !-------------------------------------------------------------- + + !-------------------------------------------------------------- + SUBROUTINE Rod_GetStateDeriv(Rod, Xd, m, p) + + Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object + Real(DbKi), INTENT(INOUT) :: Xd(:) ! state derivative vector section for this line + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects (for simplicity, since Bodies deal with Rods and Connections) + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables + + INTEGER(IntKi) :: J ! index + + Real(DbKi) :: Fnet (6) ! net force and moment about reference point + Real(DbKi) :: M_out (6,6) ! mass matrix about reference point + + Real(DbKi) :: acc(6) ! 6DOF acceleration vector about reference point + + Real(DbKi) :: Mcpl(3) ! moment in response to end A acceleration due to inertial coupling + + Real(DbKi) :: y_temp (6) ! temporary vector for LU decomposition + Real(DbKi) :: LU_temp(6,6) ! temporary matrix for LU decomposition + + ! Initialize some things to zero + y_temp = 0.0_DbKi +! FIXME: should LU_temp be set to M_out before calling LUsolve????? + LU_temp = 0.0_DbKi + + CALL Rod_GetNetForceAndMass(Rod, Rod%r(:,0), Fnet, M_out, m, p) + + + + ! TODO: add "controller" adjusting state derivatives of X(10:12) to artificially force X(10:12) to remain a unit vector <<<<<<<<<<< + + ! fill in state derivatives + IF (Rod%typeNum == 0) THEN ! free Rod type, 12 states + + ! solve for accelerations in [M]{a}={f} using LU decomposition + CALL LUsolve(6, M_out, LU_temp, Fnet, y_temp, acc) + + Xd(7:9) = Rod%v6(1:3) !Xd[6 + I] = v6[ I]; ! dxdt = V (velocities) + Xd(1:6) = acc !Xd[ I] = acc[ I]; ! dVdt = a (accelerations) + !Xd[3 + I] = acc[3+I]; ! rotational accelerations + + ! rate of change of unit vector components!! CHECK! <<<<< + Xd(10) = - Rod%v6(6)*Rod%r6(5) + Rod%v6(5)*Rod%r6(6) ! i.e. u_dot_x = -omega_z*u_y + omega_y*u_z + Xd(11) = Rod%v6(6)*Rod%r6(4) - Rod%v6(4)*Rod%r6(6) ! i.e. u_dot_y = omega_z*u_x - omega_x*u_z + Xd(12) = -Rod%v6(5)*Rod%r6(4) + Rod%v6(4)*Rod%r6(5) ! i.e. u_dot_z = -omega_y*u_x - omega_x*u_y + + ! store accelerations in case they're useful as output + Rod%a6 = acc + + ELSE ! pinned rod, 6 states (rotational only) + + ! account for moment in response to end A acceleration due to inertial coupling (off-diagonal sub-matrix terms) + !Fnet(4:6) = Fnet(4:6) - MATMUL(M_out(4:6,1:3), Rod%a6(1:3)) ! << 1) THEN + print *, " state derivatives:" + print *, Xd + + print *, "r0" + print *, Rod%r(:,0) + print *, "F" + print *, Fnet + print *, "M" + print *, M_out + print *, "acc" + print *, acc + END IF + + EXIT + END IF + END DO + + END SUBROUTINE Rod_GetStateDeriv + !-------------------------------------------------------------- + + + ! calculate the forces on the rod, including from attached lines + !-------------------------------------------------------------- + SUBROUTINE Rod_DoRHS(Rod, m, p) + + Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rodion object + TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects + TYPE(MD_ParameterType),INTENT(IN ) :: p ! Parameters + + !TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! misc/optimization variables + + INTEGER(IntKi) :: l ! index of attached lines + INTEGER(IntKi) :: I,J,K ! index + + + INTEGER(IntKi) :: N ! number of rod elements for convenience + + Real(DbKi) :: phi, beta, sinPhi, cosPhi, tanPhi, sinBeta, cosBeta ! various orientation things + Real(DbKi) :: k_hat(3) ! unit vector (redundant, not used) <<<< + Real(DbKi) :: Ftemp ! temporary force component + Real(DbKi) :: Mtemp ! temporary moment component + + Real(DbKi) :: m_i, v_i ! + Real(DbKi) :: zeta ! wave elevation above/below a given node + !Real(DbKi) :: h0 ! distance along rod centerline from end A to the waterplane + Real(DbKi) :: deltaL ! submerged length of a given segment + Real(DbKi) :: Lsum ! cumulative length along rod axis from bottom + Real(DbKi) :: dL ! length attributed to node + Real(DbKi) :: VOF ! fraction of volume associated with node that is submerged + + Real(DbKi) :: Vi(3) ! relative flow velocity over a node + Real(DbKi) :: SumSqVp, SumSqVq, MagVp, MagVq + Real(DbKi) :: Vp(3), Vq(3) ! transverse and axial components of water velocity at a given node + Real(DbKi) :: ap(3), aq(3) ! transverse and axial components of water acceleration at a given node + Real(DbKi) :: Fnet_i(3) ! force from an attached line + Real(DbKi) :: Mnet_i(3) ! moment from an attached line + Real(DbKi) :: Mass_i(3,3) ! mass from an attached line + + ! used in lumped 6DOF calculations: + Real(DbKi) :: rRel( 3) ! relative position of each node i from rRef + Real(DbKi) :: OrMat(3,3) ! rotation matrix to rotate global z to rod's axis + Real(DbKi) :: F6_i(6) ! a node's contribution to the total force vector + Real(DbKi) :: M6_i(6,6) ! a node's contribution to the total mass matrix + Real(DbKi) :: I_l ! axial inertia of rod + Real(DbKi) :: I_r ! radial inertia of rod about CG + Real(DbKi) :: Imat_l(3,3) ! inertia about CG aligned with Rod axis + Real(DbKi) :: Imat(3,3) ! inertia about CG in global frame + Real(DbKi) :: h_c ! location of CG along axis + Real(DbKi) :: r_c(3) ! 3d location of CG relative to node A + Real(DbKi) :: Fcentripetal(3) ! centripetal force + Real(DbKi) :: Mcentripetal(3) ! centripetal moment + + Real(DbKi) :: depth ! local interpolated depth from bathymetry grid + + + N = Rod%N + + ! ------------------------------ zero some things -------------------------- + + Rod%Mext = 0.0_DbKi ! zero the external moment sum + + Lsum = 0.0_DbKi + + + ! ---------------------------- initial rod and node calculations ------------------------ + + ! calculate some orientation information for the Rod as a whole + call GetOrientationAngles(Rod%r( :,0), Rod%r( :,N), phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat) + + ! save to internal roll and pitch variables for use in output <<< should check these, make Euler angles isntead of independent <<< + Rod%roll = -180.0/Pi * phi*sinBeta + Rod%pitch = 180.0/Pi * phi*cosBeta + + ! set interior node positions and velocities (stretch the nodes between the endpoints linearly) (skipped for zero-length Rods) + DO i=1,N-1 + Rod%r( :,i) = Rod%r( :,0) + (Rod%r( :,N) - Rod%r( :,0)) * (REAL(i)/REAL(N)) + Rod%rd(:,i) = Rod%rd(:,0) + (Rod%rd(:,N) - Rod%rd(:,0)) * (REAL(i)/REAL(N)) + + + Rod%V(i) = 0.25*pi * Rod%d*Rod%d * Rod%l(i) ! volume attributed to segment + END DO + + + ! --------------------------------- apply wave kinematics ------------------------------------ + + ! IF (p%WaterKin == 1) THEN ! wave kinematics interpolated from global grid in Waves object + ! DO i=0,N + ! CALL getWaveKin(p, Rod%r(1,i), Rod%r(2,i), Rod%r(3,i), Rod%time, m%WaveTi, Rod%U(:,i), Rod%Ud(:,i), Rod%zeta(i), Rod%PDyn(i)) + ! !F(i) = 1.0 ! set VOF value to one for now (everything submerged - eventually this should be element-based!!!) <<<< + ! ! <<<< currently F is not being used and instead a VOF variable is used within the node loop + ! END DO + ! END IF + + + ! ! wave kinematics not implemented yet <<< + ! ap = 0.0_DbKi + ! aq = 0.0_DbKi + ! ! set U and Ud herem as well as pDyn and zeta... + ! Rod%U = 0.0_DbKi + ! Rod%Ud = 0.0_DbKi + ! pDyn = 0.0_DbKi + ! zeta = 0.0_DbKi + + ! >>> remember to check for violated conditions, if there are any... <<< + + zeta = Rod%zeta(N)! just use the wave elevation computed at the location of the top node for now + + if ((Rod%r(3,0) < zeta) .and. (Rod%r(3,N) > zeta)) then ! check if it's crossing the water plane (should also add some limits to avoid near-horizontals at some point) + Rod%h0 = (zeta - Rod%r(3,0))/Rod%q(3) ! distance along rod centerline from end A to the waterplane + else if (Rod%r(3,0) < zeta) then + Rod%h0 = Rod%UnstrLen ! fully submerged case <<<<<< remove the 2.0 and double check there are no if statements that get changed <<<< + else + Rod%h0 = 0.0_DbKi ! fully unsubmerged case (ever applicable?) + end if + + + ! -------------------------- loop through all the nodes ----------------------------------- + DO I = 0, N + + + ! ------------------ calculate added mass matrix for each node ------------------------- + + ! get mass and volume considering adjacent segment lengths + IF (I==0) THEN + dL = 0.5*Rod%l(1) + m_i = 0.25*Pi * Rod%d*Rod%d * dL *Rod%rho ! (will be zero for zero-length Rods) + v_i = 0.5 *Rod%V(1) + ELSE IF (I==N) THEN + dL = 0.5*Rod%l(N) + m_i = 0.25*pi * Rod%d*Rod%d * dL *Rod%rho + v_i = 0.5*Rod%V(N) + ELSE + dL = 0.5*(Rod%l(I) + Rod%l(I+1)) + m_i = 0.25*pi * Rod%d*Rod%d * dL *Rod%rho + v_i = 0.5 *(Rod%V(I) + Rod%V(I+1)) + END IF + + ! get scalar for submerged portion + IF (Lsum + dL <= Rod%h0) THEN ! if fully submerged + VOF = 1.0_DbKi + ELSE IF (Lsum < Rod%h0) THEN ! if partially below waterline + VOF = (Rod%h0 - Lsum)/dL + ELSE ! must be out of water + VOF = 0.0_DbKi + END IF + + Lsum = Lsum + dL ! add length attributed to this node to the total + + ! build mass and added mass matrix + DO J=1,3 + DO K=1,3 + IF (J==K) THEN + Rod%M(K,J,I) = m_i + VOF*p%rhoW*v_i*( Rod%Can*(1 - Rod%q(J)*Rod%q(K)) + Rod%Cat*Rod%q(J)*Rod%q(K) ) + ELSE + Rod%M(K,J,I) = VOF*p%rhoW*v_i*( Rod%Can*(-Rod%q(J)*Rod%q(K)) + Rod%Cat*Rod%q(J)*Rod%q(K) ) + END IF + END DO + END DO + + ! <<<< what about accounting for offset of half segment from node location for end nodes? <<<< + + +! CALL Inverse3by3(Rod%S(:,:,I), Rod%M(:,:,I)) ! invert mass matrix + + + ! ------------------ CALCULATE FORCES ON EACH NODE ---------------------------- + + if (N > 0) then ! the following force calculations are only nonzero for finite-length rods (skipping for zero-length Rods) + + ! >>> no nodal axial elasticity loads calculated since it's assumed rigid, but should I calculate tension/compression due to other loads? <<< + + ! weight (now only the dry weight) + Rod%W(:,I) = (/ 0.0_DbKi, 0.0_DbKi, -m_i * p%g /) ! assuming g is positive + + ! buoyance (now calculated based on outside pressure, for submerged portion only) + ! radial buoyancy force from sides + Ftemp = -VOF * 0.25*Pi*dL*Rod%d*Rod%d * p%rhoW*p%g * sinPhi + Rod%Bo(:,I) = (/ Ftemp*cosBeta*cosPhi, Ftemp*sinBeta*cosPhi, -Ftemp*sinPhi /) + + !relative flow velocities + DO J = 1, 3 + Vi(J) = Rod%U(J,I) - Rod%rd(J,I) ! relative flow velocity over node -- this is where wave velicites would be added + END DO + + ! decomponse relative flow into components + SumSqVp = 0.0_DbKi ! start sums of squares at zero + SumSqVq = 0.0_DbKi + DO J = 1, 3 + Vq(J) = DOT_PRODUCT( Vi , Rod%q ) * Rod%q(J); ! tangential relative flow component + Vp(J) = Vi(J) - Vq(J) ! transverse relative flow component + SumSqVq = SumSqVq + Vq(J)*Vq(J) + SumSqVp = SumSqVp + Vp(J)*Vp(J) + END DO + MagVp = sqrt(SumSqVp) ! get magnitudes of flow components + MagVq = sqrt(SumSqVq) + + ! transverse and tangenential drag + Rod%Dp(:,I) = VOF * 0.5*p%rhoW*Rod%Cdn* Rod%d* dL * MagVp * Vp + Rod%Dq(:,I) = 0.0_DbKi ! 0.25*p%rhoW*Rod%Cdt* Pi*Rod%d* dL * MagVq * Vq <<< should these axial side loads be included? + + ! fluid acceleration components for current node + aq = DOT_PRODUCT(Rod%Ud(:,I), Rod%q) * Rod%q ! tangential component of fluid acceleration + ap = Rod%Ud(:,I) - aq ! normal component of fluid acceleration + ! transverse Froude-Krylov force + Rod%Ap(:,I) = VOF * p%rhoW*(1.0+Rod%Can)* v_i * ap ! + ! axial Froude-Krylov force + Rod%Aq(:,I) = 0.0_DbKi ! p%rhoW*(1.0+Rod%Cat)* v_i * aq ! <<< just put a taper-based term here eventually? + + ! dynamic pressure + Rod%Pd(:,I) = 0.0_DbKi ! assuming zero for sides for now, until taper comes into play + + ! bottom contact (stiffness and damping, vertical-only for now) - updated Nov 24 for general case where anchor and fairlead ends may deal with bottom contact forces + ! interpolate the local depth from the bathymetry grid + CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, Rod%r(1,I), Rod%r(2,I), depth) + + IF (Rod%r(3,I) < -depth) THEN + IF (I==0) THEN + Rod%B(3,I) = ( (-depth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*( Rod%l(I+1) ) + ELSE IF (I==N) THEN + Rod%B(3,I) = ( (-depth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) ) + ELSE + Rod%B(3,I) = ( (-depth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) + Rod%l(I+1) ) + END IF + ! IF (I==0) THEN + ! Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*( Rod%l(I+1) ) + ! ELSE IF (I==N) THEN + ! Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) ) + ! ELSE + ! Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) + Rod%l(I+1) ) + ! END IF + ELSE + Rod%B(3,I) = 0.0_DbKi + END IF + + ELSE ! zero-length (N=0) Rod case + + ! >>>>>>>>>>>>>> still need to check handling of zero length rods <<<<<<<<<<<<<<<<<<< + + ! for zero-length rods, make sure various forces are zero + Rod%W = 0.0_DbKi + Rod%Bo = 0.0_DbKi + Rod%Dp = 0.0_DbKi + Rod%Dq= 0.0_DbKi + Rod%B = 0.0_DbKi + Rod%Pd = 0.0_DbKi + + END IF + + + ! ------ now add forces, moments, and added mass from Rod end effects (these can exist even if N==0) ------- + + ! end A + IF ((I==0) .and. (Rod%h0 > 0.0_ReKi)) THEN ! if this is end A and it is submerged + + ! >>> eventually should consider a VOF approach for the ends hTilt = 0.5*Rod%d/cosPhi <<< + + ! buoyancy force + Ftemp = -VOF * 0.25*Pi*Rod%d*Rod%d * p%rhoW*p%g*Rod%r(3,I) + Rod%Bo(:,I) = Rod%Bo(:,I) + (/ Ftemp*cosBeta*sinPhi, Ftemp*sinBeta*sinPhi, Ftemp*cosPhi /) + + ! buoyancy moment + Mtemp = -VOF * 1.0/64.0*Pi*Rod%d**4 * p%rhoW*p%g * sinPhi + Rod%Mext = Rod%Mext + (/ Mtemp*sinBeta, -Mtemp*cosBeta, 0.0_DbKi /) + + ! axial drag + Rod%Dq(:,I) = Rod%Dq(:,I) + VOF * 0.25* Pi*Rod%d*Rod%d * p%rhoW*Rod%CdEnd * MagVq * Vq + + + ! >>> what about rotational drag?? <<< eqn will be Pi* Rod%d**4/16.0 omega_rel?^2... *0.5 * Cd... + + ! Froud-Krylov force + Rod%Aq(:,I) = Rod%Aq(:,I) + VOF * p%rhoW*(1.0+Rod%CaEnd)* (2.0/3.0*Pi*Rod%d**3 /8.0) * aq + + ! dynamic pressure force + Rod%Pd(:,I) = Rod%Pd(:,I) + VOF * 0.25* Pi*Rod%d*Rod%d * Rod%PDyn(I) * Rod%q + + ! added mass + DO J=1,3 + DO K=1,3 + IF (J==K) THEN + Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) + ELSE + Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) + END IF + END DO + END DO + + END IF + + IF ((I==N) .and. (Rod%h0 >= Rod%UnstrLen)) THEN ! if this end B and it is submerged (note, if N=0, both this and previous if statement are true) + + ! buoyancy force + Ftemp = VOF * 0.25*Pi*Rod%d*Rod%d * p%rhoW*p%g*Rod%r(3,I) + Rod%Bo(:,I) = Rod%Bo(:,I) + (/ Ftemp*cosBeta*sinPhi, Ftemp*sinBeta*sinPhi, Ftemp*cosPhi /) + + ! buoyancy moment + Mtemp = VOF * 1.0/64.0*Pi*Rod%d**4 * p%rhoW*p%g * sinPhi + Rod%Mext = Rod%Mext + (/ Mtemp*sinBeta, -Mtemp*cosBeta, 0.0_DbKi /) + + ! axial drag + Rod%Dq(:,I) = Rod%Dq(:,I) + VOF * 0.25* Pi*Rod%d*Rod%d * p%rhoW*Rod%CdEnd * MagVq * Vq + + ! Froud-Krylov force + Rod%Aq(:,I) = Rod%Aq(:,I) + VOF * p%rhoW*(1.0+Rod%CaEnd)* (2.0/3.0*Pi*Rod%d**3 /8.0) * aq + + ! dynamic pressure force + Rod%Pd(:,I) = Rod%Pd(:,I) - VOF * 0.25* Pi*Rod%d*Rod%d * Rod%PDyn(I) * Rod%q + + ! added mass + DO J=1,3 + DO K=1,3 + IF (J==K) THEN + Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) + ELSE + Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) + END IF + END DO + END DO + + END IF + + + + ! ---------------------------- total forces for this node ----------------------------- + + Rod%Fnet(:,I) = Rod%W(:,I) + Rod%Bo(:,I) + Rod%Dp(:,I) + Rod%Dq(:,I) & + + Rod%Ap(:,I) + Rod%Aq(:,I) + Rod%Pd(:,I) + Rod%B(:,I) + + + END DO ! I - done looping through nodes + + + ! ----- add waterplane moment of inertia moment if applicable ----- + IF ((Rod%r(3,0) < zeta) .and. (Rod%r(3,N) > zeta)) then ! check if it's crossing the water plane + Mtemp = 1.0/16.0 *Pi*Rod%d**4 * p%rhoW*p%g * sinPhi * (1.0 + 0.5* tanPhi**2) + Rod%Mext = Rod%Mext + (/ Mtemp*sinBeta, -Mtemp*cosBeta, 0.0_DbKi /) + END IF + + ! ---------------- now add in forces on end nodes from attached lines ------------------ + + ! loop through lines attached to end A + DO l=1,Rod%nAttachedA + + CALL Line_GetEndStuff(m%LineList(Rod%attachedA(l)), Fnet_i, Mnet_i, Mass_i, Rod%TopA(l)) + + ! sum quantitites + Rod%Fnet(:,0)= Rod%Fnet(:,0) + Fnet_i ! total force + Rod%Mext = Rod%Mext + Mnet_i ! externally applied moment + Rod%M(:,:,0) = Rod%M(:,:,0) + Mass_i ! mass at end node + + END DO + + ! loop through lines attached to end B + DO l=1,Rod%nAttachedB + + CALL Line_GetEndStuff(m%LineList(Rod%attachedB(l)), Fnet_i, Mnet_i, Mass_i, Rod%TopB(l)) + + ! sum quantitites + Rod%Fnet(:,N)= Rod%Fnet(:,N) + Fnet_i ! total force + Rod%Mext = Rod%Mext + Mnet_i ! externally applied moment + Rod%M(:,:,N) = Rod%M(:,:,N) + Mass_i ! mass at end node + + END DO + + ! ---------------- now lump everything in 6DOF about end A ----------------------------- + + ! question: do I really want to neglect the rotational inertia/drag/etc across the length of each segment? + + ! make sure 6DOF quantiaties are zeroed before adding them up + Rod%F6net = 0.0_DbKi + Rod%M6net = 0.0_DbKi + + ! now go through each node's contributions, put them about end A, and sum them + DO i = 0,Rod%N + + rRel = Rod%r(:,i) - Rod%r(:,0) ! vector from reference point to node + + ! convert segment net force into 6dof force about body ref point (if the Rod itself, end A) + CALL translateForce3to6DOF(rRel, Rod%Fnet(:,i), F6_i) + + ! convert segment mass matrix to 6by6 mass matrix about body ref point (if the Rod itself, end A) + CALL translateMass3to6DOF(rRel, Rod%M(:,:,i), M6_i) + + ! sum contributions + Rod%F6net = Rod%F6net + F6_i + Rod%M6net = Rod%M6net + M6_i + + END DO + + ! ------------- Calculate some items for the Rod as a whole here ----------------- + + ! >>> could some of these be precalculated just once? <<< + + ! add inertia terms for the Rod assuming it is uniform density (radial terms add to existing matrix which contains parallel-axis-theorem components only) + I_l = 0.125*Rod%mass * Rod%d*Rod%d ! axial moment of inertia + I_r = Rod%mass/12 * (0.75*Rod%d*Rod%d + (Rod%UnstrLen/Rod%N)**2 ) * Rod%N ! summed radial moment of inertia for each segment individually + + !h_c = [value from registry] + + Imat_l(1,1) = I_r ! inertia about CG in local orientations (as if Rod is vertical) + Imat_l(2,2) = I_r + Imat_l(3,3) = I_l + + OrMat = CalcOrientation(phi, beta, 0.0_DbKi) ! get rotation matrix to put things in global rather than rod-axis orientations + + Imat = RotateM3(Imat_l, OrMat) ! rotate to give inertia matrix about CG in global frame + + ! these supplementary inertias can then be added the matrix (these are the terms ASIDE from the parallel axis terms) + Rod%M6net(4:6,4:6) = Rod%M6net(4:6,4:6) + Imat + + + ! now add centripetal and gyroscopic forces/moments, and that should be everything + h_c = 0.5*Rod%UnstrLen ! distance to center of mass + r_c = h_c*Rod%q ! vector to center of mass + + ! note that Rod%v6(4:6) is the rotational velocity vector, omega + Fcentripetal = 0.0_DbKi !<<>> do we need to ensure zero moment is passed if it's pinned? <<< + !if (abs(Rod%typeNum)==1) then + ! Fnet_out(4:6) = 0.0_DbKi + !end if + + + END SUBROUTINE Rod_GetNetForceAndMass + !-------------------------------------------------------------- + + + ! this function handles assigning a line to a connection node + SUBROUTINE Rod_AddLine(Rod, lineID, TopOfLine, endB) + + Type(MD_Rod), INTENT (INOUT) :: Rod ! the Connection object + + Integer(IntKi), INTENT( IN ) :: lineID + Integer(IntKi), INTENT( IN ) :: TopOfLine + Integer(IntKi), INTENT( IN ) :: endB ! add line to end B if 1, end A if 0 + + if (endB==1) then ! attaching to end B + + IF (wordy > 0) Print*, "L", lineID, "->R", Rod%IdNum , "b" + + IF (Rod%nAttachedB <10) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. + Rod%nAttachedB = Rod%nAttachedB + 1 ! add the line to the number connected + Rod%AttachedB(Rod%nAttachedB) = lineID + Rod%TopB(Rod%nAttachedB) = TopOfLine ! attached to line ... 1 = top/fairlead(end B), 0 = bottom/anchor(end A) + ELSE + Print*, "too many lines connected to Rod ", Rod%IdNum, " in MoorDyn!" + END IF + + else ! attaching to end A + + IF (wordy > 0) Print*, "L", lineID, "->R", Rod%IdNum , "a" + + IF (Rod%nAttachedA <10) THEN ! this is currently just a maximum imposed by a fixed array size. could be improved. + Rod%nAttachedA = Rod%nAttachedA + 1 ! add the line to the number connected + Rod%AttachedA(Rod%nAttachedA) = lineID + Rod%TopA(Rod%nAttachedA) = TopOfLine ! attached to line ... 1 = top/fairlead(end B), 0 = bottom/anchor(end A) + ELSE + Print*, "too many lines connected to Rod ", Rod%IdNum, " in MoorDyn!" + END IF + + end if + + END SUBROUTINE Rod_AddLine + + + ! this function handles removing a line from a connection node + SUBROUTINE Rod_RemoveLine(Rod, lineID, TopOfLine, endB, rEnd, rdEnd) + + Type(MD_Rod), INTENT (INOUT) :: Rod ! the Connection object + + Integer(IntKi), INTENT( IN ) :: lineID + Integer(IntKi), INTENT( OUT) :: TopOfLine + Integer(IntKi), INTENT( IN ) :: endB ! end B if 1, end A if 0 + REAL(DbKi), INTENT(INOUT) :: rEnd(3) + REAL(DbKi), INTENT(INOUT) :: rdEnd(3) + + Integer(IntKi) :: l,m,J + + if (endB==1) then ! attaching to end B + + DO l = 1,Rod%nAttachedB ! look through attached lines + + IF (Rod%AttachedB(l) == lineID) THEN ! if this is the line's entry in the attachment list + + TopOfLine = Rod%TopB(l); ! record which end of the line was attached + + DO m = l,Rod%nAttachedB-1 + + Rod%AttachedB(m) = Rod%AttachedB(m+1) ! move subsequent line links forward one spot in the list to eliminate this line link + Rod%TopB( m) = Rod%TopB(m+1) + + Rod%nAttachedB = Rod%nAttachedB - 1 ! reduce attached line counter by 1 + + ! also pass back the kinematics at the end + DO J = 1,3 + rEnd( J) = Rod%r( J,Rod%N) + rdEnd(J) = Rod%rd(J,Rod%N) + END DO + + print*, "Detached line ", lineID, " from Rod ", Rod%IdNum, " end B" + + EXIT + END DO + + IF (l == Rod%nAttachedB) THEN ! detect if line not found + print *, "Error: failed to find line to remove during RemoveLine call to Rod ", Rod%IdNum, ". Line ", lineID + END IF + END IF + END DO + + else ! attaching to end A + + DO l = 1,Rod%nAttachedA ! look through attached lines + + IF (Rod%AttachedA(l) == lineID) THEN ! if this is the line's entry in the attachment list + + TopOfLine = Rod%TopA(l); ! record which end of the line was attached + + DO m = l,Rod%nAttachedA-1 + + Rod%AttachedA(m) = Rod%AttachedA(m+1) ! move subsequent line links forward one spot in the list to eliminate this line link + Rod%TopA( m) = Rod%TopA(m+1) + + Rod%nAttachedA = Rod%nAttachedA - 1 ! reduce attached line counter by 1 + + ! also pass back the kinematics at the end + DO J = 1,3 + rEnd( J) = Rod%r( J,0) + rdEnd(J) = Rod%rd(J,0) + END DO + + print*, "Detached line ", lineID, " from Rod ", Rod%IdNum, " end A" + + EXIT + END DO + + IF (l == Rod%nAttachedA) THEN ! detect if line not found + print *, "Error: failed to find line to remove during RemoveLine call to Rod ", Rod%IdNum, ". Line ", lineID + END IF + END IF + END DO + + end if + + END SUBROUTINE Rod_RemoveLine + + + + +END MODULE MoorDyn_Rod From a9ce6e907b3eaa2db7124caefbb69a28171d1d46 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Tue, 26 Oct 2021 17:17:52 -0600 Subject: [PATCH 071/242] Adding unit normal vector to seabed bathymetry interpolation: - Modified getDepthFromBathymetry subroutine to calculate unit normal vector of local seabed slope in addition to local depth. - Replaced the calculate2Dinterpolation call with a modified version of that subroutine's code that also calculates the slope, and from that the unit vector. - The unit vector output is not yet added as an output so as to not break the code, but ready to be uncommented and added to subroutine parameters once the rest of the code is updated to be compatible. My name (nvec) could be improved. --- modules/moordyn/src/MoorDyn_Misc.f90 | 65 +++++++++++++++++++++++++--- 1 file changed, 58 insertions(+), 7 deletions(-) diff --git a/modules/moordyn/src/MoorDyn_Misc.f90 b/modules/moordyn/src/MoorDyn_Misc.f90 index 1e198a7dad..4ce05ac0fe 100644 --- a/modules/moordyn/src/MoorDyn_Misc.f90 +++ b/modules/moordyn/src/MoorDyn_Misc.f90 @@ -772,7 +772,7 @@ END SUBROUTINE calculate2Dinterpolation ! :::::::::::::::::::::::::: bathymetry subroutines ::::::::::::::::::::::::::::::: - + ! interpolates local seabed depth and normal vector SUBROUTINE getDepthFromBathymetry(BathymetryGrid, BathGrid_Xs, BathGrid_Ys, LineX, LineY, depth) REAL(DbKi), INTENT(IN ) :: BathymetryGrid(:,:) ! need colons or some sort of dimension setting @@ -780,15 +780,66 @@ SUBROUTINE getDepthFromBathymetry(BathymetryGrid, BathGrid_Xs, BathGrid_Ys, Line REAL(DbKi), INTENT(IN ) :: BathGrid_Ys(:) REAL(DbKi), INTENT(IN ) :: LineX REAL(DbKi), INTENT(IN ) :: LineY - REAL(DbKi), INTENT( OUT) :: depth + REAL(DbKi), INTENT( OUT) :: depth ! local seabed depth (positive down) [m] + ! >>> to be added >>> REAL(DbKi), INTENT( OUT) :: nvec(3) ! local seabed surface normal vector (positive out) - INTEGER(IntKi) :: ix, iy ! indeces for interpolation - Real(DbKi) :: fx, fy ! interpolation fractions + INTEGER(IntKi) :: ix0, iy0 ! indeces for interpolation + INTEGER(IntKi) :: ix1, iy1 ! second indices + Real(DbKi) :: fx, fy ! interpolation fractions + REAL(DbKi) :: c00, c01, c10, c11, cx0, cx1, c0y, c1y ! temporary depth values + Real(DbKi) :: dx, dy ! x and y spacing of local grid panel [m] + Real(DbKi) :: dc_dx, dc_dy ! local slope + Real(DbKi) :: tempVector(3) ! normal vector before scaling to unit - CALL getInterpNums(BathGrid_Xs, LineX, 1, ix, fx) - CALL getInterpNums(BathGrid_Ys, LineY, 1, iy, fy) + ! get interpolation indices and fractions for the relevant grid panel + CALL getInterpNums(BathGrid_Xs, LineX, 1, ix0, fx) + CALL getInterpNums(BathGrid_Ys, LineY, 1, iy0, fy) - CALL calculate2Dinterpolation(BathymetryGrid, ix, iy, fx, fy, depth) + !CALL calculate2Dinterpolation(BathymetryGrid, ix, iy, fx, fy, depth) + + ! handle end case conditions + IF (fx == 0) THEN + ix1 = ix0 + ELSE + ix1 = min(ix0+1,size(BathymetryGrid,2)) ! don't overstep bounds + END IF + IF (fy == 0) THEN + iy1 = iy0 + ELSE + iy1 = min(iy0+1,size(BathymetryGrid,1)) ! don't overstep bounds + END IF + + ! get corner points of the panel + c00 = BathymetryGrid(iy0, ix0) + c01 = BathymetryGrid(iy1, ix0) + c10 = BathymetryGrid(iy0, ix1) + c11 = BathymetryGrid(iy1, ix1) + + ! get interpolated points and local value + cx0 = c00 *(1.0-fx) + c10 *fx + cx1 = c01 *(1.0-fx) + c11 *fx + c0y = c00 *(1.0-fy) + c01 *fx + c1y = c10 *(1.0-fy) + c11 *fx + depth = cx0 *(1.0-fy) + cx1 *fy + + ! get local slope + dx = BathGrid_Xs(ix1) - BathGrid_Xs(ix0) + dy = BathGrid_Ys(iy1) - BathGrid_Ys(iy0) + if ( dx > 0.0 ) then + dc_dx = (c1y-c0y)/dx + else + dc_dx = 0.0_DbKi ! maybe this should raise an error + end if + if ( dx > 0.0 ) then + dc_dy = (cx1-cx0)/dy + else + dc_dy = 0.0_DbKi ! maybe this should raise an error + end if + + tempVector = -dc_dx + tempVector = -dc_dy + tempVector = 1.0_DbKi + ! ScaleVector( tempVector, 1.0_DbKi, nvec ) <<< ! compute unit vector END SUBROUTINE getDepthFromBathymetry From 3e40ac156c339a18f19b53b84946787ece8396c7 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Tue, 26 Oct 2021 22:28:19 -0600 Subject: [PATCH 072/242] MoorDyn: Implementation of Line bending stiffness - Added bending moment calculations in Line::GetStateDeriv. - Required functions for transmitting moments to attached Rods already were in place. - Adjusted Rod initialization to support zero-length rods. - Not yet tested! --- modules/moordyn/src/MoorDyn.f90 | 6 +- modules/moordyn/src/MoorDyn_Line.f90 | 122 ++++++++++++++++++++--- modules/moordyn/src/MoorDyn_Misc.f90 | 29 ++++++ modules/moordyn/src/MoorDyn_Registry.txt | 2 + modules/moordyn/src/MoorDyn_Rod.f90 | 36 +++---- modules/moordyn/src/MoorDyn_Types.f90 | 120 ++++++++++++++++++++++ 6 files changed, 281 insertions(+), 34 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 0c211d6435..a2b2e44aef 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -558,7 +558,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !read into a line Line = NextLine(i) - ! parse out entries: Name Diam MassDenInAir EA cIntDamp >>EI(new)<< Can Cat Cdn Cdt + ! parse out entries: Name Diam MassDenInAir EA cIntDamp >>EI(new)<< Cdn Can Cdt Cat READ(Line,*,IOSTAT=ErrStat2) m%LineTypeList(l)%name, m%LineTypeList(l)%d, & m%LineTypeList(l)%w, tempString1, tempString2, tempString3, & m%LineTypeList(l)%Cdn, m%LineTypeList(l)%Can, m%LineTypeList(l)%Cdt, m%LineTypeList(l)%Cat @@ -1526,7 +1526,9 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !CALL CleanUp() RETURN END IF - + x%states = 0.0_DbKi + m%xTemp%states = 0.0_DbKi + m%xdTemp%states = 0.0_DbKi diff --git a/modules/moordyn/src/MoorDyn_Line.f90 b/modules/moordyn/src/MoorDyn_Line.f90 index af63cff3b6..6c25c09c39 100644 --- a/modules/moordyn/src/MoorDyn_Line.f90 +++ b/modules/moordyn/src/MoorDyn_Line.f90 @@ -153,12 +153,13 @@ SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) END IF ! allocate segment scalar quantities - ALLOCATE ( Line%l(N), Line%ld(N), Line%lstr(N), Line%lstrd(N), Line%V(N), STAT = ErrStat ) + ALLOCATE ( Line%l(N), Line%ld(N), Line%lstr(N), Line%lstrd(N), Line%Kurv(0:N), Line%V(N), STAT = ErrStat ) IF ( ErrStat /= ErrID_None ) THEN ErrMsg = ' Error allocating segment scalar quantity arrays.' !CALL CleanUp() RETURN END IF + Line%Kurv = 0.0_DbKi ! assign values for l, ld, and V DO J=1,N @@ -185,7 +186,7 @@ SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) ! allocate node force vectors ALLOCATE ( Line%W(3, 0:N), Line%Dp(3, 0:N), Line%Dq(3, 0:N), Line%Ap(3, 0:N), & - Line%Aq(3, 0:N), Line%B(3, 0:N), Line%Fnet(3, 0:N), STAT = ErrStat ) + Line%Aq(3, 0:N), Line%B(3, 0:N), Line%Bs(3, 0:N), Line%Fnet(3, 0:N), STAT = ErrStat ) IF ( ErrStat /= ErrID_None ) THEN ErrMsg = ' Error allocating node force arrays.' !CALL CleanUp() @@ -193,13 +194,9 @@ SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) END IF ! set gravity and bottom contact forces to zero initially (because the horizontal components should remain at zero) - DO J = 0,N - DO K = 1,3 - Line%W(K,J) = 0.0_DbKi - Line%B(K,J) = 0.0_DbKi - END DO - END DO - + Line%W = 0.0_DbKi + Line%B = 0.0_DbKi + ! allocate mass and inverse mass matrices for each node (including ends) ALLOCATE ( Line%S(3, 3, 0:N), Line%M(3, 3, 0:N), STAT = ErrStat ) IF ( ErrStat /= ErrID_None ) THEN @@ -1014,6 +1011,12 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, Real(DbKi) :: ld_1 ! rate of change of static stiffness portion of segment [m/s] Real(DbKi) :: EA_1 ! stiffness of 'static stiffness' portion of segment, combines with dynamic stiffness to give static stiffnes [m/s] + Real(DbKi) :: Kurvi ! temporary curvature value [1/m] + Real(DbKi) :: pvec(3) ! the p vector used in bending stiffness calcs + Real(DbKi) :: Mforce_im1(3) ! force vector for a contributor to the effect of a bending moment [N] + Real(DbKi) :: Mforce_ip1(3) ! force vector for a contributor to the effect of a bending moment [N] + Real(DbKi) :: Mforce_i( 3) ! force vector for a contributor to the effect of a bending moment [N] + Real(DbKi) :: depth ! local water depth interpolated from bathymetry grid @@ -1171,6 +1174,97 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, end do + ! Bending loads + Line%Bs = 0.0_DbKi ! zero bending forces + + if (Line%EI > 0) then + ! loop through all nodes to calculate bending forces due to bending stiffness + do i=0,N + + ! end node A case (only if attached to a Rod, i.e. a cantilever rather than pinned connection) + if (i==0) then + + if (Line%endTypeA > 0) then ! if attached to Rod i.e. cantilever connection + + Kurvi = GetCurvature(Line%lstr(i), Line%q(:,i), Line%qs(:,i)) ! curvature <<< check if this approximation works for an end (assuming rod angle is node angle which is middle of if there was a segment -1/2 + + pvec = cross_product(Line%q(:,0), Line%qs(:,i)) ! get direction of bending radius axis + + Mforce_ip1 = cross_product(Line%qs(:,i), pvec) ! get direction of resulting force from bending to apply on node i+1 + + ! record bending moment at end for potential application to attached object <<<< do double check this.... + call scalevector(pvec, Kurvi*Line%EI, Line%endMomentA) + + ! scale force direction vectors by desired moment force magnitudes to get resulting forces on adjacent nodes + call scalevector(Mforce_ip1, Kurvi*Line%EI/Line%lstr(i), Mforce_ip1) + + ! set force on node i to cancel out forces on adjacent nodes + Mforce_i = -Mforce_ip1 + + ! apply these forces to the node forces + Line%Bs(:,i ) = Line%Bs(:,i ) + Mforce_i + Line%Bs(:,i+1) = Line%Bs(:,i+1) + Mforce_ip1 + + end if + + ! end node A case (only if attached to a Rod, i.e. a cantilever rather than pinned connection) + else if (i==N) then + + if (Line%endTypeB > 0) then ! if attached to Rod i.e. cantilever connection + + Kurvi = GetCurvature(Line%lstr(i-1), Line%qs(:,i-1), Line%q(:,i)) ! curvature <<< check if this approximation works for an end (assuming rod angle is node angle which is middle of if there was a segment -1/2 + + pvec = cross_product(Line%qs(:,i-1), Line%q(:,N)) ! get direction of bending radius axis + + Mforce_im1 = cross_product(Line%qs(:,i-1), pvec) ! get direction of resulting force from bending to apply on node i-1 + + ! record bending moment at end for potential application to attached object <<<< do double check this.... + call scalevector(pvec, -Kurvi*Line%EI, Line%endMomentB ) ! note end B is oposite sign as end A + + ! scale force direction vectors by desired moment force magnitudes to get resulting forces on adjacent nodes + call scalevector(Mforce_im1, Kurvi*Line%EI/Line%lstr(i-1), Mforce_im1) + + ! set force on node i to cancel out forces on adjacent nodes + Mforce_i = Mforce_im1 + + ! apply these forces to the node forces + Line%Bs(:,i-1) = Line%Bs(:,i-1) + Mforce_im1 + Line%Bs(:,i ) = Line%Bs(:,i ) + Mforce_i + + end if + + else ! internal node + + Kurvi = GetCurvature(Line%lstr(i-1)+Line%lstr(i), Line%qs(:,i-1), Line%qs(:,i)) ! curvature <<< remember to check sign, or just take abs + + pvec = cross_product(Line%qs(:,i-1), Line%qs(:,i)) ! get direction of bending radius axis + + Mforce_im1 = cross_product(Line%qs(:,i-1), pvec) ! get direction of resulting force from bending to apply on node i-1 + Mforce_ip1 = cross_product(Line%qs(:,i ), pvec) ! get direction of resulting force from bending to apply on node i+1 + + ! scale force direction vectors by desired moment force magnitudes to get resulting forces on adjacent nodes + call scalevector(Mforce_im1, Kurvi*Line%EI/Line%lstr(i-1), Mforce_im1) + call scalevector(Mforce_ip1, Kurvi*Line%EI/Line%lstr(i ), Mforce_ip1) + + ! set force on node i to cancel out forces on adjacent nodes + Mforce_i = -Mforce_im1 - Mforce_ip1 + + ! apply these forces to the node forces + Line%Bs(:,i-1) = Line%Bs(:,i-1) + Mforce_im1 + Line%Bs(:,i ) = Line%Bs(:,i ) + Mforce_i + Line%Bs(:,i+1) = Line%Bs(:,i+1) + Mforce_ip1 + + end if + + ! record curvature at node + Line%Kurv(i) = Kurvi + + end do ! for i=0,N (looping through nodes) + end if ! if EI > 0 + + + + ! loop through the nodes DO I = 0, N @@ -1243,11 +1337,11 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, ! total forces IF (I==0) THEN - Line%Fnet(:,I) = Line%T(:,1) + Line%Td(:,1) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) + Line%Fnet(:,I) = Line%T(:,1) + Line%Td(:,1) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) + Line%Bs(:,I) ELSE IF (I==N) THEN - Line%Fnet(:,I) = -Line%T(:,N) - Line%Td(:,N) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) + Line%Fnet(:,I) = -Line%T(:,N) - Line%Td(:,N) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) + Line%Bs(:,I) ELSE - Line%Fnet(:,I) = Line%T(:,I+1) - Line%T(:,I) + Line%Td(:,I+1) - Line%Td(:,I) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) + Line%Fnet(:,I) = Line%T(:,I+1) - Line%T(:,I) + Line%Td(:,I+1) - Line%Td(:,I) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) + Line%Bs(:,I) END IF END DO ! I - done looping through nodes @@ -1422,7 +1516,7 @@ SUBROUTINE Line_SetEndOrientation(Line, qin, topOfLine, rodEndB) if (topOfLine==1) then - Line%endTypeB = 1 ! indicate attached to Rod (at every time step, just in case line get detached) + Line%endTypeB = 1 ! indicate attached to Rod (at every time step, just in case line gets detached) if (rodEndB==1) then Line%q(:,Line%N) = -qin ! -----line----->[B<==ROD==A] @@ -1431,7 +1525,7 @@ SUBROUTINE Line_SetEndOrientation(Line, qin, topOfLine, rodEndB) end if else - Line%endTypeA = 1 ! indicate attached to Rod (at every time step, just in case line get detached) ! indicate attached to Rod + Line%endTypeA = 1 ! indicate attached to Rod (at every time step, just in case line gets detached) ! indicate attached to Rod if (rodEndB==1) then Line%q(:,0 ) = qin ! [A==ROD==>B]-----line-----> diff --git a/modules/moordyn/src/MoorDyn_Misc.f90 b/modules/moordyn/src/MoorDyn_Misc.f90 index 4ce05ac0fe..37cd049629 100644 --- a/modules/moordyn/src/MoorDyn_Misc.f90 +++ b/modules/moordyn/src/MoorDyn_Misc.f90 @@ -32,6 +32,7 @@ MODULE MoorDyn_Misc PUBLIC :: UnitVector PUBLIC :: ScaleVector + PUBLIC :: GetCurvature PUBLIC :: GetOrientationAngles PUBLIC :: TransformKinematics PUBLIC :: TransformKinematicsA @@ -111,6 +112,34 @@ END SUBROUTINE ScaleVector !----------------------------------------------------------------------- + ! convenience function to calculate curvature based on adjacent segments' direction vectors and their combined length + function GetCurvature(length, q1, q2) + + real(DbKi), intent(in ) :: length + real(DbKi), intent(in ) :: q1(3) + real(DbKi), intent(in ) :: q2(3) + real(DbKi) :: GetCurvature + + + real(DbKi) :: q1_dot_q2 + + ! note "length" here is combined from both segments + + q1_dot_q2 = dot_product( q1, q2 ) + + if (q1_dot_q2 > 1.0) then ! this is just a small numerical error, so set q1_dot_q2 to 1 + GetCurvature = 0.0_DbKi ! this occurs when there's no curvature, so return zero curvature + + !else if (q1_dot_q2 < 0) ! this is a bend of more than 90 degrees, too much, call an error! + + else ! normal case + GetCurvature = 4.0/length * sqrt(0.5*(1.0 - q1_dot_q2)) ! this is the normal curvature calculation + end if + + return + end function GetCurvature + + ! calculate orientation angles of a cylindrical object !----------------------------------------------------------------------- subroutine GetOrientationAngles(p1, p2, phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat) diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 172e306422..3f78ad6ea1 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -247,6 +247,7 @@ typedef ^ ^ DbKi l {:} typedef ^ ^ DbKi ld {:} - - "segment unstretched length rate of change (used in active tensioning)" "[m]" typedef ^ ^ DbKi lstr {:} - - "segment stretched length" "[m]" typedef ^ ^ DbKi lstrd {:} - - "segment change in stretched length" "[m/s]" +typedef ^ ^ DbKi Kurv {:} - - "curvature at each node point" "[1/m]" typedef ^ ^ DbKi dl_1 {:} - - "segment stretch attributed to static stiffness portion" "[m]" typedef ^ ^ DbKi V {:} - - "segment volume" "[m^3]" typedef ^ ^ DbKi U {:}{:} - - "water velocity at node" "[m/s]" @@ -261,6 +262,7 @@ typedef ^ ^ DbKi Dq {:}{:} typedef ^ ^ DbKi Ap {:}{:} - - "node added mass forcing (transverse)" "[N]" typedef ^ ^ DbKi Aq {:}{:} - - "node added mass forcing (axial)" "[N]" typedef ^ ^ DbKi B {:}{:} - - "node bottom contact force" "[N]" +typedef ^ ^ DbKi Bs {:}{:} - - "node force due to bending moments" "[N]" typedef ^ ^ DbKi Fnet {:}{:} - - "total force on node" "[N]" typedef ^ ^ DbKi S {:}{:}{:} - - "node inverse mass matrix" "[kg]" typedef ^ ^ DbKi M {:}{:}{:} - - "node mass matrix" "[kg]" diff --git a/modules/moordyn/src/MoorDyn_Rod.f90 b/modules/moordyn/src/MoorDyn_Rod.f90 index 6bee471e54..c8165811de 100644 --- a/modules/moordyn/src/MoorDyn_Rod.f90 +++ b/modules/moordyn/src/MoorDyn_Rod.f90 @@ -58,7 +58,7 @@ SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) INTEGER, INTENT( INOUT ) :: ErrStat ! returns a non-zero value when an error occurs CHARACTER(*), INTENT( INOUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None - INTEGER(4) :: J ! Generic index + INTEGER(4) :: i ! Generic index INTEGER(4) :: K ! Generic index INTEGER(IntKi) :: N @@ -82,7 +82,11 @@ SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) IF ( ErrStat /= ErrID_None ) print *, "Alloc error 1 in MoorDyn" ! allocate segment scalar quantities - ALLOCATE ( Rod%l(N), Rod%V(N), STAT = ErrStat ) + if (Rod%N == 0) then ! special case of zero-length Rod + ALLOCATE ( Rod%l(1), Rod%V(N), STAT = ErrStat ) + else ! normal case + ALLOCATE ( Rod%l(N), Rod%V(N), STAT = ErrStat ) + end if IF ( ErrStat /= ErrID_None ) print *, "Alloc error 2 in MoorDyn" ! allocate water related vectors @@ -118,7 +122,6 @@ SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) Rod%r6(4:6) = Rod%q ! (Rod direction unit vector) Rod%v6(4:6) = 0.0_DbKi ! (rotational velocities about unrotated axes) - else if (abs(Rod%typeNum)==1) then ! for a pinned rod, just set the orientation (position will be set later by parent object) Rod%r6(4:6) = Rod%q ! (Rod direction unit vector) @@ -128,29 +131,26 @@ SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) ! otherwise (for a fixed rod) the positions will be set by the parent body or via coupling - ! save mass for future calculations >>>> should calculate I_l and I_r here in future <<<< Rod%mass = Rod%UnstrLen*RodProp%w ! assign values for l and V - DO J=1,N - Rod%l(J) = Rod%UnstrLen/REAL(N, DbKi) - Rod%V(J) = Rod%l(J)*0.25*Pi*RodProp%d*RodProp%d - END DO - + if (Rod%N == 0) then + Rod%l(1) = 0.0_DbKi + Rod%V(1) = 0.0_DbKi + else + DO i=1,N + Rod%l(i) = Rod%UnstrLen/REAL(N, DbKi) + Rod%V(i) = Rod%l(i)*0.25*Pi*RodProp%d*RodProp%d + END DO + end if ! set gravity and bottom contact forces to zero initially (because the horizontal components should remain at zero) - DO J = 0,N - DO K = 1,3 - Rod%W(K,J) = 0.0_DbKi - Rod%B(K,J) = 0.0_DbKi - END DO - END DO - - ! >>> why are the above assignments making l V W and B appear as "undefined pointer/array"s??? <<< - + Rod%W = 0.0_DbKi + Rod%B = 0.0_DbKi + IF (wordy > 0) print *, "Set up Rod ",Rod%IdNum, ", type ", Rod%typeNum ! need to add cleanup sub <<< diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index 0191b8d0b8..3f694cc4fa 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -267,6 +267,7 @@ MODULE MoorDyn_Types REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: ld !< segment unstretched length rate of change (used in active tensioning) [[m]] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: lstr !< segment stretched length [[m]] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: lstrd !< segment change in stretched length [[m/s]] + REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: Kurv !< curvature at each node point [[1/m]] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: dl_1 !< segment stretch attributed to static stiffness portion [[m]] REAL(DbKi) , DIMENSION(:), ALLOCATABLE :: V !< segment volume [[m^3]] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: U !< water velocity at node [[m/s]] @@ -281,6 +282,7 @@ MODULE MoorDyn_Types REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Ap !< node added mass forcing (transverse) [[N]] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Aq !< node added mass forcing (axial) [[N]] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: B !< node bottom contact force [[N]] + REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Bs !< node force due to bending moments [[N]] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Fnet !< total force on node [[N]] REAL(DbKi) , DIMENSION(:,:,:), ALLOCATABLE :: S !< node inverse mass matrix [[kg]] REAL(DbKi) , DIMENSION(:,:,:), ALLOCATABLE :: M !< node mass matrix [[kg]] @@ -4492,6 +4494,18 @@ SUBROUTINE MD_CopyLine( SrcLineData, DstLineData, CtrlCode, ErrStat, ErrMsg ) END IF DstLineData%lstrd = SrcLineData%lstrd ENDIF +IF (ALLOCATED(SrcLineData%Kurv)) THEN + i1_l = LBOUND(SrcLineData%Kurv,1) + i1_u = UBOUND(SrcLineData%Kurv,1) + IF (.NOT. ALLOCATED(DstLineData%Kurv)) THEN + ALLOCATE(DstLineData%Kurv(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%Kurv.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%Kurv = SrcLineData%Kurv +ENDIF IF (ALLOCATED(SrcLineData%dl_1)) THEN i1_l = LBOUND(SrcLineData%dl_1,1) i1_u = UBOUND(SrcLineData%dl_1,1) @@ -4680,6 +4694,20 @@ SUBROUTINE MD_CopyLine( SrcLineData, DstLineData, CtrlCode, ErrStat, ErrMsg ) END IF DstLineData%B = SrcLineData%B ENDIF +IF (ALLOCATED(SrcLineData%Bs)) THEN + i1_l = LBOUND(SrcLineData%Bs,1) + i1_u = UBOUND(SrcLineData%Bs,1) + i2_l = LBOUND(SrcLineData%Bs,2) + i2_u = UBOUND(SrcLineData%Bs,2) + IF (.NOT. ALLOCATED(DstLineData%Bs)) THEN + ALLOCATE(DstLineData%Bs(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstLineData%Bs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstLineData%Bs = SrcLineData%Bs +ENDIF IF (ALLOCATED(SrcLineData%Fnet)) THEN i1_l = LBOUND(SrcLineData%Fnet,1) i1_u = UBOUND(SrcLineData%Fnet,1) @@ -4776,6 +4804,9 @@ SUBROUTINE MD_DestroyLine( LineData, ErrStat, ErrMsg ) IF (ALLOCATED(LineData%lstrd)) THEN DEALLOCATE(LineData%lstrd) ENDIF +IF (ALLOCATED(LineData%Kurv)) THEN + DEALLOCATE(LineData%Kurv) +ENDIF IF (ALLOCATED(LineData%dl_1)) THEN DEALLOCATE(LineData%dl_1) ENDIF @@ -4818,6 +4849,9 @@ SUBROUTINE MD_DestroyLine( LineData, ErrStat, ErrMsg ) IF (ALLOCATED(LineData%B)) THEN DEALLOCATE(LineData%B) ENDIF +IF (ALLOCATED(LineData%Bs)) THEN + DEALLOCATE(LineData%Bs) +ENDIF IF (ALLOCATED(LineData%Fnet)) THEN DEALLOCATE(LineData%Fnet) ENDIF @@ -4939,6 +4973,11 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_BufSz = Int_BufSz + 2*1 ! lstrd upper/lower bounds for each dimension Db_BufSz = Db_BufSz + SIZE(InData%lstrd) ! lstrd END IF + Int_BufSz = Int_BufSz + 1 ! Kurv allocated yes/no + IF ( ALLOCATED(InData%Kurv) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! Kurv upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Kurv) ! Kurv + END IF Int_BufSz = Int_BufSz + 1 ! dl_1 allocated yes/no IF ( ALLOCATED(InData%dl_1) ) THEN Int_BufSz = Int_BufSz + 2*1 ! dl_1 upper/lower bounds for each dimension @@ -5009,6 +5048,11 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Int_BufSz = Int_BufSz + 2*2 ! B upper/lower bounds for each dimension Db_BufSz = Db_BufSz + SIZE(InData%B) ! B END IF + Int_BufSz = Int_BufSz + 1 ! Bs allocated yes/no + IF ( ALLOCATED(InData%Bs) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! Bs upper/lower bounds for each dimension + Db_BufSz = Db_BufSz + SIZE(InData%Bs) ! Bs + END IF Int_BufSz = Int_BufSz + 1 ! Fnet allocated yes/no IF ( ALLOCATED(InData%Fnet) ) THEN Int_BufSz = Int_BufSz + 2*2 ! Fnet upper/lower bounds for each dimension @@ -5277,6 +5321,21 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Db_Xferred = Db_Xferred + 1 END DO END IF + IF ( .NOT. ALLOCATED(InData%Kurv) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Kurv,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Kurv,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%Kurv,1), UBOUND(InData%Kurv,1) + DbKiBuf(Db_Xferred) = InData%Kurv(i1) + Db_Xferred = Db_Xferred + 1 + END DO + END IF IF ( .NOT. ALLOCATED(InData%dl_1) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -5537,6 +5596,26 @@ SUBROUTINE MD_PackLine( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz END DO END DO END IF + IF ( .NOT. ALLOCATED(InData%Bs) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Bs,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Bs,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%Bs,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Bs,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%Bs,2), UBOUND(InData%Bs,2) + DO i1 = LBOUND(InData%Bs,1), UBOUND(InData%Bs,1) + DbKiBuf(Db_Xferred) = InData%Bs(i1,i2) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF IF ( .NOT. ALLOCATED(InData%Fnet) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -5919,6 +5998,24 @@ SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) Db_Xferred = Db_Xferred + 1 END DO END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Kurv not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Kurv)) DEALLOCATE(OutData%Kurv) + ALLOCATE(OutData%Kurv(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Kurv.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%Kurv,1), UBOUND(OutData%Kurv,1) + OutData%Kurv(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END IF IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! dl_1 not allocated Int_Xferred = Int_Xferred + 1 ELSE @@ -6221,6 +6318,29 @@ SUBROUTINE MD_UnPackLine( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) END DO END DO END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Bs not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%Bs)) DEALLOCATE(OutData%Bs) + ALLOCATE(OutData%Bs(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Bs.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%Bs,2), UBOUND(OutData%Bs,2) + DO i1 = LBOUND(OutData%Bs,1), UBOUND(OutData%Bs,1) + OutData%Bs(i1,i2) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + END DO + END IF IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Fnet not allocated Int_Xferred = Int_Xferred + 1 ELSE From 7ffb95e94b3b1c9d79b7f9bca792689fb2cd31f6 Mon Sep 17 00:00:00 2001 From: shousner Date: Mon, 1 Nov 2021 15:52:21 -0600 Subject: [PATCH 073/242] Seabed Friction Implementation - See Implemenation Plan Word doc for any theory questions - Turned on the normal vector output in getDepthFromBathymetry in MoorDyn_Misc - Added all relevant parameter variables to the Registry MoorDyn.f90 - Defining parameters in the beginning of MoorDyn.f90 - - Can add/delete friction parameters as needed later - Read in the friction parameters through the OptString/OptValue process similar to the other Options parameters - Change each call to getDepthFromBathymetry to include the normal vector option - - This is done in the Rod source file as well MoorDyn_Line.f90 - Define all new friction-related parameters used for the calculations - New implementation that includes the seabed slope and the difference between transverse and axial friction into the calculation of forces on each line node - - If the line node is below the water depth, then it has a 3D normal force and a 3D friction force, with axial and transverse components - - These forces then sum into the "bottom contact" variable "B" to be included in the summation of forces --- modules/moordyn/src/MoorDyn.f90 | 27 +++++-- modules/moordyn/src/MoorDyn_Line.f90 | 91 +++++++++++++++++++----- modules/moordyn/src/MoorDyn_Misc.f90 | 8 +-- modules/moordyn/src/MoorDyn_Registry.txt | 6 +- modules/moordyn/src/MoorDyn_Rod.f90 | 5 +- modules/moordyn/src/MoorDyn_Types.f90 | 28 ++++++++ 6 files changed, 133 insertions(+), 32 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 0c211d6435..752735b84a 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -123,7 +123,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CHARACTER(40) :: TempStrings(6) ! Array of 6 strings used when parsing comma-separated items CHARACTER(1024) :: FileName ! - REAL(DbKi) :: depth ! local water depth interpolated from bathymetry grid + REAL(DbKi) :: depth ! local water depth interpolated from bathymetry grid [m] + Real(DbKi) :: nvec(3) ! local seabed surface normal vector (positive out) CHARACTER(25) :: let1 ! strings used for splitting and parsing identifiers @@ -183,11 +184,15 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er InputFileDat%TMaxIC = 60.0_DbKi InputFileDat%CdScaleIC = 4.0_ReKi InputFileDat%threshIC = 0.01_ReKi - p%WaveKin = 0_IntKi - p%Current = 0_IntKi + p%WaveKin = 0_IntKi + p%Current = 0_IntKi p%dtOut = 0.0_DbKi + p%mu_kT = 0.0_DbKi + p%mu_kA = 0.0_DbKi + p%mc = 1.0_DbKi + p%cv = 200.0_DbKi DepthValue = "" ! Start off as empty string, to only be filled if MD setting is specified (otherwise InitInp%WtrDepth is used) - ! DepthValue and InitInp%WtrDepth are processed later by getBathymetry. + ! DepthValue and InitInp%WtrDepth are processed later by setupBathymetry. WaterKinValue = "" m%PtfmInit = InitInp%PtfmInit(:,1) ! is this copying necssary in case this is an individual instance in FAST.Farm? @@ -410,7 +415,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if ( OptString == 'RHOW') then read (OptValue,*) p%rhoW else if ( OptString == 'WTRDPTH') then - read (OptValue,*) DepthValue ! water depth input read in as a string to be processed by getBathymetry + read (OptValue,*) DepthValue ! water depth input read in as a string to be processed by setupBathymetry else if ( OptString == 'KBOT') then read (OptValue,*) p%kBot else if ( OptString == 'CBOT') then @@ -427,6 +432,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er read (OptValue,*) WaterKinValue else if ( OptString == 'DTOUT') then read (OptValue,*) p%dtOut + else if ( OptString == 'MU_KT') then + read (OptValue,*) p%mu_kT + else if ( OptString == 'MU_KA') then + read (OptValue,*) p%mu_kT + else if ( OptString == 'MC') then + read (OptValue,*) p%mc + else if ( OptString == 'CV') then + read (OptValue,*) p%cv else CALL SetErrStat( ErrID_Warn, 'unable to interpret input '//trim(OptString), ErrStat, ErrMsg, RoutineName ) end if @@ -466,7 +479,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! set up seabed bathymetry CALL setupBathymetry(DepthValue, InitInp%WtrDepth, m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, ErrStat2, ErrMsg2) - CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, 0.0_DbKi, 0.0_DbKi, p%WtrDpth) ! set depth at 0,0 as nominal for waves etc + CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, 0.0_DbKi, 0.0_DbKi, p%WtrDpth, nvec) ! set depth at 0,0 as nominal for waves etc ! set up wave and current kinematics @@ -959,7 +972,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ELSE ! otherwise interpret the anchor depth value as a 'seabed' input, meaning the anchor should be on the seabed wherever the bathymetry says it should be PRINT *, "Anchor depth set for seabed; Finding the right seabed depth based on bathymetry" - CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, tempArray(1), tempArray(2), depth) + CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, tempArray(1), tempArray(2), depth, nvec) tempArray(3) = -depth END IF diff --git a/modules/moordyn/src/MoorDyn_Line.f90 b/modules/moordyn/src/MoorDyn_Line.f90 index af63cff3b6..64338944c2 100644 --- a/modules/moordyn/src/MoorDyn_Line.f90 +++ b/modules/moordyn/src/MoorDyn_Line.f90 @@ -1014,7 +1014,24 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, Real(DbKi) :: ld_1 ! rate of change of static stiffness portion of segment [m/s] Real(DbKi) :: EA_1 ! stiffness of 'static stiffness' portion of segment, combines with dynamic stiffness to give static stiffnes [m/s] - Real(DbKi) :: depth ! local water depth interpolated from bathymetry grid + Real(DbKi) :: depth ! local water depth interpolated from bathymetry grid [m] + Real(DbKi) :: nvec(3) ! local seabed surface normal vector (positive out) + Real(DbKi) :: Fn(3) ! seabed contact normal force vector + Real(DbKi) :: Vn(3) ! normal velocity of a line node relative to the seabed slope [m/s] + Real(DbKi) :: Va(3) ! velocity of a line node in the axial or "in-line" direction [m/s] + Real(DbKi) :: Vt(3) ! velocity of a line node in the transverse direction [m/s] + Real(DbKi) :: VtMag ! magnitude of the transverse velocity of a line node [m/s] + Real(DbKi) :: VaMag ! magnitude of the axial velocity of a line node [m/s] + Real(DbKi) :: FkTmax ! maximum kinetic friction force in the transverse direction (scalar) + Real(DbKi) :: FkAmax ! maximum kinetic friction force in the axial direction (scalar) + Real(DbKi) :: FkT(3) ! kinetic friction force in the transverse direction (vector) + Real(DbKi) :: FkA(3) ! kinetic friction force in the axial direction (vector) + !Real(DbKi) :: mc_T ! ratio of the transverse static friction coefficient to the transverse kinetic friction coefficient + !Real(DbKi) :: mc_A ! ratio of the axial static friction coefficient to the axial kinetic friction coefficient + Real(DbKi) :: FfT(3) ! total friction force in the transverse direction + Real(DbKi) :: FfA(3) ! total friction force in the axial direction + Real(DbKi) :: Ff(3) ! total friction force on the line node + N = Line%N ! for convenience @@ -1215,31 +1232,69 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, ! F-K force from fluid acceleration not implemented yet ! bottom contact (stiffness and damping, vertical-only for now) - updated Nov 24 for general case where anchor and fairlead ends may deal with bottom contact forces + ! bottom contact - updated throughout October 2021 for seabed bathymetry and friction models - ! interpolate the local depth from the bathymetry grid - CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, Line%r(1,I), Line%r(2,I), depth) + ! interpolate the local depth from the bathymetry grid and return the vector normal to the seabed slope + CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, Line%r(1,I), Line%r(2,I), depth, nvec) - IF (Line%r(3,I) < -depth) THEN + IF (Line%r(3,I) < -depth) THEN ! for every line node at or below the seabed + ! calculate the velocity of the node in the normal direction of the seabed slope + DO J = 1, 3 + Vn(J) = DOT_PRODUCT( Line%rd(:,I), nvec) * nvec(J) + END DO + ! calculate the normal contact force on the line node IF (I==0) THEN - Line%B(3,I) = ( (-depth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*( Line%l(I+1) ) + Fn = ( (-depth - Line%r(3,I))*nvec(2)*nvec*p%kBot - Vn*p%cBot) * 0.5*d*( Line%l(I+1) ) ELSE IF (I==N) THEN - Line%B(3,I) = ( (-depth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) ) + Fn = ( (-depth - Line%r(3,I))*nvec(2)*nvec*p%kBot - Vn*p%cBot) * 0.5*d*(Line%l(I) ) ELSE - Line%B(3,I) = ( (-depth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) + Line%l(I+1) ) - ! IF (Line%r(3,I) < -p%WtrDpth) THEN - ! IF (I==0) THEN - ! Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*( Line%l(I+1) ) - ! ELSE IF (I==N) THEN - ! Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) ) - ! ELSE - ! Line%B(3,I) = ( (-p%WtrDpth - Line%r(3,I))*p%kBot - Line%rd(3,I)*p%cBot) * 0.5*d*(Line%l(I) + Line%l(I+1) ) - - - + Fn = ( (-depth - Line%r(3,I))*nvec(2)*nvec*p%kBot - Vn*p%cBot) * 0.5*d*(Line%l(I) + Line%l(I+1) ) END IF ELSE - Line%B(3,I) = 0.0_DbKi + Fn = 0.0_DbKi + END IF + + ! calculate the axial and transverse components of the total node velocity vector (q can now have a z-component from seabed slope) + DO J = 1, 3 + Va(J) = DOT_PRODUCT( Line%rd(:,I) , Line%q(:,I) ) * Line%q(J,I) + Vt(J) = Line%rd(J,I) - Va(J) + END DO + ! calculate the magnitudes of each velocity + VaMag = SQRT(Va(1)**2+Va(2)**2+Va(3)**2) + VtMag = SQRT(Vt(1)**2+Vt(2)**2+Vt(3)**2) + + ! find the maximum possible kinetic friction force using transverse and axial kinetic friction coefficients + FkTmax = p%mu_kT*SQRT(Fn(1)**2+Fn(2)**2+Fn(3)**2) + FkAmax = p%mu_kA*SQRT(Fn(1)**2+Fn(2)**2+Fn(3)**2) + ! turn the maximum kinetic friction forces into vectors in the direction of their velocities + DO J = 1, 3 + FkT(J) = FkTmax*Vt(J)/VtMag + FkA(J) = FkAmax*Va(J)/VaMag + END DO + ! calculate the ratio between the static and kinetic coefficients of friction + !mc_T = p%mu_sT/p%mu_kT + !mc_A = p%mu_sA/p%mu_kA + + ! calculate the transverse friction force + IF (p%mu_kT*p%cv*VtMag > p%mc*FkTmax) THEN ! if the friction force of the linear curve is greater than the maximum friction force allowed adjusted for static friction, + FfT = -FkT ! then the friction force is the maximum kinetic friction force vector (constant part of the curve) + ELSE ! if the friction force of the linear curve is less than the maximum friction force allowed adjusted for static friction, + FfT = -p%mu_kT*p%cv*Vt ! then the friction force is the calculated value of the linear line END IF + ! calculate the axial friction force + IF (p%mu_kA*p%cv*VaMag > p%mc*FkAmax) THEN ! if the friction force of the linear curve is greater than the maximum friction force allowed adjusted for static friction, + FfA = -FkA ! then the friction force is the maximum kinetic friction force vector (constant part of the curve) + ELSE ! if the friction force of the linear curve is less than the maximum friction force allowed adjusted for static friction, + FfA = -p%mu_kA*p%cv*Va ! then the friction force is the calculated value of the linear line + END IF + ! NOTE: these friction forces have a negative sign here to indicate a force in the opposite direction of motion + + ! the total friction force is along the plane of the seabed slope, which is just the vector sum of the transverse and axial components + Ff = FfT + FfA + + ! the total force from bottom contact on the line node is the sum of the normal contact force and the friction force + Line%B(:,I) = Fn + Ff + ! total forces IF (I==0) THEN diff --git a/modules/moordyn/src/MoorDyn_Misc.f90 b/modules/moordyn/src/MoorDyn_Misc.f90 index 4ce05ac0fe..9d279423c8 100644 --- a/modules/moordyn/src/MoorDyn_Misc.f90 +++ b/modules/moordyn/src/MoorDyn_Misc.f90 @@ -773,7 +773,7 @@ END SUBROUTINE calculate2Dinterpolation ! :::::::::::::::::::::::::: bathymetry subroutines ::::::::::::::::::::::::::::::: ! interpolates local seabed depth and normal vector - SUBROUTINE getDepthFromBathymetry(BathymetryGrid, BathGrid_Xs, BathGrid_Ys, LineX, LineY, depth) + SUBROUTINE getDepthFromBathymetry(BathymetryGrid, BathGrid_Xs, BathGrid_Ys, LineX, LineY, depth, nvec) REAL(DbKi), INTENT(IN ) :: BathymetryGrid(:,:) ! need colons or some sort of dimension setting REAL(DbKi), INTENT(IN ) :: BathGrid_Xs(:) @@ -781,7 +781,7 @@ SUBROUTINE getDepthFromBathymetry(BathymetryGrid, BathGrid_Xs, BathGrid_Ys, Line REAL(DbKi), INTENT(IN ) :: LineX REAL(DbKi), INTENT(IN ) :: LineY REAL(DbKi), INTENT( OUT) :: depth ! local seabed depth (positive down) [m] - ! >>> to be added >>> REAL(DbKi), INTENT( OUT) :: nvec(3) ! local seabed surface normal vector (positive out) + REAL(DbKi), INTENT( OUT) :: nvec(3) ! local seabed surface normal vector (positive out) INTEGER(IntKi) :: ix0, iy0 ! indeces for interpolation INTEGER(IntKi) :: ix1, iy1 ! second indices @@ -839,7 +839,7 @@ SUBROUTINE getDepthFromBathymetry(BathymetryGrid, BathGrid_Xs, BathGrid_Ys, Line tempVector = -dc_dx tempVector = -dc_dy tempVector = 1.0_DbKi - ! ScaleVector( tempVector, 1.0_DbKi, nvec ) <<< ! compute unit vector + CALL ScaleVector( tempVector, 1.0_DbKi, nvec ) ! compute unit vector END SUBROUTINE getDepthFromBathymetry @@ -1298,7 +1298,7 @@ SUBROUTINE SetupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) CALL ReadVar( UnIn, FileName, p%dtWave , 'dtWave', 'time step for waves', ErrStat2, ErrMsg2, UnEcho); if(Failed()) return CALL ReadVar( UnIn, FileName, WaveDir , 'WaveDir' , 'wave direction', ErrStat2, ErrMsg2, UnEcho); if(Failed()) return ! X grid points - READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type + READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type READ(UnIn,*, IOSTAT=ErrStat2) entries2 ! get entries as string to be processed CALL gridAxisCoords(coordtype, entries2, p%pxWave, p%nxWave, ErrStat2, ErrMsg2) ! Y grid points diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 172e306422..56cb7a3718 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -383,7 +383,11 @@ typedef ^ ^ IntKi MDUnOut - typedef ^ ^ IntKi WaveKin - - - "Flag for whether or how to consider water kinematics" typedef ^ ^ IntKi Current - - - "Flag for whether or how to consider water kinematics" typedef ^ ^ IntKi nTurbines - - - "Number of turbines if MoorDyn is performing an array-level simulation with FAST.Farm, otherwise 0" -typedef ^ ^ ReKi TurbineRefPos {:}{:} - - "reference position of turbines in farm, shape: 3, nTurbines" - +typedef ^ ^ ReKi TurbineRefPos {:}{:} - - "reference position of turbines in farm, shape: 3, nTurbines" - +typedef ^ ^ DbKi mu_kT - - - "transverse kinetic friction coefficient" "(-)" +typedef ^ ^ DbKi mu_kA - - - "axial kinetic friction coefficient" "(-)" +typedef ^ ^ DbKi mc - - - "ratio of the static friction coefficient to the kinetic friction coefficient" "(-)" +typedef ^ ^ DbKi cv - - - "saturated damping coefficient" "(-)" # --- parameters for wave and current --- typedef ^ ^ IntKi nxWave - - - "number of x wave grid points" - typedef ^ ^ IntKi nyWave - - - "number of y wave grid points" - diff --git a/modules/moordyn/src/MoorDyn_Rod.f90 b/modules/moordyn/src/MoorDyn_Rod.f90 index 6bee471e54..aa7f141a5b 100644 --- a/modules/moordyn/src/MoorDyn_Rod.f90 +++ b/modules/moordyn/src/MoorDyn_Rod.f90 @@ -561,7 +561,8 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) Real(DbKi) :: Fcentripetal(3) ! centripetal force Real(DbKi) :: Mcentripetal(3) ! centripetal moment - Real(DbKi) :: depth ! local interpolated depth from bathymetry grid + Real(DbKi) :: depth ! local interpolated depth from bathymetry grid [m] + Real(DbKi) :: nvec(3) ! local seabed surface normal vector (positive out) N = Rod%N @@ -722,7 +723,7 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ! bottom contact (stiffness and damping, vertical-only for now) - updated Nov 24 for general case where anchor and fairlead ends may deal with bottom contact forces ! interpolate the local depth from the bathymetry grid - CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, Rod%r(1,I), Rod%r(2,I), depth) + CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, Rod%r(1,I), Rod%r(2,I), depth, nvec) IF (Rod%r(3,I) < -depth) THEN IF (I==0) THEN diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index 0191b8d0b8..e80552d555 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -416,6 +416,10 @@ MODULE MoorDyn_Types INTEGER(IntKi) :: Current !< Flag for whether or how to consider water kinematics [-] INTEGER(IntKi) :: nTurbines !< Number of turbines if MoorDyn is performing an array-level simulation with FAST.Farm, otherwise 0 [-] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: TurbineRefPos !< reference position of turbines in farm, shape: 3, nTurbines [-] + REAL(DbKi) :: mu_kT !< transverse kinetic friction coefficient [(-)] + REAL(DbKi) :: mu_kA !< axial kinetic friction coefficient [(-)] + REAL(DbKi) :: mc !< ratio of the static friction coefficient to the kinetic friction coefficient [(-)] + REAL(DbKi) :: cv !< saturated damping coefficient [(-)] INTEGER(IntKi) :: nxWave !< number of x wave grid points [-] INTEGER(IntKi) :: nyWave !< number of y wave grid points [-] INTEGER(IntKi) :: nzWave !< number of z wave grid points [-] @@ -10581,6 +10585,10 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) END IF DstParamData%TurbineRefPos = SrcParamData%TurbineRefPos ENDIF + DstParamData%mu_kT = SrcParamData%mu_kT + DstParamData%mu_kA = SrcParamData%mu_kA + DstParamData%mc = SrcParamData%mc + DstParamData%cv = SrcParamData%cv DstParamData%nxWave = SrcParamData%nxWave DstParamData%nyWave = SrcParamData%nyWave DstParamData%nzWave = SrcParamData%nzWave @@ -11033,6 +11041,10 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Int_BufSz = Int_BufSz + 2*2 ! TurbineRefPos upper/lower bounds for each dimension Re_BufSz = Re_BufSz + SIZE(InData%TurbineRefPos) ! TurbineRefPos END IF + Db_BufSz = Db_BufSz + 1 ! mu_kT + Db_BufSz = Db_BufSz + 1 ! mu_kA + Db_BufSz = Db_BufSz + 1 ! mc + Db_BufSz = Db_BufSz + 1 ! cv Int_BufSz = Int_BufSz + 1 ! nxWave Int_BufSz = Int_BufSz + 1 ! nyWave Int_BufSz = Int_BufSz + 1 ! nzWave @@ -11324,6 +11336,14 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si END DO END DO END IF + DbKiBuf(Db_Xferred) = InData%mu_kT + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%mu_kA + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%mc + Db_Xferred = Db_Xferred + 1 + DbKiBuf(Db_Xferred) = InData%cv + Db_Xferred = Db_Xferred + 1 IntKiBuf(Int_Xferred) = InData%nxWave Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%nyWave @@ -11946,6 +11966,14 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg END DO END DO END IF + OutData%mu_kT = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%mu_kA = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%mc = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + OutData%cv = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 OutData%nxWave = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 OutData%nyWave = IntKiBuf(Int_Xferred) From 0d7c83c3ad111e6cf89f6eb8ea07555fc775707d Mon Sep 17 00:00:00 2001 From: shousner Date: Mon, 1 Nov 2021 15:55:40 -0600 Subject: [PATCH 074/242] vs-build/FASTlib/FASTlib.vfproj commit - I don't know what this file means exactly so I'm pushing it here just in case its needed for anything else later, but can also be uncommitted later --- vs-build/FASTlib/FASTlib.vfproj | 1213 +++++++++++++++---------------- 1 file changed, 604 insertions(+), 609 deletions(-) diff --git a/vs-build/FASTlib/FASTlib.vfproj b/vs-build/FASTlib/FASTlib.vfproj index 1c44baea02..6ea4063e34 100644 --- a/vs-build/FASTlib/FASTlib.vfproj +++ b/vs-build/FASTlib/FASTlib.vfproj @@ -154,31 +154,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -188,31 +188,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -226,31 +226,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -264,31 +264,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -302,31 +302,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -340,31 +340,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -378,31 +378,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -437,31 +437,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -478,31 +478,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -521,31 +521,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -556,31 +556,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -600,31 +600,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -642,32 +642,32 @@ - - - - - + + + - - - - - + - + + + + + + + @@ -686,31 +686,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -724,31 +724,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -762,31 +762,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -800,31 +800,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -838,31 +838,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -876,31 +876,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -914,60 +914,60 @@ - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + @@ -990,60 +990,60 @@ - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + @@ -1094,31 +1094,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1126,31 +1126,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1179,31 +1179,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1217,31 +1217,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1255,31 +1255,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1293,31 +1293,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1331,31 +1331,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1369,31 +1369,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1407,31 +1407,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1445,31 +1445,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1483,31 +1483,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1534,31 +1534,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1575,31 +1575,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1613,31 +1613,31 @@ - + - + - + - - - - - + - + + + - + + + @@ -1654,31 +1654,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1692,35 +1692,40 @@ - + + + + + + - - - - - + + + - - - + - + + + - + + + @@ -1738,478 +1743,468 @@ - - - - - + + + + + - + - + - - - + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - - + + + + - + - + - - - + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - - + + + + - + - + - - - + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + @@ -2241,48 +2236,48 @@ - + - + - - - + + + - - - - - + + + - - - + - + + + - + + + @@ -2299,31 +2294,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -2337,31 +2332,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -2385,31 +2380,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -2433,31 +2428,31 @@ - - - - - + + + - - - + - + + + - + + + From 33382c19cf5d60e7b9091d20eb00a6f429bfa858 Mon Sep 17 00:00:00 2001 From: Stein Date: Wed, 3 Nov 2021 07:32:30 -0600 Subject: [PATCH 075/242] A couple bug fixes to the friction code - Needed an option to set FkA and FkT to zero when the velocity was zero, rather than divide by a zero velocity magnitude - Indented all the friction force calculations to be calculated when a line node is below the seabed, rather than calculating a friction force for all line nodes - Added indices to the tempVector calculation in getDepthFromBathymetry - Changed the dc_dx and dc_dy calculations to be a double negative (positive), since the c values of the bathymetry grid are positive depths --- modules/moordyn/src/MoorDyn_Line.f90 | 86 ++++++++++++++++------------ modules/moordyn/src/MoorDyn_Misc.f90 | 6 +- 2 files changed, 51 insertions(+), 41 deletions(-) diff --git a/modules/moordyn/src/MoorDyn_Line.f90 b/modules/moordyn/src/MoorDyn_Line.f90 index 64338944c2..aa5c6db582 100644 --- a/modules/moordyn/src/MoorDyn_Line.f90 +++ b/modules/moordyn/src/MoorDyn_Line.f90 @@ -1250,47 +1250,57 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, ELSE Fn = ( (-depth - Line%r(3,I))*nvec(2)*nvec*p%kBot - Vn*p%cBot) * 0.5*d*(Line%l(I) + Line%l(I+1) ) END IF - ELSE - Fn = 0.0_DbKi - END IF + + ! calculate the axial and transverse components of the total node velocity vector (q can now have a z-component from seabed slope) + DO J = 1, 3 + Va(J) = DOT_PRODUCT( Line%rd(:,I) , Line%q(:,I) ) * Line%q(J,I) + Vt(J) = Line%rd(J,I) - Va(J) + END DO + ! calculate the magnitudes of each velocity + VaMag = SQRT(Va(1)**2+Va(2)**2+Va(3)**2) + VtMag = SQRT(Vt(1)**2+Vt(2)**2+Vt(3)**2) + + ! find the maximum possible kinetic friction force using transverse and axial kinetic friction coefficients + FkTmax = p%mu_kT*SQRT(Fn(1)**2+Fn(2)**2+Fn(3)**2) + FkAmax = p%mu_kA*SQRT(Fn(1)**2+Fn(2)**2+Fn(3)**2) + ! turn the maximum kinetic friction forces into vectors in the direction of their velocities + DO J = 1, 3 + IF (VtMag==0) THEN + FkT(J) = 0.0_DbKi + ELSE + FkT(J) = FkTmax*Vt(J)/VtMag + END IF + IF (VaMag==0) THEN + FkA(J) = 0.0_DbKi + ELSE + FkA(J) = FkAmax*Va(J)/VaMag + END IF + END DO + ! calculate the ratio between the static and kinetic coefficients of friction + !mc_T = p%mu_sT/p%mu_kT + !mc_A = p%mu_sA/p%mu_kA + + ! calculate the transverse friction force + IF (p%mu_kT*p%cv*VtMag > p%mc*FkTmax) THEN ! if the friction force of the linear curve is greater than the maximum friction force allowed adjusted for static friction, + FfT = -FkT ! then the friction force is the maximum kinetic friction force vector (constant part of the curve) + ELSE ! if the friction force of the linear curve is less than the maximum friction force allowed adjusted for static friction, + FfT = -p%mu_kT*p%cv*Vt ! then the friction force is the calculated value of the linear line + END IF + ! calculate the axial friction force + IF (p%mu_kA*p%cv*VaMag > p%mc*FkAmax) THEN ! if the friction force of the linear curve is greater than the maximum friction force allowed adjusted for static friction, + FfA = -FkA ! then the friction force is the maximum kinetic friction force vector (constant part of the curve) + ELSE ! if the friction force of the linear curve is less than the maximum friction force allowed adjusted for static friction, + FfA = -p%mu_kA*p%cv*Va ! then the friction force is the calculated value of the linear line + END IF + ! NOTE: these friction forces have a negative sign here to indicate a force in the opposite direction of motion - ! calculate the axial and transverse components of the total node velocity vector (q can now have a z-component from seabed slope) - DO J = 1, 3 - Va(J) = DOT_PRODUCT( Line%rd(:,I) , Line%q(:,I) ) * Line%q(J,I) - Vt(J) = Line%rd(J,I) - Va(J) - END DO - ! calculate the magnitudes of each velocity - VaMag = SQRT(Va(1)**2+Va(2)**2+Va(3)**2) - VtMag = SQRT(Vt(1)**2+Vt(2)**2+Vt(3)**2) - - ! find the maximum possible kinetic friction force using transverse and axial kinetic friction coefficients - FkTmax = p%mu_kT*SQRT(Fn(1)**2+Fn(2)**2+Fn(3)**2) - FkAmax = p%mu_kA*SQRT(Fn(1)**2+Fn(2)**2+Fn(3)**2) - ! turn the maximum kinetic friction forces into vectors in the direction of their velocities - DO J = 1, 3 - FkT(J) = FkTmax*Vt(J)/VtMag - FkA(J) = FkAmax*Va(J)/VaMag - END DO - ! calculate the ratio between the static and kinetic coefficients of friction - !mc_T = p%mu_sT/p%mu_kT - !mc_A = p%mu_sA/p%mu_kA + ! the total friction force is along the plane of the seabed slope, which is just the vector sum of the transverse and axial components + Ff = FfT + FfA - ! calculate the transverse friction force - IF (p%mu_kT*p%cv*VtMag > p%mc*FkTmax) THEN ! if the friction force of the linear curve is greater than the maximum friction force allowed adjusted for static friction, - FfT = -FkT ! then the friction force is the maximum kinetic friction force vector (constant part of the curve) - ELSE ! if the friction force of the linear curve is less than the maximum friction force allowed adjusted for static friction, - FfT = -p%mu_kT*p%cv*Vt ! then the friction force is the calculated value of the linear line - END IF - ! calculate the axial friction force - IF (p%mu_kA*p%cv*VaMag > p%mc*FkAmax) THEN ! if the friction force of the linear curve is greater than the maximum friction force allowed adjusted for static friction, - FfA = -FkA ! then the friction force is the maximum kinetic friction force vector (constant part of the curve) - ELSE ! if the friction force of the linear curve is less than the maximum friction force allowed adjusted for static friction, - FfA = -p%mu_kA*p%cv*Va ! then the friction force is the calculated value of the linear line + ELSE + Fn = 0.0_DbKi + Ff = 0.0_DbKi END IF - ! NOTE: these friction forces have a negative sign here to indicate a force in the opposite direction of motion - - ! the total friction force is along the plane of the seabed slope, which is just the vector sum of the transverse and axial components - Ff = FfT + FfA ! the total force from bottom contact on the line node is the sum of the normal contact force and the friction force Line%B(:,I) = Fn + Ff diff --git a/modules/moordyn/src/MoorDyn_Misc.f90 b/modules/moordyn/src/MoorDyn_Misc.f90 index 9d279423c8..8a2ac33bb5 100644 --- a/modules/moordyn/src/MoorDyn_Misc.f90 +++ b/modules/moordyn/src/MoorDyn_Misc.f90 @@ -836,9 +836,9 @@ SUBROUTINE getDepthFromBathymetry(BathymetryGrid, BathGrid_Xs, BathGrid_Ys, Line dc_dy = 0.0_DbKi ! maybe this should raise an error end if - tempVector = -dc_dx - tempVector = -dc_dy - tempVector = 1.0_DbKi + tempVector(1) = dc_dx + tempVector(2) = dc_dy + tempVector(3) = 1.0_DbKi CALL ScaleVector( tempVector, 1.0_DbKi, nvec ) ! compute unit vector END SUBROUTINE getDepthFromBathymetry From 48c9e3e251be72e3ea72add71f13f986994bfe53 Mon Sep 17 00:00:00 2001 From: Stein Date: Wed, 3 Nov 2021 07:36:08 -0600 Subject: [PATCH 076/242] Changes in my vs-build FAST sln file --- vs-build/FAST/FAST.sln | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/vs-build/FAST/FAST.sln b/vs-build/FAST/FAST.sln index ec3d691059..88fddb1d0e 100644 --- a/vs-build/FAST/FAST.sln +++ b/vs-build/FAST/FAST.sln @@ -1,7 +1,7 @@  Microsoft Visual Studio Solution File, Format Version 12.00 -# Visual Studio 15 -VisualStudioVersion = 15.0.27428.2043 +# Visual Studio Version 16 +VisualStudioVersion = 16.0.31702.278 MinimumVisualStudioVersion = 10.0.40219.1 Project("{6989167D-11E4-40FE-8C1A-2192A86A7E90}") = "FAST", "FAST.vfproj", "{18AE8067-CCC6-4479-A0DB-C4089EF9FE71}" ProjectSection(ProjectDependencies) = postProject @@ -120,8 +120,8 @@ Global {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Debug_Matlab|x64.Build.0 = Release|Win32 {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Debug|Win32.ActiveCfg = Release|Win32 {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Debug|Win32.Build.0 = Release|Win32 - {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Debug|x64.ActiveCfg = Release|Win32 - {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Debug|x64.Build.0 = Release|Win32 + {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Debug|x64.ActiveCfg = Release|x64 + {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Debug|x64.Build.0 = Release|x64 {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Release_Double|Win32.ActiveCfg = Release|Win32 {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Release_Double|Win32.Build.0 = Release|Win32 {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Release_Double|x64.ActiveCfg = Release|Win32 From 24c06af965f67ac743f8f1938f68536a270f239a Mon Sep 17 00:00:00 2001 From: Stein Date: Wed, 3 Nov 2021 14:09:15 -0600 Subject: [PATCH 077/242] Fixed the wrong index for the unit normal vector --- modules/moordyn/src/MoorDyn_Line.f90 | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/modules/moordyn/src/MoorDyn_Line.f90 b/modules/moordyn/src/MoorDyn_Line.f90 index aa5c6db582..31a2d1a9b9 100644 --- a/modules/moordyn/src/MoorDyn_Line.f90 +++ b/modules/moordyn/src/MoorDyn_Line.f90 @@ -1244,11 +1244,11 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, END DO ! calculate the normal contact force on the line node IF (I==0) THEN - Fn = ( (-depth - Line%r(3,I))*nvec(2)*nvec*p%kBot - Vn*p%cBot) * 0.5*d*( Line%l(I+1) ) + Fn = ( (-depth - Line%r(3,I))*nvec(3)*nvec*p%kBot - Vn*p%cBot) * 0.5*d*( Line%l(I+1) ) ELSE IF (I==N) THEN - Fn = ( (-depth - Line%r(3,I))*nvec(2)*nvec*p%kBot - Vn*p%cBot) * 0.5*d*(Line%l(I) ) + Fn = ( (-depth - Line%r(3,I))*nvec(3)*nvec*p%kBot - Vn*p%cBot) * 0.5*d*(Line%l(I) ) ELSE - Fn = ( (-depth - Line%r(3,I))*nvec(2)*nvec*p%kBot - Vn*p%cBot) * 0.5*d*(Line%l(I) + Line%l(I+1) ) + Fn = ( (-depth - Line%r(3,I))*nvec(3)*nvec*p%kBot - Vn*p%cBot) * 0.5*d*(Line%l(I) + Line%l(I+1) ) END IF ! calculate the axial and transverse components of the total node velocity vector (q can now have a z-component from seabed slope) @@ -1305,7 +1305,6 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, ! the total force from bottom contact on the line node is the sum of the normal contact force and the friction force Line%B(:,I) = Fn + Ff - ! total forces IF (I==0) THEN Line%Fnet(:,I) = Line%T(:,1) + Line%Td(:,1) + Line%W(:,I) + Line%Dp(:,I) + Line%Dq(:,I) + Line%B(:,I) From d8e620a39a8be2baf50568519f9e31b6ceee6ca0 Mon Sep 17 00:00:00 2001 From: shousner Date: Thu, 11 Nov 2021 13:43:45 -0700 Subject: [PATCH 078/242] Bug fix in getDepthFromBathymetry: fy, not fx in interpolation --- modules/moordyn/src/MoorDyn_Misc.f90 | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/moordyn/src/MoorDyn_Misc.f90 b/modules/moordyn/src/MoorDyn_Misc.f90 index 8a2ac33bb5..6cb6c1b989 100644 --- a/modules/moordyn/src/MoorDyn_Misc.f90 +++ b/modules/moordyn/src/MoorDyn_Misc.f90 @@ -818,8 +818,8 @@ SUBROUTINE getDepthFromBathymetry(BathymetryGrid, BathGrid_Xs, BathGrid_Ys, Line ! get interpolated points and local value cx0 = c00 *(1.0-fx) + c10 *fx cx1 = c01 *(1.0-fx) + c11 *fx - c0y = c00 *(1.0-fy) + c01 *fx - c1y = c10 *(1.0-fy) + c11 *fx + c0y = c00 *(1.0-fy) + c01 *fy + c1y = c10 *(1.0-fy) + c11 *fy depth = cx0 *(1.0-fy) + cx1 *fy ! get local slope From 8f9853371b45495f23f3b19d2ef366ff0d98613f Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Thu, 2 Dec 2021 17:05:18 -0700 Subject: [PATCH 079/242] MD Driver updates and line output fix: - Added filtering in MD driver to smooth position inputs and compute smooth positions, velocities, and accelerations to drive MoorDyn with. (This does not necessarily make much difference.) - Fixed a but with line outputs where a line break would occur in a row of time series data after the node positions. --- modules/moordyn/src/MoorDyn.f90 | 2 +- modules/moordyn/src/MoorDyn_Driver.f90 | 160 +++++++++++++++++++++---- modules/moordyn/src/MoorDyn_IO.f90 | 2 +- 3 files changed, 140 insertions(+), 24 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index a2b2e44aef..fc795657c0 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -35,7 +35,7 @@ MODULE MoorDyn PRIVATE - TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a14', '15 October 2021' ) + TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a15', '16 November 2021' ) INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output diff --git a/modules/moordyn/src/MoorDyn_Driver.f90 b/modules/moordyn/src/MoorDyn_Driver.f90 index 4b5cfb3f8b..0c403fdcee 100644 --- a/modules/moordyn/src/MoorDyn_Driver.f90 +++ b/modules/moordyn/src/MoorDyn_Driver.f90 @@ -76,7 +76,11 @@ PROGRAM MoorDyn_Driver CHARACTER(100) :: Line ! String to temporarially hold value of read line REAL(ReKi), ALLOCATABLE :: PtfmMotIn(:,:) ! Variable for storing time, and DOF time series from driver input file REAL(ReKi), ALLOCATABLE :: r_in(:,:) ! Variable for storing interpolated DOF time series from driver input file + REAL(ReKi), ALLOCATABLE :: r_in2(:,:) ! used for filtering REAL(ReKi), ALLOCATABLE :: rd_in(:,:) ! Variable for storing 1st derivative of interpolate DOF time series + REAL(ReKi), ALLOCATABLE :: rd_in2(:,:) ! used for filtering + REAL(ReKi), ALLOCATABLE :: rdd_in(:,:) ! Variable for storing 2nd derivative of interpolate DOF time series + REAL(ReKi), ALLOCATABLE :: rdd_in2(:,:) ! used for filtering INTEGER(IntKi) :: ntIn ! number of time steps read from driver input file INTEGER(IntKi) :: ncIn ! number of channels read from driver input file INTEGER(IntKi) :: nt ! number of coupling time steps to use in simulation @@ -136,7 +140,7 @@ PROGRAM MoorDyn_Driver CALL CPU_TIME ( ProgStrtCPU ) ! Initial time (this zeros the start time when used as a MATLAB function) - + CALL WrScr( ' MD Driver updated 2021-11-15') ! Parse the driver input file and run the simulation based on that file CALL get_command_argument(1, drvrFilename) @@ -178,7 +182,12 @@ PROGRAM MoorDyn_Driver MD_InitInp%TurbineRefPos(1,J) = drvrInitInp%FarmPositions(1,J) MD_InitInp%TurbineRefPos(2,J) = drvrInitInp%FarmPositions(2,J) MD_InitInp%TurbineRefPos(3,J) = 0.0_DbKi - MD_InitInp%PtfmInit(:,J) = drvrInitInp%FarmPositions(3:8,J) + MD_InitInp%PtfmInit(1,J) = drvrInitInp%FarmPositions(3,J) + MD_InitInp%PtfmInit(2,J) = drvrInitInp%FarmPositions(4,J) + MD_InitInp%PtfmInit(3,J) = drvrInitInp%FarmPositions(5,J) + MD_InitInp%PtfmInit(4,J) = drvrInitInp%FarmPositions(6,J)*3.14159265/180.0 + MD_InitInp%PtfmInit(5,J) = drvrInitInp%FarmPositions(7,J)*3.14159265/180.0 + MD_InitInp%PtfmInit(6,J) = drvrInitInp%FarmPositions(8,J)*3.14159265/180.0 end do MD_interp_order = 0 @@ -309,7 +318,7 @@ PROGRAM MoorDyn_Driver ! allocate space for processed motion array - ALLOCATE ( r_in(nt, ncIn), rd_in(nt, ncIn), STAT=ErrStat2) + ALLOCATE ( r_in(nt, ncIn), r_in2(nt, ncIn), rd_in(nt, ncIn), rd_in2(nt, ncIn), rdd_in(nt, ncIn), rdd_in2(nt, ncIn), STAT=ErrStat2) IF ( ErrStat2 /= ErrID_None ) THEN ErrStat2 = ErrID_Fatal ErrMsg2 = ' Error allocating space for r_in or rd_in array.' @@ -346,7 +355,110 @@ PROGRAM MoorDyn_Driver END DO END DO - ! InputsMod == 1 + + ! ----- filter position ----- + ! now filter forward + DO i = 1,nt + DO J=1,ncIn + if (i==1) then + r_in2(i, J) = r_in(i, J) + else + r_in2(i, J) = 0.2*r_in(i, J) + 0.8*r_in2(i-1, J) + end if + END DO + END DO + ! now filter backward and save back to original variable + DO i = nt,1,-1 + DO J=1,ncIn + if (i==nt) then + r_in(i, J) = r_in2(i, J) + else + r_in(i, J) = 0.2*r_in2(i, J) + 0.8*r_in(i+1, J) + end if + END DO + END DO + + + ! now get derivative after filtering has been applied (derivative no longer needs to be calculated earlier) + DO i = 1,nt + DO J=1,ncIn + if (i==1) then + ! use forward different to estimate velocity of coupling point + rd_in(i, J) = (r_in(i+1, J) - r_in(i, J)) / dtC + else if (i==nt) then + ! use forward different to estimate velocity of coupling point + rd_in(i, J) = (r_in(i, J) - r_in(i-1, J)) / dtC + else + ! use central different to estimate velocity of coupling point + rd_in(i, J) = (r_in(i+1, J) - r_in(i-1, J)) / (2.0*dtC) + end if + END DO + END DO + + + + ! ----- filter velocity ----- + ! now filter forward + DO i = 1,nt + DO J=1,ncIn + if (i==1) then + rd_in2(i, J) = rd_in(i, J) + else + rd_in2(i, J) = 0.2*rd_in(i, J) + 0.8*rd_in2(i-1, J) + end if + END DO + END DO + ! now filter backward and save back to original variable + DO i = nt,1,-1 + DO J=1,ncIn + if (i==nt) then + rd_in(i, J) = rd_in2(i, J) + else + rd_in(i, J) = 0.2*rd_in2(i, J) + 0.8*rd_in(i+1, J) + end if + END DO + END DO + + + ! now get derivative after filtering has been applied (derivative no longer needs to be calculated earlier) + DO i = 1,nt + DO J=1,ncIn + if (i==1) then + ! use forward different to estimate velocity of coupling point + rdd_in(i, J) = (rd_in(i+1, J) - rd_in(i, J)) / dtC + else if (i==nt) then + ! use forward different to estimate velocity of coupling point + rdd_in(i, J) = (rd_in(i, J) - rd_in(i-1, J)) / dtC + else + ! use central different to estimate velocity of coupling point + rdd_in(i, J) = (rd_in(i+1, J) - rd_in(i-1, J)) / (2.0*dtC) + end if + END DO + END DO + + + ! ----- filter acceleration ----- + ! now filter forward + DO i = 1,nt + DO J=1,ncIn + if (i==1) then + rdd_in2(i, J) = rdd_in(i, J) + else + rdd_in2(i, J) = 0.2*rdd_in(i, J) + 0.8*rdd_in2(i-1, J) + end if + END DO + END DO + ! now filter backward and save back to original variable + DO i = nt,1,-1 + DO J=1,ncIn + if (i==nt) then + rdd_in(i, J) = rdd_in2(i, J) + else + rdd_in(i, J) = 0.2*rdd_in2(i, J) + 0.8*rdd_in(i+1, J) + end if + END DO + END DO + else nt = tMax/dtC - 1 ! number of coupling time steps @@ -415,42 +527,46 @@ PROGRAM MoorDyn_Driver DO iTurb = 1, MD_p%nTurbines - J = 1 ! J is the index of the coupling points in the input mesh CoupledKinematics + K = 1 ! the index of the coupling points in the input mesh CoupledKinematics + J = 1 ! the starting index of the relevant DOFs in the input array ! any coupled bodies (type -1) DO l = 1,MD_p%nCpldBodies(iTurb) - MD_u(1)%CoupledKinematics(iTurb)%TranslationDisp(:,J) = r_in(i, J:J+2) - MD_u(1)%CoupledKinematics(iTurb)%Orientation( :,:,J) = EulerConstruct( r_in(i, J+3:J+5) ) ! full Euler angle approach - MD_u(1)%CoupledKinematics(iTurb)%TranslationVel( :,J) = rd_in(i, J:J+2) - MD_u(1)%CoupledKinematics(iTurb)%RotationVel( :,J) = rd_in(i, J+3:J+5) - !a6_in(1:3) = u%CoupledKinematics(iTurb)%TranslationAcc(:,J) - !a6_in(4:6) = u%CoupledKinematics(iTurb)%RotationAcc(:,J) + MD_u(1)%CoupledKinematics(iTurb)%TranslationDisp(:,K) = r_in(i, J:J+2) - MD_u(1)%CoupledKinematics(iTurb)%Position(:,K) - MD_p%TurbineRefPos(:,iTurb) + MD_u(1)%CoupledKinematics(iTurb)%Orientation( :,:,K) = EulerConstruct( r_in(i, J+3:J+5) ) ! full Euler angle approach + MD_u(1)%CoupledKinematics(iTurb)%TranslationVel( :,K) = rd_in(i, J:J+2) + MD_u(1)%CoupledKinematics(iTurb)%RotationVel( :,K) = rd_in(i, J+3:J+5) + MD_u(1)%CoupledKinematics(iTurb)%TranslationAcc( :,K) = rdd_in(i, J:J+2) + MD_u(1)%CoupledKinematics(iTurb)%RotationAcc( :,K) = rdd_in(i, J+3:J+5) + K = K + 1 J = J + 6 END DO ! any coupled rods (type -1 or -2) >>> need to make rotations ignored if it's a pinned rod <<< DO l = 1,MD_p%nCpldRods(iTurb) - MD_u(1)%CoupledKinematics(iTurb)%TranslationDisp(:,J) = r_in(i, J:J+2) - MD_u(1)%CoupledKinematics(iTurb)%Orientation( :,:,J) = EulerConstruct( r_in(i, J+3:J+5) ) - MD_u(1)%CoupledKinematics(iTurb)%TranslationVel( :,J) = rd_in(i, J:J+2) - MD_u(1)%CoupledKinematics(iTurb)%RotationVel( :,J) = rd_in(i, J+3:J+5) + MD_u(1)%CoupledKinematics(iTurb)%TranslationDisp(:,K) = r_in(i, J:J+2) - MD_u(1)%CoupledKinematics(iTurb)%Position(:,K) - MD_p%TurbineRefPos(:,iTurb) + MD_u(1)%CoupledKinematics(iTurb)%Orientation( :,:,K) = EulerConstruct( r_in(i, J+3:J+5) ) + MD_u(1)%CoupledKinematics(iTurb)%TranslationVel( :,K) = rd_in(i, J:J+2) + MD_u(1)%CoupledKinematics(iTurb)%RotationVel( :,K) = rd_in(i, J+3:J+5) + MD_u(1)%CoupledKinematics(iTurb)%TranslationAcc( :,K) = rdd_in(i, J:J+2) + MD_u(1)%CoupledKinematics(iTurb)%RotationAcc( :,K) = rdd_in(i, J+3:J+5) + K = K + 1 J = J + 6 END DO ! any coupled points (type -1) DO l = 1, MD_p%nCpldCons(iTurb) - MD_u(1)%CoupledKinematics(iTurb)%TranslationDisp(:,J) = r_in(i, J:J+2) - MD_u(1)%CoupledKinematics(iTurb)%TranslationVel( :,J) = rd_in(i, J:J+2) - - !u%CoupledKinematics(iTurb)%TranslationVel(:,J) - !u%CoupledKinematics(iTurb)%TranslationAcc(:,J) + MD_u(1)%CoupledKinematics(iTurb)%TranslationDisp(:,K) = r_in(i, J:J+2) - MD_u(1)%CoupledKinematics(iTurb)%Position(:,K) - MD_p%TurbineRefPos(:,iTurb) + MD_u(1)%CoupledKinematics(iTurb)%TranslationVel( :,K) = rd_in(i, J:J+2) + MD_u(1)%CoupledKinematics(iTurb)%TranslationAcc( :,K) = rdd_in(i, J:J+2) !print *, u%PtFairleadDisplacement%Position(:,l) + u%PtFairleadDisplacement%TranslationDisp(:,l) !print *, u%PtFairleadDisplacement%TranslationVel(:,l) + K = K + 1 J = J + 3 END DO @@ -459,8 +575,8 @@ PROGRAM MoorDyn_Driver ! also provide any active tensioning commands do l = 1, size(MD_u(1)%DeltaL) - MD_u(1)%DeltaL( j) = r_in(i, J) - MD_u(1)%DeltaLdot(j) = rd_in(i, J) + MD_u(1)%DeltaL( l) = r_in(i, J) + MD_u(1)%DeltaLdot(l) = rd_in(i, J) J = J + 1 end do diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index 9d5d0f22d7..a61b157996 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -1391,7 +1391,7 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) + m%LineList(I)%N*SUM(m%LineList(I)%OutFlagList(10:18)) if (m%LineList(I)%OutFlagList(2) == 1) THEN ! if node positions are included, make them using a float format for higher precision - Frmt = '(F10.4,'//TRIM(Int2LStr(3*(m%LineList(I)%N + 1)))//'(A1,F12.4)),'//TRIM(Int2LStr(LineNumOuts - 3*(m%LineList(I)%N - 1)))//'(A1,e12.5))' + Frmt = '(F10.4,'//TRIM(Int2LStr(3*(m%LineList(I)%N + 1)))//'(A1,F12.4),'//TRIM(Int2LStr(LineNumOuts - 3*(m%LineList(I)%N - 1)))//'(A1,e12.5))' else Frmt = '(F10.4,'//TRIM(Int2LStr(LineNumOuts))//'(A1,e12.5))' ! should evenutally use user specified format? end if From e3eec4abf67bdae60419a8f911a4ddb917af164f Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Thu, 2 Dec 2021 22:46:58 -0700 Subject: [PATCH 080/242] FASTLib.vfproj update for new MD source files --- vs-build/FASTlib/FASTlib.vfproj | 1213 +++++++++++++++---------------- 1 file changed, 604 insertions(+), 609 deletions(-) diff --git a/vs-build/FASTlib/FASTlib.vfproj b/vs-build/FASTlib/FASTlib.vfproj index 8b50656451..62d1593276 100644 --- a/vs-build/FASTlib/FASTlib.vfproj +++ b/vs-build/FASTlib/FASTlib.vfproj @@ -154,31 +154,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -188,31 +188,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -226,31 +226,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -264,31 +264,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -302,31 +302,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -340,31 +340,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -378,31 +378,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -437,31 +437,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -478,31 +478,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -521,31 +521,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -556,31 +556,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -600,31 +600,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -642,32 +642,32 @@ - - - - - + + + - - - - - + - + + + + + + + @@ -686,31 +686,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -724,31 +724,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -762,31 +762,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -800,31 +800,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -838,31 +838,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -876,31 +876,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -914,60 +914,60 @@ - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + @@ -990,60 +990,60 @@ - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + @@ -1094,31 +1094,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1126,31 +1126,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1179,31 +1179,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1217,31 +1217,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1255,31 +1255,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1293,31 +1293,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1331,31 +1331,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1369,31 +1369,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1407,31 +1407,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1445,31 +1445,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1483,31 +1483,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1534,31 +1534,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1575,31 +1575,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1613,31 +1613,31 @@ - + - + - + - - - - - + - + + + - + + + @@ -1654,31 +1654,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -1692,35 +1692,40 @@ - + + + + + + - - - - - + + + - - - + - + + + - + + + @@ -1738,478 +1743,468 @@ - - - - - + + + + + - + - + - - - + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - - + + + + - + - + - - - + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - - + + + + - + - + - - - + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + - - - - - + + + - - - + - + + + - + + + @@ -2241,48 +2236,48 @@ - + - + - - - + + + - - - - - + + + - - - + - + + + - + + + @@ -2299,31 +2294,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -2337,31 +2332,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -2385,31 +2380,31 @@ - - - - - + + + - - - + - + + + - + + + @@ -2433,31 +2428,31 @@ - - - - - + + + - - - + - + + + - + + + From 3990bce9ce22a0eefb91287adac7c1b042282a45 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Mon, 13 Dec 2021 11:34:50 -0700 Subject: [PATCH 081/242] Update MoorDyn to new input file Body/Line format/capabilities: - Updates to bring MoorDyn input file format to hopefully its final form. - BODY section now has separate inputs for ID number and attachment type. - Body inputs now support single or 3D (separated by "|") values for CG, I, CdA, and Ca. - LINES section has had columns rearranged to the following order: ID LineType AttachA AttachB UnstrLen NumSegs Outputs - Changed input parsing of depth/bathymetry input option to avoid potential bug of it seeing an "E" for exponent and then treating the number as an input filename. Handling is now more general. - Updates have not been tested in all conditions (Bodies, bathymetry...) --- modules/moordyn/src/MoorDyn.f90 | 157 ++++++++++++++++++--------- modules/moordyn/src/MoorDyn_Body.f90 | 7 -- modules/moordyn/src/MoorDyn_Line.f90 | 7 ++ 3 files changed, 112 insertions(+), 59 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index fc795657c0..b408004dd2 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -35,7 +35,7 @@ MODULE MoorDyn PRIVATE - TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a15', '16 November 2021' ) + TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a16', '13 December 2021 (input format changes)' ) INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output @@ -120,6 +120,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CHARACTER(40) :: TempString2 ! CHARACTER(40) :: TempString3 ! CHARACTER(40) :: TempString4 ! + CHARACTER(40) :: TempString5 ! CHARACTER(40) :: TempStrings(6) ! Array of 6 strings used when parsing comma-separated items CHARACTER(1024) :: FileName ! @@ -136,7 +137,6 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er REAL(ReKi) :: rRef(6) ! used to pass positions to mesh (real type precision) REAL(DbKi) :: rRefDub(3) - CHARACTER(500) :: TempString5 ! long string used to hold CtrlChan inputs INTEGER(IntKi) :: TempIDnums(100) ! array to hold IdNums of controlled lines for each CtrlChan ! for reading output channels @@ -542,6 +542,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er if (INDEX(Line, "---") > 0) then ! look for a header line + CALL Conv2UC(Line) ! allow lowercase section header names as well !------------------------------------------------------------------------------------------- if ( ( INDEX(Line, "LINE DICTIONARY") > 0) .or. ( INDEX(Line, "LINE TYPES") > 0) ) then ! if line dictionary header @@ -558,7 +559,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !read into a line Line = NextLine(i) - ! parse out entries: Name Diam MassDenInAir EA cIntDamp >>EI(new)<< Cdn Can Cdt Cat + ! parse out entries: Name Diam MassDenInAir EA cIntDamp EI Cd Ca CdAx CaAx READ(Line,*,IOSTAT=ErrStat2) m%LineTypeList(l)%name, m%LineTypeList(l)%d, & m%LineTypeList(l)%w, tempString1, tempString2, tempString3, & m%LineTypeList(l)%Cdn, m%LineTypeList(l)%Can, m%LineTypeList(l)%Cdt, m%LineTypeList(l)%Cat @@ -587,7 +588,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er m%LineTypeList(l)%nEApoints, & m%LineTypeList(l)%stiffXs, & m%LineTypeList(l)%stiffYs, ErrStat2, ErrMsg2) - + + ! process damping coefficients CALL SplitByBars(tempString2, N, tempStrings) if (N > m%LineTypeList(l)%ElasticMod) then @@ -638,7 +640,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !read into a line Line = NextLine(i) - ! parse out entries: Name Diam MassDenInAir Can Cat Cdn Cdt + ! parse out entries: Name Diam MassDen Cd Ca CdEnd CaEnd IF (ErrStat2 == 0) THEN READ(Line,*,IOSTAT=ErrStat2) m%RodTypeList(l)%name, m%RodTypeList(l)%d, m%RodTypeList(l)%w, & m%RodTypeList(l)%Cdn, m%RodTypeList(l)%Can, m%RodTypeList(l)%CdEnd, m%RodTypeList(l)%CaEnd @@ -673,32 +675,85 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !read into a line Line = NextLine(i) - ! parse out entries: Name/ID X0 Y0 Z0 r0 p0 y0 Xcg Ycg Zcg M V IX IY IZ CdA-x,y,z Ca-x,y,z + ! parse out entries: ID Attachment X0 Y0 Z0 r0 p0 y0 M CG* I* V CdA* Ca* IF (ErrStat2 == 0) THEN - READ(Line,*,IOSTAT=ErrStat2) tempString1, & + READ(Line,*,IOSTAT=ErrStat2) m%BodyList(l)%IdNum, tempString1, & tempArray(1), tempArray(2), tempArray(3), tempArray(4), tempArray(5), tempArray(6), & - m%BodyList(l)%rCG(1), m%BodyList(l)%rCG(2), m%BodyList(l)%rCG(3), & - m%BodyList(l)%bodyM, m%BodyList(l)%bodyV, & - m%BodyList(l)%bodyI(1), m%BodyList(l)%bodyCdA(1), m%BodyList(l)%bodyCa(1) + m%BodyList(l)%bodyM, tempString2, tempString3, m%BodyList(l)%bodyV, tempString4, tempString5 END IF + + ! process CG + CALL SplitByBars(tempString2, N, tempStrings) + if (N == 1) then ! if only one entry, it is the z coordinate + m%BodyList(l)%rCG(1) = 0.0_DbKi + m%BodyList(l)%rCG(2) = 0.0_DbKi + READ(tempString2, *) m%BodyList(l)%rCG(3) + else if (N==3) then ! all three coordinates provided + READ(tempStrings(1), *) m%BodyList(l)%rCG(1) + READ(tempStrings(2), *) m%BodyList(l)%rCG(2) + READ(tempStrings(3), *) m%BodyList(l)%rCG(3) + else + CALL SetErrStat( ErrID_Fatal, 'Body '//trim(Num2LStr(l))//' CG entry (col 10) must have 1 or 3 numbers.' , ErrStat, ErrMsg, RoutineName ) + end if + ! process mements of inertia + CALL SplitByBars(tempString3, N, tempStrings) + if (N == 1) then ! if only one entry, use it for all directions + READ(tempString3, *) m%BodyList(l)%BodyI(1) + m%BodyList(l)%BodyI(2) = m%BodyList(l)%BodyI(1) + m%BodyList(l)%BodyI(3) = m%BodyList(l)%BodyI(1) + else if (N==3) then ! all three directions provided separately + READ(tempStrings(1), *) m%BodyList(l)%BodyI(1) + READ(tempStrings(2), *) m%BodyList(l)%BodyI(2) + READ(tempStrings(3), *) m%BodyList(l)%BodyI(3) + else + CALL SetErrStat( ErrID_Fatal, 'Body '//trim(Num2LStr(l))//' inertia entry (col 11) must have 1 or 3 numbers.' , ErrStat, ErrMsg, RoutineName ) + end if + ! process drag ceofficient by area product + CALL SplitByBars(tempString4, N, tempStrings) + if (N == 1) then ! if only one entry, use it for all directions + READ(tempString4, *) m%BodyList(l)%BodyCdA(1) + m%BodyList(l)%BodyCdA(2) = m%BodyList(l)%BodyCdA(1) + m%BodyList(l)%BodyCdA(3) = m%BodyList(l)%BodyCdA(1) + else if (N==3) then ! all three coordinates provided + READ(tempStrings(1), *) m%BodyList(l)%BodyCdA(1) + READ(tempStrings(2), *) m%BodyList(l)%BodyCdA(2) + READ(tempStrings(3), *) m%BodyList(l)%BodyCdA(3) + else + CALL SetErrStat( ErrID_Fatal, 'Body '//trim(Num2LStr(l))//' CdA entry (col 13) must have 1 or 3 numbers.' , ErrStat, ErrMsg, RoutineName ) + end if + ! process added mass coefficient + CALL SplitByBars(tempString5, N, tempStrings) + if (N == 1) then ! if only one entry, use it for all directions + READ(tempString5, *) m%BodyList(l)%BodyCa(1) + m%BodyList(l)%BodyCa(2) = m%BodyList(l)%BodyCa(1) + m%BodyList(l)%BodyCa(3) = m%BodyList(l)%BodyCa(1) + else if (N==3) then ! all three coordinates provided + READ(tempStrings(1), *) m%BodyList(l)%BodyCa(1) + READ(tempStrings(2), *) m%BodyList(l)%BodyCa(2) + READ(tempStrings(3), *) m%BodyList(l)%BodyCa(3) + else + CALL SetErrStat( ErrID_Fatal, 'Body '//trim(Num2LStr(l))//' Ca entry (col 14) must have 1 or 3 numbers.' , ErrStat, ErrMsg, RoutineName ) + end if + IF ( ErrStat2 /= 0 ) THEN CALL WrScr(' Unable to parse Body '//trim(Num2LStr(l))//' on row '//trim(Num2LStr(i))//' in input file.') ! Specific screen output because errors likely - CALL WrScr(' Ensure row has all 17 columns.') + CALL WrScr(' Ensure row has all 13 columns needed in MDv2 input file (13th Dec 2021).') CALL SetErrStat( ErrID_Fatal, 'Failed to read bodies.' , ErrStat, ErrMsg, RoutineName ) CALL CleanUp() RETURN END IF - !----------- process body type ----------------- - call DecomposeString(tempString1, let1, num1, let2, num2, let3) + call DecomposeString(tempString1, let1, num1, let2, num2, let3) ! note: this call is overkill (it's just a string) but leaving it here for potential future expansions - READ(num1, *) m%BodyList(l)%IdNum ! convert to int, representing parent body index - - if ((let2 == "COUPLED") .or. (let2 == "VESSEL") .or. (let1 == "CPLD") .or. (let1 == "VES")) then ! if a coupled body + if ((let1 == "ANCHOR") .or. (let1 == "FIXED") .or. (let1 == "FIX")) then ! if a fixed body (this would just be used if someone wanted to temporarly fix a body that things were attached to) + + m%BodyList(l)%typeNum = 1 + + else if ((let1 == "COUPLED") .or. (let1 == "VESSEL") .or. (let1 == "CPLD") .or. (let1 == "VES")) then ! if a coupled body m%BodyList(l)%typeNum = -1 p%nCpldBodies(1)=p%nCpldBodies(1)+1 ! add this rod to coupled list @@ -708,7 +763,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! TODO: add option for body coupling to different turbines in FAST.Farm <<< - else if ((let2 == "FREE") .or. (LEN_TRIM(let2)== 0)) then ! if a free body + else if (let1 == "FREE") then ! if a free body m%BodyList(l)%typeNum = 0 p%nFreeBodies=p%nFreeBodies+1 ! add this pinned rod to the free list because it is half free @@ -766,20 +821,32 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !read into a line Line = NextLine(i) - ! parse out entries: RodID Type/BodyID RodType Xa Ya Za Xb Yb Zb NumSegs Flags/Outputs + ! parse out entries: RodID RodType Attachment Xa Ya Za Xb Yb Zb NumSegs Flags/Outputs IF (ErrStat2 == 0) THEN READ(Line,*,IOSTAT=ErrStat2) m%RodList(l)%IdNum, tempString1, tempString2, & tempArray(1), tempArray(2), tempArray(3), tempArray(4), tempArray(5), tempArray(6), & m%RodList(l)%N, LineOutString END IF + ! find Rod properties index + DO J = 1,p%nRodTypes + IF (trim(tempString1) == trim(m%RodTypeList(J)%name)) THEN + m%RodList(l)%PropsIdNum = J + EXIT + IF (J == p%nRodTypes) THEN ! call an error if there is no match + CALL SetErrStat( ErrID_Fatal, 'Unable to find matching rod type name for Rod '//trim(Num2LStr(l))//": "//trim(tempString1), ErrStat, ErrMsg, RoutineName ) + END IF + END IF + END DO + !----------- process rod type ----------------- - call DecomposeString(tempString1, let1, num1, let2, num2, let3) + call DecomposeString(tempString2, let1, num1, let2, num2, let3) - if ((let1 == "ANCHOR") .or. (let1 == "FIXED") .or. (let1 == "FIX")) then - m%RodList(l)%typeNum = 2 + if ((let1 == "ANCHOR") .or. (let1 == "FIXED") .or. (let1 == "FIX")) then + + m%RodList(l)%typeNum = 2 CALL Body_AddRod(m%GroundBody, l, tempArray) ! add rod l to Ground body @@ -865,21 +932,9 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else - CALL SetErrStat( ErrID_Fatal, "Unidentified Type/BodyID for Rod "//trim(Num2LStr(l))//": "//trim(tempString1), ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, "Unidentified Type/BodyID for Rod "//trim(Num2LStr(l))//": "//trim(tempString2), ErrStat, ErrMsg, RoutineName ) return end if - - - ! find Rod properties index - DO J = 1,p%nRodTypes - IF (trim(tempString2) == trim(m%RodTypeList(J)%name)) THEN - m%RodList(l)%PropsIdNum = J - EXIT - IF (J == p%nRodTypes) THEN ! call an error if there is no match - CALL SetErrStat( ErrID_Fatal, 'Unable to find matching rod type name for Rod '//trim(Num2LStr(l))//": "//trim(tempString2), ErrStat, ErrMsg, RoutineName ) - END IF - END IF - END DO ! process output flag characters (LineOutString) and set line output flag array (OutFlagList) @@ -947,22 +1002,22 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !read into a line Line = NextLine(i) - ! parse out entries: Node Type X Y Z M V FX FY FZ CdA Ca + ! parse out entries: PointID Attachment X Y Z M V CdA Ca IF (ErrStat2 == 0) THEN READ(Line,*,IOSTAT=ErrStat2) m%ConnectList(l)%IdNum, tempString1, tempArray(1), & tempArray(2), tempString4, m%ConnectList(l)%conM, & m%ConnectList(l)%conV, m%ConnectList(l)%conCdA, m%ConnectList(l)%conCa + + CALL Conv2UC(tempString4) ! convert to uppercase so that matching is not case-sensitive - IF (SCAN(tempString4, "seabed") == 0) THEN - ! if the tempString of the anchor depth value does not have any letters that are found in the word seabed, it's a scalar depth value - READ(tempString4, *, IOSTAT=ErrStat2) tempArray(3) - ELSE ! otherwise interpret the anchor depth value as a 'seabed' input, meaning the anchor should be on the seabed wherever the bathymetry says it should be - PRINT *, "Anchor depth set for seabed; Finding the right seabed depth based on bathymetry" - - CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, tempArray(1), tempArray(2), depth) + if ((INDEX(tempString4, "SEABED") > 0 ) .or. (INDEX(tempString4, "GROUND") > 0 ) .or. (INDEX(tempString4, "FLOOR") > 0 )) then ! if keyword used + PRINT *, 'Point '//trim(Num2LStr(l))//' depth set to be on the seabed; finding z location based on depth/bathymetry' ! interpret the anchor depth value as a 'seabed' input + CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, tempArray(1), tempArray(2), depth) ! meaning the anchor should be at the depth of the local bathymetry tempArray(3) = -depth - - END IF + else ! if the anchor depth input isn't one of the supported keywords, + READ(tempString4, *, IOSTAT=ErrStat2) tempArray(3) ! assume it's a scalar depth value + !TODO: add error check for if the above read fails + end if ! not used m%ConnectList(l)%conFX = 0.0_DbKi @@ -973,8 +1028,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF ( ErrStat2 /= 0 ) THEN - CALL WrScr(' Unable to parse Connection '//trim(Num2LStr(I))//' row in input file.') ! Specific screen output because errors likely - CALL WrScr(' Ensure row has all 12 columns, including CdA and Ca.') ! to be caused by non-updated input file formats. + CALL WrScr(' Unable to parse Point '//trim(Num2LStr(l))//' row in input file.') ! Specific screen output because errors likely + CALL WrScr(' Ensure row has all 9 columns, including CdA and Ca.') ! to be caused by non-updated input file formats. CALL SetErrStat( ErrID_Fatal, 'Failed to read connects.' , ErrStat, ErrMsg, RoutineName ) ! would be nice to specify which line <<<<<<<<< CALL CleanUp() RETURN @@ -1104,14 +1159,12 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !read into a line Line = NextLine(i) - ! parse out entries: LineID LineType UnstrLen NumSegs NodeA NodeB Flags/Outputs + ! parse out entries: ID LineType AttachA AttachB UnstrLen NumSegs Outputs (note: order changed Dec 13, 2021 before MDv2 release) IF (ErrStat2 == 0) THEN - READ(Line,*,IOSTAT=ErrStat2) m%LineList(l)%IdNum, tempString1, m%LineList(l)%UnstrLen, & - m%LineList(l)%N, tempString2, tempString3, LineOutString + READ(Line,*,IOSTAT=ErrStat2) m%LineList(l)%IdNum, tempString1, tempString2, tempString3, & + m%LineList(l)%UnstrLen, m%LineList(l)%N, LineOutString END IF - - !note: m%LineList(I)%CtrlChan should aready be initialized to zero as per the registry - + ! identify index of line type DO J = 1,p%nLineTypes IF (trim(tempString1) == trim(m%LineTypeList(J)%name)) THEN diff --git a/modules/moordyn/src/MoorDyn_Body.f90 b/modules/moordyn/src/MoorDyn_Body.f90 index 9850d9120c..7ab12c6347 100644 --- a/modules/moordyn/src/MoorDyn_Body.f90 +++ b/modules/moordyn/src/MoorDyn_Body.f90 @@ -72,13 +72,6 @@ SUBROUTINE Body_Setup( Body, tempArray, rhoW, ErrStat, ErrMsg) Body%nAttachedC = 0 Body%nAttachedR = 0 - ! for now take one entry and apply to all three DOFs just using a single entry for all axes <<<<< - DO J=2,3 - Body%BodyI(J) = Body%BodyI(1) - Body%BodyCdA(J) = Body%BodyCdA(1) - Body%BodyCa(J) = Body%BodyCa(1) - END DO - ! set up body initial mass matrix (excluding any rods or attachements) DO J=1,3 Mtemp(J,J) = Body%bodyM ! fill in mass diff --git a/modules/moordyn/src/MoorDyn_Line.f90 b/modules/moordyn/src/MoorDyn_Line.f90 index 6c25c09c39..78e80d115b 100644 --- a/modules/moordyn/src/MoorDyn_Line.f90 +++ b/modules/moordyn/src/MoorDyn_Line.f90 @@ -101,6 +101,7 @@ SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) END DO + ! Specify specific internal damping coefficient (BA) for this line. ! Will be equal to inputted BA of LineType if input value is positive. ! If input value is negative, it is considered to be desired damping ratio (zeta) @@ -123,6 +124,12 @@ SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) !print *, 'Segment natural frequency is ', temp, ' Hz' + print *, "Line ElasticMod is ", Line%ElasticMod + print *, "EA (static value) is", Line%EA + print *, "EA_D is", Line%EA_D + print *, "BA is", Line%BA + print *, "BA_D is", Line%BA_D + ! allocate node positions and velocities (NOTE: these arrays start at ZERO) ALLOCATE ( Line%r(3, 0:N), Line%rd(3, 0:N), STAT = ErrStat ) From f21290e66d7422ee6f12c5e8d0e57c83ba39030e Mon Sep 17 00:00:00 2001 From: shousner Date: Mon, 13 Dec 2021 15:39:01 -0700 Subject: [PATCH 082/242] Bug fix of transverse/axial friction coefficients --- modules/moordyn/src/MoorDyn.f90 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index e00529e0cd..00ba7929e9 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -435,7 +435,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if ( OptString == 'MU_KT') then read (OptValue,*) p%mu_kT else if ( OptString == 'MU_KA') then - read (OptValue,*) p%mu_kT + read (OptValue,*) p%mu_kA else if ( OptString == 'MC') then read (OptValue,*) p%mc else if ( OptString == 'CV') then From 66955bf46bc2b2efd2098622ff108ccddee01927 Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Fri, 28 Jan 2022 18:28:04 -0700 Subject: [PATCH 083/242] UA: update of drag to better match HAWC2's implementation --- modules/aerodyn/src/UnsteadyAero.f90 | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/modules/aerodyn/src/UnsteadyAero.f90 b/modules/aerodyn/src/UnsteadyAero.f90 index c1e6151630..79740d8910 100644 --- a/modules/aerodyn/src/UnsteadyAero.f90 +++ b/modules/aerodyn/src/UnsteadyAero.f90 @@ -3008,6 +3008,7 @@ subroutine UA_CalcOutput( i, j, t, u_in, p, x, xd, OtherState, AFInfo, y, misc, real(ReKi) :: cn_circ, tau_vl, tV_ratio real(ReKi) :: delta_c_df_primeprime real(ReKi), parameter :: delta_c_mf_primeprime = 0.0_ReKi + real(ReKi) :: cl_circ, cd_tors TYPE(UA_ElementContinuousStateType) :: x_in ! Continuous states at t ! for BV real(ReKi) :: alphaE_L, alphaE_D ! effective angle of attack for lift and drag @@ -3141,10 +3142,12 @@ subroutine UA_CalcOutput( i, j, t, u_in, p, x, xd, OtherState, AFInfo, y, misc, delta_c_df_primeprime = 0.5_ReKi * (sqrt(fs_aE) - sqrt(x4)) - 0.25_ReKi * (fs_aE - x4) ! Eq. 81 ! bjj: do we need to check that u%alpha is between -pi and + pi? - y%Cl = x4 * cl_fa + (1.0_ReKi - x4) * cl_fs + pi * Tu * u%omega ! Eq. 78 + cl_circ = x4 * cl_fa + (1.0_ReKi - x4) * cl_fs + y%Cl = cl_circ + pi * Tu * u%omega ! Eq. 78 call AddOrSub2Pi(u%alpha, alphaE) - y%Cd = AFI_interp%Cd + (u%alpha - alphaE) * y%Cl + (AFI_interp%Cd - BL_p%Cd0) * delta_c_df_primeprime ! Eq. 79 + cd_tors = cl_circ * Tu * u%omega + y%Cd = AFI_interp%Cd + (alpha_34 - alphaE) * cl_circ + (AFI_interp%Cd - BL_p%Cd0) * delta_c_df_primeprime + cd_tors ! Eq. 79 if (AFInfo%ColCm == 0) then ! we don't have a cm column, so make everything 0 y%Cm = 0.0_ReKi From e2942a5b037a3c328db3e33025f09b653d4736e6 Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Fri, 28 Jan 2022 18:33:34 -0700 Subject: [PATCH 084/242] UA: update driver to switch between oscillation at mid/chord or AC, and velocity at 3/4 or AC --- modules/aerodyn/src/UnsteadyAero_Driver.f90 | 41 ++++++++++++++++++--- 1 file changed, 35 insertions(+), 6 deletions(-) diff --git a/modules/aerodyn/src/UnsteadyAero_Driver.f90 b/modules/aerodyn/src/UnsteadyAero_Driver.f90 index cd1591a122..30dc3c0480 100644 --- a/modules/aerodyn/src/UnsteadyAero_Driver.f90 +++ b/modules/aerodyn/src/UnsteadyAero_Driver.f90 @@ -289,18 +289,47 @@ subroutine setUAinputs(n,u,t,dt,dvrInitInp,timeArr,AOAarr,Uarr,OmegaArr) real(ReKi), intent(in) :: OmegaArr(:) integer :: indx real(ReKi) :: phase + real(ReKi) :: d_ref2AC + real(ReKi) :: alpha_ref + real(ReKi) :: U_ref + real(ReKi) :: v_ref(2) + real(ReKi) :: v_34(2) + logical, parameter :: OscillationAtMidChord=.true. ! for legacy, use false + logical, parameter :: VelocityAt34 =.true. ! for legacy, use false u%UserProp = 0 u%Re = dvrInitInp%Re if ( dvrInitInp%SimMod == 1 ) then + if (OscillationAtMidChord) then + d_ref2AC =-0.25_ReKi ! -0.25: oscillations at mid_chord + else + d_ref2AC = 0.0_ReKi ! 0: oscillations at AC + endif + U_ref = dvrInitInp%InflowVel ! m/s + t = (n-1)*dt phase = (n+dvrInitInp%Phase-1)*2*pi/dvrInitInp%StepsPerCycle - u%alpha = (dvrInitInp%Amplitude * sin(phase) + dvrInitInp%Mean)*D2R ! This needs to be in radians - ! u%omega = dvrInitInp%Amplitude * cos(phase) * dvrInitInp%Frequency * pi**2 / 90.0 ! This needs to be in radians derivative: d_alpha /d_t + alpha_ref = (dvrInitInp%Amplitude * sin(phase) + dvrInitInp%Mean)*D2R ! This needs to be in radians + v_ref(1) = sin(alpha_ref)*U_ref + v_ref(2) = cos(alpha_ref)*U_ref u%omega = dvrInitInp%Amplitude * cos(phase) * 2*pi/dvrInitInp%StepsPerCycle / dt * D2R ! This needs to be in radians derivative: d_alpha /d_t - - u%U = dvrInitInp%InflowVel ! m/s + + u%v_ac(1) = v_ref(1) + u%omega * d_ref2AC* dvrInitInp%Chord + u%v_ac(2) = v_ref(2) + + v_34(1) = u%v_ac(1) + u%omega * 0.5* dvrInitInp%Chord + v_34(2) = u%v_ac(2) + + + u%alpha = atan2(u%v_ac(1), u%v_ac(2) ) ! + if (VelocityAt34) then + u%U = sqrt(v_34(1)**2 + v_34(2)**2) ! Using U at 3/4 + else + u%U = sqrt(u%v_ac(1)**2 + u%v_ac(2)**2) ! Using U at 1/4 + endif + + else indx = min(n,size(timeArr)) indx = max(1, indx) ! use constant data at initialization @@ -315,9 +344,9 @@ subroutine setUAinputs(n,u,t,dt,dvrInitInp,timeArr,AOAarr,Uarr,OmegaArr) elseif (n < 1) then t = (n-1)*dt end if + u%v_ac(1) = sin(u%alpha)*u%U + u%v_ac(2) = cos(u%alpha)*u%U end if - u%v_ac(1) = sin(u%alpha)*u%U - u%v_ac(2) = cos(u%alpha)*u%U end subroutine setUAinputs !---------------------------------------------------------------------------------------------------- From 360fcc5d663783dc380859089c38e9e2c4a2e630 Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Sat, 29 Jan 2022 16:03:52 -0700 Subject: [PATCH 085/242] DBEMT: temporarily turning off Wdot, and changing tau1 --- modules/aerodyn/src/BEMT.f90 | 5 +++-- modules/aerodyn/src/DBEMT.f90 | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/modules/aerodyn/src/BEMT.f90 b/modules/aerodyn/src/BEMT.f90 index 379ec70d06..08e36c638a 100644 --- a/modules/aerodyn/src/BEMT.f90 +++ b/modules/aerodyn/src/BEMT.f90 @@ -944,8 +944,9 @@ subroutine SetInputs_For_DBEMT(u_DBEMT, u, p, axInduction, tanInduction, Rtip) if( allocated(u%Vx_elast_dot)) then ! only for DBEMT_Mod=DBEMT_cont_tauConst do j = 1,p%numBlades do i = 1,p%numBladeNodes - u_DBEMT%element(i,j)%vind_s_dot(1) = axInduction( i,j)*u%Vx_elast_dot(i,j) - u%omega_z(i,j)*tanInduction(i,j)*u%Vy(i,j) ! Eq. 41 - u_DBEMT%element(i,j)%vind_s_dot(2) = -tanInduction(i,j)*u%Vy_elast_dot(i,j) - u%omega_z(i,j)*axInduction( i,j)*u%Vx(i,j) ! Eq. 41 + ! TODO temporarily turning off velocity + u_DBEMT%element(i,j)%vind_s_dot(1) = 0 ! axInduction( i,j)*u%Vx_elast_dot(i,j) - u%omega_z(i,j)*tanInduction(i,j)*u%Vy(i,j) ! Eq. 41 + u_DBEMT%element(i,j)%vind_s_dot(2) = 0 !-tanInduction(i,j)*u%Vy_elast_dot(i,j) - u%omega_z(i,j)*axInduction( i,j)*u%Vx(i,j) ! Eq. 41 end do end do end if diff --git a/modules/aerodyn/src/DBEMT.f90 b/modules/aerodyn/src/DBEMT.f90 index 4d3eae6a17..9be0bc18d1 100644 --- a/modules/aerodyn/src/DBEMT.f90 +++ b/modules/aerodyn/src/DBEMT.f90 @@ -568,7 +568,7 @@ SUBROUTINE DBEMT_CalcContStateDeriv( i, j, t, u, p, x, OtherState, m, dxdt, ErrS return end if - tau1 = p%tau1_const + tau1 = p%tau1_const/2 ! TODO Temporarily changing tau 1 !call ComputeTau1( u, p, m, tau1, errStat, errMsg) call ComputeTau2(i, j, u, p, tau1, tau2) @@ -921,4 +921,4 @@ subroutine DBEMT_End( u, p, x, OtherState, m, ErrStat, ErrMsg ) END SUBROUTINE DBEMT_End -end module DBEMT \ No newline at end of file +end module DBEMT From 104f8fc69325e3422f9962ea8940b95527cd38ae Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Mon, 7 Feb 2022 15:19:33 -0700 Subject: [PATCH 086/242] Fix HD added mass on member end (Close #992) --- modules/hydrodyn/src/Morison.f90 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/hydrodyn/src/Morison.f90 b/modules/hydrodyn/src/Morison.f90 index 1a861ab5ea..505a57321a 100644 --- a/modules/hydrodyn/src/Morison.f90 +++ b/modules/hydrodyn/src/Morison.f90 @@ -2122,7 +2122,7 @@ SUBROUTINE Morison_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, In ! Constant part of the external hydrodynamic added mass term if ( Vmag > 0.0 ) then v2D(:,1) = Vn - p%AM_End(:,:,i) = (InitInp%Nodes(I)%JAxCa*InitInp%WtrDens/ Vmag)*matmul(transpose(v2D), v2D) + p%AM_End(:,:,i) = (InitInp%Nodes(I)%JAxCa*InitInp%WtrDens/ Vmag)*matmul(v2D, transpose(v2D)) end if ! Constant part of the external hydrodynamic dynamic pressure force From 5184e3b8d50fc58b10fae48676b611b9247a98eb Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Tue, 8 Feb 2022 16:04:17 -0700 Subject: [PATCH 087/242] MoorDyn waterkin and i/o updates MoorDyn: - Increased interp order of MoorDyn Driver to 1. Using 0 caused large tension discrepancies. - MoorDyn Driver active tensioning inputs are currently disabled. - Added support for alternative MoorDyn depth keywords: wtrdpth, depth, and waterdepth - Added acceleration output channels for rods and points. - Fixed up wave and current kinematics abilities. FAST.Farm: Commented out timers and print statements in OpenMP option to avoid some compile problems. --- glue-codes/fast-farm/src/FAST_Farm_Subs.f90 | 118 ++++++++++---------- modules/moordyn/src/MoorDyn.f90 | 13 +-- modules/moordyn/src/MoorDyn_Body.f90 | 8 +- modules/moordyn/src/MoorDyn_Driver.f90 | 53 +++++---- modules/moordyn/src/MoorDyn_IO.f90 | 14 ++- modules/moordyn/src/MoorDyn_Line.f90 | 45 ++++---- modules/moordyn/src/MoorDyn_Misc.f90 | 97 +++++++++++----- modules/moordyn/src/MoorDyn_Registry.txt | 2 + modules/moordyn/src/MoorDyn_Rod.f90 | 72 ++++-------- modules/moordyn/src/MoorDyn_Types.f90 | 26 +++++ 10 files changed, 251 insertions(+), 197 deletions(-) diff --git a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 index dbbf9666de..56fa954571 100644 --- a/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 +++ b/glue-codes/fast-farm/src/FAST_Farm_Subs.f90 @@ -1433,7 +1433,7 @@ SUBROUTINE Farm_ValidateInput( p, WD_InitInp, AWAE_InitInp, SC_InitInp, ErrStat, ! --- SHARED MOORING SYSTEM --- ! TODO : Verify that p%MD_FileName file exists - if ((p%DT_mooring <= 0.0_ReKi) .or. p%DT_mooring > p%DT_high)) CALL SetErrStat(ErrID_Fatal,'DT_mooring must be greater than zero and no greater than dt_high.',ErrStat,ErrMsg,RoutineName) + if ((p%DT_mooring <= 0.0_ReKi) .or. (p%DT_mooring > p%DT_high)) CALL SetErrStat(ErrID_Fatal,'DT_mooring must be greater than zero and no greater than dt_high.',ErrStat,ErrMsg,RoutineName) ! --- WAKE DYNAMICS --- IF (WD_InitInp%dr <= 0.0_ReKi) CALL SetErrStat(ErrID_Fatal,'dr (radial increment) must be larger than 0.',ErrStat,ErrMsg,RoutineName) @@ -2148,9 +2148,7 @@ subroutine FARM_UpdateStates(t, n, farm, ErrStat, ErrMsg) if (ErrStat >= AbortErrLev) return - #ifdef _OPENMP - #define printthreads - #endif + !....................................................................................... @@ -2193,51 +2191,51 @@ subroutine FARM_UpdateStates(t, n, farm, ErrStat, ErrMsg) ! Original case: no shared moorings if (farm%p%MooringMod == 0) then - #ifdef printthreads - tm1 = omp_get_wtime() - #endif + !#ifdef printthreads + ! tm1 = omp_get_wtime() + !#endif !$OMP PARALLEL DO DEFAULT(Shared) Private(nt) !Private(nt,tm2,tm3) DO nt = 1,farm%p%NumTurbines+1 if(nt.ne.farm%p%NumTurbines+1) then - #ifdef printthreads - tm3 = omp_get_wtime() - #endif + !#ifdef printthreads + ! tm3 = omp_get_wtime() + !#endif call FWrap_Increment( t, n, farm%FWrap(nt)%u, farm%FWrap(nt)%p, farm%FWrap(nt)%x, farm%FWrap(nt)%xd, farm%FWrap(nt)%z, & farm%FWrap(nt)%OtherSt, farm%FWrap(nt)%y, farm%FWrap(nt)%m, ErrStatF(nt), ErrMsgF(nt) ) - #ifdef printthreads - tm2 = omp_get_wtime() - write(*,*) ' FWrap_Increment for turbine #'//trim(num2lstr(nt))//' using thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' - #endif + !#ifdef printthreads + ! tm2 = omp_get_wtime() + ! write(*,*) ' FWrap_Increment for turbine #'//trim(num2lstr(nt))//' using thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' + !#endif else - #ifdef printthreads - tm3 = omp_get_wtime() - #endif + !#ifdef printthreads + ! tm3 = omp_get_wtime() + !#endif call AWAE_UpdateStates( t, n, farm%AWAE%u, farm%AWAE%p, farm%AWAE%x, farm%AWAE%xd, farm%AWAE%z, & farm%AWAE%OtherSt, farm%AWAE%m, ErrStatAWAE, ErrMsgAWAE ) - #ifdef printthreads - tm2 = omp_get_wtime() - write(*,*) ' AWAE_UpdateStates using thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' - #endif + !#ifdef printthreads + ! tm2 = omp_get_wtime() + ! write(*,*) ' AWAE_UpdateStates using thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' + !#endif endif END DO !$OMP END PARALLEL DO - #ifdef printthreads - tm2 = omp_get_wtime() - write(*,*) 'Total Farm_US took '//trim(num2lstr(tm2-tm1))//' seconds.' - #endif + !#ifdef printthreads + ! tm2 = omp_get_wtime() + ! write(*,*) 'Total Farm_US took '//trim(num2lstr(tm2-tm1))//' seconds.' + !#endif ! Farm-level moorings case using MoorDyn else if (farm%p%MooringMod == 3) then - #ifdef printthreads - tm1 = omp_get_wtime() - #endif + !#ifdef printthreads + ! tm1 = omp_get_wtime() + !#endif ! Set up two parallel sections - one for FAST-MoorDyn steps (FAST portion in parallel for each step), and the other for AWAE. !$OMP PARALLEL SECTIONS DEFAULT(Shared) @@ -2246,11 +2244,11 @@ subroutine FARM_UpdateStates(t, n, farm, ErrStat, ErrMsg) ! The first section, for looping through FAST and farm-level MoorDyn time steps !$OMP SECTION - #ifdef printthreads - tm3 = omp_get_wtime() - tmSF = 0.0_DbKi - tmSM = 0.0_DbKi - #endif + !#ifdef printthreads + ! tm3 = omp_get_wtime() + ! tmSF = 0.0_DbKi + ! tmSM = 0.0_DbKi + !#endif ! This is the FAST-MoorDyn farm-level substepping loop do n_ss = 1, farm%p%n_mooring ! do n_mooring substeps (number of FAST/FarmMD steps per Farm time step) @@ -2258,9 +2256,9 @@ subroutine FARM_UpdateStates(t, n, farm, ErrStat, ErrMsg) n_FMD = n*farm%p%n_mooring + n_ss - 1 ! number of the current time step of the call to FAST and MoorDyn t2 = t + farm%p%DT_mooring*(n_ss - 1) ! current time in the loop - #ifdef printthreads - tm01 = omp_get_wtime() - #endif + !#ifdef printthreads + ! tm01 = omp_get_wtime() + !#endif ! A nested parallel for loop to call each instance of OpenFAST in parallel !$OMP PARALLEL DO DEFAULT(Shared) Private(nt) @@ -2270,51 +2268,51 @@ subroutine FARM_UpdateStates(t, n, farm, ErrStat, ErrMsg) END DO !$OMP END PARALLEL DO - #ifdef printthreads - tm02 = omp_get_wtime() - #endif + !#ifdef printthreads + ! tm02 = omp_get_wtime() + !#endif ! call farm-level MoorDyn time step here (can't multithread this with FAST since it needs inputs from all FAST instances) call Farm_MD_Increment( t2, n_FMD, farm, ErrStatMD, ErrMsgMD) call SetErrStat(ErrStatMD, ErrMsgMD, ErrStat, ErrMsg, 'FARM_UpdateStates') ! MD error status <<<<< - #ifdef printthreads - tm03 = omp_get_wtime() - tmSF = tmSF + tm02-tm01 - tmSM = tmSM + tm03-tm02 - #endif + !#ifdef printthreads + ! tm03 = omp_get_wtime() + ! tmSF = tmSF + tm02-tm01 + ! tmSM = tmSM + tm03-tm02 + !#endif end do ! n_ss substepping - #ifdef printthreads - tm2 = omp_get_wtime() - write(*,*) ' Turbine and support structure simulations with parent thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' - write(*,*) ' Time on FAST sims: '//trim(num2lstr(tmSF))//' s. Time on Farm MoorDyn: '//trim(num2lstr(tmSM))//' seconds' - #endif + !#ifdef printthreads + ! tm2 = omp_get_wtime() + ! write(*,*) ' Turbine and support structure simulations with parent thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' + ! write(*,*) ' Time on FAST sims: '//trim(num2lstr(tmSF))//' s. Time on Farm MoorDyn: '//trim(num2lstr(tmSM))//' seconds' + !#endif ! The second section, for updating AWAE states on a separate thread in parallel with the FAST/MoorDyn time stepping !$OMP SECTION - #ifdef printthreads - tm3 = omp_get_wtime() - #endif + !#ifdef printthreads + ! tm3 = omp_get_wtime() + !#endif call AWAE_UpdateStates( t, n, farm%AWAE%u, farm%AWAE%p, farm%AWAE%x, farm%AWAE%xd, farm%AWAE%z, & farm%AWAE%OtherSt, farm%AWAE%m, ErrStatAWAE, ErrMsgAWAE ) - #ifdef printthreads - tm2 = omp_get_wtime() - write(*,*) ' AWAE_UpdateStates using thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' - #endif + !#ifdef printthreads + ! tm2 = omp_get_wtime() + ! write(*,*) ' AWAE_UpdateStates using thread #'//trim(num2lstr(omp_get_thread_num()))//' taking '//trim(num2lstr(tm2-tm3))//' seconds' + !#endif !$OMP END PARALLEL SECTIONS - #ifdef printthreads - tm2 = omp_get_wtime() - write(*,*) 'Total Farm_US took '//trim(num2lstr(tm2-tm1))//' seconds.' - #endif + !#ifdef printthreads + ! tm2 = omp_get_wtime() + ! write(*,*) 'Total Farm_US took '//trim(num2lstr(tm2-tm1))//' seconds.' + !#endif else CALL SetErrStat( ErrID_Fatal, 'MooringMod must be 0 or 3.', ErrStat, ErrMsg, RoutineName ) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 00ba7929e9..acb0f4ad57 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -35,7 +35,7 @@ MODULE MoorDyn PRIVATE - TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a17', '13 December 2021 (input format changes + friction)' ) + TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn-F', 'v2.a19', '6 January 2022 (input format changes)' ) INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output @@ -414,7 +414,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er read (OptValue,*) p%g else if ( OptString == 'RHOW') then read (OptValue,*) p%rhoW - else if ( OptString == 'WTRDPTH') then + else if (( OptString == 'WTRDPTH') .or. ( OptString == 'DEPTH') .or. ( OptString == 'WATERDEPTH')) then read (OptValue,*) DepthValue ! water depth input read in as a string to be processed by setupBathymetry else if ( OptString == 'KBOT') then read (OptValue,*) p%kBot @@ -688,7 +688,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !read into a line Line = NextLine(i) - ! parse out entries: ID Attachment X0 Y0 Z0 r0 p0 y0 M CG* I* V CdA* Ca* + ! parse out entries: ID Attachment X0 Y0 Z0 r0 p0 y0 M CG* I* V CdA* Ca* IF (ErrStat2 == 0) THEN READ(Line,*,IOSTAT=ErrStat2) m%BodyList(l)%IdNum, tempString1, & tempArray(1), tempArray(2), tempArray(3), tempArray(4), tempArray(5), tempArray(6), & @@ -769,7 +769,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if ((let1 == "COUPLED") .or. (let1 == "VESSEL") .or. (let1 == "CPLD") .or. (let1 == "VES")) then ! if a coupled body m%BodyList(l)%typeNum = -1 - p%nCpldBodies(1)=p%nCpldBodies(1)+1 ! add this rod to coupled list + p%nCpldBodies(1)=p%nCpldBodies(1)+1 ! add this body to coupled list m%CpldBodyIs(p%nCpldBodies(1),1) = l ! body initial position due to coupling will be adjusted later @@ -779,7 +779,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if (let1 == "FREE") then ! if a free body m%BodyList(l)%typeNum = 0 - p%nFreeBodies=p%nFreeBodies+1 ! add this pinned rod to the free list because it is half free + p%nFreeBodies=p%nFreeBodies+1 m%BodyStateIs1(p%nFreeBodies) = Nx+1 m%BodyStateIsN(p%nFreeBodies) = Nx+12 @@ -2617,8 +2617,7 @@ SUBROUTINE MD_CalcContStateDeriv( t, u, p, x, xd, z, other, m, dxdt, ErrStat, Er a_in(1:3) = u%CoupledKinematics(iTurb)%TranslationAcc(:,J) CALL Connect_SetKinematics(m%ConnectList(m%CpldConIs(l,iTurb)), r_in, rd_in, a_in, t, m) - !print *, u%PtFairleadDisplacement%Position(:,l) + u%PtFairleadDisplacement%TranslationDisp(:,l) - !print *, u%PtFairleadDisplacement%TranslationVel(:,l) + !print "(f8.5, f12.6, f12.6, f8.4, f8.4, f8.4, f8.4)", t, r_in(1), r_in(3), rd_in(1), rd_in(3), a_in(1), a_in(3) END DO diff --git a/modules/moordyn/src/MoorDyn_Body.f90 b/modules/moordyn/src/MoorDyn_Body.f90 index 7ab12c6347..3f1345c596 100644 --- a/modules/moordyn/src/MoorDyn_Body.f90 +++ b/modules/moordyn/src/MoorDyn_Body.f90 @@ -74,14 +74,14 @@ SUBROUTINE Body_Setup( Body, tempArray, rhoW, ErrStat, ErrMsg) ! set up body initial mass matrix (excluding any rods or attachements) DO J=1,3 - Mtemp(J,J) = Body%bodyM ! fill in mass - Mtemp(3+J,3+J) = Body%bodyI(J) ! fill in inertia + Mtemp(J,J) = Body%BodyM ! fill in mass + Mtemp(3+J,3+J) = Body%BodyI(J) ! fill in inertia END DO CALL TranslateMass6to6DOF(Body%rCG, Mtemp, Body%M0) ! account for potential CG offset <<< is the direction right? <<< - DO J=1,6 - Body%M0(J,J) = Body%M0(J,J) + Body%bodyV*Body%bodyCa(1) ! add added mass in each direction about ref point (so only diagonals) <<< eventually expand to multi D + DO J=1,3 + Body%M0(J,J) = Body%M0(J,J) + Body%BodyV*Body%BodyCa(J) ! add added mass in each direction about ref point (so only diagonals) <<< eventually expand to multi D END DO ! --------------- if this is an independent body (not coupled) ---------- diff --git a/modules/moordyn/src/MoorDyn_Driver.f90 b/modules/moordyn/src/MoorDyn_Driver.f90 index 0c403fdcee..93e217ac06 100644 --- a/modules/moordyn/src/MoorDyn_Driver.f90 +++ b/modules/moordyn/src/MoorDyn_Driver.f90 @@ -120,6 +120,7 @@ PROGRAM MoorDyn_Driver UnEcho=-1 UnIn =-1 + ! TODO: Sort out error handling (two sets of flags currently used) CALL NWTC_Init( ProgNameIn=version%Name ) @@ -140,7 +141,7 @@ PROGRAM MoorDyn_Driver CALL CPU_TIME ( ProgStrtCPU ) ! Initial time (this zeros the start time when used as a MATLAB function) - CALL WrScr( ' MD Driver updated 2021-11-15') + CALL WrScr( ' MD Driver updated 2022-01-12') ! Parse the driver input file and run the simulation based on that file CALL get_command_argument(1, drvrFilename) @@ -190,7 +191,7 @@ PROGRAM MoorDyn_Driver MD_InitInp%PtfmInit(6,J) = drvrInitInp%FarmPositions(8,J)*3.14159265/180.0 end do - MD_interp_order = 0 + MD_interp_order = 1 ! allocate Input and Output arrays; used for interpolation and extrapolation Allocate(MD_uTimes(MD_interp_order + 1)) @@ -228,10 +229,10 @@ PROGRAM MoorDyn_Driver OPEN(Unit=Un,FILE='MD.out',STATUS='UNKNOWN') ! call the initialization routine - CALL MD_Init( MD_InitInp, MD_u(1), MD_p, MD_x , MD_xd, MD_xc, MD_xo, MD_y, MD_m, dtC, MD_InitOut, ErrStat2, ErrMsg2 ); call AbortIfFailed() + CALL MD_Init( MD_InitInp, MD_u(1), MD_p, MD_x , MD_xd, MD_xc, MD_xo, MD_y, MD_m, dtC, MD_InitOut, ErrStat, ErrMsg2 ); call AbortIfFailed() - CALL MD_DestroyInitInput ( MD_InitInp , ErrStat2, ErrMsg2 ); call AbortIfFailed() - CALL MD_DestroyInitOutput ( MD_InitOut , ErrStat2, ErrMsg2 ); call AbortIfFailed() + CALL MD_DestroyInitInput ( MD_InitInp , ErrStat, ErrMsg ); call AbortIfFailed() + CALL MD_DestroyInitOutput ( MD_InitOut , ErrStat, ErrMsg ); call AbortIfFailed() CALL DispNVD( MD_InitOut%Ver ) @@ -250,8 +251,8 @@ PROGRAM MoorDyn_Driver if (drvrInitInp%InputsMod == 1 ) then if ( LEN( TRIM(drvrInitInp%InputsFile) ) < 1 ) then - ErrStat2 = ErrID_Fatal - ErrMsg2 = ' ERROR: MoorDyn Driver InputFile cannot be empty if InputsMode is 2.' + ErrStat = ErrID_Fatal + ErrMsg = ' ERROR: MoorDyn Driver InputFile cannot be empty if InputsMode is 2.' CALL AbortIfFailed() end if @@ -274,15 +275,17 @@ PROGRAM MoorDyn_Driver ! rewind to start of input file to re-read things now that we know how long it is REWIND(UnPtfmMotIn) - + + ErrStat2 = 0 ! reset the error state after it may be used to exit the loop above + ntIn = i-3 ! save number of lines of file ! allocate space for input motion array (including time column) ALLOCATE ( PtfmMotIn(ntIn, ncIn+1), STAT=ErrStat2) - IF ( ErrStat /= ErrID_None ) THEN - ErrStat2 = ErrID_Fatal - ErrMsg2 = ' Error allocating space for PtfmMotIn array.' + IF ( ErrStat2 /= ErrID_None ) THEN + ErrStat = ErrID_Fatal + ErrMsg = ' Error allocating space for PtfmMotIn array.' call AbortIfFailed() END IF @@ -294,8 +297,8 @@ PROGRAM MoorDyn_Driver READ (UnPtfmMotIn, *, IOSTAT=ErrStat2) (PtfmMotIn (i,J), J=1,ncIn+1) IF ( ErrStat2 /= 0 ) THEN - ErrStat2 = ErrID_Fatal - ErrMsg2 = ' Error reading the input time-series file. Expecting '//TRIM(Int2LStr(ncIn))//' channels plus time.' + ErrStat = ErrID_Fatal + ErrMsg = ' Error reading the input time-series file. Expecting '//TRIM(Int2LStr(ncIn))//' channels plus time.' call AbortIfFailed() END IF END DO @@ -321,7 +324,7 @@ PROGRAM MoorDyn_Driver ALLOCATE ( r_in(nt, ncIn), r_in2(nt, ncIn), rd_in(nt, ncIn), rd_in2(nt, ncIn), rdd_in(nt, ncIn), rdd_in2(nt, ncIn), STAT=ErrStat2) IF ( ErrStat2 /= ErrID_None ) THEN ErrStat2 = ErrID_Fatal - ErrMsg2 = ' Error allocating space for r_in or rd_in array.' + ErrMsg = ' Error allocating space for r_in or rd_in array.' call AbortIfFailed() END IF @@ -363,7 +366,7 @@ PROGRAM MoorDyn_Driver if (i==1) then r_in2(i, J) = r_in(i, J) else - r_in2(i, J) = 0.2*r_in(i, J) + 0.8*r_in2(i-1, J) + r_in2(i, J) = 0.1*r_in(i, J) + 0.9*r_in2(i-1, J) end if END DO END DO @@ -373,7 +376,7 @@ PROGRAM MoorDyn_Driver if (i==nt) then r_in(i, J) = r_in2(i, J) else - r_in(i, J) = 0.2*r_in2(i, J) + 0.8*r_in(i+1, J) + r_in(i, J) = 0.1*r_in2(i, J) + 0.9*r_in(i+1, J) end if END DO END DO @@ -404,7 +407,7 @@ PROGRAM MoorDyn_Driver if (i==1) then rd_in2(i, J) = rd_in(i, J) else - rd_in2(i, J) = 0.2*rd_in(i, J) + 0.8*rd_in2(i-1, J) + rd_in2(i, J) = 0.1*rd_in(i, J) + 0.9*rd_in2(i-1, J) end if END DO END DO @@ -414,7 +417,7 @@ PROGRAM MoorDyn_Driver if (i==nt) then rd_in(i, J) = rd_in2(i, J) else - rd_in(i, J) = 0.2*rd_in2(i, J) + 0.8*rd_in(i+1, J) + rd_in(i, J) = 0.1*rd_in2(i, J) + 0.9*rd_in(i+1, J) end if END DO END DO @@ -485,7 +488,7 @@ PROGRAM MoorDyn_Driver ! ! now add some current in x for testing ! MD_u(1)%U(1,:) = 1.0 - ! copy inputs to initialize input arrays for higher interp orders if applicable (it's not) + ! copy inputs to initialize input arrays for higher interp orders if applicable DO i = 2, MD_interp_order + 1 CALL MD_CopyInput( MD_u(1), MD_u(i), MESH_NEWCOPY, ErrStat2, ErrMsg2 ); call AbortIfFailed() END DO @@ -517,9 +520,11 @@ PROGRAM MoorDyn_Driver if ( MOD( i, 20 ) == 0 ) THEN CALL SimStatus( PrevSimTime, PrevClockTime, t, tMax ) end if - + + ! shift older inputs back in the buffer + CALL MD_CopyInput( MD_u(1), MD_u(2), MESH_NEWCOPY, ErrStat2, ErrMsg2 ); call AbortIfFailed() ! copy from 1 to 2 before 1 is updated with latest. MD_uTimes(1) = t + dtC - !MD_uTimes(2) = MD_uTimes(1) - dtC + MD_uTimes(2) = MD_uTimes(1) - dtC !MD_uTimes(3) = MD_uTimes(2) - dtC ! update coupled object kinematics iff we're reading input time series @@ -561,7 +566,7 @@ PROGRAM MoorDyn_Driver MD_u(1)%CoupledKinematics(iTurb)%TranslationDisp(:,K) = r_in(i, J:J+2) - MD_u(1)%CoupledKinematics(iTurb)%Position(:,K) - MD_p%TurbineRefPos(:,iTurb) MD_u(1)%CoupledKinematics(iTurb)%TranslationVel( :,K) = rd_in(i, J:J+2) - MD_u(1)%CoupledKinematics(iTurb)%TranslationAcc( :,K) = rdd_in(i, J:J+2) + MD_u(1)%CoupledKinematics(iTurb)%TranslationAcc( :,K) = 0.0_DbKi !rdd_in(i, J:J+2) !print *, u%PtFairleadDisplacement%Position(:,l) + u%PtFairleadDisplacement%TranslationDisp(:,l) !print *, u%PtFairleadDisplacement%TranslationVel(:,l) @@ -575,8 +580,8 @@ PROGRAM MoorDyn_Driver ! also provide any active tensioning commands do l = 1, size(MD_u(1)%DeltaL) - MD_u(1)%DeltaL( l) = r_in(i, J) - MD_u(1)%DeltaLdot(l) = rd_in(i, J) + MD_u(1)%DeltaL( l) = 0.0_DbKi ! r_in(i, J) + MD_u(1)%DeltaLdot(l) = 0.0_DbKi !rd_in(i, J) J = J + 1 end do diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index a61b157996..9fcf0e7cb4 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -1261,6 +1261,12 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%rd(2) ! y velocity CASE (VelZ) y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%rd(3) ! z velocity + CASE (AccX) + y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%a(1) ! x acceleration + CASE (AccY) + y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%a(2) ! y acceleration + CASE (AccZ) + y%WriteOutput(I) = m%ConnectList(p%OutParam(I)%ObjID)%a(3) ! z acceleration CASE (Ten) y%WriteOutput(I) = TwoNorm(m%ConnectList(p%OutParam(I)%ObjID)%Fnet) ! total force magnitude on a connect (used eg. for fairlead and anchor tensions) CASE (FX) @@ -1289,7 +1295,13 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) CASE (VelY) y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%rd(2,p%OutParam(I)%NodeID) ! y velocity CASE (VelZ) - y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%rd(3,p%OutParam(I)%NodeID) ! z velocity + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%rd(3,p%OutParam(I)%NodeID) ! z velocity + CASE (AccX) + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%a6(1) ! x acceleration <<< should this become distributed for each node? + CASE (AccY) + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%a6(2) ! y acceleration + CASE (AccZ) + y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%a6(3) ! z acceleration CASE (FX) y%WriteOutput(I) = m%RodList(p%OutParam(I)%ObjID)%F6net(1) ! total force in x - added Nov 24 CASE (FY) diff --git a/modules/moordyn/src/MoorDyn_Line.f90 b/modules/moordyn/src/MoorDyn_Line.f90 index 6234e0ed04..e32ba2ac00 100644 --- a/modules/moordyn/src/MoorDyn_Line.f90 +++ b/modules/moordyn/src/MoorDyn_Line.f90 @@ -1028,6 +1028,7 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, Real(DbKi) :: nvec(3) ! local seabed surface normal vector (positive out) Real(DbKi) :: Fn(3) ! seabed contact normal force vector Real(DbKi) :: Vn(3) ! normal velocity of a line node relative to the seabed slope [m/s] + Real(DbKi) :: Vsb(3) ! tangent velocity of a line node relative to the seabed slope [m/s] Real(DbKi) :: Va(3) ! velocity of a line node in the axial or "in-line" direction [m/s] Real(DbKi) :: Vt(3) ! velocity of a line node in the transverse direction [m/s] Real(DbKi) :: VtMag ! magnitude of the transverse velocity of a line node [m/s] @@ -1085,13 +1086,10 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, CALL UnitVector(Line%r(:,N-1), Line%r(:,N), Line%q(:,N), dummyLength) ! compute unit vector q - ! --------------------------------- apply wave kinematics ------------------------------------ - - ! IF (p%WaterKin > 0) THEN ! wave kinematics interpolated from global grid in Waves object - ! DO i=0,N - ! CALL getWaveKin(p, Line%r(1,i), Line%r(2,i), Line%r(3,i), Line%time, m%WaveTi, Line%U(:,i), Line%Ud(:,i), Line%zeta(i), Line%PDyn(i)) - ! END DO - ! END IF + ! apply wave kinematics (if there are any) + DO i=0,N + CALL getWaterKin(p, Line%r(1,i), Line%r(2,i), Line%r(3,i), Line%time, m%WaveTi, Line%U(:,i), Line%Ud(:,i), Line%zeta(i), Line%PDyn(i)) + END DO ! --------------- calculate mass (including added mass) matrix for each node ----------------- @@ -1300,9 +1298,9 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, Line%W(3,I) = pi/8.0*d*d* (Line%l(I)*(rho - p%rhoW) + Line%l(I+1)*(rho - p%rhoW) )*(-p%g) ! left in this form for future free surface handling END IF - !relative flow velocities + ! relative flow velocities DO J = 1, 3 - Vi(J) = 0.0 - Line%rd(J,I) ! relative flow velocity over node -- this is where wave velicites would be added + Vi(J) = Line%U(J,I) - Line%rd(J,I) ! relative flow velocity over node -- this is where wave velicites would be added END DO ! decomponse relative flow into components @@ -1338,10 +1336,11 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, Line%r(1,I), Line%r(2,I), depth, nvec) IF (Line%r(3,I) < -depth) THEN ! for every line node at or below the seabed - ! calculate the velocity of the node in the normal direction of the seabed slope - DO J = 1, 3 - Vn(J) = DOT_PRODUCT( Line%rd(:,I), nvec) * nvec(J) - END DO + + ! calculate the velocity components of the node relative to the seabed + Vn = DOT_PRODUCT( Line%rd(:,I), nvec) * nvec ! velocity component normal to the local seabed slope + Vsb = Line%rd(:,I) - Vn ! velocity component along (tangent to) the seabed + ! calculate the normal contact force on the line node IF (I==0) THEN Fn = ( (-depth - Line%r(3,I))*nvec(3)*nvec*p%kBot - Vn*p%cBot) * 0.5*d*( Line%l(I+1) ) @@ -1351,11 +1350,10 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, Fn = ( (-depth - Line%r(3,I))*nvec(3)*nvec*p%kBot - Vn*p%cBot) * 0.5*d*(Line%l(I) + Line%l(I+1) ) END IF - ! calculate the axial and transverse components of the total node velocity vector (q can now have a z-component from seabed slope) - DO J = 1, 3 - Va(J) = DOT_PRODUCT( Line%rd(:,I) , Line%q(:,I) ) * Line%q(J,I) - Vt(J) = Line%rd(J,I) - Va(J) - END DO + ! calculate the axial and transverse components of the node velocity vector along the seabed + Va = DOT_PRODUCT( Vsb , Line%q(:,I) ) * Line%q(:,I) + Vt = Vsb - Va + ! calculate the magnitudes of each velocity VaMag = SQRT(Va(1)**2+Va(2)**2+Va(3)**2) VtMag = SQRT(Vt(1)**2+Vt(2)**2+Vt(3)**2) @@ -1396,12 +1394,13 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, ! the total friction force is along the plane of the seabed slope, which is just the vector sum of the transverse and axial components Ff = FfT + FfA - + ELSE Fn = 0.0_DbKi Ff = 0.0_DbKi END IF - + + ! the total force from bottom contact on the line node is the sum of the normal contact force and the friction force Line%B(:,I) = Fn + Ff @@ -1421,7 +1420,7 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, DO J=1,3 ! calculate RHS constant (premultiplying force vector by inverse of mass matrix ... i.e. rhs = S*Forces) - Sum1 = 0.0_DbKi ! reset temporary accumulator + Sum1 = 0.0_DbKi ! reset temporary accumulator <<< could turn this into a Line%a array to save and output node accelerations DO K = 1, 3 Sum1 = Sum1 + Line%S(K,J,I) * Line%Fnet(K,I) ! matrix-vector multiplication [S i]{Forces i} << double check indices END DO ! K @@ -1429,6 +1428,10 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, ! update states Xd(3*N-3 + 3*I-3 + J) = Line%rd(J,I); ! dxdt = V (velocities) Xd( 3*I-3 + J) = Sum1 ! dVdt = RHS * A (accelerations) + + !if ((Line%Time > 100) .and. (Line%IdNum == 1) .and. (I==19) .and. (J==3)) then + ! print *, Line%Time, Line%r(J,I), Line%rd(J,I), Sum1 + !end if END DO ! J END DO ! I diff --git a/modules/moordyn/src/MoorDyn_Misc.f90 b/modules/moordyn/src/MoorDyn_Misc.f90 index 57ed3c2f08..7c3df24a21 100644 --- a/modules/moordyn/src/MoorDyn_Misc.f90 +++ b/modules/moordyn/src/MoorDyn_Misc.f90 @@ -796,6 +796,27 @@ SUBROUTINE calculate2Dinterpolation(f, ix0, iy0, fx, fy, c) END SUBROUTINE calculate2Dinterpolation + SUBROUTINE calculate1Dinterpolation(f, ix0, fx, c) + REAL(DbKi), INTENT (IN ) :: f(:) ! data array + INTEGER(IntKi), INTENT (IN ) :: ix0 ! indices for interpolation + REAL(DbKi), INTENT (IN ) :: fx ! interpolation fractions + REAL(DbKi), INTENT ( OUT) :: c ! the output value + + INTEGER(IntKi) :: ix1 ! second index + REAL(DbKi) :: c0, c1 + + ! handle end case conditions + IF (fx == 0) THEN + ix1 = ix0 + ELSE + ix1 = min(ix0+1,size(f,1)) ! don't overstep bounds + END IF + + c0 = f(ix0) + c1 = f(ix1) + c = c0*(1.0-fx) + c1*fx + END SUBROUTINE calculate1Dinterpolation + @@ -893,7 +914,8 @@ SUBROUTINE getWaterKin(p, x, y, z, t, tindex, U, Ud, zeta, PDyn) Real(DbKi), INTENT (INOUT) :: PDyn - INTEGER(IntKi) :: ix, iy, iz, it ! indeces for interpolation + INTEGER(IntKi) :: ix, iy, iz, it ! indices for interpolation + INTEGER(IntKi) :: iz0, iz1 ! special indices for currrent interpolation INTEGER(IntKi) :: N ! number of rod elements for convenience Real(ReKi) :: fx, fy, fz, ft ! interpolation fractions !Real(DbKi) :: qt ! used in time step interpolation @@ -906,8 +928,8 @@ SUBROUTINE getWaterKin(p, x, y, z, t, tindex, U, Ud, zeta, PDyn) CALL getInterpNumsSiKi(p%pyWave , REAL(y), 1, iy, fy) CALL getInterpNumsSiKi(p%pzWave , REAL(z), 1, iz, fz) !CALL getInterpNums(p%tWave, t, tindex, it, ft) - it = floor(t/ p%dtWave) - ft = (t - it*p%dtWave)/p%dtWave + it = floor(t/ p%dtWave) + 1 ! add 1 because Fortran indexing starts at 1 + ft = (t - (it-1)*p%dtWave)/p%dtWave tindex = it CALL calculate3Dinterpolation(p%zeta, ix, iy, it, fx, fy, ft, zeta) @@ -923,16 +945,24 @@ SUBROUTINE getWaterKin(p, x, y, z, t, tindex, U, Ud, zeta, PDyn) else U = 0.0_DbKi Ud = 0.0_DbKi + zeta = 0.0_DbKi + PDyn = 0.0_DbKi end if ! if current kinematics enabled, add interpolated current values from profile if (p%Current > 0) then - CALL getInterpNumsSiKi(p%pzCurrent, REAL(z), 1, iz, fz) + CALL getInterpNumsSiKi(p%pzCurrent, REAL(z), 1, iz0, fz) + + IF (fz == 0) THEN ! handle end case conditions + iz1 = iz0 + ELSE + iz1 = min(iz0+1,size(p%pzCurrent)) ! don't overstep bounds + END IF - U(1) = U(1) + p%uxCurrent(iz) + fz*(p%uxCurrent(iz+1)-p%uxCurrent(iz))/(p%pzCurrent(iz+1)-p%pzCurrent(iz)) - U(2) = U(2) + p%uyCurrent(iz) + fz*(p%uyCurrent(iz+1)-p%uyCurrent(iz))/(p%pzCurrent(iz+1)-p%pzCurrent(iz)) + U(1) = U(1) + (1.0-fz)*p%uxCurrent(iz0) + fz*p%uxCurrent(iz1) + U(2) = U(2) + (1.0-fz)*p%uyCurrent(iz0) + fz*p%uyCurrent(iz1) end if END SUBROUTINE getWaterKin @@ -1033,7 +1063,7 @@ SUBROUTINE CopyWaterKinFromHydroDyn(p, InitInp) p%axWave (:,I,J,K) = InitInp%WaveAcc( :,Itemp,1) p%ayWave (:,I,J,K) = InitInp%WaveAcc( :,Itemp,2) p%azWave (:,I,J,K) = InitInp%WaveAcc( :,Itemp,3) - p%PDyn(:,I,J,K) = InitInp%WavePDyn(:,Itemp) + p%PDyn( :,I,J,K) = InitInp%WavePDyn(:,Itemp) END DO END DO END DO @@ -1229,6 +1259,7 @@ SUBROUTINE WriteWaveData(p, ErrStat, ErrMsg) END SUBROUTINE WriteWaveData + ! ----- process WaterKin input value, potentially reading wave inputs and generating wave field ----- SUBROUTINE SetupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) CHARACTER(40), INTENT(IN ) :: WaterKinString ! string describing water kinematics filename @@ -1241,7 +1272,7 @@ SUBROUTINE SetupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) INTEGER(IntKi) :: ntIn ! number of time series inputs from file INTEGER(IntKi) :: UnIn ! unit number for coefficient input file INTEGER(IntKi) :: UnEcho - REAL(SiKi) :: pzCurrentTemp(100) + REAL(SiKi) :: pzCurrentTemp(100) ! current depth increments read in from input file (positive-down at this stage) REAL(SiKi) :: uxCurrentTemp(100) REAL(SiKi) :: uyCurrentTemp(100) @@ -1328,15 +1359,15 @@ SUBROUTINE SetupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) CALL ReadVar( UnIn, FileName, WaveDir , 'WaveDir' , 'wave direction', ErrStat2, ErrMsg2, UnEcho); if(Failed()) return ! X grid points READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type - READ(UnIn,*, IOSTAT=ErrStat2) entries2 ! get entries as string to be processed + READ(UnIn,'(A)', IOSTAT=ErrStat2) entries2 ! get entries as string to be processed CALL gridAxisCoords(coordtype, entries2, p%pxWave, p%nxWave, ErrStat2, ErrMsg2) ! Y grid points READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type - READ(UnIn,*, IOSTAT=ErrStat2) entries2 ! get entries as string to be processed + READ(UnIn,'(A)', IOSTAT=ErrStat2) entries2 ! get entries as string to be processed CALL gridAxisCoords(coordtype, entries2, p%pyWave, p%nyWave, ErrStat2, ErrMsg2) ! Z grid points READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type - READ(UnIn,*, IOSTAT=ErrStat2) entries2 ! get entries as string to be processed + READ(UnIn,'(A)', IOSTAT=ErrStat2) entries2 ! get entries as string to be processed CALL gridAxisCoords(coordtype, entries2, p%pzWave, p%nzWave, ErrStat2, ErrMsg2) ! ----- current ----- CALL ReadCom( UnIn, FileName, 'current header', ErrStat2, ErrMsg2, UnEcho); if(Failed()) return @@ -1448,8 +1479,10 @@ SUBROUTINE SetupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) ! allocate space for processed reference wave elevation time series - CALL AllocAry(WaveElev0, p%ntWave, 'WaveElev0', ErrStat2, ErrMsg2 ); if(Failed()) return - + ALLOCATE ( WaveElev0( 0:p%ntWave ), STAT=ErrStatTmp ) ! this has an extra entry of zero in case it needs to be padded to be even + IF (ErrStatTmp /= 0) CALL SetErrStat(ErrID_Fatal,'Cannot allocate array WaveElev0.',ErrStat,ErrMsg,RoutineName) + WaveElev0 = 0.0_SiKi + ! go through and interpolate (should replace with standard function) DO i = 1, p%ntWave t = p%dtWave*(i-1) @@ -1458,7 +1491,7 @@ SUBROUTINE SetupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) DO iIn = 1,ntIn-1 IF (WaveTimeIn(iIn+1) > t) THEN ! find the right two points to interpolate between (remember that the first column of PtfmMotIn is time) frac = (t - WaveTimeIn(iIn) )/( WaveTimeIn(iIn+1) - WaveTimeIn(iIn) ) ! interpolation fraction (0-1) between two interpolation points - WaveElev0(i) = WaveElevIn(iIn) + frac*(WaveElevIn(iIn+1) - WaveElevIn(iIn)) ! get interpolated wave elevation + WaveElev0(i-1) = WaveElevIn(iIn) + frac*(WaveElevIn(iIn+1) - WaveElevIn(iIn)) ! get interpolated wave elevation EXIT ! break out of the loop for this time step once we've done its interpolation END IF END DO @@ -1502,7 +1535,7 @@ SUBROUTINE SetupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) ! Copy values over - DO I=0,MIN(p%ntWave, NStepWave-1) + DO I=0, MIN(SIZE(WaveElev0), NStepWave)-1 TmpFFTWaveElev(I) = WaveElev0(I) ENDDO @@ -1568,22 +1601,23 @@ SUBROUTINE SetupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) do iz = 1,p%nzWave ! Compute the discrete Fourier transform of the incident wave kinematics - do I = 0, NStepWave2 ! Loop through the positive frequency components (including zero) of the discrete Fourier transforms + do i = 0, NStepWave2 ! Loop through the positive frequency components (including zero) of the discrete Fourier transforms - Omega = I*WaveDOmega + Omega = i*WaveDOmega ImagOmega = ImagNmbr*Omega - WaveElevC (i) = EXP( -ImagNmbr*WaveNmbr(i)*( p%pxWave(ix)*CosWaveDir + p%pyWave(iy)*SinWaveDir )) - WaveDynPC (i) = p%rhoW*p%g* tmpComplex(i)* WaveElevC(i) * COSHNumOvrCOSHDen( WaveNmbr(i), p%WtrDpth, DBLE(p%pzWave(iz)) ) - WaveVelCHx(i) = CosWaveDir*Omega*tmpComplex(i)* WaveElevC(i) * COSHNumOvrSINHDen( WaveNmbr(i), p%WtrDpth, DBLE(p%pzWave(iz)) ) - WaveVelCHy(i) = SinWaveDir*Omega*tmpComplex(i)* WaveElevC(i) * COSHNumOvrSINHDen( WaveNmbr(i), p%WtrDpth, DBLE(p%pzWave(iz)) ) - WaveVelCV (i) = ImagOmega*tmpComplex(i)* WaveElevC(i) * SINHNumOvrSINHDen( WaveNmbr(i), p%WtrDpth, DBLE(p%pzWave(iz)) ) - WaveAccCHx(i) = ImagOmega*WaveVelCHx(I) - WaveAccCHy(i) = ImagOmega*WaveVelCHy(I) - WaveAccCV (i) = ImagOmega*WaveVelCV (I) + WaveElevC (i) = tmpComplex(i) * EXP( -ImagNmbr*WaveNmbr(i)*( p%pxWave(ix)*CosWaveDir + p%pyWave(iy)*SinWaveDir )) + WaveDynPC (i) = p%rhoW*p%g* WaveElevC(i) * COSHNumOvrCOSHDen( WaveNmbr(i), p%WtrDpth, DBLE(p%pzWave(iz)) ) + WaveVelCHx(i) = Omega*WaveElevC(i) * COSHNumOvrSINHDen( WaveNmbr(i), p%WtrDpth, DBLE(p%pzWave(iz)) ) *CosWaveDir + WaveVelCHy(i) = Omega*WaveElevC(i) * COSHNumOvrSINHDen( WaveNmbr(i), p%WtrDpth, DBLE(p%pzWave(iz)) ) *SinWaveDir + WaveVelCV (i) = ImagOmega*WaveElevC(i) * SINHNumOvrSINHDen( WaveNmbr(i), p%WtrDpth, DBLE(p%pzWave(iz)) ) + WaveAccCHx(i) = ImagOmega*WaveVelCHx(i) + WaveAccCHy(i) = ImagOmega*WaveVelCHy(i) + WaveAccCV (i) = ImagOmega*WaveVelCV (i) end do ! I, frequencies ! now IFFT all the wave kinematics except surface elevation and save it into the grid of data + print *, 'a' CALL ApplyFFT_cx( p%PDyn (:,iz,iy,ix), WaveDynPC , FFT_Data, ErrStatTmp ); CALL SetErrStat(ErrStatTmp,'Error IFFTing WaveDynP.', ErrStat,ErrMsg,RoutineName) CALL ApplyFFT_cx( p%uxWave(:,iz,iy,ix), WaveVelCHx, FFT_Data, ErrStatTmp ); CALL SetErrStat(ErrStatTmp,'Error IFFTing WaveVelHx.',ErrStat,ErrMsg,RoutineName) CALL ApplyFFT_cx( p%uyWave(:,iz,iy,ix), WaveVelCHy, FFT_Data, ErrStatTmp ); CALL SetErrStat(ErrStatTmp,'Error IFFTing WaveVelHy.',ErrStat,ErrMsg,RoutineName) @@ -1595,8 +1629,9 @@ SUBROUTINE SetupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) end do ! iz ! IFFT wave elevation here because it's only at the surface + print *, 'b' CALL ApplyFFT_cx( p%zeta(:,iy,ix) , WaveElevC , FFT_Data, ErrStatTmp ); CALL SetErrStat(ErrStatTmp,'Error IFFTing WaveElev.', ErrStat,ErrMsg,RoutineName) - + print *, 'c' end do ! iy end do ! ix @@ -1616,11 +1651,11 @@ SUBROUTINE SetupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) CALL AllocAry( p%uxCurrent, p%nzCurrent, 'uxCurrent', ErrStat2, ErrMsg2 ); if(Failed()) return CALL AllocAry( p%uyCurrent, p%nzCurrent, 'uyCurrent', ErrStat2, ErrMsg2 ); if(Failed()) return - ! copy over data + ! copy over data, flipping sign of depth values (to be positive-up) and reversing order do i = 1,p%nzCurrent - p%pzCurrent(i) = pzCurrentTemp(i) - p%uxCurrent(i) = uxCurrentTemp(i) - p%uyCurrent(i) = uyCurrentTemp(i) + p%pzCurrent(i) = -pzCurrentTemp(p%nzCurrent + 1 - i) ! flip sign so depth is positive-up + p%uxCurrent(i) = uxCurrentTemp(p%nzCurrent + 1 - i) + p%uyCurrent(i) = uyCurrentTemp(p%nzCurrent + 1 - i) end do end if ! p%Current == 1 @@ -1943,7 +1978,7 @@ FUNCTION COSHNumOvrSINHDen ( k, h, z ) IF ( k < EPSILON(0.0_DbKi) ) THEN ! When .TRUE., the shallow water formulation is ill-conditioned; thus, HUGE(k) is returned to approximate the known value of infinity. - COSHNumOvrSINHDen = HUGE( k ) + COSHNumOvrSINHDen = 1.0E20 ! HUGE( k ) ELSEIF ( k*h > 89.4_DbKi ) THEN ! When .TRUE., the shallow water formulation will trigger a floating point overflow error; however, COSH( k*( z + h ) )/SINH( k*h ) = EXP( k*z ) + EXP( -k*( z + 2*h ) ) for large k*h. This equals the deep water formulation, EXP( k*z ), except near z = -h, because h > 14.23*wavelength (since k = 2*Pi/wavelength) in this case. diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index b1b7d0f15e..a86eab0c44 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -194,6 +194,8 @@ typedef ^ ^ DbKi Aq {:}{:} typedef ^ ^ DbKi B {:}{:} - - "node bottom contact force" "[N]" typedef ^ ^ DbKi Fnet {:}{:} - - "total force on node" "[N]" typedef ^ ^ DbKi M {:}{:}{:} - - "node mass matrix" "[kg]" +typedef ^ ^ DbKi FextA {3} - - "external forces from attached lines on/about end A " - +typedef ^ ^ DbKi FextB {3} - - "external forces from attached lines on/about end A " - typedef ^ ^ DbKi Mext {3} - - "external moment vector holding sum of any externally applied moments i.e. bending lines" - typedef ^ ^ DbKi r6 {6} - - "6 DOF position vector" - typedef ^ ^ DbKi v6 {6} - - "6 DOF velocity vector" - diff --git a/modules/moordyn/src/MoorDyn_Rod.f90 b/modules/moordyn/src/MoorDyn_Rod.f90 index ba3ebe31e5..1e4acff875 100644 --- a/modules/moordyn/src/MoorDyn_Rod.f90 +++ b/modules/moordyn/src/MoorDyn_Rod.f90 @@ -587,31 +587,18 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) DO i=1,N-1 Rod%r( :,i) = Rod%r( :,0) + (Rod%r( :,N) - Rod%r( :,0)) * (REAL(i)/REAL(N)) Rod%rd(:,i) = Rod%rd(:,0) + (Rod%rd(:,N) - Rod%rd(:,0)) * (REAL(i)/REAL(N)) - Rod%V(i) = 0.25*pi * Rod%d*Rod%d * Rod%l(i) ! volume attributed to segment END DO - ! --------------------------------- apply wave kinematics ------------------------------------ - - ! IF (p%WaterKin == 1) THEN ! wave kinematics interpolated from global grid in Waves object - ! DO i=0,N - ! CALL getWaveKin(p, Rod%r(1,i), Rod%r(2,i), Rod%r(3,i), Rod%time, m%WaveTi, Rod%U(:,i), Rod%Ud(:,i), Rod%zeta(i), Rod%PDyn(i)) - ! !F(i) = 1.0 ! set VOF value to one for now (everything submerged - eventually this should be element-based!!!) <<<< - ! ! <<<< currently F is not being used and instead a VOF variable is used within the node loop - ! END DO - ! END IF + ! apply wave kinematics (if there are any) - - ! ! wave kinematics not implemented yet <<< - ! ap = 0.0_DbKi - ! aq = 0.0_DbKi - ! ! set U and Ud herem as well as pDyn and zeta... - ! Rod%U = 0.0_DbKi - ! Rod%Ud = 0.0_DbKi - ! pDyn = 0.0_DbKi - ! zeta = 0.0_DbKi + DO i=0,N + CALL getWaterKin(p, Rod%r(1,i), Rod%r(2,i), Rod%r(3,i), Rod%time, m%WaveTi, Rod%U(:,i), Rod%Ud(:,i), Rod%zeta(i), Rod%PDyn(i)) + !F(i) = 1.0 ! set VOF value to one for now (everything submerged - eventually this should be element-based!!!) <<<< + ! <<<< currently F is not being used and instead a VOF variable is used within the node loop + END DO ! >>> remember to check for violated conditions, if there are any... <<< @@ -713,34 +700,22 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ! fluid acceleration components for current node aq = DOT_PRODUCT(Rod%Ud(:,I), Rod%q) * Rod%q ! tangential component of fluid acceleration ap = Rod%Ud(:,I) - aq ! normal component of fluid acceleration - ! transverse Froude-Krylov force + ! transverse and axial Froude-Krylov force Rod%Ap(:,I) = VOF * p%rhoW*(1.0+Rod%Can)* v_i * ap ! - ! axial Froude-Krylov force Rod%Aq(:,I) = 0.0_DbKi ! p%rhoW*(1.0+Rod%Cat)* v_i * aq ! <<< just put a taper-based term here eventually? ! dynamic pressure Rod%Pd(:,I) = 0.0_DbKi ! assuming zero for sides for now, until taper comes into play - ! bottom contact (stiffness and damping, vertical-only for now) - updated Nov 24 for general case where anchor and fairlead ends may deal with bottom contact forces + ! seabed contact (stiffness and damping, vertical-only for now) ! interpolate the local depth from the bathymetry grid CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, Rod%r(1,I), Rod%r(2,I), depth, nvec) IF (Rod%r(3,I) < -depth) THEN - IF (I==0) THEN - Rod%B(3,I) = ( (-depth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*( Rod%l(I+1) ) - ELSE IF (I==N) THEN - Rod%B(3,I) = ( (-depth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) ) - ELSE - Rod%B(3,I) = ( (-depth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) + Rod%l(I+1) ) - END IF - ! IF (I==0) THEN - ! Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*( Rod%l(I+1) ) - ! ELSE IF (I==N) THEN - ! Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) ) - ! ELSE - ! Rod%B(3,I) = ( (-p%WtrDpth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * 0.5*Rod%d*(Rod%l(I) + Rod%l(I+1) ) - ! END IF + Rod%B(3,I) = ( (-depth - Rod%r(3,I))*p%kBot - Rod%rd(3,I)*p%cBot) * Rod%d*dL ELSE + Rod%B(1,I) = 0.0_DbKi + Rod%B(2,I) = 0.0_DbKi Rod%B(3,I) = 0.0_DbKi END IF @@ -752,9 +727,11 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) Rod%W = 0.0_DbKi Rod%Bo = 0.0_DbKi Rod%Dp = 0.0_DbKi - Rod%Dq= 0.0_DbKi - Rod%B = 0.0_DbKi + Rod%Dq = 0.0_DbKi + Rod%Ap = 0.0_DbKi + Rod%Aq = 0.0_DbKi Rod%Pd = 0.0_DbKi + Rod%B = 0.0_DbKi END IF @@ -777,7 +754,6 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ! axial drag Rod%Dq(:,I) = Rod%Dq(:,I) + VOF * 0.25* Pi*Rod%d*Rod%d * p%rhoW*Rod%CdEnd * MagVq * Vq - ! >>> what about rotational drag?? <<< eqn will be Pi* Rod%d**4/16.0 omega_rel?^2... *0.5 * Cd... ! Froud-Krylov force @@ -789,11 +765,7 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ! added mass DO J=1,3 DO K=1,3 - IF (J==K) THEN - Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) - ELSE - Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) - END IF + Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) END DO END DO @@ -821,11 +793,7 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ! added mass DO J=1,3 DO K=1,3 - IF (J==K) THEN - Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) - ELSE - Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) - END IF + Rod%M(K,J,I) = Rod%M(K,J,I) + VOF*p%rhoW* Rod%CaEnd* (2.0/3.0*Pi*Rod%d**3 /8.0) *Rod%q(J)*Rod%q(K) END DO END DO @@ -850,25 +818,31 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ! ---------------- now add in forces on end nodes from attached lines ------------------ + ! zero the external force/moment sums (important!) + ! loop through lines attached to end A + Rod%FextA = 0.0_DbKi DO l=1,Rod%nAttachedA CALL Line_GetEndStuff(m%LineList(Rod%attachedA(l)), Fnet_i, Mnet_i, Mass_i, Rod%TopA(l)) ! sum quantitites Rod%Fnet(:,0)= Rod%Fnet(:,0) + Fnet_i ! total force + Rod%FextA = Rod%FextA + Fnet_i ! a copy for outputting totalled line loads Rod%Mext = Rod%Mext + Mnet_i ! externally applied moment Rod%M(:,:,0) = Rod%M(:,:,0) + Mass_i ! mass at end node END DO ! loop through lines attached to end B + Rod%FextB = 0.0_DbKi DO l=1,Rod%nAttachedB CALL Line_GetEndStuff(m%LineList(Rod%attachedB(l)), Fnet_i, Mnet_i, Mass_i, Rod%TopB(l)) ! sum quantitites Rod%Fnet(:,N)= Rod%Fnet(:,N) + Fnet_i ! total force + Rod%FextB = Rod%FextB + Fnet_i ! a copy for outputting totalled line loads Rod%Mext = Rod%Mext + Mnet_i ! externally applied moment Rod%M(:,:,N) = Rod%M(:,:,N) + Mass_i ! mass at end node diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index bc0d7f17c2..acdd15d2b0 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -214,6 +214,8 @@ MODULE MoorDyn_Types REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: B !< node bottom contact force [[N]] REAL(DbKi) , DIMENSION(:,:), ALLOCATABLE :: Fnet !< total force on node [[N]] REAL(DbKi) , DIMENSION(:,:,:), ALLOCATABLE :: M !< node mass matrix [[kg]] + REAL(DbKi) , DIMENSION(1:3) :: FextA !< external forces from attached lines on/about end A [-] + REAL(DbKi) , DIMENSION(1:3) :: FextB !< external forces from attached lines on/about end A [-] REAL(DbKi) , DIMENSION(1:3) :: Mext !< external moment vector holding sum of any externally applied moments i.e. bending lines [-] REAL(DbKi) , DIMENSION(1:6) :: r6 !< 6 DOF position vector [-] REAL(DbKi) , DIMENSION(1:6) :: v6 !< 6 DOF velocity vector [-] @@ -3009,6 +3011,8 @@ SUBROUTINE MD_CopyRod( SrcRodData, DstRodData, CtrlCode, ErrStat, ErrMsg ) END IF DstRodData%M = SrcRodData%M ENDIF + DstRodData%FextA = SrcRodData%FextA + DstRodData%FextB = SrcRodData%FextB DstRodData%Mext = SrcRodData%Mext DstRodData%r6 = SrcRodData%r6 DstRodData%v6 = SrcRodData%v6 @@ -3253,6 +3257,8 @@ SUBROUTINE MD_PackRod( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Size Int_BufSz = Int_BufSz + 2*3 ! M upper/lower bounds for each dimension Db_BufSz = Db_BufSz + SIZE(InData%M) ! M END IF + Db_BufSz = Db_BufSz + SIZE(InData%FextA) ! FextA + Db_BufSz = Db_BufSz + SIZE(InData%FextB) ! FextB Db_BufSz = Db_BufSz + SIZE(InData%Mext) ! Mext Db_BufSz = Db_BufSz + SIZE(InData%r6) ! r6 Db_BufSz = Db_BufSz + SIZE(InData%v6) ! v6 @@ -3710,6 +3716,14 @@ SUBROUTINE MD_PackRod( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Size END DO END DO END IF + DO i1 = LBOUND(InData%FextA,1), UBOUND(InData%FextA,1) + DbKiBuf(Db_Xferred) = InData%FextA(i1) + Db_Xferred = Db_Xferred + 1 + END DO + DO i1 = LBOUND(InData%FextB,1), UBOUND(InData%FextB,1) + DbKiBuf(Db_Xferred) = InData%FextB(i1) + Db_Xferred = Db_Xferred + 1 + END DO DO i1 = LBOUND(InData%Mext,1), UBOUND(InData%Mext,1) DbKiBuf(Db_Xferred) = InData%Mext(i1) Db_Xferred = Db_Xferred + 1 @@ -4273,6 +4287,18 @@ SUBROUTINE MD_UnPackRod( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) END DO END DO END IF + i1_l = LBOUND(OutData%FextA,1) + i1_u = UBOUND(OutData%FextA,1) + DO i1 = LBOUND(OutData%FextA,1), UBOUND(OutData%FextA,1) + OutData%FextA(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO + i1_l = LBOUND(OutData%FextB,1) + i1_u = UBOUND(OutData%FextB,1) + DO i1 = LBOUND(OutData%FextB,1), UBOUND(OutData%FextB,1) + OutData%FextB(i1) = DbKiBuf(Db_Xferred) + Db_Xferred = Db_Xferred + 1 + END DO i1_l = LBOUND(OutData%Mext,1) i1_u = UBOUND(OutData%Mext,1) DO i1 = LBOUND(OutData%Mext,1), UBOUND(OutData%Mext,1) From c1024dfbe33f6a317db4b629e14a077f89f1bffb Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Fri, 11 Feb 2022 17:51:29 -0700 Subject: [PATCH 088/242] DBEMT: fixing equations for continuous DBEMT --- modules/aerodyn/src/AeroDyn.f90 | 25 ++-- modules/aerodyn/src/BEMT.f90 | 30 ---- modules/aerodyn/src/BEMT_Registry.txt | 2 - modules/aerodyn/src/BEMT_Types.f90 | 166 ---------------------- modules/aerodyn/src/DBEMT.f90 | 95 +++++++------ modules/aerodyn/src/DBEMT_Registry.txt | 4 +- modules/aerodyn/src/DBEMT_Types.f90 | 54 +------ modules/openfast-library/src/FAST_Lin.f90 | 4 +- 8 files changed, 62 insertions(+), 318 deletions(-) diff --git a/modules/aerodyn/src/AeroDyn.f90 b/modules/aerodyn/src/AeroDyn.f90 index 7e7e7c4493..b01907d9b3 100644 --- a/modules/aerodyn/src/AeroDyn.f90 +++ b/modules/aerodyn/src/AeroDyn.f90 @@ -1764,13 +1764,6 @@ subroutine SetInputsForBEMT(p, u, m, indx, errStat, errMsg) m%BEMT_u(indx)%Vx(j,k) = dot_product( tmp, x_hat ) ! normal component (normal to the plane, not chord) of the inflow velocity of the jth node in the kth blade m%BEMT_u(indx)%Vy(j,k) = dot_product( tmp, y_hat ) ! tangential component (tangential to the plane, not chord) of the inflow velocity of the jth node in the kth blade - - !jmj says omega_z and PitchRate are the same things - ! inputs for DBEMT (DBEMT_Mod == DBEMT_cont_tauConst) - if (allocated(m%BEMT_u(indx)%Vx_elast_dot)) then - m%BEMT_u(indx)%Vx_elast_dot(j,k) = dot_product( u%BladeMotion(k)%TranslationAcc(:,j), x_hat ) ! normal component (normal to the plane, not chord) of the inflow velocity of the jth node in the kth blade - m%BEMT_u(indx)%Vy_elast_dot(j,k) = dot_product( u%BladeMotion(k)%TranslationAcc(:,j), y_hat ) ! tangential component (tangential to the plane, not chord) of the inflow velocity of the jth node in the kth blade - end if ! inputs for CUA (and CDBEMT): m%BEMT_u(indx)%omega_z(j,k) = dot_product( u%BladeMotion(k)%RotationVel( :,j), m%WithoutSweepPitchTwist(3,:,j,k) ) ! rotation of no-sweep-pitch coordinate system around z of the jth node in the kth blade @@ -4806,8 +4799,8 @@ SUBROUTINE RotGetOP( t, u, p, p_AD, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, do j=1,p%NumBlades ! size(x%BEMT%DBEMT%element,2) do i=1,p%NumBlNds ! size(x%BEMT%DBEMT%element,1) - do k=1,size(x%BEMT%DBEMT%element(i,j)%vind_dot) - x_op(index) = x%BEMT%DBEMT%element(i,j)%vind_dot(k) + do k=1,size(x%BEMT%DBEMT%element(i,j)%vind_1) + x_op(index) = x%BEMT%DBEMT%element(i,j)%vind_1(k) index = index + 1 end do end do @@ -4859,8 +4852,8 @@ SUBROUTINE RotGetOP( t, u, p, p_AD, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, do j=1,p%NumBlades ! size(dxdt%BEMT%DBEMT%element,2) do i=1,p%NumBlNds ! size(dxdt%BEMT%DBEMT%element,1) - do k=1,size(dxdt%BEMT%DBEMT%element(i,j)%vind_dot) - dx_op(index) = dxdt%BEMT%DBEMT%element(i,j)%vind_dot(k) + do k=1,size(dxdt%BEMT%DBEMT%element(i,j)%vind_1) + dx_op(index) = dxdt%BEMT%DBEMT%element(i,j)%vind_1(k) index = index + 1 end do end do @@ -5556,12 +5549,12 @@ SUBROUTINE Perturb_x( p, n, perturb_sign, x, dx ) if (n <= p%BEMT%DBEMT%lin_nx) then - if (n <= p%BEMT%DBEMT%lin_nx/2) then ! x_p%BEMT%DBEMT%element(i,j)%vind, else x_p%BEMT%DBEMT%element(i,j)%vind_dot + if (n <= p%BEMT%DBEMT%lin_nx/2) then ! x_p%BEMT%DBEMT%element(i,j)%vind, else x_p%BEMT%DBEMT%element(i,j)%vind_1 call GetStateIndices( n, size(x%BEMT%DBEMT%element,2), size(x%BEMT%DBEMT%element,1), size(x%BEMT%DBEMT%element(1,1)%vind), Blade, BladeNode, StateIndex ) x%BEMT%DBEMT%element(BladeNode,Blade)%vind(StateIndex) = x%BEMT%DBEMT%element(BladeNode,Blade)%vind(StateIndex) + dx * perturb_sign else - call GetStateIndices( n - p%BEMT%DBEMT%lin_nx/2, size(x%BEMT%DBEMT%element,2), size(x%BEMT%DBEMT%element,1), size(x%BEMT%DBEMT%element(1,1)%vind_dot), Blade, BladeNode, StateIndex ) - x%BEMT%DBEMT%element(BladeNode,Blade)%vind_dot(StateIndex) = x%BEMT%DBEMT%element(BladeNode,Blade)%vind_dot(StateIndex) + dx * perturb_sign + call GetStateIndices( n - p%BEMT%DBEMT%lin_nx/2, size(x%BEMT%DBEMT%element,2), size(x%BEMT%DBEMT%element,1), size(x%BEMT%DBEMT%element(1,1)%vind_1), Blade, BladeNode, StateIndex ) + x%BEMT%DBEMT%element(BladeNode,Blade)%vind_1(StateIndex) = x%BEMT%DBEMT%element(BladeNode,Blade)%vind_1(StateIndex) + dx * perturb_sign endif else @@ -5660,8 +5653,8 @@ SUBROUTINE Compute_dX(p, x_p, x_m, delta_p, delta_m, dX) do j=1,size(x_p%BEMT%DBEMT%element,2) ! number of blades do i=1,size(x_p%BEMT%DBEMT%element,1) ! number of nodes per blade - dX(indx_first:indx_first+1) = x_p%BEMT%DBEMT%element(i,j)%vind_dot - x_m%BEMT%DBEMT%element(i,j)%vind_dot - indx_first = indx_first + size(x_p%BEMT%DBEMT%element(i,j)%vind_dot) !+=2 + dX(indx_first:indx_first+1) = x_p%BEMT%DBEMT%element(i,j)%vind_1 - x_m%BEMT%DBEMT%element(i,j)%vind_1 + indx_first = indx_first + size(x_p%BEMT%DBEMT%element(i,j)%vind_1) !+=2 end do end do diff --git a/modules/aerodyn/src/BEMT.f90 b/modules/aerodyn/src/BEMT.f90 index 08e36c638a..d1c63103c7 100644 --- a/modules/aerodyn/src/BEMT.f90 +++ b/modules/aerodyn/src/BEMT.f90 @@ -357,22 +357,6 @@ subroutine BEMT_AllocInput( u, p, errStat, errMsg ) end if u%Vy = 0.0_ReKi - if (p%DBEMT_Mod==DBEMT_cont_tauConst) then - allocate ( u%Vx_elast_dot( p%numBladeNodes, p%numBlades ), STAT = errStat2 ) - if ( errStat2 /= 0 ) then - call SetErrStat( ErrID_Fatal, 'Error allocating memory for u%Vx_dot.', errStat, errMsg, RoutineName ) - return - end if - u%Vx_elast_dot = 0.0_ReKi - - allocate ( u%Vy_elast_dot( p%numBladeNodes, p%numBlades ), STAT = errStat2 ) - if ( errStat2 /= 0 ) then - call SetErrStat( ErrID_Fatal, 'Error allocating memory for u%Vy_dot.', errStat, errMsg, RoutineName ) - return - end if - u%Vy_elast_dot = 0.0_ReKi - end if - allocate ( u%omega_z( p%numBladeNodes, p%numBlades ), STAT = errStat2 ) if ( errStat2 /= 0 ) then call SetErrStat( ErrID_Fatal, 'Error allocating memory for u%omega_z.', errStat, errMsg, RoutineName ) @@ -941,16 +925,6 @@ subroutine SetInputs_For_DBEMT(u_DBEMT, u, p, axInduction, tanInduction, Rtip) end do end do - if( allocated(u%Vx_elast_dot)) then ! only for DBEMT_Mod=DBEMT_cont_tauConst - do j = 1,p%numBlades - do i = 1,p%numBladeNodes - ! TODO temporarily turning off velocity - u_DBEMT%element(i,j)%vind_s_dot(1) = 0 ! axInduction( i,j)*u%Vx_elast_dot(i,j) - u%omega_z(i,j)*tanInduction(i,j)*u%Vy(i,j) ! Eq. 41 - u_DBEMT%element(i,j)%vind_s_dot(2) = 0 !-tanInduction(i,j)*u%Vy_elast_dot(i,j) - u%omega_z(i,j)*axInduction( i,j)*u%Vx(i,j) ! Eq. 41 - end do - end do - end if - end subroutine SetInputs_For_DBEMT !.................................................................................................................................. @@ -2292,7 +2266,6 @@ subroutine WriteDEBUGValuesToFile(t, u, p, x, xd, z, OtherState, m, AFInfo) , "omega_z" & , "rLocal" , "UserProp" & , "AxInd", "TanInd" -! , "Vx_elast_dot" , "Vy_elast_dot" & end if @@ -2318,9 +2291,6 @@ subroutine WriteDEBUGValuesToFile(t, u, p, x, xd, z, OtherState, m, AFInfo) , u%UserProp( DEBUG_BLADENODE,DEBUG_BLADE) & , m%axInduction( DEBUG_BLADENODE,DEBUG_BLADE) & , m%tanInduction(DEBUG_BLADENODE,DEBUG_BLADE) -! these are not always allocated -! , u%Vx_elast_dot(DEBUG_BLADENODE,DEBUG_BLADE) & -! , u%Vy_elast_dot(DEBUG_BLADENODE,DEBUG_BLADE) & ! now write the residual function to a separate file: if ((DEBUG_nStep >= 0).AND.(DEBUG_nStep <= 450000).AND.(MOD(DEBUG_nStep,25) == 0)) then diff --git a/modules/aerodyn/src/BEMT_Registry.txt b/modules/aerodyn/src/BEMT_Registry.txt index 73b2d2cd4d..1804d6d72a 100644 --- a/modules/aerodyn/src/BEMT_Registry.txt +++ b/modules/aerodyn/src/BEMT_Registry.txt @@ -154,8 +154,6 @@ typedef ^ ^ ReKi typedef ^ ^ ReKi TSR - - - "Tip-speed ratio (to check if BEM should be turned off)" - typedef ^ ^ ReKi Vx {:}{:} - - "Local axial velocity at node" m/s typedef ^ ^ ReKi Vy {:}{:} - - "Local tangential velocity at node" m/s -typedef ^ ^ ReKi Vx_elast_dot {:}{:} - - "Local relative axial acceleration at node (for CDBEMT)" "m/s^2" -typedef ^ ^ ReKi Vy_elast_dot {:}{:} - - "Local relative tangential acceleration at node (for CDBEMT)" "m/s^2" typedef ^ ^ ReKi omega_z {:}{:} - - "rotation of no-sweep-pitch-twist coordinate system around z (for CDBEMT and CUA)" "rad/s" typedef ^ ^ ReKi rLocal {:}{:} - - "Radial distance from center-of-rotation to node" m typedef ^ InputType ReKi Un_disk - - - "disk-averaged velocity normal to the rotor disk (for input to DBEMT)" m/s diff --git a/modules/aerodyn/src/BEMT_Types.f90 b/modules/aerodyn/src/BEMT_Types.f90 index d7dd816ca9..593b7e695d 100644 --- a/modules/aerodyn/src/BEMT_Types.f90 +++ b/modules/aerodyn/src/BEMT_Types.f90 @@ -160,8 +160,6 @@ MODULE BEMT_Types REAL(ReKi) :: TSR !< Tip-speed ratio (to check if BEM should be turned off) [-] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: Vx !< Local axial velocity at node [m/s] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: Vy !< Local tangential velocity at node [m/s] - REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: Vx_elast_dot !< Local relative axial acceleration at node (for CDBEMT) [m/s^2] - REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: Vy_elast_dot !< Local relative tangential acceleration at node (for CDBEMT) [m/s^2] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: omega_z !< rotation of no-sweep-pitch-twist coordinate system around z (for CDBEMT and CUA) [rad/s] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: rLocal !< Radial distance from center-of-rotation to node [m] REAL(ReKi) :: Un_disk !< disk-averaged velocity normal to the rotor disk (for input to DBEMT) [m/s] @@ -4314,34 +4312,6 @@ SUBROUTINE BEMT_CopyInput( SrcInputData, DstInputData, CtrlCode, ErrStat, ErrMsg END IF DstInputData%Vy = SrcInputData%Vy ENDIF -IF (ALLOCATED(SrcInputData%Vx_elast_dot)) THEN - i1_l = LBOUND(SrcInputData%Vx_elast_dot,1) - i1_u = UBOUND(SrcInputData%Vx_elast_dot,1) - i2_l = LBOUND(SrcInputData%Vx_elast_dot,2) - i2_u = UBOUND(SrcInputData%Vx_elast_dot,2) - IF (.NOT. ALLOCATED(DstInputData%Vx_elast_dot)) THEN - ALLOCATE(DstInputData%Vx_elast_dot(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInputData%Vx_elast_dot.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF - END IF - DstInputData%Vx_elast_dot = SrcInputData%Vx_elast_dot -ENDIF -IF (ALLOCATED(SrcInputData%Vy_elast_dot)) THEN - i1_l = LBOUND(SrcInputData%Vy_elast_dot,1) - i1_u = UBOUND(SrcInputData%Vy_elast_dot,1) - i2_l = LBOUND(SrcInputData%Vy_elast_dot,2) - i2_u = UBOUND(SrcInputData%Vy_elast_dot,2) - IF (.NOT. ALLOCATED(DstInputData%Vy_elast_dot)) THEN - ALLOCATE(DstInputData%Vy_elast_dot(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInputData%Vy_elast_dot.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF - END IF - DstInputData%Vy_elast_dot = SrcInputData%Vy_elast_dot -ENDIF IF (ALLOCATED(SrcInputData%omega_z)) THEN i1_l = LBOUND(SrcInputData%omega_z,1) i1_u = UBOUND(SrcInputData%omega_z,1) @@ -4408,12 +4378,6 @@ SUBROUTINE BEMT_DestroyInput( InputData, ErrStat, ErrMsg ) IF (ALLOCATED(InputData%Vy)) THEN DEALLOCATE(InputData%Vy) ENDIF -IF (ALLOCATED(InputData%Vx_elast_dot)) THEN - DEALLOCATE(InputData%Vx_elast_dot) -ENDIF -IF (ALLOCATED(InputData%Vy_elast_dot)) THEN - DEALLOCATE(InputData%Vy_elast_dot) -ENDIF IF (ALLOCATED(InputData%omega_z)) THEN DEALLOCATE(InputData%omega_z) ENDIF @@ -4483,16 +4447,6 @@ SUBROUTINE BEMT_PackInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Int_BufSz = Int_BufSz + 2*2 ! Vy upper/lower bounds for each dimension Re_BufSz = Re_BufSz + SIZE(InData%Vy) ! Vy END IF - Int_BufSz = Int_BufSz + 1 ! Vx_elast_dot allocated yes/no - IF ( ALLOCATED(InData%Vx_elast_dot) ) THEN - Int_BufSz = Int_BufSz + 2*2 ! Vx_elast_dot upper/lower bounds for each dimension - Re_BufSz = Re_BufSz + SIZE(InData%Vx_elast_dot) ! Vx_elast_dot - END IF - Int_BufSz = Int_BufSz + 1 ! Vy_elast_dot allocated yes/no - IF ( ALLOCATED(InData%Vy_elast_dot) ) THEN - Int_BufSz = Int_BufSz + 2*2 ! Vy_elast_dot upper/lower bounds for each dimension - Re_BufSz = Re_BufSz + SIZE(InData%Vy_elast_dot) ! Vy_elast_dot - END IF Int_BufSz = Int_BufSz + 1 ! omega_z allocated yes/no IF ( ALLOCATED(InData%omega_z) ) THEN Int_BufSz = Int_BufSz + 2*2 ! omega_z upper/lower bounds for each dimension @@ -4617,46 +4571,6 @@ SUBROUTINE BEMT_PackInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, END DO END DO END IF - IF ( .NOT. ALLOCATED(InData%Vx_elast_dot) ) THEN - IntKiBuf( Int_Xferred ) = 0 - Int_Xferred = Int_Xferred + 1 - ELSE - IntKiBuf( Int_Xferred ) = 1 - Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%Vx_elast_dot,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Vx_elast_dot,1) - Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%Vx_elast_dot,2) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Vx_elast_dot,2) - Int_Xferred = Int_Xferred + 2 - - DO i2 = LBOUND(InData%Vx_elast_dot,2), UBOUND(InData%Vx_elast_dot,2) - DO i1 = LBOUND(InData%Vx_elast_dot,1), UBOUND(InData%Vx_elast_dot,1) - ReKiBuf(Re_Xferred) = InData%Vx_elast_dot(i1,i2) - Re_Xferred = Re_Xferred + 1 - END DO - END DO - END IF - IF ( .NOT. ALLOCATED(InData%Vy_elast_dot) ) THEN - IntKiBuf( Int_Xferred ) = 0 - Int_Xferred = Int_Xferred + 1 - ELSE - IntKiBuf( Int_Xferred ) = 1 - Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%Vy_elast_dot,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Vy_elast_dot,1) - Int_Xferred = Int_Xferred + 2 - IntKiBuf( Int_Xferred ) = LBOUND(InData%Vy_elast_dot,2) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%Vy_elast_dot,2) - Int_Xferred = Int_Xferred + 2 - - DO i2 = LBOUND(InData%Vy_elast_dot,2), UBOUND(InData%Vy_elast_dot,2) - DO i1 = LBOUND(InData%Vy_elast_dot,1), UBOUND(InData%Vy_elast_dot,1) - ReKiBuf(Re_Xferred) = InData%Vy_elast_dot(i1,i2) - Re_Xferred = Re_Xferred + 1 - END DO - END DO - END IF IF ( .NOT. ALLOCATED(InData%omega_z) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -4842,52 +4756,6 @@ SUBROUTINE BEMT_UnPackInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMs END DO END DO END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Vx_elast_dot not allocated - Int_Xferred = Int_Xferred + 1 - ELSE - Int_Xferred = Int_Xferred + 1 - i1_l = IntKiBuf( Int_Xferred ) - i1_u = IntKiBuf( Int_Xferred + 1) - Int_Xferred = Int_Xferred + 2 - i2_l = IntKiBuf( Int_Xferred ) - i2_u = IntKiBuf( Int_Xferred + 1) - Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%Vx_elast_dot)) DEALLOCATE(OutData%Vx_elast_dot) - ALLOCATE(OutData%Vx_elast_dot(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Vx_elast_dot.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF - DO i2 = LBOUND(OutData%Vx_elast_dot,2), UBOUND(OutData%Vx_elast_dot,2) - DO i1 = LBOUND(OutData%Vx_elast_dot,1), UBOUND(OutData%Vx_elast_dot,1) - OutData%Vx_elast_dot(i1,i2) = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 - END DO - END DO - END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! Vy_elast_dot not allocated - Int_Xferred = Int_Xferred + 1 - ELSE - Int_Xferred = Int_Xferred + 1 - i1_l = IntKiBuf( Int_Xferred ) - i1_u = IntKiBuf( Int_Xferred + 1) - Int_Xferred = Int_Xferred + 2 - i2_l = IntKiBuf( Int_Xferred ) - i2_u = IntKiBuf( Int_Xferred + 1) - Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%Vy_elast_dot)) DEALLOCATE(OutData%Vy_elast_dot) - ALLOCATE(OutData%Vy_elast_dot(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%Vy_elast_dot.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF - DO i2 = LBOUND(OutData%Vy_elast_dot,2), UBOUND(OutData%Vy_elast_dot,2) - DO i1 = LBOUND(OutData%Vy_elast_dot,1), UBOUND(OutData%Vy_elast_dot,1) - OutData%Vy_elast_dot(i1,i2) = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 - END DO - END DO - END IF IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! omega_z not allocated Int_Xferred = Int_Xferred + 1 ELSE @@ -6062,22 +5930,6 @@ SUBROUTINE BEMT_Input_ExtrapInterp1(u1, u2, tin, u_out, tin_out, ErrStat, ErrMsg END DO END DO END IF ! check if allocated -IF (ALLOCATED(u_out%Vx_elast_dot) .AND. ALLOCATED(u1%Vx_elast_dot)) THEN - DO i2 = LBOUND(u_out%Vx_elast_dot,2),UBOUND(u_out%Vx_elast_dot,2) - DO i1 = LBOUND(u_out%Vx_elast_dot,1),UBOUND(u_out%Vx_elast_dot,1) - b = -(u1%Vx_elast_dot(i1,i2) - u2%Vx_elast_dot(i1,i2)) - u_out%Vx_elast_dot(i1,i2) = u1%Vx_elast_dot(i1,i2) + b * ScaleFactor - END DO - END DO -END IF ! check if allocated -IF (ALLOCATED(u_out%Vy_elast_dot) .AND. ALLOCATED(u1%Vy_elast_dot)) THEN - DO i2 = LBOUND(u_out%Vy_elast_dot,2),UBOUND(u_out%Vy_elast_dot,2) - DO i1 = LBOUND(u_out%Vy_elast_dot,1),UBOUND(u_out%Vy_elast_dot,1) - b = -(u1%Vy_elast_dot(i1,i2) - u2%Vy_elast_dot(i1,i2)) - u_out%Vy_elast_dot(i1,i2) = u1%Vy_elast_dot(i1,i2) + b * ScaleFactor - END DO - END DO -END IF ! check if allocated IF (ALLOCATED(u_out%omega_z) .AND. ALLOCATED(u1%omega_z)) THEN DO i2 = LBOUND(u_out%omega_z,2),UBOUND(u_out%omega_z,2) DO i1 = LBOUND(u_out%omega_z,1),UBOUND(u_out%omega_z,1) @@ -6206,24 +6058,6 @@ SUBROUTINE BEMT_Input_ExtrapInterp2(u1, u2, u3, tin, u_out, tin_out, ErrStat, Er END DO END DO END IF ! check if allocated -IF (ALLOCATED(u_out%Vx_elast_dot) .AND. ALLOCATED(u1%Vx_elast_dot)) THEN - DO i2 = LBOUND(u_out%Vx_elast_dot,2),UBOUND(u_out%Vx_elast_dot,2) - DO i1 = LBOUND(u_out%Vx_elast_dot,1),UBOUND(u_out%Vx_elast_dot,1) - b = (t(3)**2*(u1%Vx_elast_dot(i1,i2) - u2%Vx_elast_dot(i1,i2)) + t(2)**2*(-u1%Vx_elast_dot(i1,i2) + u3%Vx_elast_dot(i1,i2)))* scaleFactor - c = ( (t(2)-t(3))*u1%Vx_elast_dot(i1,i2) + t(3)*u2%Vx_elast_dot(i1,i2) - t(2)*u3%Vx_elast_dot(i1,i2) ) * scaleFactor - u_out%Vx_elast_dot(i1,i2) = u1%Vx_elast_dot(i1,i2) + b + c * t_out - END DO - END DO -END IF ! check if allocated -IF (ALLOCATED(u_out%Vy_elast_dot) .AND. ALLOCATED(u1%Vy_elast_dot)) THEN - DO i2 = LBOUND(u_out%Vy_elast_dot,2),UBOUND(u_out%Vy_elast_dot,2) - DO i1 = LBOUND(u_out%Vy_elast_dot,1),UBOUND(u_out%Vy_elast_dot,1) - b = (t(3)**2*(u1%Vy_elast_dot(i1,i2) - u2%Vy_elast_dot(i1,i2)) + t(2)**2*(-u1%Vy_elast_dot(i1,i2) + u3%Vy_elast_dot(i1,i2)))* scaleFactor - c = ( (t(2)-t(3))*u1%Vy_elast_dot(i1,i2) + t(3)*u2%Vy_elast_dot(i1,i2) - t(2)*u3%Vy_elast_dot(i1,i2) ) * scaleFactor - u_out%Vy_elast_dot(i1,i2) = u1%Vy_elast_dot(i1,i2) + b + c * t_out - END DO - END DO -END IF ! check if allocated IF (ALLOCATED(u_out%omega_z) .AND. ALLOCATED(u1%omega_z)) THEN DO i2 = LBOUND(u_out%omega_z,2),UBOUND(u_out%omega_z,2) DO i1 = LBOUND(u_out%omega_z,1),UBOUND(u_out%omega_z,1) diff --git a/modules/aerodyn/src/DBEMT.f90 b/modules/aerodyn/src/DBEMT.f90 index 9be0bc18d1..31834f5d7a 100644 --- a/modules/aerodyn/src/DBEMT.f90 +++ b/modules/aerodyn/src/DBEMT.f90 @@ -17,6 +17,14 @@ ! See the License for the specific language governing permissions and ! limitations under the License. !********************************************************************************************************************************** +! +! References: +! [1] E. Branlard, B. Jonkman, G.R. Pirrung, K. Dixon, J. Jonkman (2022) +! Dynamic inflow and unsteady aerodynamics models for modal and stability analyses in OpenFAST, +! Journal of Physics: conference series +! [2] R. Damiani, J.Jonkman +! DBEMT Theory Rev. 3 +! Unpublished module DBEMT use NWTC_Library @@ -208,7 +216,7 @@ subroutine DBEMT_Init( InitInp, u, p, x, OtherState, m, Interval, InitOut, ErrSt end if end do - p%lin_nx = p%numNodes*p%numBlades*4 ! vind and vind_dot + p%lin_nx = p%numNodes*p%numBlades*4 ! vind and vind_1 else p%lin_nx = 0 end if @@ -241,9 +249,8 @@ subroutine DBEMT_ReInit( p, x, OtherState, m ) do j=1,size(x%element,2) do i=1,size(x%element,1) - x%element(i,j)%vind = 0.0_ReKi - x%element(i,j)%vind_dot = 0.0_ReKi - x%element(i,j)%vind_1 = 0.0_ReKi + x%element(i,j)%vind = 0.0_ReKi ! Dynamic induced velocities + x%element(i,j)%vind_1 = 0.0_ReKi ! Reduced induced velocities end do end do @@ -306,10 +313,10 @@ subroutine DBEMT_InitStates( i, j, u, p, x, OtherState ) x%element(i,j)%vind(2) = u%element(i,j)%vind_s(2) if (p%DBEMT_Mod == DBEMT_cont_tauConst) then - x%element(i,j)%vind_dot(1) = u%element(i,j)%vind_s_dot(1) - x%element(i,j)%vind_dot(2) = u%element(i,j)%vind_s_dot(2) + x%element(i,j)%vind_1(1) = (1._ReKi - p%k_0ye)*u%element(i,j)%vind_s(1) ! Reduced velocity. Eq. (6) from [1] + x%element(i,j)%vind_1(2) = (1._ReKi - p%k_0ye)*u%element(i,j)%vind_s(2) else - x%element(i,j)%vind_1(1) = u%element(i,j)%vind_s(1) + x%element(i,j)%vind_1(1) = u%element(i,j)%vind_s(1) ! Intermediate velocity x%element(i,j)%vind_1(2) = u%element(i,j)%vind_s(2) end if @@ -455,7 +462,7 @@ subroutine ComputeTau1(u, p, m, tau1, errStat, errMsg) temp = (1.0-1.3*AxInd_disk)*Un_disk - tau1 = 1.1*u%R_disk/temp ! Eqn. 1.2 (note that we've eliminated possibility of temp being 0) + tau1 = 1.1*u%R_disk/temp ! Eq. (1) from [1] (note that we've eliminated possibility of temp being 0) tau1 = min(tau1, 100.0_ReKi) ! put a limit on this time constant so it isn't unrealistically long (particularly at initialization) end if @@ -484,8 +491,8 @@ subroutine ComputeTau2(i, j, u, p, tau1, tau2, k_tau_out) spanRatio = u%spanRatio end if - k_tau = 0.39 - 0.26*spanRatio**2 ! Eqn. 1.23b - tau2 = k_tau*tau1 ! Eqn. 1.7 or Eqn 1.23a + k_tau = 0.39 - 0.26*spanRatio**2 + tau2 = k_tau*tau1 ! Eq. (1) from [1] if (present(k_tau_out) ) k_tau_out = k_tau @@ -552,7 +559,8 @@ SUBROUTINE DBEMT_CalcContStateDeriv( i, j, t, u, p, x, OtherState, m, dxdt, ErrS ! LOCAL variables CHARACTER(*), PARAMETER :: RoutineName = 'DBEMT_CalcContStateDeriv' - REAL(ReKi) :: tauConst + REAL(ReKi) :: tau1inv + REAL(ReKi) :: tau2inv REAL(ReKi) :: tau1 REAL(ReKi) :: tau2 @@ -567,20 +575,15 @@ SUBROUTINE DBEMT_CalcContStateDeriv( i, j, t, u, p, x, OtherState, m, dxdt, ErrS call SetErrStat(ErrID_Fatal,"Continuous state derivatives cannot be calculated unless DBEMT_Mod is 3.",ErrStat,ErrMsg,RoutineName) return end if - - tau1 = p%tau1_const/2 ! TODO Temporarily changing tau 1 - !call ComputeTau1( u, p, m, tau1, errStat, errMsg) + tau1 = p%tau1_const call ComputeTau2(i, j, u, p, tau1, tau2) - - ! Implement Equation 37 from E.Branlard 16-Dec-2019 doc: + tau1inv = 1.0_ReKi/(tau1) + tau2inv = 1.0_ReKi/(tau2) - dxdt%vind = x%vind_dot - - tauConst = -1.0_ReKi/(tau1 * tau2) - - dxdt%vind_dot = tauConst * ( x%vind(:) + (tau1 + tau2)*x%vind_dot(:) & - - u%vind_s(:) - p%k_0ye*tau1*u%vind_s_dot(:) ) - + ! State derivatives, Eq. (7) from [1] + dxdt%vind_1 = -tau1inv * x%vind_1(:) + (1 - p%k_0ye) * tau1inv * u%vind_s(:) + dxdt%vind = tau2inv * x%vind_1(:) - tau2inv * x%vind(:) + p%k_0ye * tau2inv * u%vind_s(:) + END SUBROUTINE DBEMT_CalcContStateDeriv !---------------------------------------------------------------------------------------------------------------------------------- !> This subroutine implements the fourth-order Runge-Kutta Method (RK4) for numerically integrating ordinary differential equations: @@ -652,11 +655,11 @@ SUBROUTINE DBEMT_RK4( i, j, t, n, u, utimes, p, x, OtherState, m, ErrStat, ErrMs IF ( ErrStat >= AbortErrLev ) RETURN - k1%vind = p%dt * k1%vind - k1%vind_dot = p%dt * k1%vind_dot + k1%vind = p%dt * k1%vind + k1%vind_1 = p%dt * k1%vind_1 - x_tmp%vind = x%element(i,j)%vind + 0.5 * k1%vind - x_tmp%vind_dot = x%element(i,j)%vind_dot + 0.5 * k1%vind_dot + x_tmp%vind = x%element(i,j)%vind + 0.5 * k1%vind + x_tmp%vind_1 = x%element(i,j)%vind_1 + 0.5 * k1%vind_1 ! interpolate u to find u_interp = u(t + dt/2) TPlusHalfDt = t+0.5_DbKi*p%dt @@ -667,20 +670,20 @@ SUBROUTINE DBEMT_RK4( i, j, t, n, u, utimes, p, x, OtherState, m, ErrStat, ErrMs ! find xdot at t + dt/2 CALL DBEMT_CalcContStateDeriv( i, j, TPlusHalfDt, u_interp, p, x_tmp, OtherState, m, k2, ErrStat2, ErrMsg2 ) - k2%vind = p%dt * k2%vind - k2%vind_dot = p%dt * k2%vind_dot + k2%vind = p%dt * k2%vind + k2%vind_1 = p%dt * k2%vind_1 - x_tmp%vind = x%element(i,j)%vind + 0.5 * k2%vind - x_tmp%vind_dot = x%element(i,j)%vind_dot + 0.5 * k2%vind_dot + x_tmp%vind = x%element(i,j)%vind + 0.5 * k2%vind + x_tmp%vind_1 = x%element(i,j)%vind_1 + 0.5 * k2%vind_1 ! find xdot at t + dt/2 (note x_tmp has changed) CALL DBEMT_CalcContStateDeriv( i, j, TPlusHalfDt, u_interp, p, x_tmp, OtherState, m, k3, ErrStat2, ErrMsg2 ) - k3%vind = p%dt * k3%vind - k3%vind_dot = p%dt * k3%vind_dot + k3%vind = p%dt * k3%vind + k3%vind_1 = p%dt * k3%vind_1 - x_tmp%vind = x%element(i,j)%vind + k3%vind - x_tmp%vind_dot = x%element(i,j)%vind_dot + k3%vind_dot + x_tmp%vind = x%element(i,j)%vind + k3%vind + x_tmp%vind_1 = x%element(i,j)%vind_1 + k3%vind_1 ! interpolate u to find u_interp = u(t + dt) TPlusDt = t + p%dt @@ -691,11 +694,11 @@ SUBROUTINE DBEMT_RK4( i, j, t, n, u, utimes, p, x, OtherState, m, ErrStat, ErrMs ! find xdot at t + dt CALL DBEMT_CalcContStateDeriv( i, j, TPlusDt, u_interp, p, x_tmp, OtherState, m, k4, ErrStat2, ErrMsg2 ) - k4%vind = p%dt * k4%vind - k4%vind_dot = p%dt * k4%vind_dot + k4%vind = p%dt * k4%vind + k4%vind_1 = p%dt * k4%vind_1 - x%element(i,j)%vind = x%element(i,j)%vind + ( k1%vind + 2. * k2%vind + 2. * k3%vind + k4%vind ) / 6. - x%element(i,j)%vind_dot = x%element(i,j)%vind_dot + ( k1%vind_dot + 2. * k2%vind_dot + 2. * k3%vind_dot + k4%vind_dot ) / 6. + x%element(i,j)%vind = x%element(i,j)%vind + ( k1%vind + 2. * k2%vind + 2. * k3%vind + k4%vind ) / 6. + x%element(i,j)%vind_1 = x%element(i,j)%vind_1 + ( k1%vind_1 + 2. * k2%vind_1 + 2. * k3%vind_1 + k4%vind_1 ) / 6. END SUBROUTINE DBEMT_RK4 !---------------------------------------------------------------------------------------------------------------------------------- @@ -779,11 +782,11 @@ SUBROUTINE DBEMT_AB4( i, j, t, n, u, utimes, p, x, OtherState, m, ErrStat, ErrMs else - x%element(i,j)%vind = x%element(i,j)%vind + p%DT/24. * ( 55.*OtherState%xdot(1)%element(i,j)%vind - 59.*OtherState%xdot(2)%element(i,j)%vind & + x%element(i,j)%vind = x%element(i,j)%vind + p%DT/24. * ( 55.*OtherState%xdot(1)%element(i,j)%vind - 59.*OtherState%xdot(2)%element(i,j)%vind & + 37.*OtherState%xdot(3)%element(i,j)%vind - 9.*OtherState%xdot(4)%element(i,j)%vind ) - x%element(i,j)%vind_dot = x%element(i,j)%vind_dot + p%DT/24. * ( 55.*OtherState%xdot(1)%element(i,j)%vind_dot - 59.*OtherState%xdot(2)%element(i,j)%vind_dot & - + 37.*OtherState%xdot(3)%element(i,j)%vind_dot - 9.*OtherState%xdot(4)%element(i,j)%vind_dot ) + x%element(i,j)%vind_1 = x%element(i,j)%vind_1 + p%DT/24. * ( 55.*OtherState%xdot(1)%element(i,j)%vind_1 - 59.*OtherState%xdot(2)%element(i,j)%vind_1 & + + 37.*OtherState%xdot(3)%element(i,j)%vind_1 - 9.*OtherState%xdot(4)%element(i,j)%vind_1 ) endif @@ -861,13 +864,13 @@ SUBROUTINE DBEMT_ABM4( i, j, t, n, u, utimes, p, x, OtherState, m, ErrStat, ErrM IF ( ErrStat >= AbortErrLev ) RETURN - x%element(i,j)%vind = x_in%vind + p%DT/24. * ( 9. * xdot_pred%vind + 19. * OtherState%xdot(1)%element(i,j)%vind & + x%element(i,j)%vind = x_in%vind + p%DT/24. * ( 9. * xdot_pred%vind + 19. * OtherState%xdot(1)%element(i,j)%vind & - 5. * OtherState%xdot(2)%element(i,j)%vind & + 1. * OtherState%xdot(3)%element(i,j)%vind ) - x%element(i,j)%vind_dot = x_in%vind_dot + p%DT/24. * ( 9. * xdot_pred%vind_dot + 19. * OtherState%xdot(1)%element(i,j)%vind_dot & - - 5. * OtherState%xdot(2)%element(i,j)%vind_dot & - + 1. * OtherState%xdot(3)%element(i,j)%vind_dot ) + x%element(i,j)%vind_1 = x_in%vind_1 + p%DT/24. * ( 9. * xdot_pred%vind_1 + 19. * OtherState%xdot(1)%element(i,j)%vind_1 & + - 5. * OtherState%xdot(2)%element(i,j)%vind_1 & + + 1. * OtherState%xdot(3)%element(i,j)%vind_1 ) endif END SUBROUTINE DBEMT_ABM4 diff --git a/modules/aerodyn/src/DBEMT_Registry.txt b/modules/aerodyn/src/DBEMT_Registry.txt index c5fa20f529..21a726a068 100644 --- a/modules/aerodyn/src/DBEMT_Registry.txt +++ b/modules/aerodyn/src/DBEMT_Registry.txt @@ -29,8 +29,7 @@ typedef ^ ^ ReKi rLocal { typedef ^ InitOutputType ProgDesc Ver - - - "This module's name, version, and date" - typedef ^ DBEMT_ElementContinuousStateType R8Ki vind {2} - - "The filtered induced velocity, [1,i,j] is the axial induced velocity (-Vx*a) at node i on blade j and [2,i,j] is the tantential induced velocity (Vy*a')" m/s -typedef ^ DBEMT_ElementContinuousStateType R8Ki vind_dot {2} - - "Time derivative of the filtered induced velocity, x%vind in CCSD" "m/s^2" -typedef ^ DBEMT_ElementContinuousStateType R8Ki vind_1 {2} - - "The filtered intermediate induced velocity" "m/s" +typedef ^ DBEMT_ElementContinuousStateType R8Ki vind_1 {2} - - "The filtered reduced or intermediate induced velocity" "m/s" # ..... States .................................................................................................................... # Define continuous (differentiable) states here: @@ -73,7 +72,6 @@ typedef ^ ParameterType IntKi DBEMT_Mod # ..... Inputs .................................................................................................................... typedef ^ DBEMT_ElementInputType ReKi vind_s {2} - - "The unfiltered induced velocity, [1] is the axial induced velocity (-Vx*a) and [2] is the tangential induced velocity (Vy*a') at node i on blade j. Note that the inputs are used only operated on at a particular node and blade, so we don't store all elements" "m/s" -typedef ^ DBEMT_ElementInputType ReKi vind_s_dot {2} - - "The first time derivative of the unfiltered induced velocity, u%vind_s" "m/s^2" typedef ^ DBEMT_ElementInputType ReKi spanRatio - - - "Normalized span location of blade node" - # Define inputs that are contained on the mesh here: # diff --git a/modules/aerodyn/src/DBEMT_Types.f90 b/modules/aerodyn/src/DBEMT_Types.f90 index 04f0fb3af0..d87e22b1a4 100644 --- a/modules/aerodyn/src/DBEMT_Types.f90 +++ b/modules/aerodyn/src/DBEMT_Types.f90 @@ -54,8 +54,7 @@ MODULE DBEMT_Types ! ========= DBEMT_ElementContinuousStateType ======= TYPE, PUBLIC :: DBEMT_ElementContinuousStateType REAL(R8Ki) , DIMENSION(1:2) :: vind !< The filtered induced velocity, [1,i,j] is the axial induced velocity (-Vx*a) at node i on blade j and [2,i,j] is the tantential induced velocity (Vy*a') [m/s] - REAL(R8Ki) , DIMENSION(1:2) :: vind_dot !< Time derivative of the filtered induced velocity, x%vind in CCSD [m/s^2] - REAL(R8Ki) , DIMENSION(1:2) :: vind_1 !< The filtered intermediate induced velocity [m/s] + REAL(R8Ki) , DIMENSION(1:2) :: vind_1 !< The filtered reduced or intermediate induced velocity [m/s] END TYPE DBEMT_ElementContinuousStateType ! ======================= ! ========= DBEMT_ContinuousStateType ======= @@ -102,7 +101,6 @@ MODULE DBEMT_Types ! ========= DBEMT_ElementInputType ======= TYPE, PUBLIC :: DBEMT_ElementInputType REAL(ReKi) , DIMENSION(1:2) :: vind_s !< The unfiltered induced velocity, [1] is the axial induced velocity (-Vx*a) and [2] is the tangential induced velocity (Vy*a') at node i on blade j. Note that the inputs are used only operated on at a particular node and blade, so we don't store all elements [m/s] - REAL(ReKi) , DIMENSION(1:2) :: vind_s_dot !< The first time derivative of the unfiltered induced velocity, u%vind_s [m/s^2] REAL(ReKi) :: spanRatio !< Normalized span location of blade node [-] END TYPE DBEMT_ElementInputType ! ======================= @@ -559,7 +557,6 @@ SUBROUTINE DBEMT_CopyElementContinuousStateType( SrcElementContinuousStateTypeDa ErrStat = ErrID_None ErrMsg = "" DstElementContinuousStateTypeData%vind = SrcElementContinuousStateTypeData%vind - DstElementContinuousStateTypeData%vind_dot = SrcElementContinuousStateTypeData%vind_dot DstElementContinuousStateTypeData%vind_1 = SrcElementContinuousStateTypeData%vind_1 END SUBROUTINE DBEMT_CopyElementContinuousStateType @@ -610,7 +607,6 @@ SUBROUTINE DBEMT_PackElementContinuousStateType( ReKiBuf, DbKiBuf, IntKiBuf, Ind Db_BufSz = 0 Int_BufSz = 0 Db_BufSz = Db_BufSz + SIZE(InData%vind) ! vind - Db_BufSz = Db_BufSz + SIZE(InData%vind_dot) ! vind_dot Db_BufSz = Db_BufSz + SIZE(InData%vind_1) ! vind_1 IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) @@ -643,10 +639,6 @@ SUBROUTINE DBEMT_PackElementContinuousStateType( ReKiBuf, DbKiBuf, IntKiBuf, Ind DbKiBuf(Db_Xferred) = InData%vind(i1) Db_Xferred = Db_Xferred + 1 END DO - DO i1 = LBOUND(InData%vind_dot,1), UBOUND(InData%vind_dot,1) - DbKiBuf(Db_Xferred) = InData%vind_dot(i1) - Db_Xferred = Db_Xferred + 1 - END DO DO i1 = LBOUND(InData%vind_1,1), UBOUND(InData%vind_1,1) DbKiBuf(Db_Xferred) = InData%vind_1(i1) Db_Xferred = Db_Xferred + 1 @@ -686,12 +678,6 @@ SUBROUTINE DBEMT_UnPackElementContinuousStateType( ReKiBuf, DbKiBuf, IntKiBuf, O OutData%vind(i1) = REAL(DbKiBuf(Db_Xferred), R8Ki) Db_Xferred = Db_Xferred + 1 END DO - i1_l = LBOUND(OutData%vind_dot,1) - i1_u = UBOUND(OutData%vind_dot,1) - DO i1 = LBOUND(OutData%vind_dot,1), UBOUND(OutData%vind_dot,1) - OutData%vind_dot(i1) = REAL(DbKiBuf(Db_Xferred), R8Ki) - Db_Xferred = Db_Xferred + 1 - END DO i1_l = LBOUND(OutData%vind_1,1) i1_u = UBOUND(OutData%vind_1,1) DO i1 = LBOUND(OutData%vind_1,1), UBOUND(OutData%vind_1,1) @@ -1972,7 +1958,6 @@ SUBROUTINE DBEMT_CopyElementInputType( SrcElementInputTypeData, DstElementInputT ErrStat = ErrID_None ErrMsg = "" DstElementInputTypeData%vind_s = SrcElementInputTypeData%vind_s - DstElementInputTypeData%vind_s_dot = SrcElementInputTypeData%vind_s_dot DstElementInputTypeData%spanRatio = SrcElementInputTypeData%spanRatio END SUBROUTINE DBEMT_CopyElementInputType @@ -2023,7 +2008,6 @@ SUBROUTINE DBEMT_PackElementInputType( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrSt Db_BufSz = 0 Int_BufSz = 0 Re_BufSz = Re_BufSz + SIZE(InData%vind_s) ! vind_s - Re_BufSz = Re_BufSz + SIZE(InData%vind_s_dot) ! vind_s_dot Re_BufSz = Re_BufSz + 1 ! spanRatio IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) @@ -2056,10 +2040,6 @@ SUBROUTINE DBEMT_PackElementInputType( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrSt ReKiBuf(Re_Xferred) = InData%vind_s(i1) Re_Xferred = Re_Xferred + 1 END DO - DO i1 = LBOUND(InData%vind_s_dot,1), UBOUND(InData%vind_s_dot,1) - ReKiBuf(Re_Xferred) = InData%vind_s_dot(i1) - Re_Xferred = Re_Xferred + 1 - END DO ReKiBuf(Re_Xferred) = InData%spanRatio Re_Xferred = Re_Xferred + 1 END SUBROUTINE DBEMT_PackElementInputType @@ -2097,12 +2077,6 @@ SUBROUTINE DBEMT_UnPackElementInputType( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, Er OutData%vind_s(i1) = ReKiBuf(Re_Xferred) Re_Xferred = Re_Xferred + 1 END DO - i1_l = LBOUND(OutData%vind_s_dot,1) - i1_u = UBOUND(OutData%vind_s_dot,1) - DO i1 = LBOUND(OutData%vind_s_dot,1), UBOUND(OutData%vind_s_dot,1) - OutData%vind_s_dot(i1) = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 - END DO OutData%spanRatio = ReKiBuf(Re_Xferred) Re_Xferred = Re_Xferred + 1 END SUBROUTINE DBEMT_UnPackElementInputType @@ -2710,10 +2684,6 @@ SUBROUTINE DBEMT_ElementInputType_ExtrapInterp1(u1, u2, tin, u_out, tin_out, Err b = -(u1%vind_s(i1) - u2%vind_s(i1)) u_out%vind_s(i1) = u1%vind_s(i1) + b * ScaleFactor END DO - DO i1 = LBOUND(u_out%vind_s_dot,1),UBOUND(u_out%vind_s_dot,1) - b = -(u1%vind_s_dot(i1) - u2%vind_s_dot(i1)) - u_out%vind_s_dot(i1) = u1%vind_s_dot(i1) + b * ScaleFactor - END DO b = -(u1%spanRatio - u2%spanRatio) u_out%spanRatio = u1%spanRatio + b * ScaleFactor END SUBROUTINE DBEMT_ElementInputType_ExtrapInterp1 @@ -2778,11 +2748,6 @@ SUBROUTINE DBEMT_ElementInputType_ExtrapInterp2(u1, u2, u3, tin, u_out, tin_out, c = ( (t(2)-t(3))*u1%vind_s(i1) + t(3)*u2%vind_s(i1) - t(2)*u3%vind_s(i1) ) * scaleFactor u_out%vind_s(i1) = u1%vind_s(i1) + b + c * t_out END DO - DO i1 = LBOUND(u_out%vind_s_dot,1),UBOUND(u_out%vind_s_dot,1) - b = (t(3)**2*(u1%vind_s_dot(i1) - u2%vind_s_dot(i1)) + t(2)**2*(-u1%vind_s_dot(i1) + u3%vind_s_dot(i1)))* scaleFactor - c = ( (t(2)-t(3))*u1%vind_s_dot(i1) + t(3)*u2%vind_s_dot(i1) - t(2)*u3%vind_s_dot(i1) ) * scaleFactor - u_out%vind_s_dot(i1) = u1%vind_s_dot(i1) + b + c * t_out - END DO b = (t(3)**2*(u1%spanRatio - u2%spanRatio) + t(2)**2*(-u1%spanRatio + u3%spanRatio))* scaleFactor c = ( (t(2)-t(3))*u1%spanRatio + t(3)*u2%spanRatio - t(2)*u3%spanRatio ) * scaleFactor u_out%spanRatio = u1%spanRatio + b + c * t_out @@ -2902,14 +2867,6 @@ SUBROUTINE DBEMT_Input_ExtrapInterp1(u1, u2, tin, u_out, tin_out, ErrStat, ErrMs ENDDO DO i02 = LBOUND(u_out%element,2),UBOUND(u_out%element,2) DO i01 = LBOUND(u_out%element,1),UBOUND(u_out%element,1) - DO i1 = LBOUND(u_out%element(i01,i02)%vind_s_dot,1),UBOUND(u_out%element(i01,i02)%vind_s_dot,1) - b = -(u1%element(i01,i02)%vind_s_dot(i1) - u2%element(i01,i02)%vind_s_dot(i1)) - u_out%element(i01,i02)%vind_s_dot(i1) = u1%element(i01,i02)%vind_s_dot(i1) + b * ScaleFactor - END DO - ENDDO - ENDDO - DO i02 = LBOUND(u_out%element,2),UBOUND(u_out%element,2) - DO i01 = LBOUND(u_out%element,1),UBOUND(u_out%element,1) b = -(u1%element(i01,i02)%spanRatio - u2%element(i01,i02)%spanRatio) u_out%element(i01,i02)%spanRatio = u1%element(i01,i02)%spanRatio + b * ScaleFactor ENDDO @@ -2995,15 +2952,6 @@ SUBROUTINE DBEMT_Input_ExtrapInterp2(u1, u2, u3, tin, u_out, tin_out, ErrStat, E ENDDO DO i02 = LBOUND(u_out%element,2),UBOUND(u_out%element,2) DO i01 = LBOUND(u_out%element,1),UBOUND(u_out%element,1) - DO i1 = LBOUND(u_out%element(i01,i02)%vind_s_dot,1),UBOUND(u_out%element(i01,i02)%vind_s_dot,1) - b = (t(3)**2*(u1%element(i01,i02)%vind_s_dot(i1) - u2%element(i01,i02)%vind_s_dot(i1)) + t(2)**2*(-u1%element(i01,i02)%vind_s_dot(i1) + u3%element(i01,i02)%vind_s_dot(i1)))* scaleFactor - c = ( (t(2)-t(3))*u1%element(i01,i02)%vind_s_dot(i1) + t(3)*u2%element(i01,i02)%vind_s_dot(i1) - t(2)*u3%element(i01,i02)%vind_s_dot(i1) ) * scaleFactor - u_out%element(i01,i02)%vind_s_dot(i1) = u1%element(i01,i02)%vind_s_dot(i1) + b + c * t_out - END DO - ENDDO - ENDDO - DO i02 = LBOUND(u_out%element,2),UBOUND(u_out%element,2) - DO i01 = LBOUND(u_out%element,1),UBOUND(u_out%element,1) b = (t(3)**2*(u1%element(i01,i02)%spanRatio - u2%element(i01,i02)%spanRatio) + t(2)**2*(-u1%element(i01,i02)%spanRatio + u3%element(i01,i02)%spanRatio))* scaleFactor c = ( (t(2)-t(3))*u1%element(i01,i02)%spanRatio + t(3)*u2%element(i01,i02)%spanRatio - t(2)*u3%element(i01,i02)%spanRatio ) * scaleFactor u_out%element(i01,i02)%spanRatio = u1%element(i01,i02)%spanRatio + b + c * t_out diff --git a/modules/openfast-library/src/FAST_Lin.f90 b/modules/openfast-library/src/FAST_Lin.f90 index 2a03635b84..b555300fee 100644 --- a/modules/openfast-library/src/FAST_Lin.f90 +++ b/modules/openfast-library/src/FAST_Lin.f90 @@ -4913,8 +4913,8 @@ SUBROUTINE PerturbOP(t, iLinTime, iMode, p_FAST, y_FAST, ED, BD, SrvD, AD, IfW, do j=1,size(AD%x(STATE_CURR)%rotors(1)%BEMT%DBEMT%element,2) do i=1,size(AD%x(STATE_CURR)%rotors(1)%BEMT%DBEMT%element,1) - indx_last = indx + size(AD%x(STATE_CURR)%rotors(1)%BEMT%DBEMT%element(i,j)%vind_dot) - 1 - call GetStateAry(p_FAST, iMode, t, AD%x(STATE_CURR)%rotors(1)%BEMT%DBEMT%element(i,j)%vind_dot, y_FAST%Lin%Modules(ThisModule)%Instance(1)%op_x_eig_mag( indx : indx_last), & + indx_last = indx + size(AD%x(STATE_CURR)%rotors(1)%BEMT%DBEMT%element(i,j)%vind_1) - 1 + call GetStateAry(p_FAST, iMode, t, AD%x(STATE_CURR)%rotors(1)%BEMT%DBEMT%element(i,j)%vind_1, y_FAST%Lin%Modules(ThisModule)%Instance(1)%op_x_eig_mag( indx : indx_last), & y_FAST%Lin%Modules(ThisModule)%Instance(1)%op_x_eig_phase(indx : indx_last) ) indx = indx_last + 1 end do From 04e9cba86b8e05ad5047c28450d76b38e0c4dce6 Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Fri, 7 Jan 2022 13:00:09 -0700 Subject: [PATCH 089/242] UA/DBEMT: Allowing linearization --- modules/aerodyn/src/AeroDyn.f90 | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/modules/aerodyn/src/AeroDyn.f90 b/modules/aerodyn/src/AeroDyn.f90 index b01907d9b3..14a93b76de 100644 --- a/modules/aerodyn/src/AeroDyn.f90 +++ b/modules/aerodyn/src/AeroDyn.f90 @@ -2460,21 +2460,17 @@ SUBROUTINE ValidateInputData( InitInp, InputFileData, NumBl, ErrStat, ErrMsg ) !.................. if (InitInp%Linearize) then if (InputFileData%AFAeroMod /= AFAeroMod_Steady) then -!bjj: REMOVE when linearization has been tested - call SetErrStat( ErrID_Fatal, 'Steady blade airfoil aerodynamics must be used for linearization. Set AFAeroMod=1.', ErrStat, ErrMsg, RoutineName ) - !if (InputFileData%UAMod /= UA_HGM) then - ! call SetErrStat( ErrID_Fatal, 'When AFAeroMod=2, UAMod must be 4 for linearization. Set AFAeroMod=1 or UAMod=4.', ErrStat, ErrMsg, RoutineName ) - !end if + if (InputFileData%UAMod /= UA_HGM .and. InputFileData%UAMod /= UA_OYE) then + call SetErrStat( ErrID_Fatal, 'When AFAeroMod=2, UAMod must be 4 for linearization. Set AFAeroMod=1 or UAMod=4.', ErrStat, ErrMsg, RoutineName ) + end if end if if (InputFileData%WakeMod == WakeMod_FVW) then call SetErrStat( ErrID_Fatal, 'FVW cannot currently be used for linearization. Set WakeMod=0 or WakeMod=1.', ErrStat, ErrMsg, RoutineName ) else if (InputFileData%WakeMod == WakeMod_DBEMT) then -!bjj: when linearization has been tested - call SetErrStat( ErrID_Fatal, 'DBEMT cannot currently be used for linearization. Set WakeMod=0 or WakeMod=1.', ErrStat, ErrMsg, RoutineName ) - !if (InputFileData%DBEMT_Mod /= DBEMT_cont_tauConst) then - ! call SetErrStat( ErrID_Fatal, 'DBEMT requires the continuous formulation with constant tau1 for linearization. Set DBEMT_Mod=3 or set WakeMod to 0 or 1.', ErrStat, ErrMsg, RoutineName ) - !end if + if (InputFileData%DBEMT_Mod /= DBEMT_cont_tauConst) then + call SetErrStat( ErrID_Fatal, 'DBEMT requires the continuous formulation with constant tau1 for linearization. Set DBEMT_Mod=3 or set WakeMod to 0 or 1.', ErrStat, ErrMsg, RoutineName ) + end if end if end if From 9cb0b1bb917e1354b439a214121c85b9cfe58024 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Thu, 7 Oct 2021 16:53:54 -0600 Subject: [PATCH 090/242] Cherry picking some overlooked linearization updates from MDv2 32bf56f4 (Merge remote-tracking branch 'andy/MDv2+SrvDLin' into MDv2) --- modules/moordyn/CMakeLists.txt | 6 +- modules/moordyn/src/MoorDyn.f90 | 306 +++++++++++++++------- modules/moordyn/src/MoorDyn_Registry.txt | 1 + modules/moordyn/src/MoorDyn_Types.f90 | 54 ++++ modules/openfast-library/src/FAST_Lin.f90 | 112 ++++++-- 5 files changed, 369 insertions(+), 110 deletions(-) diff --git a/modules/moordyn/CMakeLists.txt b/modules/moordyn/CMakeLists.txt index 4bd406ba23..6d6333139a 100644 --- a/modules/moordyn/CMakeLists.txt +++ b/modules/moordyn/CMakeLists.txt @@ -33,9 +33,9 @@ install(TARGETS moordynlib ARCHIVE DESTINATION lib LIBRARY DESTINATION lib) -set(MD_DRIVER_SOURCES src/MoorDyn_Driver.f90) -add_executable(moordyn_driver ${MD_DRIVER_SOURCES}) -target_link_libraries(moordyn_driver moordynlib nwtclibs versioninfolib ${CMAKE_DL_LIBS}) +#set(MD_DRIVER_SOURCES src/MoorDyn_Driver.f90) +#add_executable(moordyn_driver ${MD_DRIVER_SOURCES}) +#target_link_libraries(moordyn_driver moordynlib nwtclibs versioninfolib ${CMAKE_DL_LIBS}) install(TARGETS moordyn_driver RUNTIME DESTINATION bin diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index acb0f4ad57..07e7abdf3e 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -3157,7 +3157,7 @@ SUBROUTINE MD_JacobianPInput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrM ! get central difference: ! we may have had an error allocating memory, so we'll check if(Failed()) return - ! get central difference: + ! get central difference (state entries are mapped the the dXdu column in routine): call MD_Compute_dX( p, x_p, x_m, delta_p, dXdu(:,i) ) end do END IF ! dXdu @@ -3229,15 +3229,17 @@ SUBROUTINE MD_JacobianPContState( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ! make a copy of outputs because we will need two for the central difference computations (with orientations) call MD_CopyOutput( y, y_p, MESH_NEWCOPY, ErrStat2, ErrMsg2); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) call MD_CopyOutput( y, y_m, MESH_NEWCOPY, ErrStat2, ErrMsg2); if(Failed()) return - do i=1,p%Jac_nx + ! Loop over the dx dimension of the dYdx array. Perturb the corresponding state (note difference in ordering of dYdx and x%states). + ! The p%dxIdx_map2_xStateIdx(i) is the index to the state array for the given dx index + do i=1,p%Jac_nx ! index into dx dimension ! get x_op + delta x call MD_CopyContState( x, x_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) - call MD_perturb_x(p, i, 1, x_perturb, delta ) + call MD_perturb_x(p, p%dxIdx_map2_xStateIdx(i), 1, x_perturb, delta ) ! compute y at x_op + delta x call MD_CalcOutput( t, u, p, x_perturb, xd, z, OtherState, y_p, m, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) ! get x_op - delta x call MD_CopyContState( x, x_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) - call MD_perturb_x(p, i, -1, x_perturb, delta ) + call MD_perturb_x(p, p%dxIdx_map2_xStateIdx(i), -1, x_perturb, delta ) ! compute y at x_op - delta x call MD_CalcOutput( t, u, p, x_perturb, xd, z, OtherState, y_m, m, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) ! get central difference: @@ -3251,15 +3253,18 @@ SUBROUTINE MD_JacobianPContState( t, u, p, x, xd, z, OtherState, y, m, ErrStat, if (.not. allocated(dXdx)) then call AllocAry(dXdx, p%Jac_nx, p%Jac_nx, 'dXdx', ErrStat2, ErrMsg2); if(Failed()) return end if - do i=1,p%Jac_nx + ! Loop over the dx dimension of the array. Perturb the corresponding state (note difference in ordering of dXdx and x%states). + ! The resulting x_p and x_m are used to calculate the column for dXdx (mapping of state entry to dXdx row entry occurs in MD_Compute_dX) + ! The p%dxIdx_map2_xStateIdx(i) is the index to the state array for the given dx index + do i=1,p%Jac_nx ! index into dx dimension ! get x_op + delta x call MD_CopyContState( x, x_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) - call MD_perturb_x(p, i, 1, x_perturb, delta ) + call MD_perturb_x(p, p%dxIdx_map2_xStateIdx(i), 1, x_perturb, delta ) ! compute x at x_op + delta x call MD_CalcContStateDeriv( t, u, p, x_perturb, xd, z, OtherState, m, x_p, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) ! get x_op - delta x call MD_CopyContState( x, x_perturb, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) - call MD_perturb_x(p, i, -1, x_perturb, delta ) + call MD_perturb_x(p, p%dxIdx_map2_xStateIdx(i), -1, x_perturb, delta ) ! compute x at x_op - delta x call MD_CalcContStateDeriv( t, u, p, x_perturb, xd, z, OtherState, m, x_m, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) if(Failed()) return @@ -3436,7 +3441,7 @@ SUBROUTINE MD_GetOP( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, u_op, call AllocAry(x_op, p%Jac_nx,'x_op',ErrStat2,ErrMsg2); if (Failed()) return end if do i=1, p%Jac_nx - x_op(i) = x%states(i) + x_op(i) = x%states(p%dxIdx_map2_xStateIdx(i)) ! x for lin is different order, so use mapping end do END IF ! state derivatives? @@ -3446,7 +3451,7 @@ SUBROUTINE MD_GetOP( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, u_op, end if call MD_CalcContStateDeriv( t, u, p, x, xd, z, OtherState, m, dx, ErrStat2, ErrMsg2 ) ; if(Failed()) return do i=1, p%Jac_nx - dx_op(i) = dx%states(i) + dx_op(i) = dx%states(p%dxIdx_map2_xStateIdx(i)) ! x for lin is different order, so use mapping end do END IF IF ( PRESENT( xd_op ) ) THEN @@ -3473,9 +3478,9 @@ END SUBROUTINE MD_GetOP !==================================================================================================== !++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ !> This routine initializes the array that maps rows/columns of the Jacobian to specific mesh fields. -!! Do not change the order of this packing without changing subroutine ! +!! Do not change the order of this packing without changing subroutines calculating dXdx etc (MD_Compute_dX) SUBROUTINE MD_Init_Jacobian(Init, p, u, y, m, InitOut, ErrStat, ErrMsg) - TYPE(MD_InitInputType) , INTENT(IN ) :: Init !< Init + TYPE(MD_InitInputType) , INTENT(IN ) :: Init !< Init TYPE(MD_ParameterType) , INTENT(INOUT) :: p !< parameters TYPE(MD_InputType) , INTENT(IN ) :: u !< inputs TYPE(MD_OutputType) , INTENT(IN ) :: y !< outputs @@ -3532,90 +3537,210 @@ END SUBROUTINE Init_Jacobian_y !> This routine initializes the Jacobian parameters and initialization outputs for the linearized continuous states. SUBROUTINE Init_Jacobian_x() + INTEGER(IntKi) :: idx ! index into the LinNames_x array INTEGER(IntKi) :: i INTEGER(IntKi) :: l INTEGER(IntKi) :: N + p%Jac_nx = m%Nx ! size of (continuous) state vector (includes the first derivatives) ! allocate space for the row/column names and for perturbation sizes - CALL AllocAry(InitOut%LinNames_x , p%Jac_nx, 'LinNames_x' , ErrStat2, ErrMsg2); if(ErrStat/=ErrID_None) return - CALL AllocAry(InitOut%RotFrame_x , p%Jac_nx, 'RotFrame_x' , ErrStat2, ErrMsg2); if(ErrStat/=ErrID_None) return - CALL AllocAry(InitOut%DerivOrder_x, p%Jac_nx, 'DerivOrder_x', ErrStat2, ErrMsg2); if(ErrStat/=ErrID_None) return - CALL AllocAry(p%dx, p%Jac_nx, 'p%dx' , ErrStat2, ErrMsg2); if(ErrStat/=ErrID_None) return + CALL AllocAry(InitOut%LinNames_x , p%Jac_nx, 'LinNames_x' , ErrStat2, ErrMsg2); if(ErrStat/=ErrID_None) return + CALL AllocAry(InitOut%RotFrame_x , p%Jac_nx, 'RotFrame_x' , ErrStat2, ErrMsg2); if(ErrStat/=ErrID_None) return + CALL AllocAry(InitOut%DerivOrder_x , p%Jac_nx, 'DerivOrder_x' , ErrStat2, ErrMsg2); if(ErrStat/=ErrID_None) return + CALL AllocAry(p%dx , p%Jac_nx, 'p%dx' , ErrStat2, ErrMsg2); if(ErrStat/=ErrID_None) return + CALL AllocAry(p%dxIdx_map2_xStateIdx, p%Jac_nx, 'p%dxIdx_map2_xStateIdx', ErrStat2, ErrMsg2); if(ErrStat/=ErrID_None) return + + p%dxIdx_map2_xStateIdx = 0_IntKi ! all values should be overwritten by logic below + ! set linearization output names and default perturbations, p%dx: - - DO l = 1,p%nFreeBodies ! Body m%BodyList(m%FreeBodyIs(l)) - p%dx( m%BodyStateIs1(l) : m%BodyStateIs1(l)+2 ) = 0.1 ! body translational velocity [m/s] - p%dx( m%BodyStateIs1(l)+3 : m%BodyStateIs1(l)+5 ) = 0.1 ! body rotational velocity [rad/s] - p%dx( m%BodyStateIs1(l)+6 : m%BodyStateIs1(l)+8 ) = 0.2 ! body displacement [m] - p%dx( m%BodyStateIs1(l)+9 : m%BodyStateIs1(l)+11) = 0.02 ! body rotation [rad] - InitOut%LinNames_x(m%BodyStateIs1(l) ) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Vx, m/s' - InitOut%LinNames_x(m%BodyStateIs1(l)+1) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Vy, m/s' - InitOut%LinNames_x(m%BodyStateIs1(l)+2) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Vz, m/s' - InitOut%LinNames_x(m%BodyStateIs1(l)+3) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' omega_x, rad/s' - InitOut%LinNames_x(m%BodyStateIs1(l)+4) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' omega_y, rad/s' - InitOut%LinNames_x(m%BodyStateIs1(l)+5) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' omega_z, rad/s' - InitOut%LinNames_x(m%BodyStateIs1(l)+6) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Px, m' - InitOut%LinNames_x(m%BodyStateIs1(l)+7) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Py, m' - InitOut%LinNames_x(m%BodyStateIs1(l)+8) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Pz, m' - InitOut%LinNames_x(m%BodyStateIs1(l)+9) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' rot_x, rad' - InitOut%LinNames_x(m%BodyStateIs1(l)+10)= 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' rot_y, rad' - InitOut%LinNames_x(m%BodyStateIs1(l)+11)= 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' rot_z, rad' + ! NOTE: the order is different than the order of the internal states. This is to + ! match what the OpenFAST framework is expecting: all positions first, then all + ! derviatives of positions (velocity terms) second. This adds slight complexity + ! here, but considerably simplifies post processing of the full OpenFAST results + ! for linearization. + ! The p%dxIdx_map2_xStateIdx array holds the index for the x%states array + ! corresponding to the current jacobian index. + + !----------------- + ! position states + !----------------- + idx = 0 + ! Free bodies + DO l = 1,p%nFreeBodies ! Body m%BodyList(m%FreeBodyIs(l)) + p%dx(idx+1:idx+3) = 0.2 ! body displacement [m] + p%dx(idx+4:idx+6) = 0.02 ! body rotation [rad] + ! corresponds to state indices: (m%BodyStateIs1(l)+6:m%BodyStateIs1(l)+11) + InitOut%LinNames_x(idx+1) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Px, m' + InitOut%LinNames_x(idx+2) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Py, m' + InitOut%LinNames_x(idx+3) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Pz, m' + InitOut%LinNames_x(idx+4) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' rot_x, rad' + InitOut%LinNames_x(idx+5) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' rot_y, rad' + InitOut%LinNames_x(idx+6) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' rot_z, rad' + p%dxIdx_map2_xStateIdx(idx+1) = m%BodyStateIs1(l)+6 ! x%state index for Px + p%dxIdx_map2_xStateIdx(idx+2) = m%BodyStateIs1(l)+7 ! x%state index for Py + p%dxIdx_map2_xStateIdx(idx+3) = m%BodyStateIs1(l)+8 ! x%state index for Pz + p%dxIdx_map2_xStateIdx(idx+4) = m%BodyStateIs1(l)+9 ! x%state index for rot_x + p%dxIdx_map2_xStateIdx(idx+5) = m%BodyStateIs1(l)+10 ! x%state index for rot_y + p%dxIdx_map2_xStateIdx(idx+6) = m%BodyStateIs1(l)+11 ! x%state index for rot_z + idx = idx + 6 END DO - DO l = 1,p%nFreeRods ! Rod m%RodList(m%FreeRodIs(l)) - if (m%RodList(m%FreeRodIs(l))%typeNum == 1) then ! pinned rod - p%dx( m%RodStateIs1(l) : m%RodStateIs1(l)+2 ) = 0.1 ! body rotational velocity [rad/s] - p%dx( m%RodStateIs1(l)+3 : m%RodStateIs1(l)+5 ) = 0.02 ! body rotation [rad] - InitOut%LinNames_x(m%RodStateIs1(l) ) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_x, rad/s' - InitOut%LinNames_x(m%RodStateIs1(l)+1) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_y, rad/s' - InitOut%LinNames_x(m%RodStateIs1(l)+2) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_z, rad/s' - InitOut%LinNames_x(m%RodStateIs1(l)+3) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_x, rad' - InitOut%LinNames_x(m%RodStateIs1(l)+4) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_y, rad' - InitOut%LinNames_x(m%RodStateIs1(l)+5) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_z, rad' - else ! free rod - p%dx( m%RodStateIs1(l) : m%RodStateIs1(l)+2 ) = 0.1 ! body translational velocity [m/s] - p%dx( m%RodStateIs1(l)+3 : m%RodStateIs1(l)+5 ) = 0.02 ! body rotational velocity [rad/s] - p%dx( m%RodStateIs1(l)+6 : m%RodStateIs1(l)+8 ) = 0.1 ! body displacement [m] - p%dx( m%RodStateIs1(l)+9 : m%RodStateIs1(l)+11) = 0.02 ! body rotation [rad] - InitOut%LinNames_x(m%RodStateIs1(l) ) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Vx, m/s' - InitOut%LinNames_x(m%RodStateIs1(l)+1) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Vy, m/s' - InitOut%LinNames_x(m%RodStateIs1(l)+2) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Vz, m/s' - InitOut%LinNames_x(m%RodStateIs1(l)+3) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_x, rad/s' - InitOut%LinNames_x(m%RodStateIs1(l)+4) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_y, rad/s' - InitOut%LinNames_x(m%RodStateIs1(l)+5) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_z, rad/s' - InitOut%LinNames_x(m%RodStateIs1(l)+6) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Px, m' - InitOut%LinNames_x(m%RodStateIs1(l)+7) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Py, m' - InitOut%LinNames_x(m%RodStateIs1(l)+8) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Pz, m' - InitOut%LinNames_x(m%RodStateIs1(l)+9) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_x, rad' - InitOut%LinNames_x(m%RodStateIs1(l)+10)= 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_y, rad' - InitOut%LinNames_x(m%RodStateIs1(l)+11)= 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_z, rad' + + ! Rods + DO l = 1,p%nFreeRods ! Rod m%RodList(m%FreeRodIs(l)) + if (m%RodList(m%FreeRodIs(l))%typeNum == 1) then ! pinned rod + p%dx(idx+1:idx+3) = 0.02 ! body rotation [rad] + ! corresponds to state indices: (m%RodStateIs1(l)+3:m%RodStateIs1(l)+5) + InitOut%LinNames_x(idx+1) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_x, rad' + InitOut%LinNames_x(idx+2) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_y, rad' + InitOut%LinNames_x(idx+3) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_z, rad' + p%dxIdx_map2_xStateIdx(idx+4) = m%RodStateIs1(l)+3 ! x%state index for rot_x + p%dxIdx_map2_xStateIdx(idx+5) = m%RodStateIs1(l)+4 ! x%state index for rot_y + p%dxIdx_map2_xStateIdx(idx+6) = m%RodStateIs1(l)+5 ! x%state index for rot_z + idx = idx + 3 + else ! free rod + p%dx(idx+1:idx+3) = 0.1 ! body displacement [m] + p%dx(idx+4:idx+6) = 0.02 ! body rotation [rad] + ! corresponds to state indices: (m%RodStateIs1(l)+6:m%RodStateIs1(l)+11) + InitOut%LinNames_x(idx+1) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Px, m' + InitOut%LinNames_x(idx+2) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Py, m' + InitOut%LinNames_x(idx+3) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Pz, m' + InitOut%LinNames_x(idx+4) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_x, rad' + InitOut%LinNames_x(idx+5) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_y, rad' + InitOut%LinNames_x(idx+6) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_z, rad' + p%dxIdx_map2_xStateIdx(idx+1) = m%RodStateIs1(l)+6 ! x%state index for Px + p%dxIdx_map2_xStateIdx(idx+2) = m%RodStateIs1(l)+7 ! x%state index for Py + p%dxIdx_map2_xStateIdx(idx+3) = m%RodStateIs1(l)+8 ! x%state index for Pz + p%dxIdx_map2_xStateIdx(idx+4) = m%RodStateIs1(l)+9 ! x%state index for rot_x + p%dxIdx_map2_xStateIdx(idx+5) = m%RodStateIs1(l)+10 ! x%state index for rot_y + p%dxIdx_map2_xStateIdx(idx+6) = m%RodStateIs1(l)+11 ! x%state index for rot_z + idx = idx + 6 end if END DO - DO l = 1,p%nFreeCons ! Point m%ConnectList(m%FreeConIs(l)) - p%dx( m%ConStateIs1(l) : m%ConStateIs1(l)+2 ) = 0.1 ! point translational velocity [m/s] - p%dx( m%ConStateIs1(l)+3 : m%ConStateIs1(l)+5 ) = 0.1 ! point displacement [m] - InitOut%LinNames_x(m%RodStateIs1(l) ) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Vx, m/s' - InitOut%LinNames_x(m%RodStateIs1(l)+1) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Vy, m/s' - InitOut%LinNames_x(m%RodStateIs1(l)+2) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Vz, m/s' - InitOut%LinNames_x(m%RodStateIs1(l)+3) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Px, m' - InitOut%LinNames_x(m%RodStateIs1(l)+4) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Py, m' - InitOut%LinNames_x(m%RodStateIs1(l)+5) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Pz, m' - END DO - DO l = 1,p%nLines ! Line m%LineList(l) - N = m%LineList(l)%N ! number of segments in the line - p%dx( m%LineStateIs1(l) : m%LineStateIs1(l)+3*N-4 ) = 0.1 ! line internal node translational velocity [m/s] - p%dx( m%LineStateIs1(l)+3*N-3 : m%LineStateIs1(l)+6*N-7 ) = 0.1 ! line internal node displacement [m] + + ! Free Connnections + DO l = 1,p%nFreeCons ! Point m%ConnectList(m%FreeConIs(l)) + ! corresponds to state indices: (m%ConStateIs1(l)+3:m%ConStateIs1(l)+5) + p%dx(idx+1:idx+3) = 0.1 ! point displacement [m] + InitOut%LinNames_x(idx+1) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Px, m' + InitOut%LinNames_x(idx+2) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Py, m' + InitOut%LinNames_x(idx+3) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Pz, m' + p%dxIdx_map2_xStateIdx(idx+1) = m%ConStateIs1(l)+3 ! x%state index for Px + p%dxIdx_map2_xStateIdx(idx+2) = m%ConStateIs1(l)+4 ! x%state index for Py + p%dxIdx_map2_xStateIdx(idx+3) = m%ConStateIs1(l)+5 ! x%state index for Pz + idx = idx + 3 + END DO + + ! Lines + DO l = 1,p%nLines ! Line m%LineList(l) + ! corresponds to state indices: (m%LineStateIs1(l)+3*N-3:m%LineStateIs1(l)+6*N-7) -- NOTE: end nodes not included + N = m%LineList(l)%N ! number of segments in the line DO i = 0,N-2 - InitOut%LinNames_x( m%LineStateIs1(l) +3*i ) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Vx, m/s' - InitOut%LinNames_x( m%LineStateIs1(l) +3*i+1) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Vy, m/s' - InitOut%LinNames_x( m%LineStateIs1(l) +3*i+2) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Vz, m/s' - InitOut%LinNames_x( m%LineStateIs1(l)+3*N +3*i-3) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Px, m' - InitOut%LinNames_x( m%LineStateIs1(l)+3*N +3*i-2) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Py, m' - InitOut%LinNames_x( m%LineStateIs1(l)+3*N +3*i-1) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Pz, m' + p%dx(idx+1:idx+3) = 0.1 ! line internal node displacement [m] + InitOut%LinNames_x(idx+1) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Px, m' + InitOut%LinNames_x(idx+2) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Py, m' + InitOut%LinNames_x(idx+3) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Pz, m' + p%dxIdx_map2_xStateIdx(idx+1) = m%LineStateIs1(l)+3*N+3*i-3 ! x%state index for Px + p%dxIdx_map2_xStateIdx(idx+2) = m%LineStateIs1(l)+3*N+3*i-2 ! x%state index for Py + p%dxIdx_map2_xStateIdx(idx+3) = m%LineStateIs1(l)+3*N+3*i-1 ! x%state index for Pz + idx = idx + 3 END DO END DO - + + !----------------- + ! velocity states + !----------------- + ! Free bodies + DO l = 1,p%nFreeBodies ! Body m%BodyList(m%FreeBodyIs(l)) + ! corresponds to state indices: (m%BodyStateIs1(l):m%BodyStateIs1(l)+5) + p%dx(idx+1:idx+3) = 0.1 ! body translational velocity [m/s] + p%dx(idx+4:idx+6) = 0.1 ! body rotational velocity [rad/s] + InitOut%LinNames_x(idx+1) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Vx, m/s' + InitOut%LinNames_x(idx+2) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Vy, m/s' + InitOut%LinNames_x(idx+3) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Vz, m/s' + InitOut%LinNames_x(idx+4) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' omega_x, rad/s' + InitOut%LinNames_x(idx+5) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' omega_y, rad/s' + InitOut%LinNames_x(idx+6) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' omega_z, rad/s' + p%dxIdx_map2_xStateIdx(idx+1) = m%BodyStateIs1(l)+0 ! x%state index for Rx + p%dxIdx_map2_xStateIdx(idx+2) = m%BodyStateIs1(l)+1 ! x%state index for Ry + p%dxIdx_map2_xStateIdx(idx+3) = m%BodyStateIs1(l)+2 ! x%state index for Rz + p%dxIdx_map2_xStateIdx(idx+4) = m%BodyStateIs1(l)+3 ! x%state index for omega_x + p%dxIdx_map2_xStateIdx(idx+5) = m%BodyStateIs1(l)+4 ! x%state index for omega_y + p%dxIdx_map2_xStateIdx(idx+6) = m%BodyStateIs1(l)+5 ! x%state index for omega_z + idx = idx + 6 + END DO + + ! Rods + DO l = 1,p%nFreeRods ! Rod m%RodList(m%FreeRodIs(l)) + if (m%RodList(m%FreeRodIs(l))%typeNum == 1) then ! pinned rod + ! corresponds to state indices: (m%RodStateIs1(l):m%RodStateIs1(l)+2) + p%dx(idx+1:idx+3) = 0.1 ! body rotational velocity [rad/s] + InitOut%LinNames_x(idx+1) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_x, rad/s' + InitOut%LinNames_x(idx+2) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_y, rad/s' + InitOut%LinNames_x(idx+3) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_z, rad/s' + p%dxIdx_map2_xStateIdx(idx+1) = m%RodStateIs1(l)+0 ! x%state index for Vx + p%dxIdx_map2_xStateIdx(idx+2) = m%RodStateIs1(l)+1 ! x%state index for Vy + p%dxIdx_map2_xStateIdx(idx+3) = m%RodStateIs1(l)+2 ! x%state index for Vz + idx = idx + 3 + else ! free rod + ! corresponds to state indices: (m%RodStateIs1(l):m%RodStateIs1(l)+5) + p%dx(idx+1:idx+3) = 0.1 ! body translational velocity [m/s] + p%dx(idx+4:idx+6) = 0.02 ! body rotational velocity [rad/s] + InitOut%LinNames_x(idx+1) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Vx, m/s' + InitOut%LinNames_x(idx+2) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Vy, m/s' + InitOut%LinNames_x(idx+3) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Vz, m/s' + InitOut%LinNames_x(idx+4) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_x, rad/s' + InitOut%LinNames_x(idx+5) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_y, rad/s' + InitOut%LinNames_x(idx+6) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' omega_z, rad/s' + p%dxIdx_map2_xStateIdx(idx+1) = m%RodStateIs1(l)+0 ! x%state index for Vx + p%dxIdx_map2_xStateIdx(idx+2) = m%RodStateIs1(l)+1 ! x%state index for Vy + p%dxIdx_map2_xStateIdx(idx+3) = m%RodStateIs1(l)+2 ! x%state index for Vz + p%dxIdx_map2_xStateIdx(idx+4) = m%RodStateIs1(l)+3 ! x%state index for omega_x + p%dxIdx_map2_xStateIdx(idx+5) = m%RodStateIs1(l)+4 ! x%state index for omega_y + p%dxIdx_map2_xStateIdx(idx+6) = m%RodStateIs1(l)+5 ! x%state index for omega_z + idx = idx + 6 + end if + END DO + + ! Free Connnections + DO l = 1,p%nFreeCons ! Point m%ConnectList(m%FreeConIs(l)) + ! corresponds to state indices: (m%ConStateIs1(l):m%ConStateIs1(l)+2) + p%dx(idx+1:idx+3) = 0.1 ! point translational velocity [m/s] + InitOut%LinNames_x(idx+1) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Vx, m/s' + InitOut%LinNames_x(idx+2) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Vy, m/s' + InitOut%LinNames_x(idx+3) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Vz, m/s' + p%dxIdx_map2_xStateIdx(idx+1) = m%ConStateIs1(l)+0 ! x%state index for Vx + p%dxIdx_map2_xStateIdx(idx+2) = m%ConStateIs1(l)+1 ! x%state index for Vy + p%dxIdx_map2_xStateIdx(idx+3) = m%ConStateIs1(l)+2 ! x%state index for Vz + idx = idx + 3 + END DO + + ! Lines + DO l = 1,p%nLines ! Line m%LineList(l) + ! corresponds to state indices: (m%LineStateIs1(l):m%LineStateIs1(l)+3*N-4) -- NOTE: end nodes not included + N = m%LineList(l)%N ! number of segments in the line + DO i = 0,N-2 + p%dx(idx+1:idx+3) = 0.1 ! line internal node translational velocity [m/s] + InitOut%LinNames_x(idx+1) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Vx, m/s' + InitOut%LinNames_x(idx+2) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Vy, m/s' + InitOut%LinNames_x(idx+3) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Vz, m/s' + p%dxIdx_map2_xStateIdx(idx+1) = m%LineStateIs1(l)+3*i+0 ! x%state index for Vx + p%dxIdx_map2_xStateIdx(idx+2) = m%LineStateIs1(l)+3*i+1 ! x%state index for Vy + p%dxIdx_map2_xStateIdx(idx+3) = m%LineStateIs1(l)+3*i+2 ! x%state index for Vz + idx = idx + 3 + END DO + END DO + + ! If a summary file is ever made... + ! !Formatting may be needed to make it pretty + ! if(UnSum > 0) then + ! write(UnSum,*) ' Lin_Jac_x idx x%state idx' + ! do i=1,p%Jac_nx + ! write(UnSum,*) InitOut%LinNames_x(i),' ',i,' ',p%dxIdx_map2_xStateIdx(i) + ! enddo + ! endif + InitOut%RotFrame_x = .false. InitOut%DerivOrder_x = 2 END SUBROUTINE Init_Jacobian_x @@ -3769,9 +3894,9 @@ END SUBROUTINE MD_Compute_dY !! Do not change this without making sure subroutine MD_init_jacobian is consistant with this routine! SUBROUTINE MD_Perturb_x( p, i, perturb_sign, x, dx ) TYPE(MD_ParameterType) , INTENT(IN ) :: p !< parameters - INTEGER( IntKi ) , INTENT(IN ) :: i !< node number + INTEGER( IntKi ) , INTENT(IN ) :: i !< state array index number INTEGER( IntKi ) , INTENT(IN ) :: perturb_sign !< +1 or -1 (value to multiply perturbation by; positive or negative difference) - TYPE(MD_ContinuousStateType), INTENT(INOUT) :: x !< perturbed SD states + TYPE(MD_ContinuousStateType), INTENT(INOUT) :: x !< perturbed MD states REAL( R8Ki ) , INTENT( OUT) :: dx !< amount that specific state was perturbed dx=p%dx(i) @@ -3781,14 +3906,15 @@ END SUBROUTINE MD_Perturb_x !> This routine uses values of two output types to compute an array of differences. !! Do not change this packing without making sure subroutine MD_init_jacobian is consistant with this routine! SUBROUTINE MD_Compute_dX(p, x_p, x_m, delta, dX) - TYPE(MD_ParameterType) , INTENT(IN ) :: p !< parameters - TYPE(MD_ContinuousStateType), INTENT(IN ) :: x_p !< SD continuous states at \f$ u + \Delta_p u \f$ or \f$ x + \Delta_p x \f$ (p=plus) - TYPE(MD_ContinuousStateType), INTENT(IN ) :: x_m !< SD continuous states at \f$ u - \Delta_m u \f$ or \f$ x - \Delta_m x \f$ (m=minus) - REAL(R8Ki) , INTENT(IN ) :: delta !< difference in inputs or states \f$ delta_p = \Delta_p u \f$ or \f$ delta_p = \Delta_p x \f$ - REAL(R8Ki) , INTENT(INOUT) :: dX(:) !< column of dXdu or dXdx: \f$ \frac{\partial X}{\partial u_i} = \frac{x_p - x_m}{2 \, \Delta u}\f$ or \f$ \frac{\partial X}{\partial x_i} = \frac{x_p - x_m}{2 \, \Delta x}\f$ + TYPE(MD_ParameterType) , INTENT(IN ) :: p !< parameters + TYPE(MD_ContinuousStateType), INTENT(IN ) :: x_p !< This routine forms the dU^{MD}/du^{MD} block of dUdu. (i.e., how do changes in the MD outputs affect +!! the MD inputs?) +SUBROUTINE Linear_MD_InputSolve_du( p_FAST, y_FAST, u_MD, y_ED, y_SD, MeshMapData, dUdu, ErrStat, ErrMsg ) + + ! Passed variables + TYPE(FAST_ParameterType), INTENT(IN ) :: p_FAST !< FAST parameter data + TYPE(FAST_OutputFileType), INTENT(IN ) :: y_FAST !< FAST output file data (for linearization) + TYPE(MD_InputType), INTENT(INOUT) :: u_MD !< The inputs to MoorDyn + TYPE(ED_OutputType), INTENT(IN) :: y_ED !< The outputs from the ElastoDyn structural dynamics module + TYPE(SD_OutputType), INTENT(IN) :: y_SD !< The outputs from the SubDyn structural dynamics module + TYPE(FAST_ModuleMapType), INTENT(INOUT) :: MeshMapData !< Data for mapping between modules + REAL(R8Ki), INTENT(INOUT) :: dUdu(:,:) !< Jacobian matrix of which we are computing the dU^{MD}/dy^{ED} block + + INTEGER(IntKi) :: ErrStat !< Error status of the operation + CHARACTER(*) :: ErrMsg !< Error message if ErrStat /= ErrID_None + + ! Local variables: + + INTEGER(IntKi) :: MD_Start_td ! starting index of dUdu (column) where particular MD fields are located + INTEGER(IntKi) :: MD_Start_tr ! starting index of dUdu (row) where particular MD fields are located + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'Linear_MD_InputSolve_du' + + + ErrStat = ErrID_None + ErrMsg = "" + IF (u_MD%CoupledKinematics%Committed) THEN + !................................... + ! FairLead Mesh + !................................... + + if ( p_FAST%CompSub == Module_SD ) THEN + ! dU^{MD}/du^{MD} + call Linearize_Point_to_Point( y_SD%Y3Mesh, u_MD%CoupledKinematics, MeshMapData%SDy3_P_2_Mooring_P, ErrStat2, ErrMsg2 ) + + ! MD is destination in the mapping, so we want M_{tv_uD} and M_{ta_uD} + MD_Start_td = y_FAST%Lin%Modules(MODULE_MD)%Instance(1)%LinStartIndx(LIN_INPUT_COL) + MD_Start_tr = MD_Start_td + u_MD%CoupledKinematics%NNodes * 6 ! skip 2 fields (TranslationDisp and Orientation) with 3 components before translational velocity field + + ! translational velocity: + if (allocated(MeshMapData%SDy3_P_2_Mooring_P%dM%tv_uD )) then + call SetBlockMatrix( dUdu, MeshMapData%SDy3_P_2_Mooring_P%dM%tv_ud, MD_Start_tr, MD_Start_td ) + end if + + ! translational acceleration: + MD_Start_tr = MD_Start_tr + u_MD%CoupledKinematics%NNodes * 6 ! skip 2 fields ( TranslationVel and RotationVel) + if (allocated(MeshMapData%SDy3_P_2_Mooring_P%dM%ta_uD )) then + call SetBlockMatrix( dUdu, MeshMapData%SDy3_P_2_Mooring_P%dM%ta_ud, MD_Start_tr, MD_Start_td ) + end if + + else if ( p_FAST%CompSub == Module_None ) THEN + ! dU^{MD}/du^{MD} + call Linearize_Point_to_Point( y_ED%PlatformPtMesh, u_MD%CoupledKinematics, MeshMapData%ED_P_2_Mooring_P, ErrStat2, ErrMsg2 ) + + ! MD is destination in the mapping, so we want M_{tv_uD} and M_{ta_uD} + MD_Start_td = y_FAST%Lin%Modules(MODULE_MD)%Instance(1)%LinStartIndx(LIN_INPUT_COL) + MD_Start_tr = MD_Start_td + u_MD%CoupledKinematics%NNodes * 6 ! skip 2 fields (TranslationDisp and Orientation) with 3 components before translational velocity field + + ! translational velocity: + if (allocated(MeshMapData%ED_P_2_Mooring_P%dM%tv_uD )) then + call SetBlockMatrix( dUdu, MeshMapData%ED_P_2_Mooring_P%dM%tv_ud, MD_Start_tr, MD_Start_td ) + end if + + ! translational acceleration: + MD_Start_tr = MD_Start_tr + u_MD%CoupledKinematics%NNodes * 6 ! skip 2 fields ( TranslationVel and RotationVel) + if (allocated(MeshMapData%ED_P_2_Mooring_P%dM%ta_uD )) then + call SetBlockMatrix( dUdu, MeshMapData%ED_P_2_Mooring_P%dM%ta_ud, MD_Start_tr, MD_Start_td ) + end if + + end if + + + END IF +END SUBROUTINE Linear_MD_InputSolve_du + !---------------------------------------------------------------------------------------------------------------------------------- !> This routine forms the dU^{MD}/dy^{ED} block of dUdy. (i.e., how do changes in the ED outputs affect !! the MD inputs?) @@ -4073,8 +4150,6 @@ SUBROUTINE Linear_MD_InputSolve_dy( p_FAST, y_FAST, u_MD, y_ED, y_SD, MeshMapDat INTEGER(IntKi) :: MD_Start ! starting index of dUdy (column) where particular MD fields are located INTEGER(IntKi) :: ED_Out_Start! starting index of dUdy (row) where particular ED fields are located INTEGER(IntKi) :: SD_Out_Start! starting index of dUdy (row) where particular SD fields are located - INTEGER(IntKi) :: ErrStat2 - CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'Linear_MD_InputSolve_dy' @@ -4089,15 +4164,18 @@ SUBROUTINE Linear_MD_InputSolve_dy( p_FAST, y_FAST, u_MD, y_ED, y_SD, MeshMapDat if ( p_FAST%CompSub == Module_SD ) THEN ! dU^{MD}/dy^{SD} + + !!! ! This linearization was done in forming dUdu (see Linear_MD_InputSolve_du()), so we don't need to re-calculate these matrices + !!! ! while forming dUdy, too. + !!!call Linearize_Point_to_Point( y_SD%Y3Mesh, u_MD%CoupledKinematics(1), MeshMapData%SD_P_2_Mooring_P, ErrStat2, ErrMsg2 ) + SD_Out_Start = Indx_y_SD_Y3Mesh_Start(y_SD, y_FAST) ! start of y_SD%Y3Mesh%TranslationDisp field - call Linearize_Point_to_Point( y_SD%Y3Mesh, u_MD%CoupledKinematics(1), MeshMapData%SDy3_P_2_Mooring_P, ErrStat2, ErrMsg2 ) - call Assemble_dUdy_Motions( y_SD%Y3Mesh, u_MD%CoupledKinematics(1), MeshMapData%SDy3_P_2_Mooring_P, MD_Start, SD_Out_Start, dUdy, OnlyTranslationDisp=.true.) + call Assemble_dUdy_Motions( y_SD%Y3Mesh, u_MD%CoupledKinematics(1), MeshMapData%SDy3_P_2_Mooring_P, MD_Start, SD_Out_Start, dUdy) ! OnlyTranslationDisp=.true. else if ( p_FAST%CompSub == Module_None ) THEN ! dU^{MD}/dy^{ED} ED_Out_Start = Indx_y_ED_Platform_Start(y_ED, y_FAST) ! start of y_ED%PlatformPtMesh%TranslationDisp field - call Linearize_Point_to_Point( y_ED%PlatformPtMesh, u_MD%CoupledKinematics(1), MeshMapData%ED_P_2_Mooring_P, ErrStat2, ErrMsg2 ) - call Assemble_dUdy_Motions( y_ED%PlatformPtMesh, u_MD%CoupledKinematics(1), MeshMapData%ED_P_2_Mooring_P, MD_Start, ED_Out_Start, dUdy, OnlyTranslationDisp=.true.) + call Assemble_dUdy_Motions(y_ED%PlatformPtMesh, u_MD%CoupledKinematics(1), MeshMapData%ED_P_2_Mooring_P, MD_Start, ED_Out_Start, dUdy) ! OnlyTranslationDisp=.true. end if From 56604b1160bd2edb940aa3b8e2a635b9170f5eb0 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Mon, 14 Feb 2022 16:41:03 -0700 Subject: [PATCH 091/242] Adding MoorDyn driver visual studio files --- vs-build/MoorDyn/MoorDynDriver.sln | 31 +++++ vs-build/MoorDyn/MoorDynDriver.vfproj | 167 ++++++++++++++++++++++++++ 2 files changed, 198 insertions(+) create mode 100644 vs-build/MoorDyn/MoorDynDriver.sln create mode 100644 vs-build/MoorDyn/MoorDynDriver.vfproj diff --git a/vs-build/MoorDyn/MoorDynDriver.sln b/vs-build/MoorDyn/MoorDynDriver.sln new file mode 100644 index 0000000000..93af1f3831 --- /dev/null +++ b/vs-build/MoorDyn/MoorDynDriver.sln @@ -0,0 +1,31 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 16 +VisualStudioVersion = 16.0.31613.86 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{6989167D-11E4-40FE-8C1A-2192A86A7E90}") = "MoorDynDriver", "MoorDynDriver.vfproj", "{815C302F-A93D-4C22-9329-717B085113C0}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {815C302F-A93D-4C22-9329-717B085113C0}.Debug|x64.ActiveCfg = Debug|x64 + {815C302F-A93D-4C22-9329-717B085113C0}.Debug|x64.Build.0 = Debug|x64 + {815C302F-A93D-4C22-9329-717B085113C0}.Debug|x86.ActiveCfg = Debug|Win32 + {815C302F-A93D-4C22-9329-717B085113C0}.Debug|x86.Build.0 = Debug|Win32 + {815C302F-A93D-4C22-9329-717B085113C0}.Release|x64.ActiveCfg = Release|x64 + {815C302F-A93D-4C22-9329-717B085113C0}.Release|x64.Build.0 = Release|x64 + {815C302F-A93D-4C22-9329-717B085113C0}.Release|x86.ActiveCfg = Release|Win32 + {815C302F-A93D-4C22-9329-717B085113C0}.Release|x86.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {8141B2B8-9857-455F-978C-8B086DDB3E6A} + EndGlobalSection +EndGlobal diff --git a/vs-build/MoorDyn/MoorDynDriver.vfproj b/vs-build/MoorDyn/MoorDynDriver.vfproj new file mode 100644 index 0000000000..2396eb9279 --- /dev/null +++ b/vs-build/MoorDyn/MoorDynDriver.vfproj @@ -0,0 +1,167 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From e8f1a7bc4984db9b9485d155e55665a90050eb0e Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Mon, 23 Aug 2021 16:36:33 -0600 Subject: [PATCH 092/242] MD lin: missing argument to MD_JacobianPInput --- modules/openfast-library/src/FAST_Lin.f90 | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/modules/openfast-library/src/FAST_Lin.f90 b/modules/openfast-library/src/FAST_Lin.f90 index 41ce159df3..b6e079c61d 100644 --- a/modules/openfast-library/src/FAST_Lin.f90 +++ b/modules/openfast-library/src/FAST_Lin.f90 @@ -1116,7 +1116,9 @@ SUBROUTINE FAST_Linearize_OP(t_global, p_FAST, y_FAST, m_FAST, ED, BD, SrvD, AD, if ( p_FAST%CompMooring == Module_MD ) then call MD_JacobianPInput( t_global, MD%Input(1), MD%p, MD%x(STATE_CURR), MD%xd(STATE_CURR), MD%z(STATE_CURR), & - MD%OtherSt(STATE_CURR), MD%y, MD%m, ErrStat2, ErrMsg2, y_FAST%Lin%Modules(Module_MD)%Instance(1)%D ) + MD%OtherSt(STATE_CURR), MD%y, MD%m, ErrStat2, ErrMsg2, & + dXdu=y_FAST%Lin%Modules(Module_MD)%Instance(1)%B, & + dYdu=y_FAST%Lin%Modules(Module_MD)%Instance(1)%D ) call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) call MD_JacobianPContState( t_global, MD%Input(1), MD%p, MD%x(STATE_CURR), MD%xd(STATE_CURR), MD%z(STATE_CURR), MD%OtherSt(STATE_CURR), & From ccdf44d654f7a71663c4c8aa67592e366962f4a7 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Thu, 19 Aug 2021 10:39:18 -0600 Subject: [PATCH 093/242] MD Lin: add du{MD}/du{MD} term (accel + vel terms) --- modules/openfast-library/src/FAST_Lin.f90 | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/modules/openfast-library/src/FAST_Lin.f90 b/modules/openfast-library/src/FAST_Lin.f90 index b6e079c61d..ab44c44d7c 100644 --- a/modules/openfast-library/src/FAST_Lin.f90 +++ b/modules/openfast-library/src/FAST_Lin.f90 @@ -1768,6 +1768,14 @@ SUBROUTINE Glue_Jacobians( p_FAST, y_FAST, m_FAST, ED, BD, SrvD, AD, IfW, OpFM, CALL WrScr('>>> FAST_LIN: Linear_ExtPtfm_InputSolve_du, TODO') ENDIF + !............ + ! \f$ \frac{\partial U_\Lambda^{MD}}{\partial u^{MD}} \end{bmatrix} = \f$ (dUdu block row 9=MD) <<<< + !............ + if (p_FAST%CompMooring == MODULE_MD) then + call Linear_MD_InputSolve_du( p_FAST, y_FAST, MD%Input(1), ED%y, SD%y, MeshMapData, dUdu, ErrStat2, ErrMsg2 ) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + end if + ! LIN-TODO: Update the doc lines below to include SrvD, HD, SD, and MAP !..................................... ! dUdy @@ -4119,7 +4127,7 @@ SUBROUTINE Linear_MD_InputSolve_du( p_FAST, y_FAST, u_MD, y_ED, y_SD, MeshMapDat end if ! translational acceleration: - MD_Start_tr = MD_Start_tr + u_MD%CoupledKinematics%NNodes * 6 ! skip 2 fields ( TranslationVel and RotationVel) + MD_Start_tr = MD_Start_tr + u_MD%CoupledKinematics%NNodes * 6 ! skip 2 fields (TranslationDisp and Orientation) with 3 components before translational velocity field if (allocated(MeshMapData%ED_P_2_Mooring_P%dM%ta_uD )) then call SetBlockMatrix( dUdu, MeshMapData%ED_P_2_Mooring_P%dM%ta_ud, MD_Start_tr, MD_Start_td ) end if @@ -4172,12 +4180,16 @@ SUBROUTINE Linear_MD_InputSolve_dy( p_FAST, y_FAST, u_MD, y_ED, y_SD, MeshMapDat !!!call Linearize_Point_to_Point( y_SD%Y3Mesh, u_MD%CoupledKinematics(1), MeshMapData%SD_P_2_Mooring_P, ErrStat2, ErrMsg2 ) SD_Out_Start = Indx_y_SD_Y3Mesh_Start(y_SD, y_FAST) ! start of y_SD%Y3Mesh%TranslationDisp field - call Assemble_dUdy_Motions( y_SD%Y3Mesh, u_MD%CoupledKinematics(1), MeshMapData%SDy3_P_2_Mooring_P, MD_Start, SD_Out_Start, dUdy) ! OnlyTranslationDisp=.true. + call Assemble_dUdy_Motions( y_SD%Y3Mesh, u_MD%CoupledKinematics(1), MeshMapData%SDy3_P_2_Mooring_P, MD_Start, SD_Out_Start, dUdy, OnlyTranslationDisp=.false.) else if ( p_FAST%CompSub == Module_None ) THEN ! dU^{MD}/dy^{ED} + !!! ! This linearization was done in forming dUdu (see Linear_MD_InputSolve_du()), so we don't need to re-calculate these matrices + !!! ! while forming dUdy, too. + !!!call Linearize_Point_to_Point( y_ED%PlatformPtMesh, u_MD%CoupledKinematics, MeshMapData%ED_P_2_Mooring_P, ErrStat2, ErrMsg2 ) + ED_Out_Start = Indx_y_ED_Platform_Start(y_ED, y_FAST) ! start of y_ED%PlatformPtMesh%TranslationDisp field - call Assemble_dUdy_Motions(y_ED%PlatformPtMesh, u_MD%CoupledKinematics(1), MeshMapData%ED_P_2_Mooring_P, MD_Start, ED_Out_Start, dUdy) ! OnlyTranslationDisp=.true. + call Assemble_dUdy_Motions(y_ED%PlatformPtMesh, u_MD%CoupledKinematics(1), MeshMapData%ED_P_2_Mooring_P, MD_Start, ED_Out_Start, dUdy, OnlyTranslationDisp=.false.) end if From e5cb26d2405bcee0cd273f24ef9f6a9c0cf56f56 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Mon, 14 Feb 2022 18:15:31 -0700 Subject: [PATCH 094/242] Adjust merged FAST_Lin additions for MDv2-farm compatibility --- modules/openfast-library/src/FAST_Lin.f90 | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/modules/openfast-library/src/FAST_Lin.f90 b/modules/openfast-library/src/FAST_Lin.f90 index ab44c44d7c..2c526e830d 100644 --- a/modules/openfast-library/src/FAST_Lin.f90 +++ b/modules/openfast-library/src/FAST_Lin.f90 @@ -4089,18 +4089,18 @@ SUBROUTINE Linear_MD_InputSolve_du( p_FAST, y_FAST, u_MD, y_ED, y_SD, MeshMapDat ErrStat = ErrID_None ErrMsg = "" - IF (u_MD%CoupledKinematics%Committed) THEN + IF (u_MD%CoupledKinematics(1)%Committed) THEN !................................... ! FairLead Mesh !................................... if ( p_FAST%CompSub == Module_SD ) THEN ! dU^{MD}/du^{MD} - call Linearize_Point_to_Point( y_SD%Y3Mesh, u_MD%CoupledKinematics, MeshMapData%SDy3_P_2_Mooring_P, ErrStat2, ErrMsg2 ) + call Linearize_Point_to_Point( y_SD%Y3Mesh, u_MD%CoupledKinematics(1), MeshMapData%SDy3_P_2_Mooring_P, ErrStat2, ErrMsg2 ) ! MD is destination in the mapping, so we want M_{tv_uD} and M_{ta_uD} MD_Start_td = y_FAST%Lin%Modules(MODULE_MD)%Instance(1)%LinStartIndx(LIN_INPUT_COL) - MD_Start_tr = MD_Start_td + u_MD%CoupledKinematics%NNodes * 6 ! skip 2 fields (TranslationDisp and Orientation) with 3 components before translational velocity field + MD_Start_tr = MD_Start_td + u_MD%CoupledKinematics(1)%NNodes * 6 ! skip 2 fields (TranslationDisp and Orientation) with 3 components before translational velocity field ! translational velocity: if (allocated(MeshMapData%SDy3_P_2_Mooring_P%dM%tv_uD )) then @@ -4108,18 +4108,18 @@ SUBROUTINE Linear_MD_InputSolve_du( p_FAST, y_FAST, u_MD, y_ED, y_SD, MeshMapDat end if ! translational acceleration: - MD_Start_tr = MD_Start_tr + u_MD%CoupledKinematics%NNodes * 6 ! skip 2 fields ( TranslationVel and RotationVel) + MD_Start_tr = MD_Start_tr + u_MD%CoupledKinematics(1)%NNodes * 6 ! skip 2 fields ( TranslationVel and RotationVel) if (allocated(MeshMapData%SDy3_P_2_Mooring_P%dM%ta_uD )) then call SetBlockMatrix( dUdu, MeshMapData%SDy3_P_2_Mooring_P%dM%ta_ud, MD_Start_tr, MD_Start_td ) end if else if ( p_FAST%CompSub == Module_None ) THEN ! dU^{MD}/du^{MD} - call Linearize_Point_to_Point( y_ED%PlatformPtMesh, u_MD%CoupledKinematics, MeshMapData%ED_P_2_Mooring_P, ErrStat2, ErrMsg2 ) + call Linearize_Point_to_Point( y_ED%PlatformPtMesh, u_MD%CoupledKinematics(1), MeshMapData%ED_P_2_Mooring_P, ErrStat2, ErrMsg2 ) ! MD is destination in the mapping, so we want M_{tv_uD} and M_{ta_uD} MD_Start_td = y_FAST%Lin%Modules(MODULE_MD)%Instance(1)%LinStartIndx(LIN_INPUT_COL) - MD_Start_tr = MD_Start_td + u_MD%CoupledKinematics%NNodes * 6 ! skip 2 fields (TranslationDisp and Orientation) with 3 components before translational velocity field + MD_Start_tr = MD_Start_td + u_MD%CoupledKinematics(1)%NNodes * 6 ! skip 2 fields (TranslationDisp and Orientation) with 3 components before translational velocity field ! translational velocity: if (allocated(MeshMapData%ED_P_2_Mooring_P%dM%tv_uD )) then @@ -4127,7 +4127,7 @@ SUBROUTINE Linear_MD_InputSolve_du( p_FAST, y_FAST, u_MD, y_ED, y_SD, MeshMapDat end if ! translational acceleration: - MD_Start_tr = MD_Start_tr + u_MD%CoupledKinematics%NNodes * 6 ! skip 2 fields (TranslationDisp and Orientation) with 3 components before translational velocity field + MD_Start_tr = MD_Start_tr + u_MD%CoupledKinematics(1)%NNodes * 6 ! skip 2 fields (TranslationDisp and Orientation) with 3 components before translational velocity field if (allocated(MeshMapData%ED_P_2_Mooring_P%dM%ta_uD )) then call SetBlockMatrix( dUdu, MeshMapData%ED_P_2_Mooring_P%dM%ta_ud, MD_Start_tr, MD_Start_td ) end if From 44ef8924047cb92f2a84ecad74dd8c195869ae45 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Tue, 22 Feb 2022 16:30:36 -0700 Subject: [PATCH 095/242] MoorDyn: Improved error handling for Rods and WaterKin - Added/adjusted some error handling for Rods - Added some error handling for WaterKin input files, where previously file reading errors were not producing messages. HydroDyn/Morison: Added a catch to avoid divide be zeros when calculating centroid of tapered member segments (if somehow they have zero radius). --- modules/hydrodyn/src/Morison.f90 | 8 ++-- modules/moordyn/src/MoorDyn.f90 | 7 +++- modules/moordyn/src/MoorDyn_IO.f90 | 25 +++++++++-- modules/moordyn/src/MoorDyn_Misc.f90 | 48 ++++++++++++--------- modules/moordyn/src/MoorDyn_Registry.txt | 1 + modules/moordyn/src/MoorDyn_Rod.f90 | 53 +++++++++++++++--------- modules/moordyn/src/MoorDyn_Types.f90 | 11 +++++ 7 files changed, 106 insertions(+), 47 deletions(-) diff --git a/modules/hydrodyn/src/Morison.f90 b/modules/hydrodyn/src/Morison.f90 index 1a861ab5ea..834d0d0cb8 100644 --- a/modules/hydrodyn/src/Morison.f90 +++ b/modules/hydrodyn/src/Morison.f90 @@ -2240,9 +2240,11 @@ FUNCTION GetAlpha(R1,R2) REAL(ReKi), INTENT ( IN ) :: R1 ! interior radius of element at node point REAL(ReKi), INTENT ( IN ) :: R2 ! interior radius of other end of part-element - - GetAlpha = (R1*R1 + 2.0*R1*R2 + 3.0*R2*R2)/4.0/(R1*R1 + R1*R2 + R2*R2) - + if ( EqualRealNos(R1, 0.0_ReKi) .AND. EqualRealNos(R2, 0.0_ReKi) ) then ! if undefined, return 0 + GetAlpha = 0.0_ReKi + else + GetAlpha = (R1*R1 + 2.0*R1*R2 + 3.0*R2*R2)/4.0/(R1*R1 + R1*R2 + R2*R2) + end if END FUNCTION GetAlpha diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 07e7abdf3e..dd4621d681 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -74,6 +74,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! local variables TYPE(MD_InputFileType) :: InputFileDat ! Data read from input file for setup, but not stored after Init type(FileInfoType) :: FileInfo_In !< The derived type for holding the full input file for parsing -- we may pass this in the future + ! CHARACTER(1024) :: priPath ! The path to the primary MoorDyn input file REAL(DbKi) :: t ! instantaneous time, to be used during IC generation INTEGER(IntKi) :: l ! index INTEGER(IntKi) :: I ! Current line number of input file @@ -245,8 +246,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! Read the entire input file, minus any comment lines, into the FileInfo_In ! data structure in memory for further processing. call ProcessComFile( InitInp%FileName, FileInfo_In, ErrStat2, ErrMsg2 ) + CALL GetPath( InitInp%FileName, p%PriPath ) ! Input files will be relative to the path where the primary input file is located. else call NWTC_Library_CopyFileInfoType( InitInp%PassedPrimaryInputData, FileInfo_In, MESH_NEWCOPY, ErrStat2, ErrMsg2 ) + p%PriPath = "" endif if (Failed()) return; @@ -483,7 +486,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! set up wave and current kinematics - CALL setupWaterKin(WaterKinValue, p, InitInp%Tmax, ErrStat2, ErrMsg2) + CALL setupWaterKin(WaterKinValue, p, InitInp%Tmax, ErrStat2, ErrMsg2); if(Failed()) return @@ -2089,7 +2092,7 @@ END FUNCTION AllocateFailed LOGICAL FUNCTION Failed() - call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'MDIO_ReadInput') + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'MD_Init') Failed = ErrStat >= AbortErrLev if (Failed) call CleanUp() END FUNCTION Failed diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index 9fcf0e7cb4..0cc1b73962 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -1157,18 +1157,37 @@ SUBROUTINE MDIO_CloseOutput ( p, m, ErrStat, ErrMsg ) !FIXME: make sure thes are actually open before trying to close them. Segfault will occur otherwise!!!! ! This bug can be triggered by an early failure of the parsing routines, before these files were ever opened ! which returns MD to OpenFAST as ErrID_Fatal, then OpenFAST calls MD_End, which calls this. + ! close main MoorDyn output file - CLOSE( p%MDUnOut, IOSTAT = ErrStat ) + if (p%MDUnOut > 0) then + CLOSE( p%MDUnOut, IOSTAT = ErrStat ) IF ( ErrStat /= 0 ) THEN ErrMsg = 'Error closing output file' END IF - + end if + + ! close individual rod output files + DO I=1,p%NRods + if (allocated(m%RodList)) then + if (m%RodList(I)%RodUnOut > 0) then + CLOSE( m%RodList(I)%RodUnOut, IOSTAT = ErrStat ) + IF ( ErrStat /= 0 ) THEN + ErrMsg = 'Error closing rod output file' + END IF + end if + end if + END DO + ! close individual line output files DO I=1,p%NLines - CLOSE( m%LineList(I)%LineUnOut, IOSTAT = ErrStat ) + if (allocated(m%LineList)) then + if (m%LineList(I)%LineUnOut > 0) then + CLOSE( m%LineList(I)%LineUnOut, IOSTAT = ErrStat ) IF ( ErrStat /= 0 ) THEN ErrMsg = 'Error closing line output file' END IF + end if + end if END DO ! deallocate output arrays diff --git a/modules/moordyn/src/MoorDyn_Misc.f90 b/modules/moordyn/src/MoorDyn_Misc.f90 index 7c3df24a21..03c44a32fa 100644 --- a/modules/moordyn/src/MoorDyn_Misc.f90 +++ b/modules/moordyn/src/MoorDyn_Misc.f90 @@ -140,23 +140,19 @@ function GetCurvature(length, q1, q2) end function GetCurvature - ! calculate orientation angles of a cylindrical object + ! calculate orientation angles of a direction vector !----------------------------------------------------------------------- - subroutine GetOrientationAngles(p1, p2, phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat) - real(DbKi), intent(in ) :: p1(3),p2(3) + subroutine GetOrientationAngles(vec, phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat) + real(DbKi), intent(in ) :: vec(3) !p1(3),p2(3) real(DbKi), intent( out) :: phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat(3) - real(DbKi) :: vec(3), vecLen, vecLen2D - - ! calculate isntantaneous incline angle and heading, and related trig values - ! the first and last NodeIndx values point to the corresponding Joint nodes idices which are at the start of the Mesh - vec = p2 - p1 + real(DbKi) :: vecLen, vecLen2D + vecLen = SQRT(Dot_Product(vec,vec)) vecLen2D = SQRT(vec(1)**2+vec(2)**2) if ( vecLen < 0.000001 ) then - print *, "ERROR in GetOrientationAngles in MoorDyn" !call SeterrStat(ErrID_Fatal, 'An element of the Morison structure has co-located endpoints! This should never occur. Please review your model.', errStat, errMsg, 'Morison_CalcOutput' ) - print *, p1 - print *, p2 + print *, "ERROR in GetOrientationAngles in MoorDyn. Supplied vector is near zero" + print *, vec k_hat = NaN ! 1.0/0.0 else k_hat = vec / vecLen @@ -1260,7 +1256,7 @@ END SUBROUTINE WriteWaveData ! ----- process WaterKin input value, potentially reading wave inputs and generating wave field ----- - SUBROUTINE SetupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) + SUBROUTINE setupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) CHARACTER(40), INTENT(IN ) :: WaterKinString ! string describing water kinematics filename TYPE(MD_ParameterType), INTENT(INOUT) :: p ! Parameters @@ -1322,6 +1318,11 @@ SUBROUTINE SetupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) CHARACTER(120) :: ErrMsg2 CHARACTER(120) :: RoutineName = 'SetupWaveKin' + + ErrStatTmp = ErrID_None ! TODO: get rid of redundancy <<< + ErrStat2 = ErrID_None + ErrMsg2 = "" + IF (LEN_TRIM(WaterKinString) == 0) THEN ! If the input is empty (not provided), there are no water kinematics to be included p%WaveKin = 0 @@ -1338,12 +1339,20 @@ SUBROUTINE SetupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) ! otherwise interpret the input as a file name to load the bathymetry lookup data from - PRINT *, "found a letter in the depth value so will try to load a water kinematics input file" + print *, "found a letter in the WaterKin value so will try to load a water kinematics input file" ! -------- load water kinematics input file ------------- - FileName = TRIM(WaterKinString) + IF ( PathIsRelative( WaterKinString ) ) THEN ! properly handle relative path <<< + !CALL GetPath( TRIM(InitInp%InputFile), TmpPath ) + FileName = TRIM(p%PriPath)//TRIM(WaterKinString) + ELSE + FileName = trim(WaterKinString) + END IF + + + UnEcho=-1 CALL GetNewUnit( UnIn ) CALL OpenFInpFile( UnIn, FileName, ErrStat2, ErrMsg2); if(Failed()) return @@ -1418,16 +1427,17 @@ SUBROUTINE SetupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) ! --------------------- set from inputted wave elevation time series, grid approach ------------------- if (p%WaveKin == 3) then + print *, 'Setting up WaveKin 3 option: read wave elevation time series from file' IF ( LEN_TRIM( WaveKinFile ) == 0 ) THEN CALL SetErrStat( ErrID_Fatal,'WaveKinFile must not be an empty string.',ErrStat, ErrMsg, RoutineName); return RETURN END IF - !IF ( PathIsRelative( WaveKinFile ) ) THEN ! properly handle relative path <<< - ! CALL GetPath( TRIM(InitInp%InputFile), TmpPath ) - ! WaveKinFile = TRIM(TmpPath)//TRIM(WaveKinFile) - !END IF + IF ( PathIsRelative( WaveKinFile ) ) THEN ! properly handle relative path <<< + !CALL GetPath( TRIM(InitInp%InputFile), TmpPath ) + WaveKinFile = TRIM(p%PriPath)//TRIM(WaveKinFile) + END IF ! note: following is adapted from MoorDyn_Driver @@ -2078,7 +2088,7 @@ FUNCTION SINHNumOvrSINHDen ( k, h, z ) RETURN END FUNCTION SINHNumOvrSINHDen - END SUBROUTINE SetupWaterKin + END SUBROUTINE setupWaterKin diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 9e7b19d857..d2cbf1e411 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -384,6 +384,7 @@ typedef ^ ^ CHARACTER(1024) RootName - typedef ^ ^ MD_OutParmType OutParam {:} - - "Names and units (and other characteristics) of all requested output parameters" - typedef ^ ^ CHARACTER(1) Delim - - - "Column delimiter for output text files" - typedef ^ ^ IntKi MDUnOut - - - "Unit number of main output file" +typedef ^ ^ CHARACTER(1024) PriPath - - - "The path to the primary MoorDyn input file, used if looking for additional input files" typedef ^ ^ IntKi WaveKin - - - "Flag for whether or how to consider water kinematics" typedef ^ ^ IntKi Current - - - "Flag for whether or how to consider water kinematics" typedef ^ ^ IntKi nTurbines - - - "Number of turbines if MoorDyn is performing an array-level simulation with FAST.Farm, otherwise 0" diff --git a/modules/moordyn/src/MoorDyn_Rod.f90 b/modules/moordyn/src/MoorDyn_Rod.f90 index 1e4acff875..1aad886596 100644 --- a/modules/moordyn/src/MoorDyn_Rod.f90 +++ b/modules/moordyn/src/MoorDyn_Rod.f90 @@ -61,6 +61,8 @@ SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) INTEGER(4) :: i ! Generic index INTEGER(4) :: K ! Generic index INTEGER(IntKi) :: N + + INTEGER :: ErrStat2 N = Rod%N ! number of segments in this line (for code readability) @@ -74,45 +76,45 @@ SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) Rod%Cdn = RodProp%Cdn Rod%Cdt = RodProp%Cdt Rod%CaEnd = RodProp%CaEnd - Rod%CdEnd = RodProp%CdEnd - + Rod%CdEnd = RodProp%CdEnd + ! allocate node positions and velocities (NOTE: these arrays start at ZERO) - ALLOCATE ( Rod%r(3, 0:N), Rod%rd(3, 0:N), STAT = ErrStat ) ! <<<<<< add error checks here - IF ( ErrStat /= ErrID_None ) print *, "Alloc error 1 in MoorDyn" + ALLOCATE(Rod%r(3, 0:N), Rod%rd(3, 0:N), STAT=ErrStat2); if(AllocateFailed("")) return ! allocate segment scalar quantities if (Rod%N == 0) then ! special case of zero-length Rod - ALLOCATE ( Rod%l(1), Rod%V(N), STAT = ErrStat ) + ALLOCATE(Rod%l(1), Rod%V(N), STAT=ErrStat2); if(AllocateFailed("Rod: l and V")) return else ! normal case - ALLOCATE ( Rod%l(N), Rod%V(N), STAT = ErrStat ) + ALLOCATE(Rod%l(N), Rod%V(N), STAT=ErrStat2); if(AllocateFailed("Rod: l and V")) return end if - IF ( ErrStat /= ErrID_None ) print *, "Alloc error 2 in MoorDyn" ! allocate water related vectors - ALLOCATE ( Rod%U(3, 0:N), Rod%Ud(3, 0:N), Rod%zeta(0:N), Rod%PDyn(0:N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) print *, "Alloc error 3 in MoorDyn" - ! set to zero initially (important of wave kinematics are not being used) - Rod%U = 0.0_DbKi - Rod%Ud = 0.0_DbKi - Rod%zeta = 0.0_DbKi - Rod%PDyn = 0.0_DbKi + ALLOCATE(Rod%U(3, 0:N), Rod%Ud(3, 0:N), Rod%zeta(0:N), Rod%PDyn(0:N), STAT=ErrStat2) + if(AllocateFailed("Rod: U Ud zeta PDyn")) return ! allocate node force vectors - ALLOCATE ( Rod%W(3, 0:N), Rod%Bo(3, 0:N), Rod%Dp(3, 0:N), Rod%Dq(3, 0:N), Rod%Ap(3, 0:N), & - Rod%Aq(3, 0:N), Rod%Pd(3, 0:N), Rod%B(3, 0:N), Rod%Fnet(3, 0:N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) print *, "Alloc error 4 in MoorDyn" + ALLOCATE(Rod%W(3, 0:N), Rod%Bo(3, 0:N), Rod%Dp(3, 0:N), Rod%Dq(3, 0:N), Rod%Ap(3, 0:N), & + Rod%Aq(3, 0:N), Rod%Pd(3, 0:N), Rod%B(3, 0:N), Rod%Fnet(3, 0:N), STAT=ErrStat2) + if(AllocateFailed("Rod: force arrays")) return ! allocate mass and inverse mass matrices for each node (including ends) - ALLOCATE ( Rod%M(3, 3, 0:N), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) print *, "Alloc error 5 in MoorDyn" + ALLOCATE(Rod%M(3, 3, 0:N), STAT=ErrStat2); if(AllocateFailed("Rod: M")) return + ! set to zero initially (important of wave kinematics are not being used) + Rod%U = 0.0_DbKi + Rod%Ud = 0.0_DbKi + Rod%zeta = 0.0_DbKi + Rod%PDyn = 0.0_DbKi ! ------------------------- set some geometric properties and the starting kinematics ------------------------- CALL UnitVector(endCoords(1:3), endCoords(4:6), Rod%q, Rod%UnstrLen) ! get Rod axis direction vector and Rod length + print *, 'im a rod' + print *, endCoords + ! set Rod positions if applicable if (Rod%typeNum==0) then ! for an independent rod, set the position right off the bat @@ -155,6 +157,16 @@ SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) ! need to add cleanup sub <<< + + CONTAINS + + LOGICAL FUNCTION AllocateFailed(arrayName) + CHARACTER(*), INTENT(IN ) :: arrayName ! The array name + call SetErrStat(ErrStat2, "Error allocating space for "//trim(arrayName)//" array.", ErrStat, ErrMsg, 'Rod_Setup') + AllocateFailed = ErrStat2 >= AbortErrLev + !if (AllocateFailed) call CleanUp() + END FUNCTION AllocateFailed + END SUBROUTINE Rod_Setup !-------------------------------------------------------------- @@ -577,7 +589,8 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ! ---------------------------- initial rod and node calculations ------------------------ ! calculate some orientation information for the Rod as a whole - call GetOrientationAngles(Rod%r( :,0), Rod%r( :,N), phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat) + !call GetOrientationAngles(Rod%r( :,0), Rod%r( :,N), phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat) + call GetOrientationAngles(Rod%q, phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat) ! save to internal roll and pitch variables for use in output <<< should check these, make Euler angles isntead of independent <<< Rod%roll = -180.0/Pi * phi*sinBeta diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index 9de65c1ff2..0b47292b10 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -416,6 +416,7 @@ MODULE MoorDyn_Types TYPE(MD_OutParmType) , DIMENSION(:), ALLOCATABLE :: OutParam !< Names and units (and other characteristics) of all requested output parameters [-] CHARACTER(1) :: Delim !< Column delimiter for output text files [-] INTEGER(IntKi) :: MDUnOut !< Unit number of main output file [-] + CHARACTER(1024) :: PriPath !< The path to the primary MoorDyn input file, used if looking for additional input files [-] INTEGER(IntKi) :: WaveKin !< Flag for whether or how to consider water kinematics [-] INTEGER(IntKi) :: Current !< Flag for whether or how to consider water kinematics [-] INTEGER(IntKi) :: nTurbines !< Number of turbines if MoorDyn is performing an array-level simulation with FAST.Farm, otherwise 0 [-] @@ -10715,6 +10716,7 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) ENDIF DstParamData%Delim = SrcParamData%Delim DstParamData%MDUnOut = SrcParamData%MDUnOut + DstParamData%PriPath = SrcParamData%PriPath DstParamData%WaveKin = SrcParamData%WaveKin DstParamData%Current = SrcParamData%Current DstParamData%nTurbines = SrcParamData%nTurbines @@ -11195,6 +11197,7 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si END IF Int_BufSz = Int_BufSz + 1*LEN(InData%Delim) ! Delim Int_BufSz = Int_BufSz + 1 ! MDUnOut + Int_BufSz = Int_BufSz + 1*LEN(InData%PriPath) ! PriPath Int_BufSz = Int_BufSz + 1 ! WaveKin Int_BufSz = Int_BufSz + 1 ! Current Int_BufSz = Int_BufSz + 1 ! nTurbines @@ -11477,6 +11480,10 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si END DO ! I IntKiBuf(Int_Xferred) = InData%MDUnOut Int_Xferred = Int_Xferred + 1 + DO I = 1, LEN(InData%PriPath) + IntKiBuf(Int_Xferred) = ICHAR(InData%PriPath(I:I), IntKi) + Int_Xferred = Int_Xferred + 1 + END DO ! I IntKiBuf(Int_Xferred) = InData%WaveKin Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%Current @@ -12119,6 +12126,10 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg END DO ! I OutData%MDUnOut = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 + DO I = 1, LEN(OutData%PriPath) + OutData%PriPath(I:I) = CHAR(IntKiBuf(Int_Xferred)) + Int_Xferred = Int_Xferred + 1 + END DO ! I OutData%WaveKin = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 OutData%Current = IntKiBuf(Int_Xferred) From 91e9a5789fee8ef0620a8b2fa4528835577aa613 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Wed, 16 Feb 2022 19:01:58 -0700 Subject: [PATCH 096/242] Increase allowed line length in the FileInfoType parsing --- modules/nwtc-library/src/NWTC_IO.f90 | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/nwtc-library/src/NWTC_IO.f90 b/modules/nwtc-library/src/NWTC_IO.f90 index 81d768f82b..47da20580c 100644 --- a/modules/nwtc-library/src/NWTC_IO.f90 +++ b/modules/nwtc-library/src/NWTC_IO.f90 @@ -4045,7 +4045,7 @@ SUBROUTINE ParseInclInfo ( InclInfo, RelativePathFileName, FileName, RangeBeg, R INTEGER :: DashLoc ! The possible location of the dash in the range text. CHARACTER( 20) :: InclInfoUC ! InclInfo converted to upper case. - CHARACTER(512) :: Words (2) ! The two "words" parsed from the line. + CHARACTER(2048) :: Words (2) ! The two "words" parsed from the line. CHARACTER(1024) :: PriPath ! path name of primary file (RelativePathFileName) CHARACTER(*), PARAMETER :: RoutineName = 'ParseInclInfo' @@ -5409,7 +5409,7 @@ RECURSIVE SUBROUTINE ReadComFile ( FileInfo, FileIndx, AryInd, StartLine, LastLi INTEGER :: UnIn ! The unit number used for the input file. ! Should the comment characters be passed to this routine instead of being hard coded? -mlb CHARACTER(1024) :: IncFileName ! The name of a file that this one includes. - CHARACTER(512) :: Line ! The contents of a line returned from ReadLine() with comment removed. + CHARACTER(2048) :: Line ! The contents of a line returned from ReadLine() with comment removed. CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'ReadComFile' @@ -7374,7 +7374,7 @@ RECURSIVE SUBROUTINE ScanComFile ( FirstFile, ThisFile, LastFile, StartLine, Las CHARACTER(1024) :: FileName ! The name of this file being processed. CHARACTER(1024) :: IncFileName ! The name of a file that this one includes. - CHARACTER(512) :: Line ! The contents of a line returned from ReadLine() with comment removed. + CHARACTER(2048) :: Line ! The contents of a line returned from ReadLine() with comment removed. CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'ScanComFile' From c5e26157c392e6fdaee47eda61c7e4fc7984f237 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Thu, 3 Mar 2022 09:01:12 -0700 Subject: [PATCH 097/242] [BugFix] reset platformtoolset to v140 This was set to v142, which is not compatible with Visual Studio C++ 2015 that some people are still using. --- vs-build/MAPlib/MAP_dll.vcxproj | 8 ++++---- vs-build/Registry/FAST_Registry.vcxproj | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/vs-build/MAPlib/MAP_dll.vcxproj b/vs-build/MAPlib/MAP_dll.vcxproj index 0c7c4198f0..c81eb5b173 100644 --- a/vs-build/MAPlib/MAP_dll.vcxproj +++ b/vs-build/MAPlib/MAP_dll.vcxproj @@ -28,26 +28,26 @@ StaticLibrary true - v142 + v140 Unicode StaticLibrary true - v142 + v140 Unicode StaticLibrary false - v142 + v140 true Unicode StaticLibrary false - v142 + v140 true Unicode diff --git a/vs-build/Registry/FAST_Registry.vcxproj b/vs-build/Registry/FAST_Registry.vcxproj index 792f8ce14e..40649f85f0 100644 --- a/vs-build/Registry/FAST_Registry.vcxproj +++ b/vs-build/Registry/FAST_Registry.vcxproj @@ -28,27 +28,27 @@ Application true Unicode - v142 + v140 Application true Unicode - v142 + v140 Application false true Unicode - v142 + v140 Application false true Unicode - v142 + v140 From fcc732f2f037bbb9266db2e3ecbb7b095a04b44c Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Thu, 3 Mar 2022 10:36:41 -0700 Subject: [PATCH 098/242] [BugFix] Reset WindowsTargetPlatformVersion to 8.1 --- vs-build/MAPlib/MAP_dll.vcxproj | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/vs-build/MAPlib/MAP_dll.vcxproj b/vs-build/MAPlib/MAP_dll.vcxproj index c81eb5b173..22373bf523 100644 --- a/vs-build/MAPlib/MAP_dll.vcxproj +++ b/vs-build/MAPlib/MAP_dll.vcxproj @@ -22,7 +22,7 @@ {BF86702A-CB17-4050-8AE9-078CDC5910D3} Win32Proj MAP_DLL - 10.0 + 8.1 @@ -214,4 +214,4 @@ - \ No newline at end of file + From dbe0239df248b970c7fa7f4764f64abe8a368965 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Thu, 3 Mar 2022 16:23:29 -0700 Subject: [PATCH 099/242] Reset a few VS project file entries to match what is in dev Version info mostly -- this seems to be throwing off older VS setups --- vs-build/FAST-farm/FAST-Farm.vfproj | 12 ++++++------ vs-build/FAST/FAST.sln | 8 ++++---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/vs-build/FAST-farm/FAST-Farm.vfproj b/vs-build/FAST-farm/FAST-Farm.vfproj index dde73bcade..adcd8f661d 100644 --- a/vs-build/FAST-farm/FAST-Farm.vfproj +++ b/vs-build/FAST-farm/FAST-Farm.vfproj @@ -5,7 +5,7 @@ - + @@ -15,7 +15,7 @@ - + @@ -25,7 +25,7 @@ - + @@ -35,7 +35,7 @@ - + @@ -45,7 +45,7 @@ - + @@ -55,7 +55,7 @@ - + diff --git a/vs-build/FAST/FAST.sln b/vs-build/FAST/FAST.sln index 88fddb1d0e..ec3d691059 100644 --- a/vs-build/FAST/FAST.sln +++ b/vs-build/FAST/FAST.sln @@ -1,7 +1,7 @@  Microsoft Visual Studio Solution File, Format Version 12.00 -# Visual Studio Version 16 -VisualStudioVersion = 16.0.31702.278 +# Visual Studio 15 +VisualStudioVersion = 15.0.27428.2043 MinimumVisualStudioVersion = 10.0.40219.1 Project("{6989167D-11E4-40FE-8C1A-2192A86A7E90}") = "FAST", "FAST.vfproj", "{18AE8067-CCC6-4479-A0DB-C4089EF9FE71}" ProjectSection(ProjectDependencies) = postProject @@ -120,8 +120,8 @@ Global {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Debug_Matlab|x64.Build.0 = Release|Win32 {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Debug|Win32.ActiveCfg = Release|Win32 {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Debug|Win32.Build.0 = Release|Win32 - {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Debug|x64.ActiveCfg = Release|x64 - {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Debug|x64.Build.0 = Release|x64 + {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Debug|x64.ActiveCfg = Release|Win32 + {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Debug|x64.Build.0 = Release|Win32 {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Release_Double|Win32.ActiveCfg = Release|Win32 {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Release_Double|Win32.Build.0 = Release|Win32 {DA16A3A6-3297-4628-9E46-C6FA0E3C4D16}.Release_Double|x64.ActiveCfg = Release|Win32 From c825ba2446b7d265efe17e07339dd6a96339630d Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Thu, 3 Mar 2022 10:39:06 -0700 Subject: [PATCH 100/242] Fixed some duplication and initialization issues for Rods: - Reduced duplicate code with initilaization of (non)zero-length Rods. - Changed GetOrientationAngles to use a unit vector rather than end A and B coordinates. - Some edits to avoid issues with uninitialized variables and array bounds with Rods. --- modules/moordyn/src/MoorDyn_Misc.f90 | 13 ++++++++--- modules/moordyn/src/MoorDyn_Rod.f90 | 34 +++++++++++++++------------- 2 files changed, 28 insertions(+), 19 deletions(-) diff --git a/modules/moordyn/src/MoorDyn_Misc.f90 b/modules/moordyn/src/MoorDyn_Misc.f90 index 03c44a32fa..43f47ce8da 100644 --- a/modules/moordyn/src/MoorDyn_Misc.f90 +++ b/modules/moordyn/src/MoorDyn_Misc.f90 @@ -539,7 +539,11 @@ SUBROUTINE LUsolve(n, A, LU, b, y, x) sum = sum + LU(k,p)*LU(p,j) END DO - LU(k,j) = (A(k,j)-sum)/LU(k,k) + if ( EqualRealNos( LU(k,k), 0.0_DbKi) ) then + LU(k,j) = 0.0_DbKi ! avoid divide by zero <<< numerator likely zero too. check if this is safe <<< + else + LU(k,j) = (A(k,j)-sum)/LU(k,k) + end if END DO !j END DO !K @@ -552,8 +556,11 @@ SUBROUTINE LUsolve(n, A, LU, b, y, x) sum = sum + LU(i,k)*y(k); END DO - y(i) = (b(i)-sum)/LU(i,i) - + if ( EqualRealNos( LU(i,i), 0.0_DbKi) ) then + y(i) = 0.0_DbKi ! avoid divide by zero <<< numerator likely zero too. check if this is safe <<< + else + y(i) = (b(i)-sum)/LU(i,i) + end if END DO DO j=1,n ! this is actually for looping through i in reverse diff --git a/modules/moordyn/src/MoorDyn_Rod.f90 b/modules/moordyn/src/MoorDyn_Rod.f90 index 1aad886596..9c1152c895 100644 --- a/modules/moordyn/src/MoorDyn_Rod.f90 +++ b/modules/moordyn/src/MoorDyn_Rod.f90 @@ -112,7 +112,7 @@ SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) CALL UnitVector(endCoords(1:3), endCoords(4:6), Rod%q, Rod%UnstrLen) ! get Rod axis direction vector and Rod length - print *, 'im a rod' + print *, 'I am a rod' print *, endCoords ! set Rod positions if applicable @@ -198,7 +198,7 @@ SUBROUTINE Rod_Initialize(Rod, states, m) ! Pass kinematics to any attached lines (this is just like what a Connection does, except for both ends) ! so that they have the correct initial positions at this initialization stage. - if (Rod%typeNum >- 2) CALL Rod_SetDependentKin(Rod, 0.0_DbKi, m) ! don't call this for type -2 coupled Rods as it's already been called + if (Rod%typeNum >- 2) CALL Rod_SetDependentKin(Rod, 0.0_DbKi, m, .TRUE.) ! don't call this for type -2 coupled Rods as it's already been called ! assign the resulting kinematics to its part of the state vector (only matters if it's an independent Rod) @@ -249,7 +249,7 @@ SUBROUTINE Rod_SetKinematics(Rod, r6_in, v6_in, a6_in, t, m) call ScaleVector(Rod%r6(4:6), 1.0_DbKi, Rod%r6(4:6)); ! enforce direction vector to be a unit vector ! since this rod has no states and all DOFs have been set, pass its kinematics to dependent Lines - CALL Rod_SetDependentKin(Rod, t, m) + CALL Rod_SetDependentKin(Rod, t, m, .FALSE.) else if (abs(Rod%typeNum) == 1) then ! rod end A pinned to a body, or ground, or coupling point @@ -314,7 +314,7 @@ SUBROUTINE Rod_SetState(Rod, X, t, m) Rod%v6(4:6) = X(4:6) ! (rotational velocities about unrotated axes) - CALL Rod_SetDependentKin(Rod, t, m) + CALL Rod_SetDependentKin(Rod, t, m, .FALSE.) else if (abs(Rod%typeNum) == 1) then ! pinned rod type (coupled or attached to something)t previously via setPinKin) @@ -325,7 +325,7 @@ SUBROUTINE Rod_SetState(Rod, X, t, m) Rod%v6(4:6) = X(1:3) ! (rotational velocities about unrotated axes) - CALL Rod_SetDependentKin(Rod, t, m) + CALL Rod_SetDependentKin(Rod, t, m, .FALSE.) else print *, "Error: Rod::setState called for a non-free rod type in MoorDyn" ! <<< @@ -341,11 +341,12 @@ END SUBROUTINE Rod_SetState ! Set the Rod end kinematics then set the kinematics of dependent objects (any attached lines). ! This also determines the orientation of zero-length rods. !-------------------------------------------------------------- - SUBROUTINE Rod_SetDependentKin(Rod, t, m) + SUBROUTINE Rod_SetDependentKin(Rod, t, m, initial) Type(MD_Rod), INTENT(INOUT) :: Rod ! the Rod object Real(DbKi), INTENT(IN ) :: t ! instantaneous time TYPE(MD_MiscVarType), INTENT(INOUT) :: m ! passing along all mooring objects (for simplicity, since Bodies deal with Rods and Connections) + LOGICAL, INTENT(IN ) :: initial ! true if this is the call during initialization (in which case avoid calling any Lines yet) INTEGER(IntKi) :: l ! index of segments or nodes along line INTEGER(IntKi) :: J ! index @@ -385,8 +386,8 @@ SUBROUTINE Rod_SetDependentKin(Rod, t, m) END DO - ! if this is a zero-length Rod, get bending moment-related information from attached lines and compute Rod's equilibrium orientation - if (N==0) then + ! if this is a zero-length Rod and we're passed initialization, get bending moment-related information from attached lines and compute Rod's equilibrium orientation + if ((N==0) .and. (initial==.FALSE.)) then DO l=1,Rod%nAttachedA @@ -891,14 +892,15 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ! >>> could some of these be precalculated just once? <<< ! add inertia terms for the Rod assuming it is uniform density (radial terms add to existing matrix which contains parallel-axis-theorem components only) - I_l = 0.125*Rod%mass * Rod%d*Rod%d ! axial moment of inertia - I_r = Rod%mass/12 * (0.75*Rod%d*Rod%d + (Rod%UnstrLen/Rod%N)**2 ) * Rod%N ! summed radial moment of inertia for each segment individually - - !h_c = [value from registry] - - Imat_l(1,1) = I_r ! inertia about CG in local orientations (as if Rod is vertical) - Imat_l(2,2) = I_r - Imat_l(3,3) = I_l + Imat_l = 0.0_DbKi + if (Rod%N > 0) then + I_l = 0.125*Rod%mass * Rod%d*Rod%d ! axial moment of inertia + I_r = Rod%mass/12 * (0.75*Rod%d*Rod%d + (Rod%UnstrLen/Rod%N)**2 ) * Rod%N ! summed radial moment of inertia for each segment individually + + Imat_l(1,1) = I_r ! inertia about CG in local orientations (as if Rod is vertical) + Imat_l(2,2) = I_r + Imat_l(3,3) = I_l + end if OrMat = CalcOrientation(phi, beta, 0.0_DbKi) ! get rotation matrix to put things in global rather than rod-axis orientations From 643e5133281dacb78d7939d0e6f5cc9f89220d32 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Thu, 3 Mar 2022 16:40:22 -0700 Subject: [PATCH 101/242] MDv2: update CMakeLists.txt --- modules/moordyn/CMakeLists.txt | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/modules/moordyn/CMakeLists.txt b/modules/moordyn/CMakeLists.txt index 6d6333139a..19a992b42b 100644 --- a/modules/moordyn/CMakeLists.txt +++ b/modules/moordyn/CMakeLists.txt @@ -20,7 +20,12 @@ endif() set(MOORDYN_LIBS_SOURCES src/MoorDyn.f90 + src/MoorDyn_Body.f90 src/MoorDyn_IO.f90 + src/MoorDyn_Line.f90 + src/MoorDyn_Misc.f90 + src/MoorDyn_Point.f90 + src/MoorDyn_Rod.f90 src/MoorDyn_Types.f90 ) @@ -33,11 +38,12 @@ install(TARGETS moordynlib ARCHIVE DESTINATION lib LIBRARY DESTINATION lib) -#set(MD_DRIVER_SOURCES src/MoorDyn_Driver.f90) -#add_executable(moordyn_driver ${MD_DRIVER_SOURCES}) -#target_link_libraries(moordyn_driver moordynlib nwtclibs versioninfolib ${CMAKE_DL_LIBS}) +set(MD_DRIVER_SOURCES src/MoorDyn_Driver.f90) +add_executable(moordyn_driver ${MD_DRIVER_SOURCES}) +target_link_libraries(moordyn_driver moordynlib nwtclibs versioninfolib ${CMAKE_DL_LIBS}) install(TARGETS moordyn_driver RUNTIME DESTINATION bin LIBRARY DESTINATION lib ARCHIVE DESTINATION lib) + From 383ba9091eb51435c07d013055298ed06ae0a9bf Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Thu, 3 Mar 2022 16:46:28 -0700 Subject: [PATCH 102/242] MDv2: convert tabs to spaces --- modules/moordyn/src/MoorDyn_Line.f90 | 4 ++-- modules/moordyn/src/MoorDyn_Misc.f90 | 16 ++++++++-------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/modules/moordyn/src/MoorDyn_Line.f90 b/modules/moordyn/src/MoorDyn_Line.f90 index e32ba2ac00..cb2f0ac9f6 100644 --- a/modules/moordyn/src/MoorDyn_Line.f90 +++ b/modules/moordyn/src/MoorDyn_Line.f90 @@ -1195,7 +1195,7 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, end do - ! Bending loads + ! Bending loads Line%Bs = 0.0_DbKi ! zero bending forces if (Line%EI > 0) then @@ -1217,7 +1217,7 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, call scalevector(pvec, Kurvi*Line%EI, Line%endMomentA) ! scale force direction vectors by desired moment force magnitudes to get resulting forces on adjacent nodes - call scalevector(Mforce_ip1, Kurvi*Line%EI/Line%lstr(i), Mforce_ip1) + call scalevector(Mforce_ip1, Kurvi*Line%EI/Line%lstr(i), Mforce_ip1) ! set force on node i to cancel out forces on adjacent nodes Mforce_i = -Mforce_ip1 diff --git a/modules/moordyn/src/MoorDyn_Misc.f90 b/modules/moordyn/src/MoorDyn_Misc.f90 index 43f47ce8da..c04a51a09b 100644 --- a/modules/moordyn/src/MoorDyn_Misc.f90 +++ b/modules/moordyn/src/MoorDyn_Misc.f90 @@ -1374,16 +1374,16 @@ SUBROUTINE setupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) CALL ReadVar( UnIn, FileName, p%dtWave , 'dtWave', 'time step for waves', ErrStat2, ErrMsg2, UnEcho); if(Failed()) return CALL ReadVar( UnIn, FileName, WaveDir , 'WaveDir' , 'wave direction', ErrStat2, ErrMsg2, UnEcho); if(Failed()) return ! X grid points - READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type - READ(UnIn,'(A)', IOSTAT=ErrStat2) entries2 ! get entries as string to be processed + READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type + READ(UnIn,'(A)', IOSTAT=ErrStat2) entries2 ! get entries as string to be processed CALL gridAxisCoords(coordtype, entries2, p%pxWave, p%nxWave, ErrStat2, ErrMsg2) ! Y grid points - READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type - READ(UnIn,'(A)', IOSTAT=ErrStat2) entries2 ! get entries as string to be processed + READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type + READ(UnIn,'(A)', IOSTAT=ErrStat2) entries2 ! get entries as string to be processed CALL gridAxisCoords(coordtype, entries2, p%pyWave, p%nyWave, ErrStat2, ErrMsg2) ! Z grid points - READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type - READ(UnIn,'(A)', IOSTAT=ErrStat2) entries2 ! get entries as string to be processed + READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type + READ(UnIn,'(A)', IOSTAT=ErrStat2) entries2 ! get entries as string to be processed CALL gridAxisCoords(coordtype, entries2, p%pzWave, p%nzWave, ErrStat2, ErrMsg2) ! ----- current ----- CALL ReadCom( UnIn, FileName, 'current header', ErrStat2, ErrMsg2, UnEcho); if(Failed()) return @@ -1427,7 +1427,7 @@ SUBROUTINE setupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) ! ** if a grid has already been set, these will interpolate onto it, otherwise they'll make a new grid based on their provided coordinates ! NOTE: lots of partial code is available from MD-C for supporting various wave kinematics input options - + ! WaveKin and Current compatibility check could go here in future @@ -2095,7 +2095,7 @@ FUNCTION SINHNumOvrSINHDen ( k, h, z ) RETURN END FUNCTION SINHNumOvrSINHDen - END SUBROUTINE setupWaterKin + END SUBROUTINE setupWaterKin From c69c1780afd91da4c2cc6a9d7f39582e1770717c Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Thu, 3 Mar 2022 17:04:13 -0700 Subject: [PATCH 103/242] MDv2: double precision compiling working And some more rogue tab characters --- modules/moordyn/src/MoorDyn_Line.f90 | 2 +- modules/moordyn/src/MoorDyn_Misc.f90 | 16 +++---- modules/moordyn/src/MoorDyn_Registry.txt | 30 ++++++------ modules/moordyn/src/MoorDyn_Types.f90 | 60 ++++++++++++------------ 4 files changed, 54 insertions(+), 54 deletions(-) diff --git a/modules/moordyn/src/MoorDyn_Line.f90 b/modules/moordyn/src/MoorDyn_Line.f90 index cb2f0ac9f6..f527b758a8 100644 --- a/modules/moordyn/src/MoorDyn_Line.f90 +++ b/modules/moordyn/src/MoorDyn_Line.f90 @@ -1217,7 +1217,7 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, call scalevector(pvec, Kurvi*Line%EI, Line%endMomentA) ! scale force direction vectors by desired moment force magnitudes to get resulting forces on adjacent nodes - call scalevector(Mforce_ip1, Kurvi*Line%EI/Line%lstr(i), Mforce_ip1) + call scalevector(Mforce_ip1, Kurvi*Line%EI/Line%lstr(i), Mforce_ip1) ! set force on node i to cancel out forces on adjacent nodes Mforce_i = -Mforce_ip1 diff --git a/modules/moordyn/src/MoorDyn_Misc.f90 b/modules/moordyn/src/MoorDyn_Misc.f90 index c04a51a09b..ebefaf0b21 100644 --- a/modules/moordyn/src/MoorDyn_Misc.f90 +++ b/modules/moordyn/src/MoorDyn_Misc.f90 @@ -920,16 +920,16 @@ SUBROUTINE getWaterKin(p, x, y, z, t, tindex, U, Ud, zeta, PDyn) INTEGER(IntKi) :: ix, iy, iz, it ! indices for interpolation INTEGER(IntKi) :: iz0, iz1 ! special indices for currrent interpolation INTEGER(IntKi) :: N ! number of rod elements for convenience - Real(ReKi) :: fx, fy, fz, ft ! interpolation fractions + Real(SiKi) :: fx, fy, fz, ft ! interpolation fractions !Real(DbKi) :: qt ! used in time step interpolation ! if wave kinematics enabled, get interpolated values from grid if (p%WaveKin > 0) then - CALL getInterpNumsSiKi(p%pxWave , REAL(x), 1, ix, fx) - CALL getInterpNumsSiKi(p%pyWave , REAL(y), 1, iy, fy) - CALL getInterpNumsSiKi(p%pzWave , REAL(z), 1, iz, fz) + CALL getInterpNumsSiKi(p%pxWave , REAL(x,SiKi), 1, ix, fx) + CALL getInterpNumsSiKi(p%pyWave , REAL(y,SiKi), 1, iy, fy) + CALL getInterpNumsSiKi(p%pzWave , REAL(z,SiKi), 1, iz, fz) !CALL getInterpNums(p%tWave, t, tindex, it, ft) it = floor(t/ p%dtWave) + 1 ! add 1 because Fortran indexing starts at 1 ft = (t - (it-1)*p%dtWave)/p%dtWave @@ -956,7 +956,7 @@ SUBROUTINE getWaterKin(p, x, y, z, t, tindex, U, Ud, zeta, PDyn) ! if current kinematics enabled, add interpolated current values from profile if (p%Current > 0) then - CALL getInterpNumsSiKi(p%pzCurrent, REAL(z), 1, iz0, fz) + CALL getInterpNumsSiKi(p%pzCurrent, REAL(z,SiKi), 1, iz0, fz) IF (fz == 0) THEN ! handle end case conditions iz1 = iz0 @@ -1378,11 +1378,11 @@ SUBROUTINE setupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) READ(UnIn,'(A)', IOSTAT=ErrStat2) entries2 ! get entries as string to be processed CALL gridAxisCoords(coordtype, entries2, p%pxWave, p%nxWave, ErrStat2, ErrMsg2) ! Y grid points - READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type + READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type READ(UnIn,'(A)', IOSTAT=ErrStat2) entries2 ! get entries as string to be processed CALL gridAxisCoords(coordtype, entries2, p%pyWave, p%nyWave, ErrStat2, ErrMsg2) ! Z grid points - READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type + READ(UnIn,*, IOSTAT=ErrStat2) coordtype ! get the entry type READ(UnIn,'(A)', IOSTAT=ErrStat2) entries2 ! get entries as string to be processed CALL gridAxisCoords(coordtype, entries2, p%pzWave, p%nzWave, ErrStat2, ErrMsg2) ! ----- current ----- @@ -1690,7 +1690,7 @@ SUBROUTINE gridAxisCoords(coordtype, entries, coordarray, n, ErrStat, ErrMsg) INTEGER(IntKi), INTENT(IN ) :: coordtype CHARACTER(*), INTENT(INOUT) :: entries - REAL(ReKi), ALLOCATABLE, INTENT(INOUT) :: coordarray(:) + REAL(SiKi), ALLOCATABLE, INTENT(INOUT) :: coordarray(:) INTEGER(IntKi), INTENT( OUT) :: n diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index d2cbf1e411..a009242fc4 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -398,22 +398,22 @@ typedef ^ ^ IntKi nxWave - typedef ^ ^ IntKi nyWave - - - "number of y wave grid points" - typedef ^ ^ IntKi nzWave - - - "number of z wave grid points" - typedef ^ ^ IntKi ntWave - - - "number of wave time steps" - -typedef ^ ^ ReKi pxWave {:} - - "x location of wave grid points" - -typedef ^ ^ ReKi pyWave {:} - - "y location of wave grid points" - -typedef ^ ^ ReKi pzWave {:} - - "z location of wave grid points" - -typedef ^ ^ ReKi dtWave - - - "wave data time step" - -typedef ^ ^ ReKi uxWave {:}{:}{:}{:} - - "wave velocities time series at each grid point" - -typedef ^ ^ ReKi uyWave {:}{:}{:}{:} - - "wave velocities time series at each grid point" - -typedef ^ ^ ReKi uzWave {:}{:}{:}{:} - - "wave velocities time series at each grid point" - -typedef ^ ^ ReKi axWave {:}{:}{:}{:} - - "wave accelerations time series at each grid point" - -typedef ^ ^ ReKi ayWave {:}{:}{:}{:} - - "wave accelerations time series at each grid point" - -typedef ^ ^ ReKi azWave {:}{:}{:}{:} - - "wave accelerations time series at each grid point" - -typedef ^ ^ ReKi PDyn {:}{:}{:}{:} - - "wave dynamic pressure time series at each grid point" - -typedef ^ ^ ReKi zeta {:}{:}{:} - - "wave surface elevations time series at each surface grid point" - +typedef ^ ^ SiKi pxWave {:} - - "x location of wave grid points" - +typedef ^ ^ SiKi pyWave {:} - - "y location of wave grid points" - +typedef ^ ^ SiKi pzWave {:} - - "z location of wave grid points" - +typedef ^ ^ SiKi dtWave - - - "wave data time step" - +typedef ^ ^ SiKi uxWave {:}{:}{:}{:} - - "wave velocities time series at each grid point" - +typedef ^ ^ SiKi uyWave {:}{:}{:}{:} - - "wave velocities time series at each grid point" - +typedef ^ ^ SiKi uzWave {:}{:}{:}{:} - - "wave velocities time series at each grid point" - +typedef ^ ^ SiKi axWave {:}{:}{:}{:} - - "wave accelerations time series at each grid point" - +typedef ^ ^ SiKi ayWave {:}{:}{:}{:} - - "wave accelerations time series at each grid point" - +typedef ^ ^ SiKi azWave {:}{:}{:}{:} - - "wave accelerations time series at each grid point" - +typedef ^ ^ SiKi PDyn {:}{:}{:}{:} - - "wave dynamic pressure time series at each grid point" - +typedef ^ ^ SiKi zeta {:}{:}{:} - - "wave surface elevations time series at each surface grid point" - typedef ^ ^ IntKi nzCurrent - - - "number of z current grid points" - -typedef ^ ^ ReKi pzCurrent {:} - - "z location of current grid points" - -typedef ^ ^ ReKi uxCurrent {:} - - "current velocities time series at each grid point" - -typedef ^ ^ ReKi uyCurrent {:} - - "current velocities time series at each grid point" - +typedef ^ ^ SiKi pzCurrent {:} - - "z location of current grid points" - +typedef ^ ^ SiKi uxCurrent {:} - - "current velocities time series at each grid point" - +typedef ^ ^ SiKi uyCurrent {:} - - "current velocities time series at each grid point" - # --- Parameters for linearization --- typedef ^ ^ Integer Nx0 - - - "copy of initial size of system state vector, for linearization routines" - typedef ^ ^ Integer Jac_u_indx {:}{:} - - "matrix to help fill/pack the u vector in computing the jacobian" - diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index 0b47292b10..788414bf92 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -429,22 +429,22 @@ MODULE MoorDyn_Types INTEGER(IntKi) :: nyWave !< number of y wave grid points [-] INTEGER(IntKi) :: nzWave !< number of z wave grid points [-] INTEGER(IntKi) :: ntWave !< number of wave time steps [-] - REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: pxWave !< x location of wave grid points [-] - REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: pyWave !< y location of wave grid points [-] - REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: pzWave !< z location of wave grid points [-] - REAL(ReKi) :: dtWave !< wave data time step [-] - REAL(ReKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: uxWave !< wave velocities time series at each grid point [-] - REAL(ReKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: uyWave !< wave velocities time series at each grid point [-] - REAL(ReKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: uzWave !< wave velocities time series at each grid point [-] - REAL(ReKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: axWave !< wave accelerations time series at each grid point [-] - REAL(ReKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: ayWave !< wave accelerations time series at each grid point [-] - REAL(ReKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: azWave !< wave accelerations time series at each grid point [-] - REAL(ReKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: PDyn !< wave dynamic pressure time series at each grid point [-] - REAL(ReKi) , DIMENSION(:,:,:), ALLOCATABLE :: zeta !< wave surface elevations time series at each surface grid point [-] + REAL(SiKi) , DIMENSION(:), ALLOCATABLE :: pxWave !< x location of wave grid points [-] + REAL(SiKi) , DIMENSION(:), ALLOCATABLE :: pyWave !< y location of wave grid points [-] + REAL(SiKi) , DIMENSION(:), ALLOCATABLE :: pzWave !< z location of wave grid points [-] + REAL(SiKi) :: dtWave !< wave data time step [-] + REAL(SiKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: uxWave !< wave velocities time series at each grid point [-] + REAL(SiKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: uyWave !< wave velocities time series at each grid point [-] + REAL(SiKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: uzWave !< wave velocities time series at each grid point [-] + REAL(SiKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: axWave !< wave accelerations time series at each grid point [-] + REAL(SiKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: ayWave !< wave accelerations time series at each grid point [-] + REAL(SiKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: azWave !< wave accelerations time series at each grid point [-] + REAL(SiKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: PDyn !< wave dynamic pressure time series at each grid point [-] + REAL(SiKi) , DIMENSION(:,:,:), ALLOCATABLE :: zeta !< wave surface elevations time series at each surface grid point [-] INTEGER(IntKi) :: nzCurrent !< number of z current grid points [-] - REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: pzCurrent !< z location of current grid points [-] - REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: uxCurrent !< current velocities time series at each grid point [-] - REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: uyCurrent !< current velocities time series at each grid point [-] + REAL(SiKi) , DIMENSION(:), ALLOCATABLE :: pzCurrent !< z location of current grid points [-] + REAL(SiKi) , DIMENSION(:), ALLOCATABLE :: uxCurrent !< current velocities time series at each grid point [-] + REAL(SiKi) , DIMENSION(:), ALLOCATABLE :: uyCurrent !< current velocities time series at each grid point [-] INTEGER(IntKi) :: Nx0 !< copy of initial size of system state vector, for linearization routines [-] INTEGER(IntKi) , DIMENSION(:,:), ALLOCATABLE :: Jac_u_indx !< matrix to help fill/pack the u vector in computing the jacobian [-] REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: du !< vector that determines size of perturbation for u (inputs) [-] @@ -12189,7 +12189,7 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg RETURN END IF DO i1 = LBOUND(OutData%pxWave,1), UBOUND(OutData%pxWave,1) - OutData%pxWave(i1) = ReKiBuf(Re_Xferred) + OutData%pxWave(i1) = REAL(ReKiBuf(Re_Xferred), SiKi) Re_Xferred = Re_Xferred + 1 END DO END IF @@ -12207,7 +12207,7 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg RETURN END IF DO i1 = LBOUND(OutData%pyWave,1), UBOUND(OutData%pyWave,1) - OutData%pyWave(i1) = ReKiBuf(Re_Xferred) + OutData%pyWave(i1) = REAL(ReKiBuf(Re_Xferred), SiKi) Re_Xferred = Re_Xferred + 1 END DO END IF @@ -12225,11 +12225,11 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg RETURN END IF DO i1 = LBOUND(OutData%pzWave,1), UBOUND(OutData%pzWave,1) - OutData%pzWave(i1) = ReKiBuf(Re_Xferred) + OutData%pzWave(i1) = REAL(ReKiBuf(Re_Xferred), SiKi) Re_Xferred = Re_Xferred + 1 END DO END IF - OutData%dtWave = ReKiBuf(Re_Xferred) + OutData%dtWave = REAL(ReKiBuf(Re_Xferred), SiKi) Re_Xferred = Re_Xferred + 1 IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! uxWave not allocated Int_Xferred = Int_Xferred + 1 @@ -12257,7 +12257,7 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg DO i3 = LBOUND(OutData%uxWave,3), UBOUND(OutData%uxWave,3) DO i2 = LBOUND(OutData%uxWave,2), UBOUND(OutData%uxWave,2) DO i1 = LBOUND(OutData%uxWave,1), UBOUND(OutData%uxWave,1) - OutData%uxWave(i1,i2,i3,i4) = ReKiBuf(Re_Xferred) + OutData%uxWave(i1,i2,i3,i4) = REAL(ReKiBuf(Re_Xferred), SiKi) Re_Xferred = Re_Xferred + 1 END DO END DO @@ -12290,7 +12290,7 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg DO i3 = LBOUND(OutData%uyWave,3), UBOUND(OutData%uyWave,3) DO i2 = LBOUND(OutData%uyWave,2), UBOUND(OutData%uyWave,2) DO i1 = LBOUND(OutData%uyWave,1), UBOUND(OutData%uyWave,1) - OutData%uyWave(i1,i2,i3,i4) = ReKiBuf(Re_Xferred) + OutData%uyWave(i1,i2,i3,i4) = REAL(ReKiBuf(Re_Xferred), SiKi) Re_Xferred = Re_Xferred + 1 END DO END DO @@ -12323,7 +12323,7 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg DO i3 = LBOUND(OutData%uzWave,3), UBOUND(OutData%uzWave,3) DO i2 = LBOUND(OutData%uzWave,2), UBOUND(OutData%uzWave,2) DO i1 = LBOUND(OutData%uzWave,1), UBOUND(OutData%uzWave,1) - OutData%uzWave(i1,i2,i3,i4) = ReKiBuf(Re_Xferred) + OutData%uzWave(i1,i2,i3,i4) = REAL(ReKiBuf(Re_Xferred), SiKi) Re_Xferred = Re_Xferred + 1 END DO END DO @@ -12356,7 +12356,7 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg DO i3 = LBOUND(OutData%axWave,3), UBOUND(OutData%axWave,3) DO i2 = LBOUND(OutData%axWave,2), UBOUND(OutData%axWave,2) DO i1 = LBOUND(OutData%axWave,1), UBOUND(OutData%axWave,1) - OutData%axWave(i1,i2,i3,i4) = ReKiBuf(Re_Xferred) + OutData%axWave(i1,i2,i3,i4) = REAL(ReKiBuf(Re_Xferred), SiKi) Re_Xferred = Re_Xferred + 1 END DO END DO @@ -12389,7 +12389,7 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg DO i3 = LBOUND(OutData%ayWave,3), UBOUND(OutData%ayWave,3) DO i2 = LBOUND(OutData%ayWave,2), UBOUND(OutData%ayWave,2) DO i1 = LBOUND(OutData%ayWave,1), UBOUND(OutData%ayWave,1) - OutData%ayWave(i1,i2,i3,i4) = ReKiBuf(Re_Xferred) + OutData%ayWave(i1,i2,i3,i4) = REAL(ReKiBuf(Re_Xferred), SiKi) Re_Xferred = Re_Xferred + 1 END DO END DO @@ -12422,7 +12422,7 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg DO i3 = LBOUND(OutData%azWave,3), UBOUND(OutData%azWave,3) DO i2 = LBOUND(OutData%azWave,2), UBOUND(OutData%azWave,2) DO i1 = LBOUND(OutData%azWave,1), UBOUND(OutData%azWave,1) - OutData%azWave(i1,i2,i3,i4) = ReKiBuf(Re_Xferred) + OutData%azWave(i1,i2,i3,i4) = REAL(ReKiBuf(Re_Xferred), SiKi) Re_Xferred = Re_Xferred + 1 END DO END DO @@ -12455,7 +12455,7 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg DO i3 = LBOUND(OutData%PDyn,3), UBOUND(OutData%PDyn,3) DO i2 = LBOUND(OutData%PDyn,2), UBOUND(OutData%PDyn,2) DO i1 = LBOUND(OutData%PDyn,1), UBOUND(OutData%PDyn,1) - OutData%PDyn(i1,i2,i3,i4) = ReKiBuf(Re_Xferred) + OutData%PDyn(i1,i2,i3,i4) = REAL(ReKiBuf(Re_Xferred), SiKi) Re_Xferred = Re_Xferred + 1 END DO END DO @@ -12484,7 +12484,7 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg DO i3 = LBOUND(OutData%zeta,3), UBOUND(OutData%zeta,3) DO i2 = LBOUND(OutData%zeta,2), UBOUND(OutData%zeta,2) DO i1 = LBOUND(OutData%zeta,1), UBOUND(OutData%zeta,1) - OutData%zeta(i1,i2,i3) = ReKiBuf(Re_Xferred) + OutData%zeta(i1,i2,i3) = REAL(ReKiBuf(Re_Xferred), SiKi) Re_Xferred = Re_Xferred + 1 END DO END DO @@ -12506,7 +12506,7 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg RETURN END IF DO i1 = LBOUND(OutData%pzCurrent,1), UBOUND(OutData%pzCurrent,1) - OutData%pzCurrent(i1) = ReKiBuf(Re_Xferred) + OutData%pzCurrent(i1) = REAL(ReKiBuf(Re_Xferred), SiKi) Re_Xferred = Re_Xferred + 1 END DO END IF @@ -12524,7 +12524,7 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg RETURN END IF DO i1 = LBOUND(OutData%uxCurrent,1), UBOUND(OutData%uxCurrent,1) - OutData%uxCurrent(i1) = ReKiBuf(Re_Xferred) + OutData%uxCurrent(i1) = REAL(ReKiBuf(Re_Xferred), SiKi) Re_Xferred = Re_Xferred + 1 END DO END IF @@ -12542,7 +12542,7 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg RETURN END IF DO i1 = LBOUND(OutData%uyCurrent,1), UBOUND(OutData%uyCurrent,1) - OutData%uyCurrent(i1) = ReKiBuf(Re_Xferred) + OutData%uyCurrent(i1) = REAL(ReKiBuf(Re_Xferred), SiKi) Re_Xferred = Re_Xferred + 1 END DO END IF From 23227948697d977147298c185d0850311d6e4cf8 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Fri, 4 Mar 2022 08:46:09 -0700 Subject: [PATCH 104/242] MDv2: fix non-standard fortran 'if' statement Could not compile with certain compilers (i.e. gcc7, gcc8, gcc9, gcc10). --- modules/moordyn/src/MoorDyn_Rod.f90 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/moordyn/src/MoorDyn_Rod.f90 b/modules/moordyn/src/MoorDyn_Rod.f90 index 9c1152c895..e20d870d5b 100644 --- a/modules/moordyn/src/MoorDyn_Rod.f90 +++ b/modules/moordyn/src/MoorDyn_Rod.f90 @@ -387,7 +387,7 @@ SUBROUTINE Rod_SetDependentKin(Rod, t, m, initial) ! if this is a zero-length Rod and we're passed initialization, get bending moment-related information from attached lines and compute Rod's equilibrium orientation - if ((N==0) .and. (initial==.FALSE.)) then + if ((N==0) .and. (.not. initial)) then DO l=1,Rod%nAttachedA From 5d2c848327f8fdd92496af33613fd6a16ca880a0 Mon Sep 17 00:00:00 2001 From: Bonnie Jonkman Date: Thu, 9 Dec 2021 09:37:00 -0700 Subject: [PATCH 105/242] AD: do not turn off BEM for all negative TSRs The check to turn off BEM for low TSRs should have been using `ABS(TSR)` instead of `TSR` so that BEM is not disabled for all negative TSR values. https://github.com/OpenFAST/openfast/discussions/940 --- modules/aerodyn/src/BEMT.f90 | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/aerodyn/src/BEMT.f90 b/modules/aerodyn/src/BEMT.f90 index be879948c5..4caee0b8f8 100644 --- a/modules/aerodyn/src/BEMT.f90 +++ b/modules/aerodyn/src/BEMT.f90 @@ -1139,9 +1139,9 @@ subroutine check_turnOffBEMT(p, u, Weight, axInduction, tanInduction, FirstWarn) integer(IntKi) :: i !< blade node counter integer(IntKi) :: j !< blade counter - if( u%TSR < BEMT_upperBoundTSR ) then + if( abs(u%TSR) < BEMT_upperBoundTSR ) then - Weight = BlendCosine( u%TSR, BEMT_lowerBoundTSR, BEMT_upperBoundTSR ) + Weight = BlendCosine( abs(u%TSR), BEMT_lowerBoundTSR, BEMT_upperBoundTSR ) if (FirstWarn) then if (Weight < 1.0_ReKi) then From cf58377c890add573dfb2f332cbb7952ba7c0b11 Mon Sep 17 00:00:00 2001 From: Bonnie Jonkman Date: Tue, 8 Mar 2022 14:24:02 -0700 Subject: [PATCH 106/242] AD/AA: remove unused variables --- modules/aerodyn/src/AeroAcoustics.f90 | 38 +++++++++------------------ 1 file changed, 13 insertions(+), 25 deletions(-) diff --git a/modules/aerodyn/src/AeroAcoustics.f90 b/modules/aerodyn/src/AeroAcoustics.f90 index dbc0c6b214..58d11b8d34 100644 --- a/modules/aerodyn/src/AeroAcoustics.f90 +++ b/modules/aerodyn/src/AeroAcoustics.f90 @@ -1101,7 +1101,7 @@ SUBROUTINE CalcAeroAcousticsOutput(u,p,m,xd,y,errStat,errMsg) ! Amiet's Inflow Noise Model is Calculated as long as InflowNoise is On CALL InflowNoise(AlphaNoise,p%BlChord(J,I),Unoise,m%ChordAngleLE(K,J,I),m%SpanAngleLE(K,J,I),& - elementspan,m%rLEtoObserve(K,J,I),xd%MeanVxVyVz(J,I),xd%TIVx(J,I),m%LE_Location(3,J,I),0.050,p,m%SPLti,errStat2,errMsg2 ) + elementspan,m%rLEtoObserve(K,J,I),xd%TIVx(J,I),p,m%SPLti,errStat2,errMsg2 ) CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) ! If Guidati model (simplified or full version) is also on then the 'SPL correction' to Amiet's model will be added IF ( p%IInflow .EQ. 2 ) THEN @@ -1648,30 +1648,19 @@ SUBROUTINE TIPNOIS(ALPHTIP,ALPRAT2,C,U ,THETA,PHI, R,p,SPLTIP, errStat, errMsg) ENDDO END SUBROUTINE TipNois !==================================================================================================================================! -SUBROUTINE InflowNoise(AlphaNoise,Chord,U,THETA,PHI,d,RObs,MeanVNoise,TINoise,LE_Location,dissip,p,SPLti,errStat,errMsg) -! REAL(ReKi), INTENT(IN ) :: AlphaNoise ! AOA -! REAL(ReKi), INTENT(IN ) :: Chord ! Chord Length -! REAL(ReKi), INTENT(IN ) :: U ! -! REAL(ReKi), INTENT(IN ) :: d ! element span -! REAL(ReKi), INTENT(IN ) :: RObs ! distance to observer -! REAL(ReKi), INTENT(IN ) :: THETA ! -! REAL(ReKi), INTENT(IN ) :: PHI ! Spanwise directivity angle +SUBROUTINE InflowNoise(AlphaNoise,Chord,U,THETA,PHI,d,RObs,TINoise,p,SPLti,errStat,errMsg) + REAL(ReKi), INTENT(IN ) :: AlphaNoise ! AOA + REAL(ReKi), INTENT(IN ) :: Chord ! Chord Length + REAL(ReKi), INTENT(IN ) :: U ! + REAL(ReKi), INTENT(IN ) :: THETA ! + REAL(ReKi), INTENT(IN ) :: PHI ! Spanwise directivity angle + REAL(ReKi), INTENT(IN ) :: d ! element span + REAL(ReKi), INTENT(IN ) :: RObs ! distance to observer ! REAL(ReKi), INTENT(IN ) :: MeanVNoise ! -! REAL(ReKi), INTENT(IN ) :: TINoise ! + REAL(ReKi), INTENT(IN ) :: TINoise ! ! REAL(ReKi), INTENT(IN ) :: LE_Location ! - - REAL(ReKi) :: AlphaNoise ! AOA - REAL(ReKi) :: Chord ! Chord Length - REAL(ReKi) :: U ! - REAL(ReKi) :: d ! element span - REAL(ReKi) :: RObs ! distance to observer - REAL(ReKi) :: THETA ! - REAL(ReKi) :: PHI ! Spanwise directivity angle - REAL(ReKi) :: MeanVNoise ! - REAL(ReKi) :: TINoise ! - REAL(ReKi) :: LE_Location ! - - REAL(ReKi), INTENT(IN ) :: dissip ! + +! REAL(ReKi), INTENT(IN ) :: dissip ! TYPE(AA_ParameterType), INTENT(IN ) :: p ! Parameters REAL(ReKi),DIMENSION(size(p%FreqList)), INTENT( OUT) :: SPLti ! INTEGER(IntKi), INTENT( OUT) :: errStat ! Error status of the operation @@ -2481,8 +2470,7 @@ SUBROUTINE Aero_Tests() !CALL TIPNOIS(AlphaNoise,p%ALpRAT,p%BlChord(J,I),UNoise,m%ChordAngleTE(K,J,I),m%SpanAngleTE(K,J,I), & ! m%rTEtoObserve(K,J,I), p, m%SPLTIP,ErrStat2,errMsg2) !--------Inflow Turbulence Noise ------------------------------------------------! - !CALL InflowNoise(3.0d0,0.22860d0,63.920d0,90.0d0,90.0d0,0.5090d0,1.220d0, & - ! xd%MeanVrel(J,I),0.050d0,0.050d0,p,m%SPLti,ErrStat2,errMsg2 ) + !CALL InflowNoise(3.0d0,0.22860d0,63.920d0,90.0d0,90.0d0,0.5090d0,1.220d0, xd%TIVx(J,I),0.050d0,p,m%SPLti,ErrStat2,errMsg2 ) !CALL FullGuidati(3.0d0,63.920d0,0.22860d0,0.5090d0,1.220d0,90.0d0,90.0d0,xd%MeanVrel(J,I),xd%TIVrel(J,I), & ! p,p%BlAFID(J,I),m%SPLTIGui,ErrStat2 ) !CALL Simple_Guidati(UNoise,0.22860d0,0.120d0,0.020d0,p,m%SPLTIGui,ErrStat2,errMsg2 ) From 17795d7c06361d002847fa73e0acfaa749b81e32 Mon Sep 17 00:00:00 2001 From: Bonnie Jonkman Date: Wed, 9 Mar 2022 14:35:15 -0700 Subject: [PATCH 107/242] AD sync: sort BEMT/FVW outputs in AeroDyn_IO.f90 - some outputs weren't calculated for both FVW and BEMT but could be (merged into one subroutine) - also adds a couple of variables to help sync with Envision --- modules/aerodyn/src/AeroDyn.f90 | 154 +++++++-- modules/aerodyn/src/AeroDyn_IO.f90 | 473 ++++++++++++-------------- modules/aerodyn/src/BEMT_Registry.txt | 1 + modules/aerodyn/src/BEMT_Types.f90 | 54 +++ 4 files changed, 397 insertions(+), 285 deletions(-) diff --git a/modules/aerodyn/src/AeroDyn.f90 b/modules/aerodyn/src/AeroDyn.f90 index 6a0598be7f..29f3a07cfc 100644 --- a/modules/aerodyn/src/AeroDyn.f90 +++ b/modules/aerodyn/src/AeroDyn.f90 @@ -276,7 +276,7 @@ subroutine AD_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, InitOut if (Failed()) return; NumBlades(iR) = InitInp%rotors(iR)%NumBlades p%rotors(iR)%NumBlades = InitInp%rotors(iR)%NumBlades - if (nRotors>1) then + if (nRotors > 1) then p%rotors(iR)%RootName = TRIM(InitInp%RootName)//'.AD.R'//trim(num2lstr(iR)) else p%rotors(iR)%RootName = TRIM(InitInp%RootName)//'.AD' @@ -1160,7 +1160,8 @@ subroutine AD_End( u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) TYPE(AD_MiscVarType), INTENT(INOUT) :: m !< Misc/optimization variables INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status of the operation CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message if ErrStat /= ErrID_None - integer :: iW + + integer :: iW @@ -1513,7 +1514,7 @@ subroutine RotWriteOutputs( t, u, p, p_AD, x, xd, z, OtherState, y, m, m_AD, iRo ! get values to output to file: !------------------------------------------------------- if (p%NumOuts > 0) then - call Calc_WriteOutput( p, p_AD, u, m, m_AD, y, OtherState, xd, indx, iRot, ErrStat2, ErrMsg2 ) + call Calc_WriteOutput( p, p_AD, u, x, m, m_AD, y, OtherState, xd, indx, iRot, ErrStat2, ErrMsg2 ) call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) !............................................................................................................................... @@ -1722,6 +1723,7 @@ subroutine SetInputs(p, p_AD, u, m, indx, errStat, errMsg) endif end subroutine SetInputs +!---------------------------------------------------------------------------------------------------------------------------------- !> Disturbed inflow on the blade if tower shadow or tower influence are enabled subroutine SetDisturbedInflow(p, u, m, errStat, errMsg) type(RotParameterType), intent(in ) :: p !< AD parameters @@ -1777,14 +1779,15 @@ subroutine SetInputsForBEMT(p, u, m, indx, errStat, errMsg) ! note ErrStat and ErrMsg are set in GeomWithoutSweepPitchTwist: ! Get disk average values and orientations - call DiskAvgValues(p, u, m, x_hat_disk, y_hat_disk, z_hat_disk, Azimuth) - call GeomWithoutSweepPitchTwist(p,u,m,thetaBladeNds,ErrStat,ErrMsg) + call DiskAvgValues(p, u, m, x_hat_disk, y_hat_disk, z_hat_disk, Azimuth) ! also sets m%V_diskAvg, m%V_dot_x + call GeomWithoutSweepPitchTwist(p,u,x_hat_disk,m,thetaBladeNds,ErrStat,ErrMsg) if (ErrStat >= AbortErrLev) return ! Velocity in disk normal m%BEMT_u(indx)%Un_disk = m%V_dot_x - ! "Angular velocity of rotor" rad/s + + ! "Angular velocity of rotor" rad/s m%BEMT_u(indx)%omega = dot_product( u%HubMotion%RotationVel(:,1), x_hat_disk ) ! "Angle between the vector normal to the rotor plane and the wind vector (e.g., the yaw angle in the case of no tilt)" rad @@ -1850,8 +1853,10 @@ subroutine SetInputsForBEMT(p, u, m, indx, errStat, errMsg) m%BEMT_u(indx)%UserProp = u%UserProp - ! ................ TSR ..................... + !.......................... + ! TSR + !.......................... if ( EqualRealNos( m%V_dot_x, 0.0_ReKi ) ) then m%BEMT_u(indx)%TSR = 0.0_ReKi else @@ -1924,9 +1929,58 @@ subroutine DiskAvgValues(p, u, m, x_hat_disk, y_hat_disk, z_hat_disk, Azimuth) end subroutine DiskAvgValues !---------------------------------------------------------------------------------------------------------------------------------- -subroutine GeomWithoutSweepPitchTwist(p,u,m,thetaBladeNds,ErrStat,ErrMsg) +subroutine Calculate_MeshOrientation_Rel2Hub(Mesh1, HubMotion, x_hat_disk, orientationAnnulus, elemPosRelToHub_save, elemPosRotorProj_save) + TYPE(MeshType), intent(in) :: Mesh1 !< either BladeMotion or BladeRootMotion mesh + TYPE(MeshType), intent(in) :: HubMotion !< HubMotion mesh + REAL(R8Ki), intent(in) :: x_hat_disk(3) + REAL(ReKi), intent(out) :: orientationAnnulus(3,3,Mesh1%NNodes) + real(R8Ki), optional, intent(out) :: elemPosRelToHub_save( 3,Mesh1%NNodes) + real(R8Ki), optional, intent(out) :: elemPosRotorProj_save(3,Mesh1%NNodes) + + real(R8Ki) :: x_hat_annulus(3) ! rotor normal unit vector (local rotor reference frame) + real(R8Ki) :: y_hat_annulus(3) ! annulus tangent unit vector (local rotor reference frame) + real(R8Ki) :: z_hat_annulus(3) ! annulus radial unit vector (local rotor reference frame) +! real(R8Ki) :: chordVec(3) + + integer(intKi) :: j ! loop counter for nodes + + REAL(R8Ki) :: HubAbsPosition(3) + real(R8Ki) :: elemPosRelToHub(3) ! local copies of + real(R8Ki) :: elemPosRotorProj(3) ! local copies of + + + HubAbsPosition = HubMotion%Position(:,1) + HubMotion%TranslationDisp(:,1) + + !.......................... + ! orientation + !.......................... + + do j=1,Mesh1%NNodes + !chordVec(:,j) = Mesh1%orientation(:,2,j) + ! Project element position onto the rotor plane + elemPosRelToHub = Mesh1%Position(:,j) + Mesh1%TranslationDisp(:,j) - HubAbsPosition ! + 0.00_ReKi*chordVec(:,j)*p%BEMT%chord(j,k) + elemPosRotorProj = elemPosRelToHub - x_hat_disk * dot_product( x_hat_disk, elemPosRelToHub ) + + ! Get unit vectors of the local annulus reference frame + z_hat_annulus = elemPosRotorProj / TwoNorm( elemPosRotorProj ) + x_hat_annulus = x_hat_disk + y_hat_annulus = cross_product( z_hat_annulus, x_hat_annulus ) + + ! Form a orientation matrix for the annulus reference frame + orientationAnnulus(1,:,j) = x_hat_annulus + orientationAnnulus(2,:,j) = y_hat_annulus + orientationAnnulus(3,:,j) = z_hat_annulus + + if (present(elemPosRelToHub_save) ) elemPosRelToHub_save( :,j) = elemPosRelToHub + if (present(elemPosRotorProj_save)) elemPosRotorProj_save(:,j) = elemPosRotorProj + end do + +end subroutine Calculate_MeshOrientation_Rel2Hub +!---------------------------------------------------------------------------------------------------------------------------------- +subroutine GeomWithoutSweepPitchTwist(p,u,x_hat_disk,m,thetaBladeNds,ErrStat,ErrMsg) type(RotParameterType), intent(in ) :: p !< AD parameters type(RotInputType), intent(in ) :: u !< AD Inputs at Time + real(R8Ki), intent(in ) :: x_hat_disk(3) type(RotMiscVarType), intent(inout) :: m !< Misc/optimization variables real(R8Ki), intent( out) :: thetaBladeNds(p%NumBlNds,p%NumBlades) integer(IntKi), intent( out) :: ErrStat !< Error status of the operation @@ -1945,6 +1999,7 @@ subroutine GeomWithoutSweepPitchTwist(p,u,m,thetaBladeNds,ErrStat,ErrMsg) ErrMsg = "" if (p%AeroProjMod==0) then + ! theta, "Twist angle (includes all sources of twist)" rad ! Vx, "Local axial velocity at node" m/s ! Vy, "Local tangential velocity at node" m/s @@ -1974,22 +2029,18 @@ subroutine GeomWithoutSweepPitchTwist(p,u,m,thetaBladeNds,ErrStat,ErrMsg) call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) theta = EulerExtract( orientation ) !root(k)WithoutPitch_theta(j)_blade(k) - thetaBladeNds(j,k) = -theta(3) ! local pitch + twist (aerodyanmic + elastic) angle of the jth node in the kth blade - + m%Curve( j,k) = theta(2) ! save value for possible output later + thetaBladeNds(j,k) = -theta(3) ! local pitch + twist (aerodyanmic + elastic) angle of the jth node in the kth blade theta(1) = 0.0_ReKi theta(3) = 0.0_ReKi - m%Curve(j,k) = theta(2) ! save value for possible output later m%WithoutSweepPitchTwist(:,:,j,k) = matmul( EulerConstruct( theta ), orientation_nopitch ) ! WithoutSweepPitch+Twist_theta(j)_Blade(k) end do !j=nodes end do !k=blades + else if (p%AeroProjMod==1) then - !m%AllOuts( BPitch( k) ) = 0.0_ReKi ! save this value of pitch for potential output; ill-defined, TODO - ! - !m%hub_theta_x_root(k) = 0.0_ReKi ! ill-defined, TODO - do k=1,p%NumBlades call LAPACK_gemm( 'n', 't', 1.0_R8Ki, u%BladeRootMotion(k)%Orientation(:,:,1), u%HubMotion%Orientation(:,:,1), 0.0_R8Ki, orientation, errStat2, errMsg2) call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) @@ -2000,16 +2051,25 @@ subroutine GeomWithoutSweepPitchTwist(p,u,m,thetaBladeNds,ErrStat,ErrMsg) theta(3) = 0.0_ReKi m%hub_theta_x_root(k) = theta(1) ! save this value for FAST.Farm end do - - ! Generic blade, we don't assume where the axes are, and we keep the default orientation + + do k=1,p%NumBlades - + call Calculate_MeshOrientation_Rel2Hub(u%BladeMotion(k), u%HubMotion, x_hat_disk, m%WithoutSweepPitchTwist(:,:,:,k)) + do j=1,p%NumBlNds - thetaBladeNds(j,k) = 0.0_ReKi ! local pitch + twist (aerodyanmic + elastic) angle of the jth node in the kth blade - m%Curve(j,k) = 0.0_ReKi ! ill-defined, TODO m%WithoutSweepPitchTwist(:,:,j,k) = u%BladeMotion(k)%Orientation(:,:,j) enddo enddo + + do k=1,p%NumBlades + do j=1,p%NumBlNds + orientation = matmul( u%BladeMotion(k)%Orientation(:,:,j), transpose( m%WithoutSweepPitchTwist(:,:,j,k) ) ) + theta = EulerExtract( orientation ) + m%Curve( j,k) = theta(2) + thetaBladeNds(j,k) = -theta(3) + enddo + enddo + else ErrStat = ErrID_Fatal ErrMsg ='GeomWithoutSweepPitchTwist: AeroProjMod not supported '//trim(num2lstr(p%AeroProjMod)) @@ -2040,7 +2100,7 @@ subroutine SetInputsForFVW(p, u, m, errStat, errMsg) ! Get disk average values and orientations ! NOTE: needed because it sets m%V_diskAvg and m%V_dot_x, needed by CalcOutput.. call DiskAvgValues(p%rotors(iR), u(tIndx)%rotors(iR), m%rotors(iR), x_hat_disk) ! also sets m%V_diskAvg and m%V_dot_x - call GeomWithoutSweepPitchTwist(p%rotors(iR),u(tIndx)%rotors(iR), m%rotors(iR), thetaBladeNds,ErrStat,ErrMsg) + call GeomWithoutSweepPitchTwist(p%rotors(iR),u(tIndx)%rotors(iR), x_hat_disk, m%rotors(iR), thetaBladeNds,ErrStat,ErrMsg) if (ErrStat >= AbortErrLev) return ! Rather than use a meshcopy, we will just copy what we need to the WingsMesh @@ -2310,7 +2370,7 @@ subroutine SetOutputsFromFVW(t, u, p, OtherState, x, xd, m, y, ErrStat, ErrMsg) end subroutine SetOutputsFromFVW !---------------------------------------------------------------------------------------------------------------------------------- -!> This routine validates the inputs from the AeroDyn input files. +!> This routine validates the number of blades on each rotor. SUBROUTINE ValidateNumBlades( NumBl, ErrStat, ErrMsg ) integer(IntKi), intent(in) :: NumBl !< Number of blades integer(IntKi), intent(out) :: ErrStat !< Error status @@ -2342,6 +2402,17 @@ SUBROUTINE ValidateInputData( InitInp, InputFileData, NumBl, ErrStat, ErrMsg ) ErrStat = ErrID_None ErrMsg = "" +! do iR = 1,size(NumBl) +! if (NumBl(iR) < 1) then +! call SetErrStat( ErrID_Fatal, 'Number of blades must be at least 1.', ErrStat, ErrMsg, RoutineName ) +! return ! return early because InputFileData%BladeProps may not be allocated properly otherwise... +! else +! if (NumBl(iR) > AD_MaxBl_Out .and. InitInp%Linearize) then +! call SetErrStat( ErrID_Fatal, 'Number of blades must be no larger than '//trim(num2lstr(AD_MaxBl_Out))//' for linearizaton analysis.', ErrStat, ErrMsg, RoutineName ) +! return ! return early because InputFileData%BladeProps may not be allocated properly otherwise... +! end if +! end if +! end do if (InputFileData%DTAero <= 0.0) call SetErrStat ( ErrID_Fatal, 'DTAero must be greater than zero.', ErrStat, ErrMsg, RoutineName ) if (InputFileData%WakeMod /= WakeMod_None .and. InputFileData%WakeMod /= WakeMod_BEMT .and. InputFileData%WakeMod /= WakeMod_DBEMT .and. InputFileData%WakeMod /= WakeMod_FVW) then @@ -2748,6 +2819,7 @@ SUBROUTINE Init_BEMTmodule( InputFileData, RotInputFileData, u_AD, u, p, p_AD, x real(ReKi) :: tmp(3), tmp_sz_y, tmp_sz real(ReKi) :: y_hat_disk(3) real(ReKi) :: z_hat_disk(3) + real(ReKi) :: position(3) real(ReKi) :: rMax real(ReKi) :: frac integer(IntKi) :: ErrStat2 @@ -2786,7 +2858,7 @@ SUBROUTINE Init_BEMTmodule( InputFileData, RotInputFileData, u_AD, u, p, p_AD, x call AllocAry(InitInp%zLocal,InitInp%numBladeNodes,InitInp%numBlades,'zLocal', ErrStat2,ErrMsg2); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) call AllocAry(InitInp%rLocal,InitInp%numBladeNodes,InitInp%numBlades,'rLocal', ErrStat2,ErrMsg2); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) call AllocAry(InitInp%zTip, InitInp%numBlades,'zTip', ErrStat2,ErrMsg2); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) - + call AllocAry(InitInp%rTipFix, InitInp%numBlades,'rTipFix',ErrStat2,ErrMsg2); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) call AllocAry(InitInp%UAOff_innerNode, InitInp%numBlades,'UAOff_innerNode',ErrStat2,ErrMsg2); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) call AllocAry(InitInp%UAOff_outerNode, InitInp%numBlades,'UAOff_outerNode',ErrStat2,ErrMsg2); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) @@ -2796,7 +2868,7 @@ SUBROUTINE Init_BEMTmodule( InputFileData, RotInputFileData, u_AD, u, p, p_AD, x end if - ! Compute zLocal, zHub, zTip, rLocal, rMax + ! Compute zLocal, zHub, zTip, rLocal, rMax, rTipFix rMax = 0.0_ReKi do k=1,p%numBlades @@ -2823,7 +2895,25 @@ SUBROUTINE Init_BEMTmodule( InputFileData, RotInputFileData, u_AD, u, p, p_AD, x tmp_sz = dot_product( tmp, z_hat_disk )**2 InitInp%rLocal(j,k) = sqrt( tmp_sz + tmp_sz_y ) rMax = max(rMax, InitInp%rLocal(j,k)) - end do !j=nodes + end do !j=nodes + + + !......... + ! compute fixed rLocal at tip node (without prebend) for Bladed-like calculations: + !......... + tmp(1) = 0.0_ReKi !RotInputFile%BladeProps(k)%BlCrvAC(p%NumBlNds) + tmp(2) = 0.0_ReKi !RotInputFile%BladeProps(k)%BlSwpAC(p%NumBlNds) + tmp(3) = RotInputFileData%BladeProps(k)%BlSpn(p%NumBlNds) + position = u_AD%BladeRootMotion(k)%Position(:,1) + matmul(tmp,u_AD%BladeRootMotion(k)%RefOrientation(:,:,1)) ! note that because positionL is a 1-D array, we're doing the transpose of matmul(transpose(u%BladeRootMotion(k)%RefOrientation),positionL) + + ! position of the coned tip node in the kth blade relative to the hub: + tmp = position - u_AD%HubMotion%Position(:,1) + + ! local radius (normalized distance from rotor centerline) + tmp_sz_y = dot_product( tmp, y_hat_disk )**2 + tmp_sz = dot_product( tmp, z_hat_disk )**2 + InitInp%rTipFix(k) = sqrt( tmp_sz + tmp_sz_y ) + end do !k=blades @@ -3313,7 +3403,7 @@ FUNCTION CalculateTowerInfluence(p, xbar, ybar, zbar, W_tower, TwrCd, TwrTI) RES u_TwrShadow = -TwrCd / denom * cos( PiBy2*ybar / denom )**2 end if end if - case (TwrShadow_Eames) + case (TwrShadow_Eames) if ( xbar > 0.0_ReKi .and. abs(zbar) < 1.0_ReKi) then exponential = ( ybar / (TwrTI * xbar) )**2 denom = TwrTI * xbar * sqrt( TwoPi ) @@ -3395,6 +3485,9 @@ SUBROUTINE getLocalTowerProps(p, u, BladeNodePosition, theta_tower_trans, W_towe ! Inside the tower, or very close, (will happen for vortex elements) we keep undisturbed inflow ! We don't want to reach the stagnation points DisturbInflow = .false. + !elseif ( TwrClrnc<= 0.0_ReKi) then + ! ! Tower strike + ! DisturbInflow = .false. else DisturbInflow = .true. end if @@ -3884,7 +3977,7 @@ SUBROUTINE Rot_JacobianPInput( t, u, p, p_AD, x, xd, z, OtherState, y, m, m_AD, ! get central difference: - call Compute_dY( p, y_p, y_m, delta_p, delta_m, dYdu(:,i) ) + call Compute_dY( p, p_AD, y_p, y_m, delta_p, delta_m, dYdu(:,i) ) end do @@ -4166,7 +4259,7 @@ SUBROUTINE RotJacobianPContState( t, u, p, p_AD, x, xd, z, OtherState, y, m, m_A ! get central difference: - call Compute_dY( p, y_p, y_m, delta_p, delta_m, dYdx(:,i) ) + call Compute_dY( p, p_AD, y_p, y_m, delta_p, delta_m, dYdx(:,i) ) end do @@ -4533,7 +4626,7 @@ SUBROUTINE RotJacobianPConstrState( t, u, p, p_AD, x, xd, z, OtherState, y, m, m ! get central difference: - call Compute_dY( p, y_p, y_m, delta_p, delta_m, dYdz(:,i) ) + call Compute_dY( p, p_AD, y_p, y_m, delta_p, delta_m, dYdz(:,i) ) ! put z_perturb back (for next iteration): @@ -5645,9 +5738,10 @@ END SUBROUTINE Perturb_x !---------------------------------------------------------------------------------------------------------------------------------- !> This routine uses values of two output types to compute an array of differences. !! Do not change this packing without making sure subroutine aerodyn::init_jacobian is consistant with this routine! -SUBROUTINE Compute_dY(p, y_p, y_m, delta_p, delta_m, dY) +SUBROUTINE Compute_dY(p, p_AD, y_p, y_m, delta_p, delta_m, dY) TYPE(RotParameterType) , INTENT(IN ) :: p !< parameters + TYPE(AD_ParameterType) , INTENT(IN ) :: p_AD !< parameters TYPE(RotOutputType) , INTENT(IN ) :: y_p !< AD outputs at \f$ u + \Delta_p u \f$ or \f$ x + \Delta_p x \f$ (p=plus) TYPE(RotOutputType) , INTENT(IN ) :: y_m !< AD outputs at \f$ u - \Delta_m u \f$ or \f$ x - \Delta_m x \f$ (m=minus) REAL(R8Ki) , INTENT(IN ) :: delta_p !< difference in inputs or states \f$ delta_p = \Delta_p u \f$ or \f$ delta_p = \Delta_p x \f$ diff --git a/modules/aerodyn/src/AeroDyn_IO.f90 b/modules/aerodyn/src/AeroDyn_IO.f90 index 9273b345ec..e31a73161d 100644 --- a/modules/aerodyn/src/AeroDyn_IO.f90 +++ b/modules/aerodyn/src/AeroDyn_IO.f90 @@ -1654,89 +1654,98 @@ END FUNCTION Calc_Chi0 !---------------------------------------------------------------------------------------------------------------------------------- -SUBROUTINE Calc_WriteOutput( p, p_AD, u, m, m_AD, y, OtherState, xd, indx, iRot, ErrStat, ErrMsg ) +SUBROUTINE Calc_WriteOutput( p, p_AD, u, x, m, m_AD, y, OtherState, xd, indx, iRot, ErrStat, ErrMsg ) - TYPE(RotParameterType), INTENT(IN ) :: p ! The rotor parameters - TYPE(AD_ParameterType), INTENT(IN ) :: p_AD ! The module parameters - TYPE(RotInputType), INTENT(IN ) :: u ! inputs - TYPE(RotMiscVarType), INTENT(INOUT) :: m ! misc variables - TYPE(AD_MiscVarType), INTENT(INOUT) :: m_AD ! misc variables - TYPE(RotOutputType), INTENT(IN ) :: y ! outputs - TYPE(RotOtherStateType), INTENT(IN ) :: OtherState ! other states at t (for DBEMT and UA) - TYPE(RotDiscreteStateType),INTENT(IN ) :: xd ! Discrete states - integer, intent(in ) :: indx ! index into m%BEMT_u(indx) array; 1=t and 2=t+dt (but not checked here) - integer, intent(in ) :: iRot ! Rotor index, needed for FVW - INTEGER(IntKi), INTENT( OUT) :: ErrStat ! The error status code - CHARACTER(*), INTENT( OUT) :: ErrMsg ! The error message, if an error occurred + TYPE(RotParameterType), INTENT(IN ) :: p ! The rotor parameters + TYPE(AD_ParameterType), INTENT(IN ) :: p_AD ! The module parameters + TYPE(RotInputType), INTENT(IN ) :: u ! inputs + TYPE(RotContinuousStateType), INTENT(IN ) :: x !< Continuous states at t + TYPE(RotMiscVarType), INTENT(INOUT) :: m ! misc variables + TYPE(AD_MiscVarType), INTENT(INOUT) :: m_AD ! misc variables + TYPE(RotOutputType), INTENT(IN ) :: y ! outputs + TYPE(RotOtherStateType), INTENT(IN ) :: OtherState ! other states at t (for DBEMT and UA) + TYPE(RotDiscreteStateType), INTENT(IN ) :: xd ! Discrete states + integer, intent(in ) :: indx ! index into m%BEMT_u(indx) array; 1=t and 2=t+dt (but not checked here) + integer, intent(in ) :: iRot ! Rotor index, needed for FVW + INTEGER(IntKi), INTENT( OUT) :: ErrStat ! The error status code + CHARACTER(*), INTENT( OUT) :: ErrMsg ! The error message, if an error occurred ! local variables - CHARACTER(*), PARAMETER :: RoutineName = 'Calc_WriteOutput' - INTEGER(intKi) :: ErrStat2 - CHARACTER(ErrMsgLen) :: ErrMsg2 - - INTEGER(IntKi) :: j,k,beta - REAL(ReKi) :: tmp(3) - REAL(ReKi) :: force(3) - REAL(ReKi) :: moment(3) - REAL(ReKi) :: denom, rmax - REAL(ReKi) :: ct, st ! cosine, sine of theta - REAL(ReKi) :: cp, sp ! cosine, sine of phi - + CHARACTER(*), PARAMETER :: RoutineName = 'Calc_WriteOutput' + INTEGER(intKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + INTEGER(IntKi) :: j,k,beta + REAL(ReKi) :: tmp(3) + REAL(ReKi) :: force(3) + REAL(ReKi) :: moment(3) + REAL(ReKi) :: denom, rmax, omega + REAL(ReKi) :: ct, st ! cosine, sine of theta + REAL(ReKi) :: cp, sp ! cosine, sine of phi + ! start routine: ErrStat = ErrID_None ErrMsg = "" - ! tower outputs - do beta=1,p%NTwOuts - j = p%TwOutNd(beta) - - tmp = matmul( u%TowerMotion%Orientation(:,:,j) , u%InflowOnTower(:,j) ) - m%AllOuts( TwNVUnd(:,beta) ) = tmp - - tmp = matmul( u%TowerMotion%Orientation(:,:,j) , u%TowerMotion%TranslationVel(:,j) ) - m%AllOuts( TwNSTV(:,beta) ) = tmp - - m%AllOuts( TwNVrel(beta) ) = m%W_Twr(j) ! relative velocity - m%AllOuts( TwNDynP(beta) ) = 0.5 * p%AirDens * m%W_Twr(j)**2 ! dynamic pressure - m%AllOuts( TwNRe( beta) ) = p%TwrDiam(j) * m%W_Twr(j) / p%KinVisc / 1.0E6 ! reynolds number (in millions) - m%AllOuts( TwNM( beta) ) = m%W_Twr(j) / p%SpdSound ! Mach number - m%AllOuts( TwNFdx( beta) ) = m%X_Twr(j) - m%AllOuts( TwNFdy( beta) ) = m%Y_Twr(j) - - end do ! out nodes + ! Compute max radius and rotor speed if (p_AD%WakeMod /= WakeMod_FVW) then - call Calc_WriteOutput_BEMT + rmax = 0.0_ReKi + do k=1,p%NumBlades + do j=1,p%NumBlNds + rmax = max(rmax, m%BEMT_u(indx)%rLocal(j,k) ) + end do !j=nodes + end do !k=blades + +! rmax = p%BEMT%rTipFixMax + omega = m%BEMT_u(indx)%omega else - call Calc_WriteOutput_FVW + rmax = Calc_MaxRadius(p, u) + omega = Calc_Omega(u) endif + + call Calc_WriteOutput_AD() ! need to call this before calling the BEMT vs FVW versions of outputs so that the integrated output quantities are known + + if (p_AD%WakeMod /= WakeMod_FVW) then + call Calc_WriteOutput_BEMT() + else + call Calc_WriteOutput_FVW() + endif - ! blade node tower clearance (requires tower influence calculation): - if (p%TwrPotent /= TwrPotent_none .or. p%TwrShadow /= TwrShadow_none) then - do k=1,p%numBlades - do beta=1,p%NBlOuts - j=p%BlOutNd(beta) - m%AllOuts( BNClrnc( beta,k) ) = m%TwrClrnc(j,k) - end do - end do - end if + ! set these for debugging +! m%AllOuts( Debug1 ) = 0.0_ReKi !TwoNorm( m%BEMT%u_SkewWake(1)%v_qsw ) +! m%AllOuts( Debug2 ) = 0.0_ReKi !TwoNorm( x%BEMT%v_w ) +! m%AllOuts( Debug3 ) = 0.0_ReKi CONTAINS !.......................................................................................... - subroutine Calc_WriteOutput_BEMT - real(ReKi) :: omega - omega = m%BEMT_u(indx)%omega + subroutine Calc_WriteOutput_AD() + + ! tower outputs + do beta=1,p%NTwOuts + j = p%TwOutNd(beta) + + tmp = matmul( u%TowerMotion%Orientation(:,:,j) , u%InflowOnTower(:,j) ) + m%AllOuts( TwNVUnd(:,beta) ) = tmp + + tmp = matmul( u%TowerMotion%Orientation(:,:,j) , u%TowerMotion%TranslationVel(:,j) ) + m%AllOuts( TwNSTV(:,beta) ) = tmp + + m%AllOuts( TwNVrel(beta) ) = m%W_Twr(j) ! relative velocity + m%AllOuts( TwNDynP(beta) ) = 0.5 * p%AirDens * m%W_Twr(j)**2 ! dynamic pressure + m%AllOuts( TwNRe( beta) ) = p%TwrDiam(j) * m%W_Twr(j) / p%KinVisc / 1.0E6 ! reynolds number (in millions) + m%AllOuts( TwNM( beta) ) = m%W_Twr(j) / p%SpdSound ! Mach number + m%AllOuts( TwNFdx( beta) ) = m%X_Twr(j) + m%AllOuts( TwNFdy( beta) ) = m%Y_Twr(j) + + end do ! out nodes + ! blade outputs do k=1,min(p%numBlades,3) ! limit this - m%AllOuts( BAzimuth(k) ) = MODULO( m%BEMT_u(indx)%psi(k)*R2D, 360.0_ReKi ) - ! m%AllOuts( BPitch( k) ) = calculated in SetInputsForBEMT - do beta=1,p%NBlOuts - j=p%BlOutNd(beta) tmp = matmul( m%WithoutSweepPitchTwist(:,:,j,k), u%InflowOnBlade(:,j,k) ) @@ -1753,72 +1762,39 @@ subroutine Calc_WriteOutput_BEMT m%AllOuts( BNSTVx( beta,k) ) = tmp(1) m%AllOuts( BNSTVy( beta,k) ) = tmp(2) m%AllOuts( BNSTVz( beta,k) ) = tmp(3) - - m%AllOuts( BNVrel( beta,k) ) = m%BEMT_y%Vrel(j,k) - m%AllOuts( BNDynP( beta,k) ) = 0.5 * p%airDens * m%BEMT_y%Vrel(j,k)**2 - m%AllOuts( BNRe( beta,k) ) = p%BEMT%chord(j,k) * m%BEMT_y%Vrel(j,k) / p%KinVisc / 1.0E6 - m%AllOuts( BNM( beta,k) ) = m%BEMT_y%Vrel(j,k) / p%SpdSound - - m%AllOuts( BNVIndx(beta,k) ) = - m%BEMT_u(indx)%Vx(j,k) * m%BEMT_y%axInduction( j,k) - m%AllOuts( BNVIndy(beta,k) ) = m%BEMT_u(indx)%Vy(j,k) * m%BEMT_y%tanInduction(j,k) - - m%AllOuts( BNAxInd(beta,k) ) = m%BEMT_y%axInduction(j,k) - m%AllOuts( BNTnInd(beta,k) ) = m%BEMT_y%tanInduction(j,k) - - m%AllOuts( BNAlpha(beta,k) ) = Rad2M180to180Deg( m%BEMT_y%phi(j,k) - m%BEMT_u(indx)%theta(j,k) ) - m%AllOuts( BNTheta(beta,k) ) = m%BEMT_u(indx)%theta(j,k)*R2D - m%AllOuts( BNPhi( beta,k) ) = m%BEMT_y%phi(j,k)*R2D + m%AllOuts( BNCurve(beta,k) ) = m%Curve(j,k)*R2D - - !m%AllOuts( BNCl( beta,k) ) = m%BEMT_y%Cl(j,k) - !m%AllOuts( BNCd( beta,k) ) = m%BEMT_y%Cd(j,k) - - m%AllOuts( BNCpmin( beta,k) ) = m%BEMT_y%Cpmin(j,k) + m%AllOuts( BNSigCr( beta,k) ) = m%SigmaCavitCrit(j,k) m%AllOuts( BNSgCav( beta,k) ) = m%SigmaCavit(j,k) - cp=cos(m%BEMT_y%phi(j,k)) - sp=sin(m%BEMT_y%phi(j,k)) - m%AllOuts( BNCl( beta,k) ) = m%BEMT_y%Cx(j,k)*cp + m%BEMT_y%Cy(j,k)*sp - m%AllOuts( BNCd( beta,k) ) = m%BEMT_y%Cx(j,k)*sp - m%BEMT_y%Cy(j,k)*cp - m%AllOuts( BNCm( beta,k) ) = m%BEMT_y%Cm(j,k) - m%AllOuts( BNCx( beta,k) ) = m%BEMT_y%Cx(j,k) - m%AllOuts( BNCy( beta,k) ) = m%BEMT_y%Cy(j,k) - - ct=cos(m%BEMT_u(indx)%theta(j,k)) - st=sin(m%BEMT_u(indx)%theta(j,k)) - m%AllOuts( BNCn( beta,k) ) = m%BEMT_y%Cx(j,k)*ct + m%BEMT_y%Cy(j,k)*st - m%AllOuts( BNCt( beta,k) ) =-m%BEMT_y%Cx(j,k)*st + m%BEMT_y%Cy(j,k)*ct - - m%AllOuts( BNFl( beta,k) ) = m%X(j,k)*cp - m%Y(j,k)*sp - m%AllOuts( BNFd( beta,k) ) = m%X(j,k)*sp + m%Y(j,k)*cp - m%AllOuts( BNMm( beta,k) ) = m%M(j,k) - m%AllOuts( BNFx( beta,k) ) = m%X(j,k) - m%AllOuts( BNFy( beta,k) ) = -m%Y(j,k) - m%AllOuts( BNFn( beta,k) ) = m%X(j,k)*ct - m%Y(j,k)*st - m%AllOuts( BNFt( beta,k) ) = -m%X(j,k)*st - m%Y(j,k)*ct - - m%AllOuts( BNGam( beta,k) ) = 0.5_ReKi * p%BEMT%chord(j,k) * m%BEMT_y%Vrel(j,k) * m%BEMT_y%Cl(j,k) ! "Gam" [m^2/s] end do ! nodes end do ! blades + - ! rotor outputs: - rmax = 0.0_ReKi - do k=1,p%NumBlades - do j=1,p%NumBlNds - rmax = max(rmax, m%BEMT_u(indx)%rLocal(j,k) ) - end do !j=nodes - end do !k=blades - m%AllOuts( RtSpeed ) = m%BEMT_u(indx)%omega*RPS2RPM - m%AllOuts( RtArea ) = pi*rmax**2 + ! blade node tower clearance (requires tower influence calculation): + if (p%TwrPotent /= TwrPotent_none .or. p%TwrShadow /= TwrShadow_none) then + do k=1,p%numBlades + do beta=1,p%NBlOuts + j=p%BlOutNd(beta) + m%AllOuts( BNClrnc( beta,k) ) = m%TwrClrnc(j,k) + end do + end do + end if + + + + m%AllOuts( RtSpeed ) = omega*RPS2RPM + m%AllOuts( RtArea ) = pi * rmax**2 + tmp = matmul( u%HubMotion%Orientation(:,:,1), m%V_DiskAvg ) m%AllOuts( RtVAvgxh ) = tmp(1) m%AllOuts( RtVAvgyh ) = tmp(2) m%AllOuts( RtVAvgzh ) = tmp(3) - m%AllOuts( RtSkew ) = m%BEMT_u(indx)%chi0*R2D + ! integrate force/moments over blades by performing mesh transfer to hub point: force = 0.0_ReKi @@ -1827,76 +1803,155 @@ subroutine Calc_WriteOutput_BEMT call Transfer_Line2_to_Point( y%BladeLoad(k), m%HubLoad, m%B_L_2_H_P(k), ErrStat2, ErrMsg2, u%BladeMotion(k), u%HubMotion ) force = force + m%HubLoad%force( :,1) moment = moment + m%HubLoad%moment(:,1) - - if (k<=4) then + + if (k<=size(BAeroFxg)) then ! Power contribution of blade wrt hub tmp = matmul( u%HubMotion%Orientation(:,:,1), m%HubLoad%moment(:,1) ) - m%AllOuts( BAeroPwr(k) ) = omega * tmp(1) - - ! In global, wrt hub! + m%AllOuts( BAeroPwr(k) ) = omega * tmp(1) + + ! In global, wrt hub! m%AllOuts( BAeroFxg(k) ) = m%HubLoad%force(1,1) m%AllOuts( BAeroFyg(k) ) = m%HubLoad%force(2,1) m%AllOuts( BAeroFzg(k) ) = m%HubLoad%force(3,1) m%AllOuts( BAeroMxg(k) ) = m%HubLoad%moment(1,1) m%AllOuts( BAeroMyg(k) ) = m%HubLoad%moment(2,1) m%AllOuts( BAeroMzg(k) ) = m%HubLoad%moment(3,1) - - endif + end if end do - ! In global + + ! In global m%AllOuts( RtAeroFxg ) = force(1) m%AllOuts( RtAeroFyg ) = force(2) m%AllOuts( RtAeroFzg ) = force(3) m%AllOuts( RtAeroMxg ) = moment(1) m%AllOuts( RtAeroMyg ) = moment(2) m%AllOuts( RtAeroMzg ) = moment(3) - ! In hub coord tmp = matmul( u%HubMotion%Orientation(:,:,1), force ) m%AllOuts( RtAeroFxh ) = tmp(1) m%AllOuts( RtAeroFyh ) = tmp(2) m%AllOuts( RtAeroFzh ) = tmp(3) - + tmp = matmul( u%HubMotion%Orientation(:,:,1), moment ) m%AllOuts( RtAeroMxh ) = tmp(1) m%AllOuts( RtAeroMyh ) = tmp(2) m%AllOuts( RtAeroMzh ) = tmp(3) - - m%AllOuts( RtAeroPwr ) = m%BEMT_u(indx)%omega * m%AllOuts( RtAeroMxh ) - - - - m%AllOuts( RtTSR ) = m%BEMT_u(indx)%TSR - - if ( EqualRealNos( m%V_dot_x, 0.0_ReKi ) ) then - m%AllOuts( RtTSR ) = 0.0_ReKi - m%AllOuts( RtAeroCp ) = 0.0_ReKi - m%AllOuts( RtAeroCq ) = 0.0_ReKi - m%AllOuts( RtAeroCt ) = 0.0_ReKi - else - denom = 0.5*p%AirDens*m%AllOuts( RtArea )*m%V_dot_x**2 - m%AllOuts( RtTSR ) = m%BEMT_u(indx)%omega * rmax / m%V_dot_x - - m%AllOuts( RtAeroCp ) = m%AllOuts( RtAeroPwr ) / (denom * m%V_dot_x) - m%AllOuts( RtAeroCq ) = m%AllOuts( RtAeroMxh ) / (denom * rmax) - m%AllOuts( RtAeroCt ) = m%AllOuts( RtAeroFxh ) / denom - end if + + m%AllOuts( RtAeroPwr ) = omega * m%AllOuts( RtAeroMxh ) + + ! Integrate force/moments over blades by performing mesh transfer to blade root points: - do k=1,min(p%NumBlades,4) ! Temporary hack for at least one more blae outputs + do k=1,p%NumBlades call Transfer_Line2_to_Point( y%BladeLoad(k), m%BladeRootLoad(k), m%B_L_2_R_P(k), ErrStat2, ErrMsg2, u%BladeMotion(k), u%BladeRootMotion(k) ) + end do + do k=1,min(p%NumBlades,size(BAeroFx)) ! Transform force vector to blade root coordinate system tmp = matmul( u%BladeRootMotion(k)%Orientation(:,:,1), m%BladeRootLoad(k)%force( :,1) ) m%AllOuts( BAeroFx(k) ) = tmp(1) m%AllOuts( BAeroFy(k) ) = tmp(2) m%AllOuts( BAeroFz(k) ) = tmp(3) + ! Transform moment vector to blade root coordinate system tmp = matmul( u%BladeRootMotion(k)%Orientation(:,:,1), m%BladeRootLoad(k)%moment( :,1) ) m%AllOuts( BAeroMx(k) ) = tmp(1) m%AllOuts( BAeroMy(k) ) = tmp(2) m%AllOuts( BAeroMz(k) ) = tmp(3) - end do ! k=blades + end do ! k=blades + + ! rotor outputs + if ( EqualRealNos( m%V_dot_x, 0.0_ReKi ) ) then + m%AllOuts( RtTSR ) = 0.0_ReKi + m%AllOuts( RtAeroCp ) = 0.0_ReKi + m%AllOuts( RtAeroCq ) = 0.0_ReKi + m%AllOuts( RtAeroCt ) = 0.0_ReKi + else + m%AllOuts( RtTSR ) = omega * rmax / m%V_dot_x + + denom = 0.5*p%AirDens*m%AllOuts( RtArea )*m%V_dot_x**2 + m%AllOuts( RtAeroCp ) = m%AllOuts( RtAeroPwr ) / (denom * m%V_dot_x) + m%AllOuts( RtAeroCq ) = m%AllOuts( RtAeroMxh ) / (denom * rmax ) + m%AllOuts( RtAeroCt ) = m%AllOuts( RtAeroFxh ) / denom + end if + + + end subroutine Calc_WriteOutput_AD + !.......................................................................................... + subroutine Calc_WriteOutput_BEMT() + REAL(R8Ki) :: orient(3,3) + REAL(R8Ki) :: theta(3) + REAL(ReKi) :: denom !, rmax + REAL(ReKi) :: ct, st ! cosine, sine of theta + REAL(ReKi) :: cp, sp ! cosine, sine of phi + + + + + ! blade outputs + do k=1,min(p%numBlades,size(BAzimuth) ) ! limit this + m%AllOuts( BAzimuth(k) ) = MODULO( m%BEMT_u(indx)%psi(k)*R2D, 360.0_ReKi ) + ! m%AllOuts( BPitch( k) ) = calculated in SetInputsForBEMT + + do beta=1,p%NBlOuts + + j=p%BlOutNd(beta) + + m%AllOuts( BNVrel( beta,k) ) = m%BEMT_y%Vrel(j,k) + m%AllOuts( BNDynP( beta,k) ) = 0.5 * p%airDens * m%BEMT_y%Vrel(j,k)**2 + m%AllOuts( BNRe( beta,k) ) = p%BEMT%chord(j,k) * m%BEMT_y%Vrel(j,k) / p%KinVisc / 1.0E6 + m%AllOuts( BNM( beta,k) ) = m%BEMT_y%Vrel(j,k) / p%SpdSound + + m%AllOuts( BNVIndx(beta,k) ) = - m%BEMT_u(indx)%Vx(j,k) * m%BEMT_y%axInduction( j,k) + m%AllOuts( BNVIndy(beta,k) ) = m%BEMT_u(indx)%Vy(j,k) * m%BEMT_y%tanInduction(j,k) + + m%AllOuts( BNAxInd(beta,k) ) = m%BEMT_y%axInduction(j,k) + m%AllOuts( BNTnInd(beta,k) ) = m%BEMT_y%tanInduction(j,k) + + m%AllOuts( BNAlpha(beta,k) ) = Rad2M180to180Deg( m%BEMT_y%phi(j,k) - m%BEMT_u(indx)%theta(j,k) ) + m%AllOuts( BNTheta(beta,k) ) = m%BEMT_u(indx)%theta(j,k)*R2D + m%AllOuts( BNPhi( beta,k) ) = m%BEMT_y%phi(j,k)*R2D + + ! m%AllOuts( BNCurve(beta,k) ) = m%Curve(j,k)*R2D + + m%AllOuts( BNCpmin( beta,k) ) = m%BEMT_y%Cpmin(j,k) + ! m%AllOuts( BNSigCr( beta,k) ) = m%SigmaCavitCrit(j,k) + ! m%AllOuts( BNSgCav( beta,k) ) = m%SigmaCavit(j,k) + + !m%AllOuts( BNCl( beta,k) ) = m%BEMT_y%Cl(j,k) + !m%AllOuts( BNCd( beta,k) ) = m%BEMT_y%Cd(j,k) + cp=cos(m%BEMT_y%phi(j,k)) + sp=sin(m%BEMT_y%phi(j,k)) + m%AllOuts( BNCl( beta,k) ) = m%BEMT_y%Cx(j,k)*cp + m%BEMT_y%Cy(j,k)*sp + m%AllOuts( BNCd( beta,k) ) = m%BEMT_y%Cx(j,k)*sp - m%BEMT_y%Cy(j,k)*cp + m%AllOuts( BNCm( beta,k) ) = m%BEMT_y%Cm(j,k) + m%AllOuts( BNCx( beta,k) ) = m%BEMT_y%Cx(j,k) + m%AllOuts( BNCy( beta,k) ) = m%BEMT_y%Cy(j,k) + + ct=cos(m%BEMT_u(indx)%theta(j,k)) + st=sin(m%BEMT_u(indx)%theta(j,k)) + m%AllOuts( BNCn( beta,k) ) = m%BEMT_y%Cx(j,k)*ct + m%BEMT_y%Cy(j,k)*st + m%AllOuts( BNCt( beta,k) ) =-m%BEMT_y%Cx(j,k)*st + m%BEMT_y%Cy(j,k)*ct + + m%AllOuts( BNFl( beta,k) ) = m%X(j,k)*cp - m%Y(j,k)*sp + m%AllOuts( BNFd( beta,k) ) = m%X(j,k)*sp + m%Y(j,k)*cp + m%AllOuts( BNMm( beta,k) ) = m%M(j,k) + m%AllOuts( BNFx( beta,k) ) = m%X(j,k) + m%AllOuts( BNFy( beta,k) ) = -m%Y(j,k) + m%AllOuts( BNFn( beta,k) ) = m%X(j,k)*ct - m%Y(j,k)*st + m%AllOuts( BNFt( beta,k) ) = -m%X(j,k)*st - m%Y(j,k)*ct + + m%AllOuts( BNGam( beta,k) ) = 0.5_ReKi * p%BEMT%chord(j,k) * m%BEMT_y%Vrel(j,k) * m%BEMT_y%Cl(j,k) ! "Gam" [m^2/s] + + end do ! nodes + end do ! blades + + + ! rotor outputs: + + m%AllOuts( RtSkew ) = m%BEMT_u(indx)%chi0*R2D +! m%AllOuts( RtTSR ) = m%BEMT_u(indx)%TSR m%AllOuts( DBEMTau1 ) = OtherState%BEMT%DBEMT%tau1 + end subroutine Calc_WriteOutput_BEMT @@ -1907,11 +1962,6 @@ end subroutine Calc_WriteOutput_BEMT !! Make sure these are set! subroutine Calc_WriteOutput_FVW integer :: iW - real(ReKi) :: rmax, omega - - ! Compute max radius and rotor speed - rmax = Calc_MaxRadius(p, u) - omega = Calc_Omega(u) ! blade outputs do k=1,min(p%numBlades,3) @@ -1920,22 +1970,6 @@ subroutine Calc_WriteOutput_FVW do beta=1,p%NBlOuts j=p%BlOutNd(beta) - ! --- Setting AD outputs - tmp = matmul( m%WithoutSweepPitchTwist(:,:,j,k), u%InflowOnBlade(:,j,k) ) - m%AllOuts( BNVUndx(beta,k) ) = tmp(1) - m%AllOuts( BNVUndy(beta,k) ) = tmp(2) - m%AllOuts( BNVUndz(beta,k) ) = tmp(3) - - tmp = matmul( m%WithoutSweepPitchTwist(:,:,j,k), m%DisturbedInflow(:,j,k) ) - m%AllOuts( BNVDisx(beta,k) ) = tmp(1) - m%AllOuts( BNVDisy(beta,k) ) = tmp(2) - m%AllOuts( BNVDisz(beta,k) ) = tmp(3) - - tmp = matmul( m%WithoutSweepPitchTwist(:,:,j,k), u%BladeMotion(k)%TranslationVel(:,j) ) - m%AllOuts( BNSTVx( beta,k) ) = tmp(1) - m%AllOuts( BNSTVy( beta,k) ) = tmp(2) - m%AllOuts( BNSTVz( beta,k) ) = tmp(3) - m%AllOuts( BNVrel( beta,k) ) = m_AD%FVW%W(iW)%BN_Vrel(j) m%AllOuts( BNDynP( beta,k) ) = 0.5 * p%airDens * m_AD%FVW%W(iW)%BN_Vrel(j)**2 m%AllOuts( BNRe( beta,k) ) = m_AD%FVW%W(iW)%BN_Re(j) / 1.0E6 @@ -1953,9 +1987,6 @@ subroutine Calc_WriteOutput_FVW ! m%AllOuts( BNCurve(beta,k) ) = m%Curve(j,k)*R2D ! TODO ! m%AllOuts( BNCpmin( beta,k) ) = m%BEMT_y%Cpmin(jk) ! TODO - m%AllOuts( BNSigCr( beta,k) ) = m%SigmaCavitCrit(j,k) - m%AllOuts( BNSgCav( beta,k) ) = m%SigmaCavit(j,k) - m%AllOuts( BNCl( beta,k) ) = m_AD%FVW%W(iW)%BN_Cl(j) m%AllOuts( BNCd( beta,k) ) = m_AD%FVW%W(iW)%BN_Cd(j) m%AllOuts( BNCm( beta,k) ) = m_AD%FVW%W(iW)%BN_Cm(j) @@ -1982,84 +2013,10 @@ subroutine Calc_WriteOutput_FVW end do ! blades - m%AllOuts( RtSpeed ) = omega*RPS2RPM - m%AllOuts( RtArea ) = pi*rmax**2 ! TODO vertical axis - - tmp = matmul( u%HubMotion%Orientation(:,:,1), m%V_DiskAvg ) - m%AllOuts( RtVAvgxh ) = tmp(1) - m%AllOuts( RtVAvgyh ) = tmp(2) - m%AllOuts( RtVAvgzh ) = tmp(3) - +! m%AllOuts( RtArea ) = pi*rmax**2 ! TODO vertical axis m%AllOuts( RtSkew ) = Calc_Chi0(m%V_diskAvg, m%V_dot_x) * R2D - ! integrate force/moments over blades by performing mesh transfer to hub point: - force = 0.0_ReKi - moment = 0.0_ReKi - do k=1,p%NumBlades - call Transfer_Line2_to_Point( y%BladeLoad(k), m%HubLoad, m%B_L_2_H_P(k), ErrStat2, ErrMsg2, u%BladeMotion(k), u%HubMotion ) - force = force + m%HubLoad%force( :,1) - moment = moment + m%HubLoad%moment(:,1) - - if (k<=4) then - ! Power contribution of blade wrt hub - tmp = matmul( u%HubMotion%Orientation(:,:,1), m%HubLoad%moment(:,1) ) - m%AllOuts( BAeroPwr(k) ) = omega * tmp(1) - ! In global, wrt hub! - m%AllOuts( BAeroFxg(k) ) = m%HubLoad%force(1,1) - m%AllOuts( BAeroFyg(k) ) = m%HubLoad%force(2,1) - m%AllOuts( BAeroFzg(k) ) = m%HubLoad%force(3,1) - m%AllOuts( BAeroMxg(k) ) = m%HubLoad%moment(1,1) - m%AllOuts( BAeroMyg(k) ) = m%HubLoad%moment(2,1) - m%AllOuts( BAeroMzg(k) ) = m%HubLoad%moment(3,1) - endif - end do - ! In global - m%AllOuts( RtAeroFxg ) = force(1) - m%AllOuts( RtAeroFyg ) = force(2) - m%AllOuts( RtAeroFzg ) = force(3) - m%AllOuts( RtAeroMxg ) = moment(1) - m%AllOuts( RtAeroMyg ) = moment(2) - m%AllOuts( RtAeroMzg ) = moment(3) - ! In hub coord - tmp = matmul( u%HubMotion%Orientation(:,:,1), force ) - m%AllOuts( RtAeroFxh ) = tmp(1) - m%AllOuts( RtAeroFyh ) = tmp(2) - m%AllOuts( RtAeroFzh ) = tmp(3) - - tmp = matmul( u%HubMotion%Orientation(:,:,1), moment ) - m%AllOuts( RtAeroMxh ) = tmp(1) - m%AllOuts( RtAeroMyh ) = tmp(2) - m%AllOuts( RtAeroMzh ) = tmp(3) - - m%AllOuts( RtAeroPwr ) = omega * m%AllOuts( RtAeroMxh ) - - if ( EqualRealNos( m%V_dot_x, 0.0_ReKi ) ) then - m%AllOuts( RtTSR ) = 0.0_ReKi - m%AllOuts( RtAeroCp ) = 0.0_ReKi - m%AllOuts( RtAeroCq ) = 0.0_ReKi - m%AllOuts( RtAeroCt ) = 0.0_ReKi - else - denom = 0.5*p%AirDens*m%AllOuts( RtArea )*m%V_dot_x**2 - m%AllOuts( RtTSR ) = omega * rmax / m%V_dot_x - m%AllOuts( RtAeroCp ) = m%AllOuts( RtAeroPwr ) / (denom * m%V_dot_x) - m%AllOuts( RtAeroCq ) = m%AllOuts( RtAeroMxh ) / (denom * rmax) - m%AllOuts( RtAeroCt ) = m%AllOuts( RtAeroFxh ) / denom - end if - - ! Integrate force/moments over blades by performing mesh transfer to blade root points: - do k=1,min(p%NumBlades,4) - call Transfer_Line2_to_Point( y%BladeLoad(k), m%BladeRootLoad(k), m%B_L_2_R_P(k), ErrStat2, ErrMsg2, u%BladeMotion(k), u%BladeRootMotion(k) ) - ! Transform force vector to blade root coordinate system - tmp = matmul( u%BladeRootMotion(k)%Orientation(:,:,1), m%BladeRootLoad(k)%force( :,1) ) - m%AllOuts( BAeroFx(k) ) = tmp(1) - m%AllOuts( BAeroFy(k) ) = tmp(2) - m%AllOuts( BAeroFz(k) ) = tmp(3) - ! Transform moment vector to blade root coordinate system - tmp = matmul( u%BladeRootMotion(k)%Orientation(:,:,1), m%BladeRootLoad(k)%moment( :,1) ) - m%AllOuts( BAeroMx(k) ) = tmp(1) - m%AllOuts( BAeroMy(k) ) = tmp(2) - m%AllOuts( BAeroMz(k) ) = tmp(3) - end do ! k=blades +! m%AllOuts( DBEMTau1 ) = 0.0_ReKi ! not valid with FVW end subroutine Calc_WriteOutput_FVW @@ -2123,7 +2080,7 @@ SUBROUTINE ReadInputFiles( InputFileName, InputFileData, Default_DT, OutFileRoot iBld = iBld+1 ! Increment blade counter END DO - ENDDO ! Loop on rotors + end do ! loop on rotors CALL Cleanup ( ) @@ -2661,8 +2618,12 @@ SUBROUTINE AD_PrintSum( InputFileData, p, p_AD, u, y, ErrStat, ErrMsg ) CHARACTER(*), PARAMETER :: FmtDatT = '(A,T35,1(:,F13.8))' ! Format for outputting time steps. CHARACTER(30) :: OutPFmt ! Format to print list of selected output channels to summary file + CHARACTER(30) :: OutPFmtS ! Format to print list of selected output channels to summary file CHARACTER(100) :: Msg ! temporary string for writing appropriate text to summary file + CHARACTER(ChanLen),PARAMETER :: TitleStr(2) = (/ 'Parameter', 'Units ' /) + CHARACTER(ChanLen),PARAMETER :: TitleStrLines(2) = (/ '---------------', '---------------' /) + ! Open the summary file and give it a heading. CALL GetNewUnit( UnSu, ErrStat, ErrMsg ) @@ -2845,7 +2806,7 @@ SUBROUTINE AD_PrintSum( InputFileData, p, p_AD, u, y, ErrStat, ErrMsg ) Msg = 'Stieg Oye dynamic stall model' case (UA_BV) Msg = 'Boeing-Vertol dynamic stall model (e.g. used in CACTUS)' - case default + case default Msg = 'unknown' end select WRITE (UnSu,Ec_IntFrmt) InputFileData%UAMod, 'UAMod', 'Unsteady Aero Model: '//TRIM(Msg) @@ -2887,11 +2848,12 @@ SUBROUTINE AD_PrintSum( InputFileData, p, p_AD, u, y, ErrStat, ErrMsg ) end if - OutPFmt = '( 15x, I4, 2X, A '//TRIM(Num2LStr(ChanLen))//',1 X, A'//TRIM(Num2LStr(ChanLen))//' )' + OutPFmt = '( 15x, I4, 3X,A '//TRIM(Num2LStr(ChanLen))//',1 X, A'//TRIM(Num2LStr(ChanLen))//' )' + OutPFmtS = '( 15x, A4, 3X,A '//TRIM(Num2LStr(ChanLen))//',1 X, A'//TRIM(Num2LStr(ChanLen))//' )' + WRITE (UnSu,'(15x,A)') WRITE (UnSu,'(15x,A)') 'Requested Output Channels:' - WRITE (UnSu,'(15x,A)') 'Col Parameter Units' - WRITE (UnSu,'(15x,A)') '---- -------------- -----' - + WRITE (UnSu,OutPFmtS ) "Col", TitleStr + WRITE (UnSu,OutPFmtS ) "---", TitleStrLines DO I = 0,p%NumOuts WRITE (UnSu,OutPFmt) I, p%OutParam(I)%Name, p%OutParam(I)%Units END DO @@ -2899,11 +2861,12 @@ SUBROUTINE AD_PrintSum( InputFileData, p, p_AD, u, y, ErrStat, ErrMsg ) WRITE (UnSu,'(15x,A)') WRITE (UnSu,'(15x,A)') WRITE (UnSu,'(15x,A)') 'Requested Output Channels at each blade station:' - WRITE (UnSu,'(15x,A)') 'Col Parameter Units' - WRITE (UnSu,'(15x,A)') '---- -------------- -----' + WRITE (UnSu,OutPFmtS ) "Col", TitleStr + WRITE (UnSu,OutPFmtS ) "---", TitleStrLines DO I = 1,p%BldNd_NumOuts WRITE (UnSu,OutPFmt) I, p%BldNd_OutParam(I)%Name, p%BldNd_OutParam(I)%Units - END DO + END DO + CLOSE(UnSu) @@ -3445,7 +3408,7 @@ SUBROUTINE SetOutParam(OutList, p, p_AD, ErrStat, ErrMsg ) ! BNClrnc is set only when we're computing the tower influence do i = 1,size(BNClrnc,2) ! all blades (need to do this in a loop because we need the index of InvalidOutput to be an array of rank one) - InvalidOutput( BNClrnc(:,i) ) = .true. + InvalidOutput( BNClrnc(:,i) ) = .true. end do end if diff --git a/modules/aerodyn/src/BEMT_Registry.txt b/modules/aerodyn/src/BEMT_Registry.txt index 73b2d2cd4d..e3b28be8f5 100644 --- a/modules/aerodyn/src/BEMT_Registry.txt +++ b/modules/aerodyn/src/BEMT_Registry.txt @@ -47,6 +47,7 @@ typedef ^ ^ ReKi typedef ^ ^ ReKi zLocal {:}{:} - - "Distance to blade node, measured along the blade" m typedef ^ ^ ReKi zTip {:} - - "Distance to blade tip, measured along the blade" m typedef ^ ^ ReKi rLocal {:}{:} - - "Radial distance to blade node from the center of rotation, measured in the rotor plane, needed for DBEMT" m +typedef ^ ^ ReKi rTipFix {:} - - "Nominally the coned rotor diameter (without prebend)" m typedef ^ ^ INTEGER UAMod - - - "Model for the dynamic stall equations [1 = Leishman/Beddoes, 2 = Gonzalez, 3 = Minnema]" - typedef ^ ^ LOGICAL UA_Flag - - - "logical flag indicating whether to use UnsteadyAero" - typedef ^ ^ LOGICAL Flookup - - - "Use table lookup for f' and f'' " - diff --git a/modules/aerodyn/src/BEMT_Types.f90 b/modules/aerodyn/src/BEMT_Types.f90 index d7dd816ca9..bb5752eff9 100644 --- a/modules/aerodyn/src/BEMT_Types.f90 +++ b/modules/aerodyn/src/BEMT_Types.f90 @@ -57,6 +57,7 @@ MODULE BEMT_Types REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: zLocal !< Distance to blade node, measured along the blade [m] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: zTip !< Distance to blade tip, measured along the blade [m] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: rLocal !< Radial distance to blade node from the center of rotation, measured in the rotor plane, needed for DBEMT [m] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: rTipFix !< Nominally the coned rotor diameter (without prebend) [m] INTEGER(IntKi) :: UAMod !< Model for the dynamic stall equations [1 = Leishman/Beddoes, 2 = Gonzalez, 3 = Minnema] [-] LOGICAL :: UA_Flag !< logical flag indicating whether to use UnsteadyAero [-] LOGICAL :: Flookup !< Use table lookup for f' and f'' [-] @@ -296,6 +297,18 @@ SUBROUTINE BEMT_CopyInitInput( SrcInitInputData, DstInitInputData, CtrlCode, Err END IF END IF DstInitInputData%rLocal = SrcInitInputData%rLocal +ENDIF +IF (ALLOCATED(SrcInitInputData%rTipFix)) THEN + i1_l = LBOUND(SrcInitInputData%rTipFix,1) + i1_u = UBOUND(SrcInitInputData%rTipFix,1) + IF (.NOT. ALLOCATED(DstInitInputData%rTipFix)) THEN + ALLOCATE(DstInitInputData%rTipFix(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstInitInputData%rTipFix.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstInitInputData%rTipFix = SrcInitInputData%rTipFix ENDIF DstInitInputData%UAMod = SrcInitInputData%UAMod DstInitInputData%UA_Flag = SrcInitInputData%UA_Flag @@ -359,6 +372,9 @@ SUBROUTINE BEMT_DestroyInitInput( InitInputData, ErrStat, ErrMsg ) IF (ALLOCATED(InitInputData%rLocal)) THEN DEALLOCATE(InitInputData%rLocal) ENDIF +IF (ALLOCATED(InitInputData%rTipFix)) THEN + DEALLOCATE(InitInputData%rTipFix) +ENDIF IF (ALLOCATED(InitInputData%UAOff_innerNode)) THEN DEALLOCATE(InitInputData%UAOff_innerNode) ENDIF @@ -445,6 +461,11 @@ SUBROUTINE BEMT_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrM IF ( ALLOCATED(InData%rLocal) ) THEN Int_BufSz = Int_BufSz + 2*2 ! rLocal upper/lower bounds for each dimension Re_BufSz = Re_BufSz + SIZE(InData%rLocal) ! rLocal + END IF + Int_BufSz = Int_BufSz + 1 ! rTipFix allocated yes/no + IF ( ALLOCATED(InData%rTipFix) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! rTipFix upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%rTipFix) ! rTipFix END IF Int_BufSz = Int_BufSz + 1 ! UAMod Int_BufSz = Int_BufSz + 1 ! UA_Flag @@ -629,6 +650,21 @@ SUBROUTINE BEMT_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrM Re_Xferred = Re_Xferred + 1 END DO END DO + END IF + IF ( .NOT. ALLOCATED(InData%rTipFix) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%rTipFix,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%rTipFix,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%rTipFix,1), UBOUND(InData%rTipFix,1) + ReKiBuf(Re_Xferred) = InData%rTipFix(i1) + Re_Xferred = Re_Xferred + 1 + END DO END IF IntKiBuf(Int_Xferred) = InData%UAMod Int_Xferred = Int_Xferred + 1 @@ -866,6 +902,24 @@ SUBROUTINE BEMT_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, E Re_Xferred = Re_Xferred + 1 END DO END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! rTipFix not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%rTipFix)) DEALLOCATE(OutData%rTipFix) + ALLOCATE(OutData%rTipFix(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%rTipFix.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%rTipFix,1), UBOUND(OutData%rTipFix,1) + OutData%rTipFix(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO END IF OutData%UAMod = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 From 5c39dbe08d9b1a85e313fb034fc75f5b41eb88ce Mon Sep 17 00:00:00 2001 From: Bonnie Jonkman Date: Thu, 10 Mar 2022 09:39:32 -0700 Subject: [PATCH 108/242] AD: merge more of `TwrInfl` and `TwrInflArray` routines Make BEM and FVW routines calculate tower influence in the same way, without duplicating as much code. --- modules/aerodyn/src/AeroDyn.f90 | 259 +++++++++++++++----------------- 1 file changed, 118 insertions(+), 141 deletions(-) diff --git a/modules/aerodyn/src/AeroDyn.f90 b/modules/aerodyn/src/AeroDyn.f90 index e8facc7c4f..6b76bfd58d 100644 --- a/modules/aerodyn/src/AeroDyn.f90 +++ b/modules/aerodyn/src/AeroDyn.f90 @@ -3094,15 +3094,11 @@ SUBROUTINE TwrInfl( p, u, m, ErrStat, ErrMsg ) real(ReKi) :: BladeNodePosition(3) ! local blade node position - - real(ReKi) :: u_TwrShadow ! axial velocity deficit fraction from tower shadow - real(ReKi) :: u_TwrPotent ! axial velocity deficit fraction from tower potential flow - real(ReKi) :: v_TwrPotent ! transverse velocity deficit fraction from tower potential flow - - real(ReKi) :: denom ! denominator - real(ReKi) :: exponential ! exponential term real(ReKi) :: v(3) ! temp vector + logical :: FirstWarn_TowerStrike + logical :: DisturbInflow + integer(IntKi) :: j, k ! loop counters for elements, blades integer(intKi) :: ErrStat2 character(ErrMsgLen) :: ErrMsg2 @@ -3112,6 +3108,7 @@ SUBROUTINE TwrInfl( p, u, m, ErrStat, ErrMsg ) ErrStat = ErrID_None ErrMsg = "" + FirstWarn_TowerStrike = .true. ! these models are valid for only small tower deflections; check for potential division-by-zero errors: call CheckTwrInfl( u, ErrStat2, ErrMsg2 ) @@ -3126,70 +3123,17 @@ SUBROUTINE TwrInfl( p, u, m, ErrStat, ErrMsg ) BladeNodePosition = u%BladeMotion(k)%Position(:,j) + u%BladeMotion(k)%TranslationDisp(:,j) - call getLocalTowerProps(p, u, BladeNodePosition, theta_tower_trans, W_tower, xbar, ybar, zbar, TwrCd, TwrTI, m%TwrClrnc(j,k), ErrStat2, ErrMsg2) + call getLocalTowerProps(p, u, BladeNodePosition, theta_tower_trans, W_tower, xbar, ybar, zbar, TwrCd, TwrTI, m%TwrClrnc(j,k), FirstWarn_TowerStrike, DisturbInflow, ErrStat2, ErrMsg2) call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + if (.not. FirstWarn_TowerStrike) call SetErrStat(ErrID_Fatal, "Tower strike.", ErrStat, ErrMsg, RoutineName ) if (ErrStat >= AbortErrLev) return - - - ! calculate tower influence: - if ( abs(zbar) < 1.0_ReKi .and. p%TwrPotent /= TwrPotent_none ) then - if ( p%TwrPotent == TwrPotent_baseline ) then - - denom = (xbar**2 + ybar**2)**2 - - if (equalRealNos(denom,0.0_ReKi)) then - u_TwrPotent = 0.0_ReKi - v_TwrPotent = 0.0_ReKi - else - u_TwrPotent = ( -1.0*xbar**2 + ybar**2 ) / denom - v_TwrPotent = ( -2.0*xbar * ybar ) / denom - end if - - elseif (p%TwrPotent == TwrPotent_Bak) then - - xbar = xbar + 0.1 - - denom = (xbar**2 + ybar**2)**2 - if (equalRealNos(denom,0.0_ReKi)) then - u_TwrPotent = 0.0_ReKi - v_TwrPotent = 0.0_ReKi - else - u_TwrPotent = ( -1.0*xbar**2 + ybar**2 ) / denom - v_TwrPotent = ( -2.0*xbar * ybar ) / denom - - denom = TwoPi*(xbar**2 + ybar**2) - u_TwrPotent = u_TwrPotent + TwrCd*xbar / denom - v_TwrPotent = v_TwrPotent + TwrCd*ybar / denom - end if - - end if + + if ( DisturbInflow ) then + v = CalculateTowerInfluence(p, xbar, ybar, zbar, W_tower, TwrCd, TwrTI) + m%DisturbedInflow(:,j,k) = u%InflowOnBlade(:,j,k) + matmul( theta_tower_trans, v ) else - u_TwrPotent = 0.0_ReKi - v_TwrPotent = 0.0_ReKi + m%DisturbedInflow(:,j,k) = u%InflowOnBlade(:,j,k) end if - - u_TwrShadow = 0.0_ReKi - select case (p%TwrShadow) - case (TwrShadow_Powles) - if ( xbar > 0.0_ReKi .and. abs(zbar) < 1.0_ReKi) then - denom = sqrt( sqrt( xbar**2 + ybar**2 ) ) - if ( abs(ybar) < denom ) then - u_TwrShadow = -TwrCd / denom * cos( PiBy2*ybar / denom )**2 - end if - end if - case (TwrShadow_Eames) - if ( xbar > 0.0_ReKi .and. abs(zbar) < 1.0_ReKi) then - exponential = ( ybar / (TwrTI * xbar) )**2 - denom = TwrTI * xbar * sqrt( TwoPi ) - u_TwrShadow = -TwrCd / denom * exp ( -0.5_ReKi * exponential ) - end if - end select - - v(1) = (u_TwrPotent + u_TwrShadow)*W_tower - v(2) = v_TwrPotent*W_tower - v(3) = 0.0_ReKi - - m%DisturbedInflow(:,j,k) = u%InflowOnBlade(:,j,k) + matmul( theta_tower_trans, v ) end do !j=NumBlNds end do ! NumBlades @@ -3217,109 +3161,125 @@ SUBROUTINE TwrInflArray( p, u, m, Positions, Inflow, ErrStat, ErrMsg ) real(ReKi) :: TwrTI ! local tower TI (for Eames tower shadow model) real(ReKi) :: W_tower ! local relative wind speed normal to the tower real(ReKi) :: Pos(3) ! current point - real(ReKi) :: u_TwrShadow ! axial velocity deficit fraction from tower shadow - real(ReKi) :: u_TwrPotent ! axial velocity deficit fraction from tower potential flow - real(ReKi) :: v_TwrPotent ! transverse velocity deficit fraction from tower potential flow - real(ReKi) :: denom ! denominator - real(ReKi) :: exponential ! exponential term real(ReKi) :: v(3) ! temp vector integer(IntKi) :: i ! loop counters for points real(ReKi) :: TwrClrnc ! local tower clearance - real(ReKi) :: r_TowerBlade(3) ! distance vector from tower to blade - real(ReKi) :: TwrDiam ! local tower diameter - logical :: found + logical :: FirstWarn_TowerStrike + logical :: DisturbInflow integer(intKi) :: ErrStat2 character(ErrMsgLen) :: ErrMsg2 character(*), parameter :: RoutineName = 'TwrInflArray' ErrStat = ErrID_None ErrMsg = "" + + + FirstWarn_TowerStrike = .false. ! we aren't going to end due to an assumed "tower-strike" + ! these models are valid for only small tower deflections; check for potential division-by-zero errors: call CheckTwrInfl( u, ErrStat2, ErrMsg2 ); call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ); if (ErrStat >= AbortErrLev) return !$OMP PARALLEL default(shared) - !$OMP do private(i,Pos,r_TowerBlade,theta_tower_trans,W_tower,xbar,ybar,zbar,TwrCd,TwrTI,TwrClrnc,TwrDiam,found,denom,exponential,u_TwrPotent,v_TwrPotent,u_TwrShadow,v) schedule(runtime) + !$OMP do private(i,Pos,theta_tower_trans,W_tower,xbar,ybar,zbar,TwrCd,TwrTI,TwrClrnc,FirstWarn_TowerStrike,DisturbInflow,v) schedule(runtime) do i = 1, size(Positions,2) Pos=Positions(1:3,i) ! Find nearest line2 element or node of the tower (see getLocalTowerProps) ! values are found for the deflected tower, returning theta_tower, W_tower, xbar, ybar, zbar, and TowerCd: - ! option 1: nearest line2 element - call TwrInfl_NearestLine2Element(p, u, Pos, r_TowerBlade, theta_tower_trans, W_tower, xbar, ybar, zbar, TwrCd, TwrTI, TwrDiam, found) - if ( .not. found) then - ! option 2: nearest node - call TwrInfl_NearestPoint(p, u, Pos, r_TowerBlade, theta_tower_trans, W_tower, xbar, ybar, zbar, TwrCd, TwrTI, TwrDiam) + call getLocalTowerProps(p, u, Pos, theta_tower_trans, W_tower, xbar, ybar, zbar, TwrCd, TwrTI, TwrClrnc, FirstWarn_TowerStrike, DisturbInflow, ErrStat2, ErrMsg2) + + if ( DisturbInflow ) then + v = CalculateTowerInfluence(p, xbar, ybar, zbar, W_tower, TwrCd, TwrTI) + Inflow(1:3,i) = Inflow(1:3,i) + matmul( theta_tower_trans, v ) end if - TwrClrnc = TwoNorm(r_TowerBlade) - 0.5_ReKi*TwrDiam + + enddo ! loop on points + !$OMP END DO + !$OMP END PARALLEL +END SUBROUTINE TwrInflArray +!---------------------------------------------------------------------------------------------------------------------------------- +FUNCTION CalculateTowerInfluence(p, xbar_in, ybar, zbar, W_tower, TwrCd, TwrTI) RESULT(v) - if ( TwrClrnc>20*TwrDiam) then - ! Far away, we skip the computation and keep undisturbed inflow - elseif ( TwrClrnc<=0.01_ReKi*TwrDiam) then - ! Inside the tower, or very close, (will happen for vortex elements) we keep undisturbed inflow - ! We don't want to reach the stagnation points - else - ! calculate tower influence: - if ( abs(zbar) < 1.0_ReKi .and. p%TwrPotent /= TwrPotent_none ) then - - if ( p%TwrPotent == TwrPotent_baseline ) then - denom = (xbar**2 + ybar**2)**2 - u_TwrPotent = ( -1.0*xbar**2 + ybar**2 ) / denom - v_TwrPotent = ( -2.0*xbar * ybar ) / denom - - elseif (p%TwrPotent == TwrPotent_Bak) then - xbar = xbar + 0.1 - denom = (xbar**2 + ybar**2)**2 - u_TwrPotent = ( -1.0*xbar**2 + ybar**2 ) / denom - v_TwrPotent = ( -2.0*xbar * ybar ) / denom - denom = TwoPi*(xbar**2 + ybar**2) - u_TwrPotent = u_TwrPotent + TwrCd*xbar / denom - v_TwrPotent = v_TwrPotent + TwrCd*ybar / denom + TYPE(RotParameterType), INTENT(IN ) :: p !< Parameters + real(ReKi), intent(in ) :: xbar_in ! local x^ component of r_TowerBlade (distance from tower to blade) normalized by tower radius + real(ReKi), intent(in) :: ybar ! local y^ component of r_TowerBlade (distance from tower to blade) normalized by tower radius + real(ReKi), intent(in) :: zbar ! local z^ component of r_TowerBlade (distance from tower to blade) normalized by tower radius + real(ReKi), intent(in) :: W_tower ! local relative wind speed normal to the tower + real(ReKi), intent(in) :: TwrCd ! local tower drag coefficient + real(ReKi), intent(in) :: TwrTI ! local tower TI (for Eames tower shadow model) + real(ReKi) :: v(3) ! modified velocity vector + + real(ReKi) :: denom ! denominator + real(ReKi) :: exponential ! exponential term + real(ReKi) :: xbar ! potentially modified version of xbar_in + real(ReKi) :: u_TwrShadow ! axial velocity deficit fraction from tower shadow + real(ReKi) :: u_TwrPotent ! axial velocity deficit fraction from tower potential flow + real(ReKi) :: v_TwrPotent ! transverse velocity deficit fraction from tower potential flow + + + u_TwrShadow = 0.0_ReKi + u_TwrPotent = 0.0_ReKi + v_TwrPotent = 0.0_ReKi + xbar = xbar_in + + ! calculate tower influence: + if ( abs(zbar) < 1.0_ReKi .and. p%TwrPotent /= TwrPotent_none ) then + + if ( p%TwrPotent == TwrPotent_baseline ) then + denom = (xbar**2 + ybar**2)**2 + u_TwrPotent = ( -1.0*xbar**2 + ybar**2 ) / denom + v_TwrPotent = ( -2.0*xbar * ybar ) / denom + + elseif (p%TwrPotent == TwrPotent_Bak) then + xbar = xbar + 0.1 + denom = (xbar**2 + ybar**2)**2 + u_TwrPotent = ( -1.0*xbar**2 + ybar**2 ) / denom + v_TwrPotent = ( -2.0*xbar * ybar ) / denom + denom = TwoPi*(xbar**2 + ybar**2) + u_TwrPotent = u_TwrPotent + TwrCd*xbar / denom + v_TwrPotent = v_TwrPotent + TwrCd*ybar / denom + end if + end if + + select case (p%TwrShadow) + case (TwrShadow_Powles) + if ( xbar > 0.0_ReKi .and. abs(zbar) < 1.0_ReKi) then + denom = sqrt( sqrt( xbar**2 + ybar**2 ) ) + if ( abs(ybar) < denom ) then + u_TwrShadow = -TwrCd / denom * cos( PiBy2*ybar / denom )**2 end if - else - u_TwrPotent = 0.0_ReKi - v_TwrPotent = 0.0_ReKi end if + case (TwrShadow_Eames) + if ( xbar > 0.0_ReKi .and. abs(zbar) < 1.0_ReKi) then + exponential = ( ybar / (TwrTI * xbar) )**2 + denom = TwrTI * xbar * sqrt( TwoPi ) + u_TwrShadow = -TwrCd / denom * exp ( -0.5_ReKi * exponential ) + end if + end select + + ! We limit the deficit to avoid having too much flow reversal and accumulation of vorticity behind the tower + ! Limit to -0.5 the wind speed at the tower + u_TwrShadow =max(u_TwrShadow, -0.5_ReKi) - u_TwrShadow = 0.0_ReKi - select case (p%TwrShadow) - case (TwrShadow_Powles) - if ( xbar > 0.0_ReKi .and. abs(zbar) < 1.0_ReKi) then - denom = sqrt( sqrt( xbar**2 + ybar**2 ) ) - if ( abs(ybar) < denom ) then - u_TwrShadow = -TwrCd / denom * cos( PiBy2*ybar / denom )**2 - end if - end if - case (TwrShadow_Eames) - if ( xbar > 0.0_ReKi .and. abs(zbar) < 1.0_ReKi) then - exponential = ( ybar / (TwrTI * xbar) )**2 - denom = TwrTI * xbar * sqrt( TwoPi ) - u_TwrShadow = -TwrCd / denom * exp ( -0.5_ReKi * exponential ) - end if - ! We limit the deficit to avoid having too much flow reversal and accumulation of vorticity behind the tower - ! Limit to -0.5 the wind speed at the tower - u_TwrShadow =max(u_TwrShadow, -0.5) - end select - - v(1) = (u_TwrPotent + u_TwrShadow)*W_tower - v(2) = v_TwrPotent*W_tower - v(3) = 0.0_ReKi - Inflow(1:3,i) = Inflow(1:3,i) + matmul( theta_tower_trans, v ) - endif ! Check if point far away or in tower - enddo ! loop on points - !$OMP END DO - !$OMP END PARALLEL -END SUBROUTINE TwrInflArray + v(1) = (u_TwrPotent + u_TwrShadow)*W_tower + v(2) = v_TwrPotent*W_tower + v(3) = 0.0_ReKi + + +END FUNCTION CalculateTowerInfluence !---------------------------------------------------------------------------------------------------------------------------------- !> This routine returns the tower constants necessary to compute the tower influence. !! if u%TowerMotion does not have any nodes there will be serious problems. I assume that has been checked earlier. -SUBROUTINE getLocalTowerProps(p, u, BladeNodePosition, theta_tower_trans, W_tower, xbar, ybar, zbar, TwrCd, TwrTI, TwrClrnc, ErrStat, ErrMsg) +SUBROUTINE getLocalTowerProps(p, u, BladeNodePosition, theta_tower_trans, W_tower, xbar, ybar, zbar, TwrCd, TwrTI, TwrClrnc, FirstWarn_TowerStrike, DisturbInflow, ErrStat, ErrMsg) !.................................................................................................................................. TYPE(RotInputType), INTENT(IN ) :: u !< Inputs at Time t TYPE(RotParameterType), INTENT(IN ) :: p !< Parameters REAL(ReKi) ,INTENT(IN ) :: BladeNodePosition(3) !< local blade node position REAL(ReKi) ,INTENT( OUT) :: theta_tower_trans(3,3) !< transpose of local tower orientation expressed as a DCM + LOGICAL ,INTENT(INOUT) :: FirstWarn_TowerStrike !< Whether we should check and warn for a tower strike + LOGICAL ,INTENT( OUT) :: DisturbInflow !< Whether tower clearance is in the range of values where it should disturb the inflow REAL(ReKi) ,INTENT( OUT) :: W_tower !< local relative wind speed normal to the tower REAL(ReKi) ,INTENT( OUT) :: xbar !< local x^ component of r_TowerBlade normalized by tower radius REAL(ReKi) ,INTENT( OUT) :: ybar !< local y^ component of r_TowerBlade normalized by tower radius @@ -3354,11 +3314,28 @@ SUBROUTINE getLocalTowerProps(p, u, BladeNodePosition, theta_tower_trans, W_towe end if TwrClrnc = TwoNorm(r_TowerBlade) - 0.5_ReKi*TwrDiam - if ( TwrClrnc <= 0.0_ReKi ) then - call SetErrStat(ErrID_Fatal, "Tower strike.", ErrStat, ErrMsg, RoutineName) + + if (FirstWarn_TowerStrike) then + if ( TwrClrnc <= 0.0_ReKi ) then + !call SetErrStat(ErrID_Fatal, "Tower strike.", ErrStat, ErrMsg, RoutineName) + !call SetErrStat(ErrID_Severe, NewLine//NewLine//"** WARNING: Tower strike. ** This warning will not be repeated though the condition may persist."//NewLine//NewLine//, ErrStat, ErrMsg, RoutineName) + call WrScr( NewLine//NewLine//"** WARNING: Tower strike. ** This warning will not be repeated though the condition may persist."//NewLine//NewLine ) + FirstWarn_TowerStrike = .false. + end if end if + - + if ( TwrClrnc>20.0_ReKi*TwrDiam) then + ! Far away, we skip the computation and keep undisturbed inflow + DisturbInflow = .false. + elseif ( TwrClrnc<=0.01_ReKi*TwrDiam) then + ! Inside the tower, or very close, (will happen for vortex elements) we keep undisturbed inflow + ! We don't want to reach the stagnation points + DisturbInflow = .false. + else + DisturbInflow = .true. + end if + END SUBROUTINE getLocalTowerProps !---------------------------------------------------------------------------------------------------------------------------------- !> Option 1: Find the nearest-neighbor line2 element of the tower mesh for which the blade line2-element node projects orthogonally onto @@ -3516,7 +3493,7 @@ SUBROUTINE TwrInfl_NearestPoint(p, u, BladeNodePosition, r_TowerBlade, theta_tow REAL(ReKi) ,INTENT( OUT) :: ybar !< local y^ component of r_TowerBlade normalized by tower radius REAL(ReKi) ,INTENT( OUT) :: zbar !< local z^ component of r_TowerBlade normalized by tower radius REAL(ReKi) ,INTENT( OUT) :: TwrCd !< local tower drag coefficient - REAL(ReKi) ,INTENT( OUT) :: TwrTI !< local tower TI (for Eeames tower shadow model) + REAL(ReKi) ,INTENT( OUT) :: TwrTI !< local tower TI (for Eames tower shadow model) REAL(ReKi) ,INTENT( OUT) :: TwrDiam !< local tower diameter ! local variables From 4cc8f6d419235a9891d3ff0a255b05dfb4ed4ae2 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Thu, 10 Mar 2022 12:45:51 -0700 Subject: [PATCH 109/242] Bug fixes for nonlinear tension-strain capability --- modules/moordyn/src/MoorDyn_IO.f90 | 18 ++++++++++++------ modules/moordyn/src/MoorDyn_Line.f90 | 2 +- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index 3178060095..88f550a4c4 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -241,7 +241,7 @@ SUBROUTINE getCoefficientOrCurve(inputString, LineProp_c, LineProp_npoints, Line CALL GetNewUnit( UnCoef ) CALL OpenFInpFile( UnCoef, TRIM(inputString), ErrStat4, ErrMsg4 ) ! add error handling? - READ(UnCoef,'(A)',IOSTAT=ErrStat4) Line2 ! skip the first three lines (title, names, and units) then parse + READ(UnCoef,'(A)',IOSTAT=ErrStat4) Line2 ! skip the first two lines (title, names, and units) then parse READ(UnCoef,'(A)',IOSTAT=ErrStat4) Line2 READ(UnCoef,'(A)',IOSTAT=ErrStat4) Line2 @@ -249,19 +249,25 @@ SUBROUTINE getCoefficientOrCurve(inputString, LineProp_c, LineProp_npoints, Line READ(UnCoef,'(A)',IOSTAT=ErrStat4) Line2 !read into a line - IF (ErrStat4 > 0) EXIT - - READ(Line2,*,IOSTAT=ErrStat4) LineProp_Xs(I), LineProp_Ys(I) - + IF (ErrStat4 > 0) then + print *, "Error while reading lookup table file" + EXIT + ELSE IF (ErrStat4 < 0) then + print *, "Read ", I-1, " data lines from lookup table file" + EXIT + ELSE + READ(Line2,*,IOSTAT=ErrStat4) LineProp_Xs(I), LineProp_Ys(I) + END IF END DO if (I < 2) then ErrStat3 = ErrID_Fatal ErrMsg3 = "Less than the minimum of 2 data lines found in file "//TRIM(inputString)//" (first 3 lines are headers)." + LineProp_npoints = 0 Close (UnCoef) RETURN else - LineProp_npoints = I; + LineProp_npoints = I-1 Close (UnCoef) end if diff --git a/modules/moordyn/src/MoorDyn_Line.f90 b/modules/moordyn/src/MoorDyn_Line.f90 index f527b758a8..cb383331fe 100644 --- a/modules/moordyn/src/MoorDyn_Line.f90 +++ b/modules/moordyn/src/MoorDyn_Line.f90 @@ -1127,7 +1127,7 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, ! handle nonlinear stiffness if needed if (Line%nEApoints > 0) then - Xi = Line%l(I)/Line%lstr(I) - 1.0 ! strain rate based on inputs + Xi = Line%lstr(I)/Line%l(I) - 1.0 ! strain rate based on inputs Yi = 0.0_DbKi ! find stress based on strain From 81df6721b1625db9a4d1595b2b845569fa97ed78 Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Fri, 11 Mar 2022 12:06:32 -0700 Subject: [PATCH 110/242] HD: Fix HD added mass on member end (scaling factor) (Close #992) --- modules/hydrodyn/src/Morison.f90 | 2 ++ 1 file changed, 2 insertions(+) diff --git a/modules/hydrodyn/src/Morison.f90 b/modules/hydrodyn/src/Morison.f90 index 505a57321a..b88f5e821d 100644 --- a/modules/hydrodyn/src/Morison.f90 +++ b/modules/hydrodyn/src/Morison.f90 @@ -2105,6 +2105,8 @@ SUBROUTINE Morison_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, In END IF END DO !J = 1, InitInp%InpJoints(I)%NConnections + + Vn = Vn*TwoPi/3.0_ReKi ! Semisphere volume is Vn = 2/3 pi \sum (r_MG^3 k) p%An_End(:,i) = An_drag Amag_drag = Dot_Product(An_drag ,An_drag) From 78efc1bd2374f0fe15419d959813205c2b01ca2f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Paul=20Sch=C3=BCnemann?= <47893479+pschuenemann@users.noreply.github.com> Date: Fri, 11 Mar 2022 15:34:22 +0100 Subject: [PATCH 111/242] Improve and correct some error messages and warnings to be more precise and helpful --- modules/moordyn/src/MoorDyn.f90 | 11 ++++++----- modules/moordyn/src/MoorDyn_IO.f90 | 8 ++++---- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 4c7fb98e85..ed892029c4 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -793,7 +793,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er m%BodyList(l)%r6 = tempArray ! set initial body position and orientation else - CALL SetErrStat( ErrID_Fatal, "Unidentified Body type string for Body "//trim(Num2LStr(l))//": "//trim(tempString2), ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, "Unidentified Body type string for Body "//trim(Num2LStr(l))//": "//trim(tempString1), ErrStat, ErrMsg, RoutineName ) return end if @@ -849,9 +849,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF (trim(tempString1) == trim(m%RodTypeList(J)%name)) THEN m%RodList(l)%PropsIdNum = J EXIT - IF (J == p%nRodTypes) THEN ! call an error if there is no match - CALL SetErrStat( ErrID_Fatal, 'Unable to find matching rod type name for Rod '//trim(Num2LStr(l))//": "//trim(tempString1), ErrStat, ErrMsg, RoutineName ) - END IF + END IF + IF (J == p%nRodTypes) THEN ! call an error if there is no match + CALL SetErrStat( ErrID_Fatal, 'Unable to find matching rod type name for Rod '//trim(Num2LStr(l))//": "//trim(tempString1), ErrStat, ErrMsg, RoutineName ) + RETURN END IF END DO @@ -1151,7 +1152,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF ( ErrStat2 /= 0 ) THEN - CALL SetErrStat( ErrID_Fatal, 'Failed to read rod data for Connection '//trim(Num2LStr(l)), ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, 'Failed to read data for Connection '//trim(Num2LStr(l)), ErrStat, ErrMsg, RoutineName ) CALL CleanUp() RETURN END IF diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index 88f550a4c4..db03558bb5 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -1269,7 +1269,7 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) CASE DEFAULT y%WriteOutput(I) = 0.0_ReKi ErrStat = ErrID_Warn - ErrMsg = ' Unsupported output quantity '//TRIM(Num2Lstr(p%OutParam(I)%QType))//' requested from Line '//TRIM(Num2Lstr(p%OutParam(I)%ObjID))//'.' + ErrMsg = ' Unsupported output quantity '//TRIM(p%OutParam(I)%Name)//' requested from Line '//TRIM(Num2Lstr(p%OutParam(I)%ObjID))//'.' END SELECT ELSE IF (p%OutParam(I)%OType == 2) THEN ! if dealing with a Connect output @@ -1303,7 +1303,7 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) CASE DEFAULT y%WriteOutput(I) = 0.0_ReKi ErrStat = ErrID_Warn - ErrMsg = ' Unsupported output quantity '//TRIM(Num2Lstr(p%OutParam(I)%QType))//' requested from Connection '//TRIM(Num2Lstr(p%OutParam(I)%ObjID))//'.' + ErrMsg = ' Unsupported output quantity '//TRIM(p%OutParam(I)%Name)//' requested from Connection '//TRIM(Num2Lstr(p%OutParam(I)%ObjID))//'.' END SELECT ELSE IF (p%OutParam(I)%OType == 3) THEN ! if dealing with a Rod output @@ -1342,7 +1342,7 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) CASE DEFAULT y%WriteOutput(I) = 0.0_ReKi ErrStat = ErrID_Warn - ErrMsg = ' Unsupported output quantity '//TRIM(Num2Lstr(p%OutParam(I)%QType))//' requested from Rod '//TRIM(Num2Lstr(p%OutParam(I)%ObjID))//'.' + ErrMsg = ' Unsupported output quantity '//TRIM(p%OutParam(I)%Name)//' requested from Rod '//TRIM(Num2Lstr(p%OutParam(I)%ObjID))//'.' END SELECT ELSE IF (p%OutParam(I)%OType == 4) THEN ! if dealing with a Body output @@ -1374,7 +1374,7 @@ SUBROUTINE MDIO_WriteOutputs( Time, p, m, y, ErrStat, ErrMsg ) CASE DEFAULT y%WriteOutput(I) = 0.0_ReKi ErrStat = ErrID_Warn - ErrMsg = ' Unsupported output quantity '//TRIM(Num2Lstr(p%OutParam(I)%QType))//' requested from Body '//TRIM(Num2Lstr(p%OutParam(I)%ObjID))//'.' + ErrMsg = ' Unsupported output quantity '//TRIM(p%OutParam(I)%Name)//' requested from Body '//TRIM(Num2Lstr(p%OutParam(I)%ObjID))//'.' END SELECT From c0c1c29e237a97877895275abc3e49b31a55f917 Mon Sep 17 00:00:00 2001 From: Bonnie Jonkman Date: Mon, 14 Mar 2022 20:55:36 -0600 Subject: [PATCH 112/242] GitHub template: update list of binaries for Windows executable --- .github/PULL_REQUEST_TEMPLATE.md | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index 2a941bef2b..48085ccba8 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -25,7 +25,6 @@ - [ ] Create a merge commit in r-test and add a corresponding tag - [ ] Compile executables for Windows builds - [ ] FAST_SFunc.mexw64 - - [ ] MAP_X64.dll - [ ] OpenFAST-Simulink_x64.dll - [ ] openfast_x64.exe - [ ] DISCON.dll From 4f4b5a8fb45558f758188dede3519b6b1cf1d0ea Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Tue, 15 Mar 2022 13:57:47 -0600 Subject: [PATCH 113/242] HD: allways allocating A,B,C (Closes #1046) --- modules/hydrodyn/src/HydroDyn.f90 | 4 ---- 1 file changed, 4 deletions(-) diff --git a/modules/hydrodyn/src/HydroDyn.f90 b/modules/hydrodyn/src/HydroDyn.f90 index 5b0e990a3b..93a110ba83 100644 --- a/modules/hydrodyn/src/HydroDyn.f90 +++ b/modules/hydrodyn/src/HydroDyn.f90 @@ -2579,8 +2579,6 @@ SUBROUTINE HD_JacobianPInput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrM ! For the case where either RdtnMod=0 and ExtcnMod=0 and hence %SS_Rdtn data or %SS_Exctn data is not valid then we do not have states, so simply return ! The key here is to never allocate the dXdu and related state Jacobian arrays because then the glue-code will behave properly - if ( p%totalStates == 0 ) return - ! Calculate the partial derivative of the continuous state functions (X) with respect to the inputs (u) here: ! allocate dXdu if necessary @@ -2753,8 +2751,6 @@ SUBROUTINE HD_JacobianPContState( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrStat = ErrID_None ErrMsg = '' - if ( p%totalStates == 0 ) return - ! Calculate the partial derivative of the output functions (Y) with respect to the continuous states (x) here: From b820815ef7ccb0ea04be732f49469af955360d4f Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Fri, 18 Mar 2022 08:08:39 -0600 Subject: [PATCH 114/242] Bugfixes for cable bending stiffness - Fixes behavior of zero-length rods and fixes a bug in kinematics of coupled rods. --- modules/moordyn/src/MoorDyn.f90 | 18 ++++++------------ modules/moordyn/src/MoorDyn_Registry.txt | 3 --- modules/moordyn/src/MoorDyn_Rod.f90 | 21 +++++---------------- 3 files changed, 11 insertions(+), 31 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index ed892029c4..d41c1e8158 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -1122,7 +1122,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL SetErrStat( ErrID_Fatal, "No number provided for Connection "//trim(Num2LStr(l))//" Turbine attachment.", ErrStat, ErrMsg, RoutineName ) return end if - + else CALL SetErrStat( ErrID_Fatal, "Unidentified Type/BodyID for Connection "//trim(Num2LStr(l))//": "//trim(tempString2), ErrStat, ErrMsg, RoutineName ) return @@ -1132,13 +1132,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er m%ConnectList(l)%rd(1) = 0.0_DbKi m%ConnectList(l)%rd(2) = 0.0_DbKi m%ConnectList(l)%rd(3) = 0.0_DbKi - - ! possibly redundant <<< should revisit - m%ConnectList(l)%conX = tempArray(1) - m%ConnectList(l)%conY = tempArray(2) - m%ConnectList(l)%conZ = tempArray(3) - - + !also set number of attached lines to zero initially m%ConnectList(l)%nAttached = 0 @@ -1680,7 +1674,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er u%CoupledKinematics(iTurb)%Orientation(:,:,J) = OrMat2 ! set absolute initial positions in MoorDyn - m%BodyList(m%CpldBodyIs(l,iTurb))%r6(1:3) = u%CoupledKinematics(iTurb)%Position(:,iTurb) + u%CoupledKinematics(iTurb)%TranslationDisp(:,iTurb) + p%TurbineRefPos(:,iTurb) + m%BodyList(m%CpldBodyIs(l,iTurb))%r6(1:3) = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + p%TurbineRefPos(:,iTurb) m%BodyList(m%CpldBodyIs(l,iTurb))%r6(4:6) = EulerExtract(MATMUL(OrMat, TRANSPOSE( EulerConstruct( rRef(4:6))))) ! apply rotation from PtfmInit onto input file's body orientation to get its true initial orientation CALL MeshConstructElement(u%CoupledKinematics(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, J) ! set node as point element @@ -1702,7 +1696,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er u%CoupledKinematics(iTurb)%TranslationDisp(3,J) = InitInp%PtfmInit(3,iTurb) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) ! set absolute initial positions in MoorDyn - m%RodList(m%CpldRodIs(l,iTurb))%r6(1:3) = u%CoupledKinematics(iTurb)%Position(:,iTurb) + u%CoupledKinematics(iTurb)%TranslationDisp(:,iTurb) + p%TurbineRefPos(:,iTurb) + m%RodList(m%CpldRodIs(l,iTurb))%r6(1:3) = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + p%TurbineRefPos(:,iTurb) ! >>> still need to set Rod initial orientations accounting for PtfmInit rotation <<< @@ -1718,14 +1712,14 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! set reference position as per input file <<< what about turbine positions in array? rRef(1:3) = m%ConnectList(m%CpldConIs(l,iTurb))%r CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef(1:3), ErrStat2, ErrMsg2) - + ! calculate initial point relative position, adjusted due to initial platform rotations and translations <<< could convert to array math u%CoupledKinematics(iTurb)%TranslationDisp(1,J) = InitInp%PtfmInit(1,iTurb) + OrMat(1,1)*rRef(1) + OrMat(2,1)*rRef(2) + OrMat(3,1)*rRef(3) - rRef(1) u%CoupledKinematics(iTurb)%TranslationDisp(2,J) = InitInp%PtfmInit(2,iTurb) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2) u%CoupledKinematics(iTurb)%TranslationDisp(3,J) = InitInp%PtfmInit(3,iTurb) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) ! set absolute initial positions in MoorDyn - m%ConnectList(m%CpldConIs(l,iTurb))%r = u%CoupledKinematics(iTurb)%Position(:,iTurb) + u%CoupledKinematics(iTurb)%TranslationDisp(:,iTurb) + p%TurbineRefPos(:,iTurb) + m%ConnectList(m%CpldConIs(l,iTurb))%r = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + p%TurbineRefPos(:,iTurb) CALL MeshConstructElement(u%CoupledKinematics(iTurb), ELEMENT_POINT, ErrStat2, ErrMsg2, J) diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index a009242fc4..5f4d21cbb1 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -125,9 +125,6 @@ typedef ^ ^ IntKi typeNum - typedef ^ ^ IntKi Attached {10} - - "list of IdNums of lines attached to this connection node" typedef ^ ^ IntKi Top {10} - - "list of ints specifying whether each line is attached at 1 = top/fairlead(end B), 0 = bottom/anchor(end A)" typedef ^ ^ IntKi nAttached - 0 - "number of attached lines" -typedef ^ ^ DbKi conX - - - "" -typedef ^ ^ DbKi conY - - - "" -typedef ^ ^ DbKi conZ - - - "" typedef ^ ^ DbKi conM - - - "" typedef ^ ^ DbKi conV - - - "" typedef ^ ^ DbKi conFX - - - "" diff --git a/modules/moordyn/src/MoorDyn_Rod.f90 b/modules/moordyn/src/MoorDyn_Rod.f90 index e20d870d5b..d4b75e6843 100644 --- a/modules/moordyn/src/MoorDyn_Rod.f90 +++ b/modules/moordyn/src/MoorDyn_Rod.f90 @@ -115,23 +115,12 @@ SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) print *, 'I am a rod' print *, endCoords - ! set Rod positions if applicable - if (Rod%typeNum==0) then ! for an independent rod, set the position right off the bat - - Rod%r6(1:3) = endCoords(1:3) ! (end A coordinates) - Rod%v6(1:3) = 0.0_DbKi ! (end A velocity, unrotated axes) + ! set Rod positions (some or all may be overwritten depending on if the Rod is coupled or attached to a Body) + Rod%r6(1:3) = endCoords(1:3) ! (end A coordinates) + Rod%v6(1:3) = 0.0_DbKi ! (end A velocity, unrotated axes) - Rod%r6(4:6) = Rod%q ! (Rod direction unit vector) - Rod%v6(4:6) = 0.0_DbKi ! (rotational velocities about unrotated axes) - - else if (abs(Rod%typeNum)==1) then ! for a pinned rod, just set the orientation (position will be set later by parent object) - - Rod%r6(4:6) = Rod%q ! (Rod direction unit vector) - Rod%v6(4:6) = 0.0_DbKi ! (rotational velocities about unrotated axes) - - end if - ! otherwise (for a fixed rod) the positions will be set by the parent body or via coupling - + Rod%r6(4:6) = Rod%q ! (Rod direction unit vector) + Rod%v6(4:6) = 0.0_DbKi ! (rotational velocities about unrotated axes) ! save mass for future calculations >>>> should calculate I_l and I_r here in future <<<< Rod%mass = Rod%UnstrLen*RodProp%w From c51e7c0ea93a371350ff55119524caa293858e0d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Paul=20Sch=C3=BCnemann?= <47893479+pschuenemann@users.noreply.github.com> Date: Tue, 15 Mar 2022 16:08:06 +0100 Subject: [PATCH 115/242] MDv2: BugFix to ensure correct output of solver options warning and matching line type error - Store status of ALLOCATE and MeshCommit operations in ErrStat2 instead of ErrStat to prevent overwriting of ErrStat = ErrID_Warn thrown when an unknown solver option is given. - Repairing throw of error when a non-matching line type was found by restructuring if-conditions. --- modules/moordyn/src/MoorDyn.f90 | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index d41c1e8158..a5290150f8 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -444,7 +444,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if ( OptString == 'CV') then read (OptValue,*) p%cv else - CALL SetErrStat( ErrID_Warn, 'unable to interpret input '//trim(OptString), ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Warn, 'Unable to interpret input '//trim(OptString)//' in OPTIONS section.', ErrStat, ErrMsg, RoutineName ) end if nOpts = nOpts + 1 @@ -1181,10 +1181,11 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF (trim(tempString1) == trim(m%LineTypeList(J)%name)) THEN m%LineList(l)%PropsIdNum = J EXIT + END IF IF (J == p%nLineTypes) THEN ! call an error if there is no match CALL SetErrStat( ErrID_Fatal, 'Unable to find matching line type name for Line '//trim(Num2LStr(l)), ErrStat, ErrMsg, RoutineName ) + RETURN END IF - END IF END DO ! account for states of line @@ -1584,8 +1585,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF (wordy > 0) print *, "allocating state vectors to size ", Nx ! allocate state vector and temporary state vectors based on size just calculated - ALLOCATE ( x%states(m%Nx), m%xTemp%states(m%Nx), m%xdTemp%states(m%Nx), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN + ALLOCATE ( x%states(m%Nx), m%xTemp%states(m%Nx), m%xdTemp%states(m%Nx), STAT = ErrStat2 ) + IF ( ErrStat2 /= ErrID_None ) THEN ErrMsg = ' Error allocating state vectors.' !CALL CleanUp() RETURN @@ -1619,13 +1620,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! (set up initial condition of each coupled object based on values specified by glue code) ! Also create i/o meshes - ALLOCATE ( u%CoupledKinematics(p%nTurbines), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN + ALLOCATE ( u%CoupledKinematics(p%nTurbines), STAT = ErrStat2 ) + IF ( ErrStat2 /= ErrID_None ) THEN CALL CheckError(ErrID_Fatal, ' Error allocating CoupledKinematics input array.') RETURN END IF - ALLOCATE ( y%CoupledLoads(p%nTurbines), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN + ALLOCATE ( y%CoupledLoads(p%nTurbines), STAT = ErrStat2 ) + IF ( ErrStat2 /= ErrID_None ) THEN CALL CheckError(ErrID_Fatal, ' Error allocating CoupledLoads output array.') RETURN END IF @@ -1746,7 +1747,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er u%CoupledKinematics(iTurb)%RotationVel = 0.0_ReKi u%CoupledKinematics(iTurb)%RotationAcc = 0.0_ReKi - CALL MeshCommit ( u%CoupledKinematics(iTurb), ErrStat, ErrMsg ) + CALL MeshCommit ( u%CoupledKinematics(iTurb), ErrStat2, ErrMsg ) CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN @@ -1935,8 +1936,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er END DO ! allocate array holding 10 latest fairlead tensions - ALLOCATE ( FairTensIC(p%nLines, 10), STAT = ErrStat ) - IF ( ErrStat /= ErrID_None ) THEN + ALLOCATE ( FairTensIC(p%nLines, 10), STAT = ErrStat2 ) + IF ( ErrStat2 /= ErrID_None ) THEN CALL CheckError( ErrID_Fatal, ErrMsg2 ) RETURN END IF From a612d2a6cb7bb2d7c9f72b20f5e925b678977f71 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Paul=20Sch=C3=BCnemann?= <47893479+pschuenemann@users.noreply.github.com> Date: Tue, 15 Mar 2022 16:23:26 +0100 Subject: [PATCH 116/242] MDv2: Fix error message for unidentified Type/BodyID of Connections to show the correct, related wrong user input --- modules/moordyn/src/MoorDyn.f90 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index a5290150f8..6a9ab8af19 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -1124,7 +1124,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er end if else - CALL SetErrStat( ErrID_Fatal, "Unidentified Type/BodyID for Connection "//trim(Num2LStr(l))//": "//trim(tempString2), ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, "Unidentified Type/BodyID for Connection "//trim(Num2LStr(l))//": "//trim(tempString1), ErrStat, ErrMsg, RoutineName ) return end if From 48084bcbfb7fe860e8eac9819391dcea3d132e7f Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Mon, 21 Mar 2022 14:04:58 -0600 Subject: [PATCH 117/242] FAST lin: update `Indx_y_ED_Nacelle_Start` with suggestion from bjonkman --- modules/openfast-library/src/FAST_Lin.f90 | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/modules/openfast-library/src/FAST_Lin.f90 b/modules/openfast-library/src/FAST_Lin.f90 index 4da273c484..2ce92b2ee1 100644 --- a/modules/openfast-library/src/FAST_Lin.f90 +++ b/modules/openfast-library/src/FAST_Lin.f90 @@ -4995,8 +4995,7 @@ FUNCTION Indx_y_ED_Nacelle_Start(y_ED, y_FAST) RESULT(ED_Out_Start) INTEGER :: ED_Out_Start !< starting index of this blade mesh in ElastoDyn outputs - ED_Out_Start = Indx_y_ED_BladeRoot_Start(y_ED, y_FAST, size(y_ED%BladeRootMotion)) ! start of last blade root - ED_Out_Start = ED_Out_Start + y_ED%BladeRootMotion(size(y_ED%BladeRootMotion))%NNodes*18 ! N blade roots, 6 fields with 3 components per blade. + ED_Out_Start = Indx_y_ED_BladeRoot_Start(y_ED, y_FAST, MaxNBlades+1) END FUNCTION Indx_y_ED_Nacelle_Start !---------------------------------------------------------------------------------------------------------------------------------- !> This routine returns the starting index for y_ED%Yaw in the FAST linearization outputs. From 8156e6c2cd80f6b4f9fd28ab1edb2d9c5365b00e Mon Sep 17 00:00:00 2001 From: Bonnie Jonkman Date: Mon, 21 Mar 2022 14:54:48 -0600 Subject: [PATCH 118/242] AD: fix some typos + convert `pi/180` to `D2R` --- modules/aerodyn/src/AeroDyn_AllBldNdOuts_IO.f90 | 2 +- modules/aerodyn/src/AeroDyn_Driver_Subs.f90 | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/aerodyn/src/AeroDyn_AllBldNdOuts_IO.f90 b/modules/aerodyn/src/AeroDyn_AllBldNdOuts_IO.f90 index 3255a0475e..5569b3a641 100644 --- a/modules/aerodyn/src/AeroDyn_AllBldNdOuts_IO.f90 +++ b/modules/aerodyn/src/AeroDyn_AllBldNdOuts_IO.f90 @@ -1253,7 +1253,7 @@ SUBROUTINE AllBldNdOuts_SetParameters( InputFileData, p, p_AD, ErrStat, ErrMsg ) IF ( (InputFileData%BldNd_BladesOut < 0_IntKi) ) then p%BldNd_BladesOut = 0_IntKi ELSE IF ((InputFileData%BldNd_BladesOut > p%NumBlades) ) THEN - CALL SetErrStat( ErrID_Warn, " Number of blades to output data at all blade nodes (BldNd_BladesOut) must be less than "//TRIM(Num2LStr(p%NumBlades))//".", ErrStat, ErrMsg, RoutineName) + CALL SetErrStat( ErrID_Warn, " Number of blades to output data at all blade nodes (BldNd_BladesOut) must be no more than the total number of blades, "//TRIM(Num2LStr(p%NumBlades))//".", ErrStat, ErrMsg, RoutineName) p%BldNd_BladesOut = p%NumBlades ! NOTE: we are forgiving and plateau to numBlades ELSE p%BldNd_BladesOut = InputFileData%BldNd_BladesOut diff --git a/modules/aerodyn/src/AeroDyn_Driver_Subs.f90 b/modules/aerodyn/src/AeroDyn_Driver_Subs.f90 index ce50e3f811..163e4a7cba 100644 --- a/modules/aerodyn/src/AeroDyn_Driver_Subs.f90 +++ b/modules/aerodyn/src/AeroDyn_Driver_Subs.f90 @@ -1392,8 +1392,8 @@ subroutine Dvr_ReadInputFile(fileName, dvr, errStat, errMsg ) call ParseAry(FileInfo_In, CurLine, 'nacOrigin_t'//sWT , wt%nac%origin_t, 3 , errStat2, errMsg2, unEc); if(Failed()) return call ParseAry(FileInfo_In, CurLine, 'hubOrigin_n'//sWT , wt%hub%origin_n, 3 , errStat2, errMsg2, unEc); if(Failed()) return call ParseAry(FileInfo_In, CurLine, 'hubOrientation_n'//sWT , wt%hub%orientation_n, 3 , errStat2, errMsg2, unEc); if(Failed()) return - wt%hub%orientation_n = wt%hub%orientation_n*Pi/180_ReKi - wt%orientationInit = wt%orientationInit*Pi/180_ReKi + wt%hub%orientation_n = wt%hub%orientation_n*D2R + wt%orientationInit = wt%orientationInit*D2R ! Blades call ParseCom(FileInfo_In, CurLine, Line, errStat2, errMsg2, unEc); if(Failed()) return call ParseVar(FileInfo_In, CurLine, 'numBlades'//sWT , wt%numBlades, errStat2, errMsg2, unEc); if(Failed()) return @@ -1837,7 +1837,7 @@ subroutine Dvr_InitializeDriverOutputs(dvr, errStat, errMsg) dvr%out%WriteOutputHdr(j) = 'ShearExp' if (dvr%CompInflow==1) then - dvr%out%WriteOutputUnt(j) = '(NVALID)'; j=j+1 + dvr%out%WriteOutputUnt(j) = '(INVALID)'; j=j+1 else dvr%out%WriteOutputUnt(j) = '(-)'; j=j+1 endif From 8c12ac0e76c76fa9cee10b797f355c943b978a6a Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Tue, 29 Mar 2022 10:30:52 -0600 Subject: [PATCH 119/242] MDv2: update MoorDyn_Types.f90 after regenerating --- modules/moordyn/src/MoorDyn_Types.f90 | 21 --------------------- 1 file changed, 21 deletions(-) diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index 788414bf92..4a88d808c9 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -143,9 +143,6 @@ MODULE MoorDyn_Types INTEGER(IntKi) , DIMENSION(1:10) :: Attached !< list of IdNums of lines attached to this connection node [-] INTEGER(IntKi) , DIMENSION(1:10) :: Top !< list of ints specifying whether each line is attached at 1 = top/fairlead(end B), 0 = bottom/anchor(end A) [-] INTEGER(IntKi) :: nAttached = 0 !< number of attached lines [-] - REAL(DbKi) :: conX !< [-] - REAL(DbKi) :: conY !< [-] - REAL(DbKi) :: conZ !< [-] REAL(DbKi) :: conM !< [-] REAL(DbKi) :: conV !< [-] REAL(DbKi) :: conFX !< [-] @@ -2353,9 +2350,6 @@ SUBROUTINE MD_CopyConnect( SrcConnectData, DstConnectData, CtrlCode, ErrStat, Er DstConnectData%Attached = SrcConnectData%Attached DstConnectData%Top = SrcConnectData%Top DstConnectData%nAttached = SrcConnectData%nAttached - DstConnectData%conX = SrcConnectData%conX - DstConnectData%conY = SrcConnectData%conY - DstConnectData%conZ = SrcConnectData%conZ DstConnectData%conM = SrcConnectData%conM DstConnectData%conV = SrcConnectData%conV DstConnectData%conFX = SrcConnectData%conFX @@ -2441,9 +2435,6 @@ SUBROUTINE MD_PackConnect( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Int_BufSz = Int_BufSz + SIZE(InData%Attached) ! Attached Int_BufSz = Int_BufSz + SIZE(InData%Top) ! Top Int_BufSz = Int_BufSz + 1 ! nAttached - Db_BufSz = Db_BufSz + 1 ! conX - Db_BufSz = Db_BufSz + 1 ! conY - Db_BufSz = Db_BufSz + 1 ! conZ Db_BufSz = Db_BufSz + 1 ! conM Db_BufSz = Db_BufSz + 1 ! conV Db_BufSz = Db_BufSz + 1 ! conFX @@ -2510,12 +2501,6 @@ SUBROUTINE MD_PackConnect( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, END DO IntKiBuf(Int_Xferred) = InData%nAttached Int_Xferred = Int_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%conX - Db_Xferred = Db_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%conY - Db_Xferred = Db_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%conZ - Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%conM Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%conV @@ -2631,12 +2616,6 @@ SUBROUTINE MD_UnPackConnect( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMs END DO OutData%nAttached = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 - OutData%conX = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 - OutData%conY = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 - OutData%conZ = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 OutData%conM = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 OutData%conV = DbKiBuf(Db_Xferred) From 605de108fdc5f35f52c5f6906a7d077546950546 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Thu, 31 Mar 2022 13:38:22 -0600 Subject: [PATCH 120/242] MDv2-farm: update regression test cases from @erickaloz --- reg_tests/r-test | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/reg_tests/r-test b/reg_tests/r-test index d5dd4299d7..19eba75fc1 160000 --- a/reg_tests/r-test +++ b/reg_tests/r-test @@ -1 +1 @@ -Subproject commit d5dd4299d7d7d19155a383442ac5788e44b87dc1 +Subproject commit 19eba75fc1871a914e65820fa49789ad34ce185a From 45843898741af0e8b481e538cee32f47915293cb Mon Sep 17 00:00:00 2001 From: Hannah Ross Date: Fri, 1 Apr 2022 20:55:26 -0600 Subject: [PATCH 121/242] Enable cavitation calculation using FVW model --- modules/aerodyn/src/AeroDyn.f90 | 67 ++++++++++++------- .../aerodyn/src/AeroDyn_AllBldNdOuts_IO.f90 | 4 +- modules/aerodyn/src/AeroDyn_IO.f90 | 2 +- modules/aerodyn/src/FVW.f90 | 1 + modules/aerodyn/src/FVW_Registry.txt | 1 + modules/aerodyn/src/FVW_Types.f90 | 54 +++++++++++++++ 6 files changed, 100 insertions(+), 29 deletions(-) diff --git a/modules/aerodyn/src/AeroDyn.f90 b/modules/aerodyn/src/AeroDyn.f90 index dacdb038e4..01ffbd2b17 100644 --- a/modules/aerodyn/src/AeroDyn.f90 +++ b/modules/aerodyn/src/AeroDyn.f90 @@ -1383,6 +1383,9 @@ subroutine AD_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) endif + ! Cavitation check + call AD_CavtCrit(u, p, m, errStat2, errMsg2) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) !------------------------------------------------------- ! get values to output to file: @@ -1477,47 +1480,59 @@ subroutine RotCalcOutput( t, u, p, p_AD, x, xd, z, OtherState, y, m, ErrStat, Er call ADTwr_CalcOutput(p, u, m, y, ErrStat2, ErrMsg2 ) call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) endif - - call AD_CavtCrit(u, p, m, errStat2, errMsg2) - call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) end subroutine RotCalcOutput subroutine AD_CavtCrit(u, p, m, errStat, errMsg) - TYPE(RotInputType), INTENT(IN ) :: u !< Inputs at Time t - TYPE(RotParameterType), INTENT(IN ) :: p !< Parameters - TYPE(RotMiscVarType), INTENT(INOUT) :: m !< Misc/optimization variables - !! nectivity information does not have to be recalculated) + TYPE(AD_InputType), INTENT(IN ) :: u !< Inputs at time t + TYPE(AD_ParameterType), INTENT(IN ) :: p !< Parameters + TYPE(AD_MiscVarType), INTENT(INOUT) :: m !< Misc/optimization variables INTEGER(IntKi), INTENT( OUT) :: errStat !< Error status of the operation CHARACTER(*), INTENT( OUT) :: errMsg !< Error message if ErrStat /= ErrID_None - integer :: i,j - real(ReKi) :: SigmaCavitCrit, SigmaCavit + + ! Local variables + integer :: i, j + integer(intKi) :: iR, iW + real(ReKi) :: SigmaCavitCrit, SigmaCavit + real(ReKi) :: Vreltemp + real(ReKi) :: Cpmintemp errStat = ErrID_None errMsg = '' - if ( p%CavitCheck ) then ! Calculate the cavitation number for the airfoil at the node in quesiton, and compare to the critical cavitation number based on the vapour pressure and submerged depth - do j = 1,p%numBlades ! Loop through all blades - do i = 1,p%NumBlNds ! Loop through all nodes + do iR = 1,size(p%rotors) + if ( p%rotors(iR)%CavitCheck ) then ! Calculate the cavitation number for the airfoil at the node in quesiton, and compare to the critical cavitation number based on the vapour pressure and submerged depth + do j = 1,p%rotors(iR)%numBlades ! Loop through all blades + do i = 1,p%rotors(iR)%NumBlNds ! Loop through all nodes - if ( EqualRealNos( m%BEMT_y%Vrel(i,j), 0.0_ReKi ) ) call SetErrStat( ErrID_Fatal, 'Vrel cannot be zero to do a cavitation check', ErrStat, ErrMsg, 'AD_CavtCrit') - if (ErrStat >= AbortErrLev) return + if ( p%WakeMod == WakeMod_BEMT .or. p%WakeMod == WakeMod_DBEMT ) then + Vreltemp = m%rotors(iR)%BEMT_y%Vrel(i,j) + Cpmintemp = m%rotors(iR)%BEMT_y%Cpmin(i,j) + else if ( p%WakeMod == WakeMod_FVW ) then + iW = p%FVW%Bld2Wings(iR,j) + Vreltemp = m%FVW%W(iW)%BN_Vrel(i) + Cpmintemp = m%FVW%W(iW)%BN_Cpmin(i) + end if + + if ( EqualRealNos( Vreltemp, 0.0_ReKi ) ) call SetErrStat( ErrID_Fatal, 'Vrel cannot be zero to do a cavitation check', ErrStat, ErrMsg, 'AD_CavtCrit' ) + if ( ErrStat >= AbortErrLev ) return - SigmaCavit= -1* m%BEMT_y%Cpmin(i,j) ! Local cavitation number on node j - SigmaCavitCrit= ( ( p%Patm + ( p%Gravity * (p%WtrDpth - ( u%HubMotion%Position(3,1)+u%HubMotion%TranslationDisp(3,1) ) - ( u%BladeMotion(j)%Position(3,i) + u%BladeMotion(j)%TranslationDisp(3,i) - u%HubMotion%Position(3,1))) * p%airDens) - p%Pvap ) / ( 0.5_ReKi * p%airDens * m%BEMT_y%Vrel(i,j)**2)) ! Critical value of Sigma, cavitation occurs if local cavitation number is greater than this + SigmaCavit = -1 * Cpmintemp ! Local cavitation number on node j + SigmaCavitCrit = ( p%rotors(iR)%Patm + ( p%rotors(iR)%Gravity * ( p%rotors(iR)%WtrDpth - ( u%rotors(iR)%BladeMotion(j)%Position(3,i) + u%rotors(iR)%BladeMotion(j)%TranslationDisp(3,i) ) ) * p%rotors(iR)%airDens ) - p%rotors(iR)%Pvap ) / ( 0.5_ReKi * p%rotors(iR)%airDens * Vreltemp**2 ) ! Critical value of Sigma, cavitation occurs if local cavitation number is greater than this - if ( (SigmaCavitCrit < SigmaCavit) .and. (.not. (m%CavitWarnSet(i,j)) ) ) then - call WrScr( NewLine//'Cavitation occurred at blade '//trim(num2lstr(j))//' and node '//trim(num2lstr(i))//'.' ) - m%CavitWarnSet(i,j) = .true. + if ( ( SigmaCavitCrit < SigmaCavit ) .and. ( .not. ( m%rotors(iR)%CavitWarnSet(i,j) ) ) ) then + call WrScr( NewLine//'Cavitation occurred at blade '//trim(num2lstr(j))//' and node '//trim(num2lstr(i))//'.' ) + m%rotors(iR)%CavitWarnSet(i,j) = .true. end if - m%SigmaCavit(i,j)= SigmaCavit - m%SigmaCavitCrit(i,j)=SigmaCavitCrit + m%rotors(iR)%SigmaCavit(i,j) = SigmaCavit + m%rotors(iR)%SigmaCavitCrit(i,j) = SigmaCavitCrit - end do ! p%NumBlNds - end do ! p%numBlades - end if ! Cavitation check + end do ! p%NumBlNds + end do ! p%numBlades + end if ! Cavitation check + end do ! p%numRotors end subroutine AD_CavtCrit !---------------------------------------------------------------------------------------------------------------------------------- @@ -2143,7 +2158,7 @@ subroutine SetOutputsFromFVW(t, u, p, OtherState, x, xd, m, y, ErrStat, ErrMsg) type(AFI_OutputType) :: AFI_interp ! Resulting values from lookup table real(ReKi) :: UrelWind_s(3) ! Relative wind (wind+str) in section coords real(ReKi) :: Cx, Cy - real(ReKi) :: Cl_Static, Cd_Static, Cm_Static + real(ReKi) :: Cl_Static, Cd_Static, Cm_Static, Cpmin real(ReKi) :: Cl_dyn, Cd_dyn, Cm_dyn type(UA_InputType), pointer :: u_UA ! Alias to shorten notations integer(IntKi), parameter :: InputIndex=1 ! we will always use values at t in this routine @@ -2177,6 +2192,7 @@ subroutine SetOutputsFromFVW(t, u, p, OtherState, x, xd, m, y, ErrStat, ErrMsg) Cl_Static = AFI_interp%Cl Cd_Static = AFI_interp%Cd Cm_Static = AFI_interp%Cm + Cpmin = AFI_interp%Cpmin ! Set dynamic to the (will be same as static if UA_Flag is false) Cl_dyn = AFI_interp%Cl @@ -2231,6 +2247,7 @@ subroutine SetOutputsFromFVW(t, u, p, OtherState, x, xd, m, y, ErrStat, ErrMsg) m%FVW%W(iW)%BN_Cl_Static(j) = Cl_Static m%FVW%W(iW)%BN_Cd_Static(j) = Cd_Static m%FVW%W(iW)%BN_Cm_Static(j) = Cm_Static + m%FVW%W(iW)%BN_Cpmin(j) = Cpmin m%FVW%W(iW)%BN_Cl(j) = Cl_dyn m%FVW%W(iW)%BN_Cd(j) = Cd_dyn m%FVW%W(iW)%BN_Cm(j) = Cm_dyn diff --git a/modules/aerodyn/src/AeroDyn_AllBldNdOuts_IO.f90 b/modules/aerodyn/src/AeroDyn_AllBldNdOuts_IO.f90 index 7110eeb837..7918e878f4 100644 --- a/modules/aerodyn/src/AeroDyn_AllBldNdOuts_IO.f90 +++ b/modules/aerodyn/src/AeroDyn_AllBldNdOuts_IO.f90 @@ -1046,8 +1046,7 @@ SUBROUTINE Calc_WriteAllBldNdOutput( p, p_AD, u, m, m_AD, x, y, OtherState, Indx DO IdxBlade=1,p%BldNd_BladesOut iW = p_AD%FVW%Bld2Wings(iRot, IdxBlade) DO IdxNode=1,u%BladeMotion(IdxBlade)%NNodes -!NOT available in FVW yet - y%WriteOutput( OutIdx ) = 0.0_ReKi + y%WriteOutput( OutIdx ) = m_AD%FVW%W(iW)%BN_Cpmin(IdxNode) OutIdx = OutIdx + 1 ENDDO ENDDO @@ -1392,7 +1391,6 @@ SUBROUTINE BldNdOuts_SetOutParam(BldNd_OutList, p, p_AD, ErrStat, ErrMsg ) ! The following are invalid for free vortex wake InvalidOutput( BldNd_Chi ) = .true. InvalidOutput( BldNd_Curve ) = .true. - InvalidOutput( BldNd_CpMin ) = .true. InvalidOutput( BldNd_GeomPhi ) = .true. ! applies only to BEM endif diff --git a/modules/aerodyn/src/AeroDyn_IO.f90 b/modules/aerodyn/src/AeroDyn_IO.f90 index 219538ed20..4b61b9dded 100644 --- a/modules/aerodyn/src/AeroDyn_IO.f90 +++ b/modules/aerodyn/src/AeroDyn_IO.f90 @@ -1985,7 +1985,7 @@ subroutine Calc_WriteOutput_FVW m%AllOuts( BNPhi( beta,k) ) = m_AD%FVW%W(iW)%BN_phi(j)*R2D ! m%AllOuts( BNCurve(beta,k) ) = m%Curve(j,k)*R2D ! TODO -! m%AllOuts( BNCpmin( beta,k) ) = m%BEMT_y%Cpmin(jk) ! TODO + m%AllOuts( BNCpmin(beta,k) ) = m_AD%FVW%W(iW)%BN_Cpmin(j) m%AllOuts( BNCl( beta,k) ) = m_AD%FVW%W(iW)%BN_Cl(j) m%AllOuts( BNCd( beta,k) ) = m_AD%FVW%W(iW)%BN_Cd(j) m%AllOuts( BNCm( beta,k) ) = m_AD%FVW%W(iW)%BN_Cm(j) diff --git a/modules/aerodyn/src/FVW.f90 b/modules/aerodyn/src/FVW.f90 index 1e0b97173f..64aecb29c7 100644 --- a/modules/aerodyn/src/FVW.f90 +++ b/modules/aerodyn/src/FVW.f90 @@ -240,6 +240,7 @@ subroutine FVW_InitMiscVars( p, m, ErrStat, ErrMsg ) call AllocAry( m%W(iW)%BN_Cl_Static , p%W(iW)%nSpan+1 , 'Coefficient lift - no UA', ErrStat2, ErrMsg2 );call SetErrStat ( ErrStat2, ErrMsg2, ErrStat,ErrMsg,RoutineName ); m%W(iW)%BN_Cl_Static = -999999_ReKi; call AllocAry( m%W(iW)%BN_Cd_Static , p%W(iW)%nSpan+1 , 'Coefficient drag - no UA', ErrStat2, ErrMsg2 );call SetErrStat ( ErrStat2, ErrMsg2, ErrStat,ErrMsg,RoutineName ); m%W(iW)%BN_Cd_Static = -999999_ReKi; call AllocAry( m%W(iW)%BN_Cm_Static , p%W(iW)%nSpan+1 , 'Coefficient moment - no UA', ErrStat2, ErrMsg2 );call SetErrStat ( ErrStat2, ErrMsg2, ErrStat,ErrMsg,RoutineName ); m%W(iW)%BN_Cm_Static = -999999_ReKi; + call AllocAry( m%W(iW)%BN_Cpmin , p%W(iW)%nSpan+1 , 'Coefficient minimum pressure - no UA', ErrStat2, ErrMsg2 );call SetErrStat ( ErrStat2, ErrMsg2, ErrStat,ErrMsg,RoutineName ); m%W(iW)%BN_Cpmin = -999999_ReKi; call AllocAry( m%W(iW)%BN_Cl , p%W(iW)%nSpan+1 , 'Coefficient lift - with UA', ErrStat2, ErrMsg2 );call SetErrStat ( ErrStat2, ErrMsg2, ErrStat,ErrMsg,RoutineName ); m%W(iW)%BN_Cl = -999999_ReKi; call AllocAry( m%W(iW)%BN_Cd , p%W(iW)%nSpan+1 , 'Coefficient drag - with UA', ErrStat2, ErrMsg2 );call SetErrStat ( ErrStat2, ErrMsg2, ErrStat,ErrMsg,RoutineName ); m%W(iW)%BN_Cd = -999999_ReKi; call AllocAry( m%W(iW)%BN_Cm , p%W(iW)%nSpan+1 , 'Coefficient moment - with UA', ErrStat2, ErrMsg2 );call SetErrStat ( ErrStat2, ErrMsg2, ErrStat,ErrMsg,RoutineName ); m%W(iW)%BN_Cm = -999999_ReKi; diff --git a/modules/aerodyn/src/FVW_Registry.txt b/modules/aerodyn/src/FVW_Registry.txt index 4b820161e3..6083c2609d 100644 --- a/modules/aerodyn/src/FVW_Registry.txt +++ b/modules/aerodyn/src/FVW_Registry.txt @@ -165,6 +165,7 @@ typedef ^ ^ ReKi typedef ^ ^ ReKi BN_Cl_Static : - - "Coefficient lift, excluding unsteady aero effects" - typedef ^ ^ ReKi BN_Cd_Static : - - "Coefficient drag. excluding unsteady aero effects" - typedef ^ ^ ReKi BN_Cm_Static : - - "Coefficient moment, excluding unsteady aero effects" - +typedef ^ ^ ReKi BN_Cpmin : - - "Coefficient minimum pressure, excluding unsteady aero effects" - typedef ^ ^ ReKi BN_Cl : - - "Coefficient lift, including unsteady aero effects" - typedef ^ ^ ReKi BN_Cd : - - "Coefficient drag, including unsteady aero effects" - typedef ^ ^ ReKi BN_Cm : - - "Coefficient moment, including unsteady aero effects" - diff --git a/modules/aerodyn/src/FVW_Types.f90 b/modules/aerodyn/src/FVW_Types.f90 index 72df451fa7..6f287de750 100644 --- a/modules/aerodyn/src/FVW_Types.f90 +++ b/modules/aerodyn/src/FVW_Types.f90 @@ -194,6 +194,7 @@ MODULE FVW_Types REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: BN_Cl_Static !< Coefficient lift, excluding unsteady aero effects [-] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: BN_Cd_Static !< Coefficient drag. excluding unsteady aero effects [-] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: BN_Cm_Static !< Coefficient moment, excluding unsteady aero effects [-] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: BN_Cpmin !< Coefficient minimum pressure, excluding unsteady aero effects [-] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: BN_Cl !< Coefficient lift, including unsteady aero effects [-] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: BN_Cd !< Coefficient drag, including unsteady aero effects [-] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: BN_Cm !< Coefficient moment, including unsteady aero effects [-] @@ -4070,6 +4071,18 @@ SUBROUTINE FVW_CopyWng_MiscVarType( SrcWng_MiscVarTypeData, DstWng_MiscVarTypeDa END IF DstWng_MiscVarTypeData%BN_Cm_Static = SrcWng_MiscVarTypeData%BN_Cm_Static ENDIF +IF (ALLOCATED(SrcWng_MiscVarTypeData%BN_Cpmin)) THEN + i1_l = LBOUND(SrcWng_MiscVarTypeData%BN_Cpmin,1) + i1_u = UBOUND(SrcWng_MiscVarTypeData%BN_Cpmin,1) + IF (.NOT. ALLOCATED(DstWng_MiscVarTypeData%BN_Cpmin)) THEN + ALLOCATE(DstWng_MiscVarTypeData%BN_Cpmin(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstWng_MiscVarTypeData%BN_Cpmin.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstWng_MiscVarTypeData%BN_Cpmin = SrcWng_MiscVarTypeData%BN_Cpmin +ENDIF IF (ALLOCATED(SrcWng_MiscVarTypeData%BN_Cl)) THEN i1_l = LBOUND(SrcWng_MiscVarTypeData%BN_Cl,1) i1_u = UBOUND(SrcWng_MiscVarTypeData%BN_Cl,1) @@ -4248,6 +4261,9 @@ SUBROUTINE FVW_DestroyWng_MiscVarType( Wng_MiscVarTypeData, ErrStat, ErrMsg ) IF (ALLOCATED(Wng_MiscVarTypeData%BN_Cm_Static)) THEN DEALLOCATE(Wng_MiscVarTypeData%BN_Cm_Static) ENDIF +IF (ALLOCATED(Wng_MiscVarTypeData%BN_Cpmin)) THEN + DEALLOCATE(Wng_MiscVarTypeData%BN_Cpmin) +ENDIF IF (ALLOCATED(Wng_MiscVarTypeData%BN_Cl)) THEN DEALLOCATE(Wng_MiscVarTypeData%BN_Cl) ENDIF @@ -4539,6 +4555,11 @@ SUBROUTINE FVW_PackWng_MiscVarType( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, Int_BufSz = Int_BufSz + 2*1 ! BN_Cm_Static upper/lower bounds for each dimension Re_BufSz = Re_BufSz + SIZE(InData%BN_Cm_Static) ! BN_Cm_Static END IF + Int_BufSz = Int_BufSz + 1 ! BN_Cpmin allocated yes/no + IF ( ALLOCATED(InData%BN_Cpmin) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! BN_Cpmin upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%BN_Cpmin) ! BN_Cpmin + END IF Int_BufSz = Int_BufSz + 1 ! BN_Cl allocated yes/no IF ( ALLOCATED(InData%BN_Cl) ) THEN Int_BufSz = Int_BufSz + 2*1 ! BN_Cl upper/lower bounds for each dimension @@ -5320,6 +5341,21 @@ SUBROUTINE FVW_PackWng_MiscVarType( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, Re_Xferred = Re_Xferred + 1 END DO END IF + IF ( .NOT. ALLOCATED(InData%BN_Cpmin) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%BN_Cpmin,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%BN_Cpmin,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%BN_Cpmin,1), UBOUND(InData%BN_Cpmin,1) + ReKiBuf(Re_Xferred) = InData%BN_Cpmin(i1) + Re_Xferred = Re_Xferred + 1 + END DO + END IF IF ( .NOT. ALLOCATED(InData%BN_Cl) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -6302,6 +6338,24 @@ SUBROUTINE FVW_UnPackWng_MiscVarType( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrSt Re_Xferred = Re_Xferred + 1 END DO END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! BN_Cpmin not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%BN_Cpmin)) DEALLOCATE(OutData%BN_Cpmin) + ALLOCATE(OutData%BN_Cpmin(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%BN_Cpmin.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%BN_Cpmin,1), UBOUND(OutData%BN_Cpmin,1) + OutData%BN_Cpmin(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END IF IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! BN_Cl not allocated Int_Xferred = Int_Xferred + 1 ELSE From de041bb306b24be4fcbc0a6e1bbe5bd4ea19e1cf Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Thu, 7 Apr 2022 14:17:48 -0600 Subject: [PATCH 122/242] MoorDyn cylinder hydrodynamics capability - Added new support for horizontal Rods crossing the free surface, which blends with the previous support for surface-piercing Rods based on the incline angle. - Added initial log file capabilities (controlled with "writeLog" option). --- modules/moordyn/src/MoorDyn.f90 | 87 +++++++++++++++++++++-- modules/moordyn/src/MoorDyn_Body.f90 | 9 +++ modules/moordyn/src/MoorDyn_Line.f90 | 10 +-- modules/moordyn/src/MoorDyn_Misc.f90 | 3 - modules/moordyn/src/MoorDyn_Registry.txt | 2 + modules/moordyn/src/MoorDyn_Rod.f90 | 89 ++++++++++++++++++------ modules/moordyn/src/MoorDyn_Types.f90 | 35 ++++------ 7 files changed, 178 insertions(+), 57 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 6a9ab8af19..b1c84f0777 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -106,8 +106,8 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! Local variables for reading file input (Previously in MDIO_ReadInput) INTEGER(IntKi) :: UnEc ! The local unit number for this module's echo file - INTEGER(IntKi) :: UnOut ! for outputing wave kinematics data - CHARACTER(200) :: Frmt ! a string to hold a format statement + INTEGER(IntKi) :: UnOut ! for outputing wave kinematics data + CHARACTER(200) :: Frmt ! a string to hold a format statement CHARACTER(1024) :: EchoFile ! Name of MoorDyn echo file CHARACTER(1024) :: Line ! String to temporarially hold value of read line @@ -411,7 +411,19 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL Conv2UC(OptString) ! check all possible options types and see if OptString is one of them, in which case set the variable. - if ( OptString == 'DTM') THEN + if ( OptString == 'WRITELOG') THEN + read (OptValue,*) p%writeLog + if (p%writeLog > 0) then ! if not zero, open a log file for output + CALL GetNewUnit( p%UnLog ) + CALL OpenFOutFile ( p%UnLog, TRIM(p%RootName)//'.log', ErrStat, ErrMsg ) + IF ( ErrStat > AbortErrLev ) THEN + ErrMsg = ' Failed to open MoorDyn log file: '//TRIM(ErrMsg) + RETURN + END IF + write(p%UnLog,'(A)', IOSTAT=ErrStat2) "MoorDyn v2 log file with output level "//TRIM(Num2LStr(p%writeLog)) + write(p%UnLog,'(A)', IOSTAT=ErrStat2) "Note: options above the writeLog line in the input file will not be recorded." + end if + else if ( OptString == 'DTM') THEN read (OptValue,*) p%dtM0 else if ( OptString == 'G') then read (OptValue,*) p%g @@ -630,7 +642,18 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! specify IdNum of line type for error checking m%LineTypeList(l)%IdNum = l - + + ! write lineType information to log file + if (p%writeLog > 1) then + write(p%UnLog, '(A12,A20)' ) " LineType"//trim(num2lstr(l))//":" + write(p%UnLog, '(A12,A20)' ) " name: ", m%LineTypeList(l)%name + write(p%UnLog, '(A12,f12.4)') " d : ", m%LineTypeList(l)%d + write(p%UnLog, '(A12,f12.4)') " w : ", m%LineTypeList(l)%w + write(p%UnLog, '(A12,f12.4)') " Cdn : ", m%LineTypeList(l)%Cdn + write(p%UnLog, '(A12,f12.4)') " Can : ", m%LineTypeList(l)%Can + write(p%UnLog, '(A12,f12.4)') " Cdt : ", m%LineTypeList(l)%Cdt + write(p%UnLog, '(A12,f12.4)') " Cat : ", m%LineTypeList(l)%Cat + end if IF ( ErrStat2 /= ErrID_None ) THEN CALL SetErrStat( ErrID_Fatal, ErrMsg2, ErrStat, ErrMsg, RoutineName ) @@ -667,7 +690,18 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! specify IdNum of rod type for error checking m%RodTypeList(l)%IdNum = l - + + ! write lineType information to log file + if (p%writeLog > 1) then + write(p%UnLog, '(A12,A20)' ) " RodType"//trim(num2lstr(l))//":" + write(p%UnLog, '(A12,A20)' ) " name: ", m%RodTypeList(l)%name + write(p%UnLog, '(A12,f12.4)') " d : ", m%RodTypeList(l)%d + write(p%UnLog, '(A12,f12.4)') " w : ", m%RodTypeList(l)%w + write(p%UnLog, '(A12,f12.4)') " Cdn : ", m%RodTypeList(l)%Cdn + write(p%UnLog, '(A12,f12.4)') " Can : ", m%RodTypeList(l)%Can + write(p%UnLog, '(A12,f12.4)') " Cdt : ", m%RodTypeList(l)%CdEnd + write(p%UnLog, '(A12,f12.4)') " Cat : ", m%RodTypeList(l)%CaEnd + end if IF ( ErrStat2 /= ErrID_None ) THEN CALL SetErrStat( ErrID_Fatal, 'Failed to process rod type properties for rod '//trim(Num2LStr(l)), ErrStat, ErrMsg, RoutineName ) @@ -1522,6 +1556,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF (wordy > 2) print *, "CpldConIs are ", m%CpldConIs + ! write system description to log file + if (p%writeLog > 1) then + write(p%UnLog, '(A)') "----- MoorDyn Model Summary (to be written) -----" + end if + + + !------------------------------------------------------------------------------------ ! fill in state vector index record holders !------------------------------------------------------------------------------------ @@ -1920,6 +1961,38 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! END IF ! END DO + ! ------------------------------------------------------------------- + ! if log file, compute and write some object properties + ! ------------------------------------------------------------------- + if (p%writeLog > 1) then + + write(p%UnLog, '(A)' ) " Bodies:" + DO l = 1,p%nBodies + write(p%UnLog, '(A)' ) " Body"//trim(num2lstr(l))//":" + write(p%UnLog, '(A12, f12.4)') " mass: ", m%BodyList(l)%M(1,1) + END DO + + write(p%UnLog, '(A)' ) " Rods:" + DO l = 1,p%nRods + write(p%UnLog, '(A)' ) " Rod"//trim(num2lstr(l))//":" + ! m%RodList(l) + END DO + + write(p%UnLog, '(A)' ) " Points:" + DO l = 1,p%nFreeCons + write(p%UnLog, '(A)' ) " Point"//trim(num2lstr(l))//":" + ! m%ConnectList(l) + END DO + + write(p%UnLog, '(A)' ) " Lines:" + DO l = 1,p%nLines + write(p%UnLog, '(A)' ) " Line"//trim(num2lstr(l))//":" + ! m%LineList(l) + END DO + + end if + + ! -------------------------------------------------------------------- ! do dynamic relaxation to get ICs ! -------------------------------------------------------------------- @@ -2134,7 +2207,7 @@ END SUBROUTINE CheckError SUBROUTINE CleanUp() ! ErrStat = ErrID_Fatal call MD_DestroyInputFileType( InputFileDat, ErrStat2, ErrMsg2 ) ! Ignore any error messages from this - ! IF (InitInp%Echo) CLOSE( UnEc ) + IF (p%UnLog) CLOSE( p%UnLog ) END SUBROUTINE !> If for some reason the file is truncated, it is possible to get into an infinite loop @@ -2832,7 +2905,7 @@ SUBROUTINE MD_End(u, p, x, xd, z, other, y, m, ErrStat , ErrMsg) CALL MD_DestroyMisc(m, ErrStat2, ErrMsg2) CALL CheckError( ErrStat2, ErrMsg2 ) - + IF (p%UnLog) CLOSE( p%UnLog ) ! close log file if it's open !TODO: any need to specifically deallocate things like m%xTemp%states in the above? <<<< ! IF ( ErrStat==ErrID_None) THEN diff --git a/modules/moordyn/src/MoorDyn_Body.f90 b/modules/moordyn/src/MoorDyn_Body.f90 index 3f1345c596..aa6385d8bc 100644 --- a/modules/moordyn/src/MoorDyn_Body.f90 +++ b/modules/moordyn/src/MoorDyn_Body.f90 @@ -393,6 +393,7 @@ SUBROUTINE Body_DoRHS(Body, m, p) ! Initialize variables U = 0.0_DbKi ! Set to zero for now + Body%F6net = 0.0_DbKi ! First, the body's own mass matrix must be adjusted based on its orientation so that ! we have a mass matrix in the global orientation frame @@ -429,6 +430,10 @@ SUBROUTINE Body_DoRHS(Body, m, p) ! get net force and mass from Connection on body ref point (global orientation) CALL Connect_GetNetForceAndMass( m%ConnectList(Body%attachedC(l)), Body%r6(1:3), F6_i, M6_i, m, p) + if (ABS(F6_i(5)) > 1.0E12) then + print *, "Warning: extreme pitch moment from body-attached Point ", l + end if + ! sum quantitites Body%F6net = Body%F6net + F6_i Body%M = Body%M + M6_i @@ -440,6 +445,10 @@ SUBROUTINE Body_DoRHS(Body, m, p) ! get net force and mass from Rod on body ref point (global orientation) CALL Rod_GetNetForceAndMass(m%RodList(Body%attachedR(l)), Body%r6(1:3), F6_i, M6_i, m, p) + if (ABS(F6_i(5)) > 1.0E12) then + print *, "Warning: extreme pitch moment from body-attached Rod ", l + end if + ! sum quantitites Body%F6net = Body%F6net + F6_i Body%M = Body%M + M6_i diff --git a/modules/moordyn/src/MoorDyn_Line.f90 b/modules/moordyn/src/MoorDyn_Line.f90 index cb383331fe..b5dcf51500 100644 --- a/modules/moordyn/src/MoorDyn_Line.f90 +++ b/modules/moordyn/src/MoorDyn_Line.f90 @@ -124,11 +124,11 @@ SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) !print *, 'Segment natural frequency is ', temp, ' Hz' - print *, "Line ElasticMod is ", Line%ElasticMod - print *, "EA (static value) is", Line%EA - print *, "EA_D is", Line%EA_D - print *, "BA is", Line%BA - print *, "BA_D is", Line%BA_D + !print *, "Line ElasticMod is ", Line%ElasticMod + !print *, "EA (static value) is", Line%EA + !print *, "EA_D is", Line%EA_D + !print *, "BA is", Line%BA + !print *, "BA_D is", Line%BA_D ! allocate node positions and velocities (NOTE: these arrays start at ZERO) diff --git a/modules/moordyn/src/MoorDyn_Misc.f90 b/modules/moordyn/src/MoorDyn_Misc.f90 index ebefaf0b21..0f98c1d3b1 100644 --- a/modules/moordyn/src/MoorDyn_Misc.f90 +++ b/modules/moordyn/src/MoorDyn_Misc.f90 @@ -1634,7 +1634,6 @@ SUBROUTINE setupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) end do ! I, frequencies ! now IFFT all the wave kinematics except surface elevation and save it into the grid of data - print *, 'a' CALL ApplyFFT_cx( p%PDyn (:,iz,iy,ix), WaveDynPC , FFT_Data, ErrStatTmp ); CALL SetErrStat(ErrStatTmp,'Error IFFTing WaveDynP.', ErrStat,ErrMsg,RoutineName) CALL ApplyFFT_cx( p%uxWave(:,iz,iy,ix), WaveVelCHx, FFT_Data, ErrStatTmp ); CALL SetErrStat(ErrStatTmp,'Error IFFTing WaveVelHx.',ErrStat,ErrMsg,RoutineName) CALL ApplyFFT_cx( p%uyWave(:,iz,iy,ix), WaveVelCHy, FFT_Data, ErrStatTmp ); CALL SetErrStat(ErrStatTmp,'Error IFFTing WaveVelHy.',ErrStat,ErrMsg,RoutineName) @@ -1646,9 +1645,7 @@ SUBROUTINE setupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) end do ! iz ! IFFT wave elevation here because it's only at the surface - print *, 'b' CALL ApplyFFT_cx( p%zeta(:,iy,ix) , WaveElevC , FFT_Data, ErrStatTmp ); CALL SetErrStat(ErrStatTmp,'Error IFFTing WaveElev.', ErrStat,ErrMsg,RoutineName) - print *, 'c' end do ! iy end do ! ix diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 5f4d21cbb1..06f0d6d429 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -382,6 +382,8 @@ typedef ^ ^ MD_OutParmType OutParam {:} typedef ^ ^ CHARACTER(1) Delim - - - "Column delimiter for output text files" - typedef ^ ^ IntKi MDUnOut - - - "Unit number of main output file" typedef ^ ^ CHARACTER(1024) PriPath - - - "The path to the primary MoorDyn input file, used if looking for additional input files" +typedef ^ ^ IntKi writeLog - - - "Switch for level of log file output" +typedef ^ ^ IntKi UnLog - - - "Unit number of log file" typedef ^ ^ IntKi WaveKin - - - "Flag for whether or how to consider water kinematics" typedef ^ ^ IntKi Current - - - "Flag for whether or how to consider water kinematics" typedef ^ ^ IntKi nTurbines - - - "Number of turbines if MoorDyn is performing an array-level simulation with FAST.Farm, otherwise 0" diff --git a/modules/moordyn/src/MoorDyn_Rod.f90 b/modules/moordyn/src/MoorDyn_Rod.f90 index d4b75e6843..60c3131482 100644 --- a/modules/moordyn/src/MoorDyn_Rod.f90 +++ b/modules/moordyn/src/MoorDyn_Rod.f90 @@ -541,6 +541,14 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) Real(DbKi) :: dL ! length attributed to node Real(DbKi) :: VOF ! fraction of volume associated with node that is submerged + Real(DbKi) :: VOF0 ! original VOF based only on axis before refinement + Real(DbKi) :: z1hi ! highest elevation of cross section at node [m] + Real(DbKi) :: z1lo ! lowest elevation of cross section at node [m] + Real(DbKi) :: G ! distance normal to axis from bottom edge of cross section to waterplane [m] + Real(DbKi) :: A ! area of cross section at node that is below the waterline [m2] + Real(DbKi) :: zA ! crude approximation to z value of centroid of submerged cross section at node [m] + + Real(DbKi) :: Vi(3) ! relative flow velocity over a node Real(DbKi) :: SumSqVp, SumSqVq, MagVp, MagVq Real(DbKi) :: Vp(3), Vq(3) ! transverse and axial components of water velocity at a given node @@ -607,11 +615,14 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) zeta = Rod%zeta(N)! just use the wave elevation computed at the location of the top node for now - if ((Rod%r(3,0) < zeta) .and. (Rod%r(3,N) > zeta)) then ! check if it's crossing the water plane (should also add some limits to avoid near-horizontals at some point) + ! get approximate location of waterline crossing along Rod axis (note: negative h0 indicates end A is above end B, and measures -distance from end A to waterline crossing) + if ((Rod%r(3,0) < zeta) .and. (Rod%r(3,N) < zeta)) then ! fully submerged case + Rod%h0 = Rod%UnstrLen + else if ((Rod%r(3,0) < zeta) .and. (Rod%r(3,N) > zeta)) then ! check if it's crossing the water plane (should also add some limits to avoid near-horizontals at some point) Rod%h0 = (zeta - Rod%r(3,0))/Rod%q(3) ! distance along rod centerline from end A to the waterplane - else if (Rod%r(3,0) < zeta) then - Rod%h0 = Rod%UnstrLen ! fully submerged case <<<<<< remove the 2.0 and double check there are no if statements that get changed <<<< - else + else if ((Rod%r(3,N) < zeta) .and. (Rod%r(3,0) > zeta)) then ! check if it's crossing the water plane but upside down + Rod%h0 = -(zeta - Rod%r(3,0))/Rod%q(3) ! negative distance along rod centerline from end A to the waterplane + else Rod%h0 = 0.0_DbKi ! fully unsubmerged case (ever applicable?) end if @@ -638,16 +649,50 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) END IF ! get scalar for submerged portion - IF (Lsum + dL <= Rod%h0) THEN ! if fully submerged - VOF = 1.0_DbKi - ELSE IF (Lsum < Rod%h0) THEN ! if partially below waterline - VOF = (Rod%h0 - Lsum)/dL - ELSE ! must be out of water - VOF = 0.0_DbKi - END IF + if (Rod%h0 < 0.0_DbKi) then ! upside down partially-submerged Rod case + IF (Lsum >= -Rod%h0) THEN ! if fully submerged + VOF0 = 1.0_DbKi + ELSE IF (Lsum + dL > -Rod%h0) THEN ! if partially below waterline + VOF0 = (Lsum+dL + Rod%h0)/dL + ELSE ! must be out of water + VOF0 = 0.0_DbKi + END IF + else + IF (Lsum + dL <= Rod%h0) THEN ! if fully submerged + VOF0 = 1.0_DbKi + ELSE IF (Lsum < Rod%h0) THEN ! if partially below waterline + VOF0 = (Rod%h0 - Lsum)/dL + ELSE ! must be out of water + VOF0 = 0.0_DbKi + END IF + end if Lsum = Lsum + dL ! add length attributed to this node to the total + ! get submerged cross sectional area and centroid for each node + z1hi = Rod%r(3,I) + 0.5*Rod%d*abs(sinPhi) ! highest elevation of cross section at node + z1lo = Rod%r(3,I) - 0.5*Rod%d*abs(sinPhi) ! lowest elevation of cross section at node + + if (z1lo > zeta) then ! fully out of water + A = 0.0 ! area + zA = 0 ! centroid depth + else if (z1hi < zeta) then ! fully submerged + A = Pi*0.25*Rod%d**2 + zA = Rod%r(3,I) + else ! if z1hi*z1lo < 0.0: # if cross section crosses waterplane + if (abs(sinPhi) < 0.001) then + A = 0.5_DbKi + zA = 0.0_DbKi + else + G = (-z1lo+zeta)/abs(sinPhi) ! distance from node to waterline cross at same axial location [m] + A = 0.25*Rod%d**2*acos((Rod%d - 2.0*G)/Rod%d) - (0.5*Rod%d-G)*sqrt(Rod%d*G-G**2) ! area of circular cross section that is below waterline [m^2] + zA = (z1lo-zeta)/2 ! very crude approximation of centroid for now... <<< need to double check zeta bit <<< + end if + end if + + VOF = VOF0*cosPhi**2 + A/(0.25*Pi*Rod%d**2)*sinPhi**2 ! this is a more refined VOF-type measure that can work for any incline + + ! build mass and added mass matrix DO J=1,3 DO K=1,3 @@ -674,9 +719,8 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ! weight (now only the dry weight) Rod%W(:,I) = (/ 0.0_DbKi, 0.0_DbKi, -m_i * p%g /) ! assuming g is positive - ! buoyance (now calculated based on outside pressure, for submerged portion only) - ! radial buoyancy force from sides - Ftemp = -VOF * 0.25*Pi*dL*Rod%d*Rod%d * p%rhoW*p%g * sinPhi + ! radial buoyancy force from sides (now calculated based on outside pressure, for submerged portion only) + Ftemp = -VOF * v_i * p%rhoW*p%g * sinPhi ! magnitude of radial buoyancy force at this node Rod%Bo(:,I) = (/ Ftemp*cosBeta*cosPhi, Ftemp*sinBeta*cosPhi, -Ftemp*sinPhi /) !relative flow velocities @@ -747,7 +791,7 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ! >>> eventually should consider a VOF approach for the ends hTilt = 0.5*Rod%d/cosPhi <<< ! buoyancy force - Ftemp = -VOF * 0.25*Pi*Rod%d*Rod%d * p%rhoW*p%g*Rod%r(3,I) + Ftemp = -VOF * 0.25*Pi*Rod%d*Rod%d * p%rhoW*p%g* zA Rod%Bo(:,I) = Rod%Bo(:,I) + (/ Ftemp*cosBeta*sinPhi, Ftemp*sinBeta*sinPhi, Ftemp*cosPhi /) ! buoyancy moment @@ -773,17 +817,17 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) END DO END IF - - IF ((I==N) .and. (Rod%h0 >= Rod%UnstrLen)) THEN ! if this end B and it is submerged (note, if N=0, both this and previous if statement are true) + + IF ((I==N) .and. ((Rod%h0 >= Rod%UnstrLen) .or. (Rod%h0 < 0.0_DbKi))) THEN ! if this end B and it is submerged (note, if N=0, both this and previous if statement are true) ! buoyancy force - Ftemp = VOF * 0.25*Pi*Rod%d*Rod%d * p%rhoW*p%g*Rod%r(3,I) + Ftemp = VOF * 0.25*Pi*Rod%d*Rod%d * p%rhoW*p%g* zA Rod%Bo(:,I) = Rod%Bo(:,I) + (/ Ftemp*cosBeta*sinPhi, Ftemp*sinBeta*sinPhi, Ftemp*cosPhi /) ! buoyancy moment Mtemp = VOF * 1.0/64.0*Pi*Rod%d**4 * p%rhoW*p%g * sinPhi Rod%Mext = Rod%Mext + (/ Mtemp*sinBeta, -Mtemp*cosBeta, 0.0_DbKi /) - + ! axial drag Rod%Dq(:,I) = Rod%Dq(:,I) + VOF * 0.25* Pi*Rod%d*Rod%d * p%rhoW*Rod%CdEnd * MagVq * Vq @@ -815,9 +859,12 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ! ----- add waterplane moment of inertia moment if applicable ----- IF ((Rod%r(3,0) < zeta) .and. (Rod%r(3,N) > zeta)) then ! check if it's crossing the water plane - Mtemp = 1.0/16.0 *Pi*Rod%d**4 * p%rhoW*p%g * sinPhi * (1.0 + 0.5* tanPhi**2) + ! >>> could scale the below based on whether part of the end cap is crossing the water plane... + !Mtemp = 1.0/16.0 *Pi*Rod%d**4 * p%rhoW*p%g * sinPhi * (1.0 + 0.5* tanPhi**2) ! original (goes to infinity at 90 deg) + Mtemp = 1.0/16.0 *Pi*Rod%d**4 * p%rhoW*p%g * sinPhi * cosPhi ! simple alternative that goes to 0 at 90 deg then reverses sign beyond that Rod%Mext = Rod%Mext + (/ Mtemp*sinBeta, -Mtemp*cosBeta, 0.0_DbKi /) END IF + ! ---------------- now add in forces on end nodes from attached lines ------------------ @@ -850,7 +897,7 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) Rod%M(:,:,N) = Rod%M(:,:,N) + Mass_i ! mass at end node END DO - + ! ---------------- now lump everything in 6DOF about end A ----------------------------- ! question: do I really want to neglect the rotational inertia/drag/etc across the length of each segment? diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index 788414bf92..f306dc104a 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -143,9 +143,6 @@ MODULE MoorDyn_Types INTEGER(IntKi) , DIMENSION(1:10) :: Attached !< list of IdNums of lines attached to this connection node [-] INTEGER(IntKi) , DIMENSION(1:10) :: Top !< list of ints specifying whether each line is attached at 1 = top/fairlead(end B), 0 = bottom/anchor(end A) [-] INTEGER(IntKi) :: nAttached = 0 !< number of attached lines [-] - REAL(DbKi) :: conX !< [-] - REAL(DbKi) :: conY !< [-] - REAL(DbKi) :: conZ !< [-] REAL(DbKi) :: conM !< [-] REAL(DbKi) :: conV !< [-] REAL(DbKi) :: conFX !< [-] @@ -417,6 +414,8 @@ MODULE MoorDyn_Types CHARACTER(1) :: Delim !< Column delimiter for output text files [-] INTEGER(IntKi) :: MDUnOut !< Unit number of main output file [-] CHARACTER(1024) :: PriPath !< The path to the primary MoorDyn input file, used if looking for additional input files [-] + INTEGER(IntKi) :: writeLog !< Switch for level of log file output [-] + INTEGER(IntKi) :: UnLog !< Unit number of log file [-] INTEGER(IntKi) :: WaveKin !< Flag for whether or how to consider water kinematics [-] INTEGER(IntKi) :: Current !< Flag for whether or how to consider water kinematics [-] INTEGER(IntKi) :: nTurbines !< Number of turbines if MoorDyn is performing an array-level simulation with FAST.Farm, otherwise 0 [-] @@ -2353,9 +2352,6 @@ SUBROUTINE MD_CopyConnect( SrcConnectData, DstConnectData, CtrlCode, ErrStat, Er DstConnectData%Attached = SrcConnectData%Attached DstConnectData%Top = SrcConnectData%Top DstConnectData%nAttached = SrcConnectData%nAttached - DstConnectData%conX = SrcConnectData%conX - DstConnectData%conY = SrcConnectData%conY - DstConnectData%conZ = SrcConnectData%conZ DstConnectData%conM = SrcConnectData%conM DstConnectData%conV = SrcConnectData%conV DstConnectData%conFX = SrcConnectData%conFX @@ -2441,9 +2437,6 @@ SUBROUTINE MD_PackConnect( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Int_BufSz = Int_BufSz + SIZE(InData%Attached) ! Attached Int_BufSz = Int_BufSz + SIZE(InData%Top) ! Top Int_BufSz = Int_BufSz + 1 ! nAttached - Db_BufSz = Db_BufSz + 1 ! conX - Db_BufSz = Db_BufSz + 1 ! conY - Db_BufSz = Db_BufSz + 1 ! conZ Db_BufSz = Db_BufSz + 1 ! conM Db_BufSz = Db_BufSz + 1 ! conV Db_BufSz = Db_BufSz + 1 ! conFX @@ -2510,12 +2503,6 @@ SUBROUTINE MD_PackConnect( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, END DO IntKiBuf(Int_Xferred) = InData%nAttached Int_Xferred = Int_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%conX - Db_Xferred = Db_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%conY - Db_Xferred = Db_Xferred + 1 - DbKiBuf(Db_Xferred) = InData%conZ - Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%conM Db_Xferred = Db_Xferred + 1 DbKiBuf(Db_Xferred) = InData%conV @@ -2631,12 +2618,6 @@ SUBROUTINE MD_UnPackConnect( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMs END DO OutData%nAttached = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 - OutData%conX = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 - OutData%conY = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 - OutData%conZ = DbKiBuf(Db_Xferred) - Db_Xferred = Db_Xferred + 1 OutData%conM = DbKiBuf(Db_Xferred) Db_Xferred = Db_Xferred + 1 OutData%conV = DbKiBuf(Db_Xferred) @@ -10717,6 +10698,8 @@ SUBROUTINE MD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) DstParamData%Delim = SrcParamData%Delim DstParamData%MDUnOut = SrcParamData%MDUnOut DstParamData%PriPath = SrcParamData%PriPath + DstParamData%writeLog = SrcParamData%writeLog + DstParamData%UnLog = SrcParamData%UnLog DstParamData%WaveKin = SrcParamData%WaveKin DstParamData%Current = SrcParamData%Current DstParamData%nTurbines = SrcParamData%nTurbines @@ -11198,6 +11181,8 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Int_BufSz = Int_BufSz + 1*LEN(InData%Delim) ! Delim Int_BufSz = Int_BufSz + 1 ! MDUnOut Int_BufSz = Int_BufSz + 1*LEN(InData%PriPath) ! PriPath + Int_BufSz = Int_BufSz + 1 ! writeLog + Int_BufSz = Int_BufSz + 1 ! UnLog Int_BufSz = Int_BufSz + 1 ! WaveKin Int_BufSz = Int_BufSz + 1 ! Current Int_BufSz = Int_BufSz + 1 ! nTurbines @@ -11484,6 +11469,10 @@ SUBROUTINE MD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si IntKiBuf(Int_Xferred) = ICHAR(InData%PriPath(I:I), IntKi) Int_Xferred = Int_Xferred + 1 END DO ! I + IntKiBuf(Int_Xferred) = InData%writeLog + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%UnLog + Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%WaveKin Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%Current @@ -12130,6 +12119,10 @@ SUBROUTINE MD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg OutData%PriPath(I:I) = CHAR(IntKiBuf(Int_Xferred)) Int_Xferred = Int_Xferred + 1 END DO ! I + OutData%writeLog = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%UnLog = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 OutData%WaveKin = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 OutData%Current = IntKiBuf(Int_Xferred) From f2629d685f6bd303e139665198992f7d79993f07 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Paul=20Sch=C3=BCnemann?= <47893479+pschuenemann@users.noreply.github.com> Date: Fri, 18 Mar 2022 15:56:41 +0100 Subject: [PATCH 123/242] MDv2: Make parsing of OutList from input file more robust - allow for descriptions/comments on the output parameters in input file, when output parameters are put in quotes (copy and adapt some code from NWTC_IO::ReadOutputListFromFileInfo() to parse quoted strings) - add some checks for Line, Rod, Connect and Body output specifiers and throw warnings if the specifiers are missing any ID --- modules/moordyn/src/MoorDyn.f90 | 11 +++++++++++ modules/moordyn/src/MoorDyn_IO.f90 | 26 +++++++++++++++++++++----- 2 files changed, 32 insertions(+), 5 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index b1c84f0777..233343f951 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -146,6 +146,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er INTEGER :: MaxAryLen = 1000 ! Maximum length of the array being read INTEGER :: NumWords ! Number of words contained on a line INTEGER :: Nx + INTEGER :: QuoteCh ! Character position. CHARACTER(*), PARAMETER :: RoutineName = 'MD_Init' @@ -1450,11 +1451,21 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er DO ! read a line Line = NextLine(i) + Line = adjustl(trim(Line)) ! remove leading whitespace CALL Conv2UC(Line) ! convert to uppercase for easy string matching if ((INDEX(Line, "---") > 0) .or. (INDEX(Line, "END") > 0)) EXIT ! stop if we hit a header line or the keyword "END" + ! Check if we have a quoted string at the beginning. Ignore anything outside the quotes if so (this is the ReadVar behaviour for quoted strings). + IF (SCAN(Line(1:1), '''"' ) == 1_IntKi ) THEN + QuoteCh = SCAN( Line(2:), '''"' ) ! last quote + IF (QuoteCh < 1) QuoteCh = LEN_TRIM(Line) ! in case no end quote + Line(QuoteCh+2:) = ' ' ! blank out everything after last quote + ELSE + CALL WrScr('Warning: Could not find a quoted string in line: '//trim(Line)//'. Output specifier(s) should be enclosed in quotes for proper parsing of outlist.') + END IF + NumWords = CountWords( Line ) ! The number of words in Line. p%NumOuts = p%NumOuts + NumWords ! The total number of output channels read in so far. diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index db03558bb5..faf6b823bb 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -516,8 +516,14 @@ SUBROUTINE MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat, ErrMsg ) IF (let1(1:1) == 'L') THEN ! Look for L?N?xxxx p%OutParam(I)%OType = 1 ! Line object type ! for now we'll just assume the next character(s) are "n" to represent node number or "s" to represent segment number - READ (num2,*) nID ! node or segment ID - p%OutParam(I)%NodeID = nID + IF (num2/=" ") THEN + READ (num2,*) nID ! node or segment ID + p%OutParam(I)%NodeID = nID + ELSE + CALL DenoteInvalidOutput(p%OutParam(I)) ! flag as invalid + CALL WrScr('Warning: invalid output specifier '//trim(OutListTmp)//'. Line ID or Node ID missing.') + CYCLE + END IF qVal = let3 ! quantity type string ! Connect case @@ -531,10 +537,14 @@ SUBROUTINE MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat, ErrMsg ) IF (LEN_TRIM(let3)== 0) THEN ! No third character cluster indicates this is a whole-rod channel p%OutParam(I)%NodeID = 0 qVal = let2 ! quantity type string - ELSE + ELSE IF (num2/=" ") THEN READ (num2,*) nID ! rod node ID p%OutParam(I)%NodeID = nID qVal = let3 ! quantity type string + ELSE + CALL DenoteInvalidOutput(p%OutParam(I)) ! flag as invalid + CALL WrScr('Warning: invalid output specifier '//trim(OutListTmp)//'. Rod ID or Node ID missing.') + CYCLE END IF ! Body case @@ -552,8 +562,14 @@ SUBROUTINE MDIO_ProcessOutList(OutList, p, m, y, InitOut, ErrStat, ErrMsg ) END IF ! object number - READ (num1,*) oID - p%OutParam(I)%ObjID = oID ! line or connect ID number + IF (num1/=" ") THEN + READ (num1,*) oID + p%OutParam(I)%ObjID = oID ! line or connect ID number + ELSE + CALL DenoteInvalidOutput(p%OutParam(I)) ! flag as invalid + CALL WrScr('Warning: invalid output specifier '//trim(OutListTmp)//'. Object ID missing.') + CYCLE + END IF ! which kind of quantity? IF (qVal == 'PX') THEN From f7b799e5e086795a0cee76e29332b076d5a6acd2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Paul=20Sch=C3=BCnemann?= <47893479+pschuenemann@users.noreply.github.com> Date: Fri, 18 Mar 2022 15:57:40 +0100 Subject: [PATCH 124/242] MDv2: More strict parsing of Type/BodyID for pinned Rods Consider a Rod only as pinned if input is exactly "PINNED" or "PIN". Anything else now results in fatal error. Thus, "pinnedx", "pinxned" or similar which used to be interpreted as a pinned rod now result in an error. Also, if BodyID is followed by any characters it will now result in an error. Before, those characters were ignored and the Rod interpreted as fixed. --- modules/moordyn/src/MoorDyn.f90 | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 233343f951..21406803ba 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -924,7 +924,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL Body_AddRod(m%BodyList(J), l, tempArray) ! add rod l to the body - if ((INDEX(let2, "PINNED") > 0) .or. (INDEX(let2, "PIN") > 0)) then + if ( (let2 == "PINNED") .or. (let2 == "PIN") ) then m%RodList(l)%typeNum = 1 p%nFreeRods=p%nFreeRods+1 ! add this pinned rod to the free list because it is half free @@ -935,8 +935,12 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er m%FreeRodIs(p%nFreeRods) = l - else + else if (let2 == " ") then ! rod is not requested to be pinned, so add this rod as a fixed one m%RodList(l)%typeNum = 2 + + else + CALL SetErrStat( ErrID_Fatal, "Unidentified Type/BodyID for Rod "//trim(Num2LStr(l))//": "//trim(tempString2), ErrStat, ErrMsg, RoutineName ) + return end if else From e908c4b4f9b1687a591271493f57f52b0ed2c5c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Paul=20Sch=C3=BCnemann?= <47893479+pschuenemann@users.noreply.github.com> Date: Fri, 18 Mar 2022 15:58:55 +0100 Subject: [PATCH 125/242] MDv2: Check for correct number of columns in each row of each input table -> throw fatal error if wrong number of columns to prevent program crashes and incorrect processing of data --- modules/moordyn/src/MoorDyn.f90 | 44 ++++++++++++++++++++++++++++++++- 1 file changed, 43 insertions(+), 1 deletion(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 21406803ba..aad318d52e 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -587,6 +587,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !read into a line Line = NextLine(i) + + ! check for correct number of columns in current line + IF ( CountWords( Line ) /= 10 ) THEN + CALL SetErrStat( ErrID_Fatal, ' Unable to parse Line type '//trim(Num2LStr(l))//' on row '//trim(Num2LStr(i))//' in input file. Row has wrong number of columns. Must be 10 columns.', ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF ! parse out entries: Name Diam MassDenInAir EA cIntDamp EI Cd Ca CdAx CaAx READ(Line,*,IOSTAT=ErrStat2) m%LineTypeList(l)%name, m%LineTypeList(l)%d, & @@ -680,6 +687,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !read into a line Line = NextLine(i) + ! check for correct number of columns in current line + IF ( CountWords( Line ) /= 7 ) THEN + CALL SetErrStat( ErrID_Fatal, ' Unable to parse Rod Type '//trim(Num2LStr(l))//' on row '//trim(Num2LStr(i))//' in input file. Row has wrong number of columns. Must be 7 columns.', ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF + ! parse out entries: Name Diam MassDen Cd Ca CdEnd CaEnd IF (ErrStat2 == 0) THEN READ(Line,*,IOSTAT=ErrStat2) m%RodTypeList(l)%name, m%RodTypeList(l)%d, m%RodTypeList(l)%w, & @@ -705,7 +719,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er end if IF ( ErrStat2 /= ErrID_None ) THEN - CALL SetErrStat( ErrID_Fatal, 'Failed to process rod type properties for rod '//trim(Num2LStr(l)), ErrStat, ErrMsg, RoutineName ) + CALL SetErrStat( ErrID_Fatal, 'Failed to process rod type properties for rod '//trim(Num2LStr(l))//'. Check formatting and correct number of columns.', ErrStat, ErrMsg, RoutineName ) CALL CleanUp() RETURN END IF @@ -725,6 +739,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !read into a line Line = NextLine(i) + + ! check for correct number of columns in current line + IF ( CountWords( Line ) /= 14 ) THEN + CALL SetErrStat( ErrID_Fatal, ' Unable to parse Body '//trim(Num2LStr(l))//' on row '//trim(Num2LStr(i))//' in input file. Row has wrong number of columns. Must be 14 columns.', ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF ! parse out entries: ID Attachment X0 Y0 Z0 r0 p0 y0 M CG* I* V CdA* Ca* IF (ErrStat2 == 0) THEN @@ -872,6 +893,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !read into a line Line = NextLine(i) + ! check for correct number of columns in current line + IF ( CountWords( Line ) /= 11 ) THEN + CALL SetErrStat( ErrID_Fatal, ' Unable to parse Rod '//trim(Num2LStr(l))//' on row '//trim(Num2LStr(i))//' in input file. Row has wrong number of columns. Must be 11 columns.', ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF + ! parse out entries: RodID RodType Attachment Xa Ya Za Xb Yb Zb NumSegs Flags/Outputs IF (ErrStat2 == 0) THEN READ(Line,*,IOSTAT=ErrStat2) m%RodList(l)%IdNum, tempString1, tempString2, & @@ -1058,6 +1086,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !read into a line Line = NextLine(i) + ! check for correct number of columns in current line + IF ( CountWords( Line ) /= 9 ) THEN + CALL SetErrStat( ErrID_Fatal, ' Unable to parse Point '//trim(Num2LStr(l))//' on row '//trim(Num2LStr(i))//' in input file. Row has wrong number of columns. Must be 9 columns.', ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF + ! parse out entries: PointID Attachment X Y Z M V CdA Ca IF (ErrStat2 == 0) THEN READ(Line,*,IOSTAT=ErrStat2) m%ConnectList(l)%IdNum, tempString1, tempArray(1), & @@ -1209,6 +1244,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !read into a line Line = NextLine(i) + ! check for correct number of columns in current line + IF ( CountWords( Line ) /= 7 ) THEN + CALL SetErrStat( ErrID_Fatal, ' Unable to parse Line '//trim(Num2LStr(l))//' on row '//trim(Num2LStr(i))//' in input file. Row has wrong number of columns. Must be 7 columns.', ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF + ! parse out entries: ID LineType AttachA AttachB UnstrLen NumSegs Outputs (note: order changed Dec 13, 2021 before MDv2 release) IF (ErrStat2 == 0) THEN READ(Line,*,IOSTAT=ErrStat2) m%LineList(l)%IdNum, tempString1, tempString2, tempString3, & From 490bd8e852d310893ead94734c68fc5be588a6dc Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Thu, 7 Apr 2022 14:50:06 -0600 Subject: [PATCH 126/242] MoorDyn: restoring backward-compatible output names - Modified Paul's contribution of quoted output channel names to also allow the original unquoted format rather than throwing error. - Minor console output cleanup. --- modules/moordyn/src/MoorDyn.f90 | 2 -- modules/moordyn/src/MoorDyn_IO.f90 | 2 +- modules/moordyn/src/MoorDyn_Misc.f90 | 2 +- modules/moordyn/src/MoorDyn_Rod.f90 | 3 --- 4 files changed, 2 insertions(+), 7 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index aad318d52e..6e9ef9a5f9 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -1508,8 +1508,6 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er QuoteCh = SCAN( Line(2:), '''"' ) ! last quote IF (QuoteCh < 1) QuoteCh = LEN_TRIM(Line) ! in case no end quote Line(QuoteCh+2:) = ' ' ! blank out everything after last quote - ELSE - CALL WrScr('Warning: Could not find a quoted string in line: '//trim(Line)//'. Output specifier(s) should be enclosed in quotes for proper parsing of outlist.') END IF NumWords = CountWords( Line ) ! The number of words in Line. diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index faf6b823bb..16391b3592 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -159,7 +159,7 @@ SUBROUTINE setupBathymetry(inputString, defaultDepth, BathGrid, BathGrid_Xs, Bat BathGrid_Ys(1) = 0.0_DbKi ELSE ! otherwise interpret the input as a file name to load the bathymetry lookup data from - PRINT *, "found a letter in the depth value so will try to load a bathymetry file" + PRINT *, " The depth input contains letters so will load a bathymetry file." ! load lookup table data from file CALL GetNewUnit( UnCoef ) ! unit number for coefficient input file diff --git a/modules/moordyn/src/MoorDyn_Misc.f90 b/modules/moordyn/src/MoorDyn_Misc.f90 index 0f98c1d3b1..012f185fe7 100644 --- a/modules/moordyn/src/MoorDyn_Misc.f90 +++ b/modules/moordyn/src/MoorDyn_Misc.f90 @@ -1346,7 +1346,7 @@ SUBROUTINE setupWaterKin(WaterKinString, p, Tmax, ErrStat, ErrMsg) ! otherwise interpret the input as a file name to load the bathymetry lookup data from - print *, "found a letter in the WaterKin value so will try to load a water kinematics input file" + print *, " The waterKin input contains letters so will load a water kinematics input file" ! -------- load water kinematics input file ------------- diff --git a/modules/moordyn/src/MoorDyn_Rod.f90 b/modules/moordyn/src/MoorDyn_Rod.f90 index 60c3131482..8046764174 100644 --- a/modules/moordyn/src/MoorDyn_Rod.f90 +++ b/modules/moordyn/src/MoorDyn_Rod.f90 @@ -112,9 +112,6 @@ SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) CALL UnitVector(endCoords(1:3), endCoords(4:6), Rod%q, Rod%UnstrLen) ! get Rod axis direction vector and Rod length - print *, 'I am a rod' - print *, endCoords - ! set Rod positions (some or all may be overwritten depending on if the Rod is coupled or attached to a Body) Rod%r6(1:3) = endCoords(1:3) ! (end A coordinates) Rod%v6(1:3) = 0.0_DbKi ! (end A velocity, unrotated axes) From a8b1d091e5ac00806893669fdec5513bdc0af85e Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Mon, 11 Apr 2022 09:31:08 -0600 Subject: [PATCH 127/242] Enabling and fixing cable bending stiffness capability: - Turned on Line%EI value (previously set at zero). - Corrected indexing errors in bending stiffness application at Line end nodes. - Expanded Options keywords: rhow+rho, kbot+kb, cbot+cb. - Corrected unit conversion in Rod roll and pitch outputs. --- modules/moordyn/src/MoorDyn.f90 | 8 +-- modules/moordyn/src/MoorDyn_Line.f90 | 80 +++++++++++++--------------- modules/moordyn/src/MoorDyn_Rod.f90 | 4 +- 3 files changed, 43 insertions(+), 49 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 6e9ef9a5f9..eff7bf9c43 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -35,7 +35,7 @@ MODULE MoorDyn PRIVATE - TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn', '', '' ) + TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn', 'v2.a25', '2022-04-11' ) INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output @@ -428,13 +428,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er read (OptValue,*) p%dtM0 else if ( OptString == 'G') then read (OptValue,*) p%g - else if ( OptString == 'RHOW') then + else if (( OptString == 'RHOW') .or. ( OptString == 'RHO')) then read (OptValue,*) p%rhoW else if (( OptString == 'WTRDPTH') .or. ( OptString == 'DEPTH') .or. ( OptString == 'WATERDEPTH')) then read (OptValue,*) DepthValue ! water depth input read in as a string to be processed by setupBathymetry - else if ( OptString == 'KBOT') then + else if (( OptString == 'KBOT') .or. ( OptString == 'KB')) then read (OptValue,*) p%kBot - else if ( OptString == 'CBOT') then + else if (( OptString == 'CBOT') .or. ( OptString == 'CB')) then read (OptValue,*) p%cBot else if ( OptString == 'DTIC') then read (OptValue,*) InputFileDat%dtIC diff --git a/modules/moordyn/src/MoorDyn_Line.f90 b/modules/moordyn/src/MoorDyn_Line.f90 index b5dcf51500..434c981bbf 100644 --- a/modules/moordyn/src/MoorDyn_Line.f90 +++ b/modules/moordyn/src/MoorDyn_Line.f90 @@ -72,7 +72,7 @@ SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) ! note: Line%BA is set later Line%EA_D = LineProp%EA_D Line%BA_D = LineProp%BA_D - !Line%EI = LineProp%EI <<< for bending stiffness + Line%EI = LineProp%EI !<<< for bending stiffness Line%Can = LineProp%Can Line%Cat = LineProp%Cat @@ -1078,19 +1078,24 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, ! Line%V(I) = Pi/4.0 * d*d*Line%l(I) !volume attributed to segment END DO - !calculate unit tangent vectors (q) for each node (including ends) note: I think these are pointing toward 0 rather than N! - CALL UnitVector(Line%r(:,0), Line%r(:,1), Line%q(:,0), dummyLength) ! compute unit vector q + !calculate unit tangent vectors (q) for each internal node based on adjacent node positions DO I = 1, N-1 - CALL UnitVector(Line%r(:,I-1), Line%r(:,I+1), Line%q(:,I), dummyLength) ! compute unit vector q ... using adjacent two nodes! + CALL UnitVector(Line%r(:,I-1), Line%r(:,I+1), Line%q(:,I), dummyLength) END DO - CALL UnitVector(Line%r(:,N-1), Line%r(:,N), Line%q(:,N), dummyLength) ! compute unit vector q - - + + ! calculate unit tangent vectors for either end node if the line has no bending stiffness of if either end is pinned (otherwise it's already been set via setEndStateFromRod) + if ((Line%endTypeA==0) .or. (Line%EI==0.0)) then + CALL UnitVector(Line%r(:,0), Line%r(:,1), Line%q(:,0), dummyLength) + end if + if ((Line%endTypeB==0) .or. (Line%EI==0.0)) then + CALL UnitVector(Line%r(:,N-1), Line%r(:,N), Line%q(:,N), dummyLength) + end if + ! apply wave kinematics (if there are any) DO i=0,N CALL getWaterKin(p, Line%r(1,i), Line%r(2,i), Line%r(3,i), Line%time, m%WaveTi, Line%U(:,i), Line%Ud(:,i), Line%zeta(i), Line%PDyn(i)) END DO - + ! --------------- calculate mass (including added mass) matrix for each node ----------------- DO I = 0, N @@ -1207,20 +1212,17 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, if (Line%endTypeA > 0) then ! if attached to Rod i.e. cantilever connection - Kurvi = GetCurvature(Line%lstr(i), Line%q(:,i), Line%qs(:,i)) ! curvature <<< check if this approximation works for an end (assuming rod angle is node angle which is middle of if there was a segment -1/2 + Kurvi = GetCurvature(Line%lstr(1), Line%q(:,0), Line%qs(:,1)) ! curvature (assuming rod angle is node angle which is middle of if there was a segment -1/2) - pvec = cross_product(Line%q(:,0), Line%qs(:,i)) ! get direction of bending radius axis + pvec = cross_product(Line%q(:,0), Line%qs(:,1)) ! get direction of bending radius axis - Mforce_ip1 = cross_product(Line%qs(:,i), pvec) ! get direction of resulting force from bending to apply on node i+1 - - ! record bending moment at end for potential application to attached object <<<< do double check this.... - call scalevector(pvec, Kurvi*Line%EI, Line%endMomentA) - - ! scale force direction vectors by desired moment force magnitudes to get resulting forces on adjacent nodes - call scalevector(Mforce_ip1, Kurvi*Line%EI/Line%lstr(i), Mforce_ip1) + Mforce_ip1 = cross_product(Line%qs(:,1), pvec) ! get direction of resulting force from bending to apply on node i+1 + + call scalevector(pvec, Kurvi*Line%EI, Line%endMomentA) ! record bending moment at end for potential application to attached object + + call scalevector(Mforce_ip1, Kurvi*Line%EI/Line%lstr(1), Mforce_ip1) ! scale force direction vectors by desired moment force magnitudes to get resulting forces on adjacent nodes - ! set force on node i to cancel out forces on adjacent nodes - Mforce_i = -Mforce_ip1 + Mforce_i = -Mforce_ip1 ! set force on node i to cancel out forces on adjacent nodes ! apply these forces to the node forces Line%Bs(:,i ) = Line%Bs(:,i ) + Mforce_i @@ -1233,20 +1235,17 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, if (Line%endTypeB > 0) then ! if attached to Rod i.e. cantilever connection - Kurvi = GetCurvature(Line%lstr(i-1), Line%qs(:,i-1), Line%q(:,i)) ! curvature <<< check if this approximation works for an end (assuming rod angle is node angle which is middle of if there was a segment -1/2 + Kurvi = GetCurvature(Line%lstr(N), Line%qs(:,N), Line%q(:,N)) ! curvature (assuming rod angle is node angle which is middle of if there was a segment -1/2 - pvec = cross_product(Line%qs(:,i-1), Line%q(:,N)) ! get direction of bending radius axis + pvec = cross_product(Line%qs(:,N), Line%q(:,N)) ! get direction of bending radius axis - Mforce_im1 = cross_product(Line%qs(:,i-1), pvec) ! get direction of resulting force from bending to apply on node i-1 + Mforce_im1 = cross_product(Line%qs(:,N), pvec) ! get direction of resulting force from bending to apply on node i-1 - ! record bending moment at end for potential application to attached object <<<< do double check this.... - call scalevector(pvec, -Kurvi*Line%EI, Line%endMomentB ) ! note end B is oposite sign as end A + call scalevector(pvec, -Kurvi*Line%EI, Line%endMomentB ) ! record bending moment at end (note end B is oposite sign as end A) - ! scale force direction vectors by desired moment force magnitudes to get resulting forces on adjacent nodes - call scalevector(Mforce_im1, Kurvi*Line%EI/Line%lstr(i-1), Mforce_im1) - - ! set force on node i to cancel out forces on adjacent nodes - Mforce_i = Mforce_im1 + call scalevector(Mforce_im1, Kurvi*Line%EI/Line%lstr(N), Mforce_im1) ! scale force direction vectors by desired moment force magnitudes to get resulting forces on adjacent nodes + + Mforce_i = Mforce_im1 ! set force on node i to cancel out forces on adjacent nodes ! apply these forces to the node forces Line%Bs(:,i-1) = Line%Bs(:,i-1) + Mforce_im1 @@ -1256,19 +1255,18 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, else ! internal node - Kurvi = GetCurvature(Line%lstr(i-1)+Line%lstr(i), Line%qs(:,i-1), Line%qs(:,i)) ! curvature <<< remember to check sign, or just take abs + Kurvi = GetCurvature(Line%lstr(i)+Line%lstr(i+1), Line%qs(:,i), Line%qs(:,i+1)) ! curvature - pvec = cross_product(Line%qs(:,i-1), Line%qs(:,i)) ! get direction of bending radius axis + pvec = cross_product(Line%qs(:,i), Line%qs(:,i+1)) ! get direction of bending radius axis - Mforce_im1 = cross_product(Line%qs(:,i-1), pvec) ! get direction of resulting force from bending to apply on node i-1 - Mforce_ip1 = cross_product(Line%qs(:,i ), pvec) ! get direction of resulting force from bending to apply on node i+1 + Mforce_im1 = cross_product(Line%qs(:,i ), pvec) ! get direction of resulting force from bending to apply on node i-1 + Mforce_ip1 = cross_product(Line%qs(:,i+1), pvec) ! get direction of resulting force from bending to apply on node i+1 ! scale force direction vectors by desired moment force magnitudes to get resulting forces on adjacent nodes - call scalevector(Mforce_im1, Kurvi*Line%EI/Line%lstr(i-1), Mforce_im1) - call scalevector(Mforce_ip1, Kurvi*Line%EI/Line%lstr(i ), Mforce_ip1) - - ! set force on node i to cancel out forces on adjacent nodes - Mforce_i = -Mforce_im1 - Mforce_ip1 + call scalevector(Mforce_im1, Kurvi*Line%EI/Line%lstr(i ), Mforce_im1) + call scalevector(Mforce_ip1, Kurvi*Line%EI/Line%lstr(i+1), Mforce_ip1) + + Mforce_i = -Mforce_im1 - Mforce_ip1 ! set force on node i to cancel out forces on adjacent nodes ! apply these forces to the node forces Line%Bs(:,i-1) = Line%Bs(:,i-1) + Mforce_im1 @@ -1429,10 +1427,6 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, Xd(3*N-3 + 3*I-3 + J) = Line%rd(J,I); ! dxdt = V (velocities) Xd( 3*I-3 + J) = Sum1 ! dVdt = RHS * A (accelerations) - !if ((Line%Time > 100) .and. (Line%IdNum == 1) .and. (I==19) .and. (J==3)) then - ! print *, Line%Time, Line%r(J,I), Line%rd(J,I), Sum1 - !end if - END DO ! J END DO ! I @@ -1578,7 +1572,7 @@ END SUBROUTINE Line_GetEndSegmentInfo !-------------------------------------------------------------- - ! set end node unit vector of a line (this is called by an attached to a Rod, only applicable for bending stiffness) + ! set end node unit vector of a line (this is called when attached to a Rod, only applicable for bending stiffness) !-------------------------------------------------------------- SUBROUTINE Line_SetEndOrientation(Line, qin, topOfLine, rodEndB) diff --git a/modules/moordyn/src/MoorDyn_Rod.f90 b/modules/moordyn/src/MoorDyn_Rod.f90 index 8046764174..15bab8760e 100644 --- a/modules/moordyn/src/MoorDyn_Rod.f90 +++ b/modules/moordyn/src/MoorDyn_Rod.f90 @@ -588,8 +588,8 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) call GetOrientationAngles(Rod%q, phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat) ! save to internal roll and pitch variables for use in output <<< should check these, make Euler angles isntead of independent <<< - Rod%roll = -180.0/Pi * phi*sinBeta - Rod%pitch = 180.0/Pi * phi*cosBeta + Rod%roll = -phi*sinBeta + Rod%pitch = phi*cosBeta ! set interior node positions and velocities (stretch the nodes between the endpoints linearly) (skipped for zero-length Rods) DO i=1,N-1 From 3b4f10d231782021b03cf34b96b3f92ec7234f69 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Tue, 19 Apr 2022 08:47:02 -0600 Subject: [PATCH 128/242] MDv2: remove tab characters --- modules/moordyn/src/MoorDyn.f90 | 46 ++++++++++++++-------------- modules/moordyn/src/MoorDyn_Line.f90 | 2 +- 2 files changed, 24 insertions(+), 24 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index eff7bf9c43..35fd95c689 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -423,7 +423,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er END IF write(p%UnLog,'(A)', IOSTAT=ErrStat2) "MoorDyn v2 log file with output level "//TRIM(Num2LStr(p%writeLog)) write(p%UnLog,'(A)', IOSTAT=ErrStat2) "Note: options above the writeLog line in the input file will not be recorded." - end if + end if else if ( OptString == 'DTM') THEN read (OptValue,*) p%dtM0 else if ( OptString == 'G') then @@ -652,24 +652,24 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er m%LineTypeList(l)%IdNum = l ! write lineType information to log file - if (p%writeLog > 1) then + if (p%writeLog > 1) then write(p%UnLog, '(A12,A20)' ) " LineType"//trim(num2lstr(l))//":" write(p%UnLog, '(A12,A20)' ) " name: ", m%LineTypeList(l)%name - write(p%UnLog, '(A12,f12.4)') " d : ", m%LineTypeList(l)%d - write(p%UnLog, '(A12,f12.4)') " w : ", m%LineTypeList(l)%w - write(p%UnLog, '(A12,f12.4)') " Cdn : ", m%LineTypeList(l)%Cdn - write(p%UnLog, '(A12,f12.4)') " Can : ", m%LineTypeList(l)%Can - write(p%UnLog, '(A12,f12.4)') " Cdt : ", m%LineTypeList(l)%Cdt - write(p%UnLog, '(A12,f12.4)') " Cat : ", m%LineTypeList(l)%Cat - end if + write(p%UnLog, '(A12,f12.4)') " d : ", m%LineTypeList(l)%d + write(p%UnLog, '(A12,f12.4)') " w : ", m%LineTypeList(l)%w + write(p%UnLog, '(A12,f12.4)') " Cdn : ", m%LineTypeList(l)%Cdn + write(p%UnLog, '(A12,f12.4)') " Can : ", m%LineTypeList(l)%Can + write(p%UnLog, '(A12,f12.4)') " Cdt : ", m%LineTypeList(l)%Cdt + write(p%UnLog, '(A12,f12.4)') " Cat : ", m%LineTypeList(l)%Cat + end if - IF ( ErrStat2 /= ErrID_None ) THEN - CALL SetErrStat( ErrID_Fatal, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - CALL CleanUp() - RETURN - END IF + IF ( ErrStat2 /= ErrID_None ) THEN + CALL SetErrStat( ErrID_Fatal, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + CALL CleanUp() + RETURN + END IF - END DO + END DO !------------------------------------------------------------------------------------------- @@ -707,16 +707,16 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er m%RodTypeList(l)%IdNum = l ! write lineType information to log file - if (p%writeLog > 1) then + if (p%writeLog > 1) then write(p%UnLog, '(A12,A20)' ) " RodType"//trim(num2lstr(l))//":" write(p%UnLog, '(A12,A20)' ) " name: ", m%RodTypeList(l)%name - write(p%UnLog, '(A12,f12.4)') " d : ", m%RodTypeList(l)%d - write(p%UnLog, '(A12,f12.4)') " w : ", m%RodTypeList(l)%w - write(p%UnLog, '(A12,f12.4)') " Cdn : ", m%RodTypeList(l)%Cdn - write(p%UnLog, '(A12,f12.4)') " Can : ", m%RodTypeList(l)%Can - write(p%UnLog, '(A12,f12.4)') " Cdt : ", m%RodTypeList(l)%CdEnd - write(p%UnLog, '(A12,f12.4)') " Cat : ", m%RodTypeList(l)%CaEnd - end if + write(p%UnLog, '(A12,f12.4)') " d : ", m%RodTypeList(l)%d + write(p%UnLog, '(A12,f12.4)') " w : ", m%RodTypeList(l)%w + write(p%UnLog, '(A12,f12.4)') " Cdn : ", m%RodTypeList(l)%Cdn + write(p%UnLog, '(A12,f12.4)') " Can : ", m%RodTypeList(l)%Can + write(p%UnLog, '(A12,f12.4)') " Cdt : ", m%RodTypeList(l)%CdEnd + write(p%UnLog, '(A12,f12.4)') " Cat : ", m%RodTypeList(l)%CaEnd + end if IF ( ErrStat2 /= ErrID_None ) THEN CALL SetErrStat( ErrID_Fatal, 'Failed to process rod type properties for rod '//trim(Num2LStr(l))//'. Check formatting and correct number of columns.', ErrStat, ErrMsg, RoutineName ) diff --git a/modules/moordyn/src/MoorDyn_Line.f90 b/modules/moordyn/src/MoorDyn_Line.f90 index 434c981bbf..8efe941133 100644 --- a/modules/moordyn/src/MoorDyn_Line.f90 +++ b/modules/moordyn/src/MoorDyn_Line.f90 @@ -1083,7 +1083,7 @@ SUBROUTINE Line_GetStateDeriv(Line, Xd, m, p) !, FairFtot, FairMtot, AnchFtot, CALL UnitVector(Line%r(:,I-1), Line%r(:,I+1), Line%q(:,I), dummyLength) END DO - ! calculate unit tangent vectors for either end node if the line has no bending stiffness of if either end is pinned (otherwise it's already been set via setEndStateFromRod) + ! calculate unit tangent vectors for either end node if the line has no bending stiffness of if either end is pinned (otherwise it's already been set via setEndStateFromRod) if ((Line%endTypeA==0) .or. (Line%EI==0.0)) then CALL UnitVector(Line%r(:,0), Line%r(:,1), Line%q(:,0), dummyLength) end if From ea6bc8fe683cccf0882cf8cad2b8ad7e618f6897 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Tue, 19 Apr 2022 08:50:36 -0600 Subject: [PATCH 129/242] MDv2: Fix compile issue with the UnLog --- modules/moordyn/src/MoorDyn.f90 | 4 ++-- modules/moordyn/src/MoorDyn_Registry.txt | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index 35fd95c689..fe47605c0a 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -2262,7 +2262,7 @@ END SUBROUTINE CheckError SUBROUTINE CleanUp() ! ErrStat = ErrID_Fatal call MD_DestroyInputFileType( InputFileDat, ErrStat2, ErrMsg2 ) ! Ignore any error messages from this - IF (p%UnLog) CLOSE( p%UnLog ) + IF (p%UnLog > 0_IntKi) CLOSE( p%UnLog ) ! Remove this when the log file is kept open during the full simulation END SUBROUTINE !> If for some reason the file is truncated, it is possible to get into an infinite loop @@ -2960,7 +2960,7 @@ SUBROUTINE MD_End(u, p, x, xd, z, other, y, m, ErrStat , ErrMsg) CALL MD_DestroyMisc(m, ErrStat2, ErrMsg2) CALL CheckError( ErrStat2, ErrMsg2 ) - IF (p%UnLog) CLOSE( p%UnLog ) ! close log file if it's open + IF (p%UnLog > 0_IntKi) CLOSE( p%UnLog ) ! close log file if it's open !TODO: any need to specifically deallocate things like m%xTemp%states in the above? <<<< ! IF ( ErrStat==ErrID_None) THEN diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 06f0d6d429..63f808db29 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -383,7 +383,8 @@ typedef ^ ^ CHARACTER(1) Delim - typedef ^ ^ IntKi MDUnOut - - - "Unit number of main output file" typedef ^ ^ CHARACTER(1024) PriPath - - - "The path to the primary MoorDyn input file, used if looking for additional input files" typedef ^ ^ IntKi writeLog - - - "Switch for level of log file output" -typedef ^ ^ IntKi UnLog - - - "Unit number of log file" +#NOTE: there may be an issue with start/restart with the UnLog stored in parameters. We'll ignore this for now -- ADP +typedef ^ ^ IntKi UnLog - -1 - "Unit number of log file" typedef ^ ^ IntKi WaveKin - - - "Flag for whether or how to consider water kinematics" typedef ^ ^ IntKi Current - - - "Flag for whether or how to consider water kinematics" typedef ^ ^ IntKi nTurbines - - - "Number of turbines if MoorDyn is performing an array-level simulation with FAST.Farm, otherwise 0" From 461fd87ea295efdab19f4f4b73225e83c1a23097 Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Tue, 19 Apr 2022 16:01:06 -0600 Subject: [PATCH 130/242] MoorDyn updates: linearization, wave stretching, buoyancy, print: - Implemented a basic method for automatically setting the linearization perturbation distance (displacement only) to avoid any line segments going slack (as long as they aren't slack to start with). Needs testing. - Adjusted wave kinematics interpolation function getWaterKin to include Wheeler wave kinematics stretching. - Some bug fixes to Rod buoyancy calculations for horizontal or upside down Rods. - Improved accuracy of Rod end center-of-pressure calculation when crossing the instantaneous free surface. - Replaced print statements that always are always called with WrScr. - A bug fix for UnLog (duplicates fix by Andy) - Enabled mooring line segment unstretched length output ("l" in Lines output) --- modules/moordyn/src/MoorDyn.f90 | 72 ++++++++++++++++-------- modules/moordyn/src/MoorDyn_Body.f90 | 2 +- modules/moordyn/src/MoorDyn_IO.f90 | 10 ++-- modules/moordyn/src/MoorDyn_Misc.f90 | 27 ++++++--- modules/moordyn/src/MoorDyn_Point.f90 | 6 +- modules/moordyn/src/MoorDyn_Registry.txt | 5 ++ modules/moordyn/src/MoorDyn_Rod.f90 | 37 ++++++------ 7 files changed, 98 insertions(+), 61 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index fe47605c0a..f4c7daf1d5 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -35,7 +35,7 @@ MODULE MoorDyn PRIVATE - TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn', 'v2.a25', '2022-04-11' ) + TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn', 'v2.a26', '2022-04-13' ) INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output @@ -634,7 +634,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er else if (N==2) then ! visco-elastic case when two BA values provided read(tempStrings(2), *) m%LineTypeList(l)%BA_D else if (m%LineTypeList(l)%ElasticMod == 2) then ! case where there is no dynamic damping for viscoelastic model (will it work)? - print *, "Warning, viscoelastic model being used with zero damping on the dynamic stiffness." + CALL WrScr("Warning, viscoelastic model being used with zero damping on the dynamic stiffness.") end if ! get the regular/static coefficient or relation in all cases (can be from a lookup table?) CALL getCoefficientOrCurve(tempStrings(1), m%LineTypeList(l)%BA, & @@ -1102,7 +1102,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er CALL Conv2UC(tempString4) ! convert to uppercase so that matching is not case-sensitive if ((INDEX(tempString4, "SEABED") > 0 ) .or. (INDEX(tempString4, "GROUND") > 0 ) .or. (INDEX(tempString4, "FLOOR") > 0 )) then ! if keyword used - PRINT *, 'Point '//trim(Num2LStr(l))//' depth set to be on the seabed; finding z location based on depth/bathymetry' ! interpret the anchor depth value as a 'seabed' input + CALL WrScr('Point '//trim(Num2LStr(l))//' depth set to be on the seabed; finding z location based on depth/bathymetry') ! interpret the anchor depth value as a 'seabed' input CALL getDepthFromBathymetry(m%BathymetryGrid, m%BathGrid_Xs, m%BathGrid_Ys, tempArray(1), tempArray(2), depth, nvec) ! meaning the anchor should be at the depth of the local bathymetry tempArray(3) = -depth else ! if the anchor depth input isn't one of the supported keywords, @@ -1185,7 +1185,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er m%ConnectList(l)%TypeNum = -1 ! set as coupled type p%nCpldCons(J) = p%nCpldCons(J) + 1 ! increment counter for the appropriate turbine m%CpldConIs(p%nCpldCons(J),J) = l - print *, ' added connection ', l, ' as fairlead for turbine ', J + CALL WrScr(' added connection '//TRIM(int2lstr(l))//' as fairlead for turbine '//trim(int2lstr(J))) else @@ -1379,6 +1379,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er IF ( scan( LineOutString, 'c') > 0 ) m%LineList(l)%OutFlagList(11) = 1 ! segment internal damping force IF ( scan( LineOutString, 's') > 0 ) m%LineList(l)%OutFlagList(12) = 1 ! Segment strain IF ( scan( LineOutString, 'd') > 0 ) m%LineList(l)%OutFlagList(13) = 1 ! Segment strain rate + IF ( scan( LineOutString, 'l') > 0 ) m%LineList(l)%OutFlagList(14) = 1 ! Segment stretched length IF (SUM(m%LineList(l)%OutFlagList) > 0) m%LineList(l)%OutFlagList(1) = 1 ! this first entry signals whether to create any output file at all ! the above letter-index combinations define which OutFlagList entry corresponds to which output type @@ -1439,12 +1440,12 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er if (TempIDnums(J) <= p%nLines) then ! ensure line ID is in range if (m%LineList( TempIDnums(J) )%CtrlChan == 0) then ! ensure line doesn't already have a CtrlChan assigned m%LineList( TempIDnums(J) )%CtrlChan = Itemp - print *, 'Assigned Line ', TempIDnums(J), ' to control channel ', Itemp + CALL WrScr('Assigned Line '//TRIM(Int2LStr(TempIDnums(J)))//' to control channel '//TRIM(Int2LStr(Itemp))) else - print *, 'Error: Line ', TempIDnums(J), ' already is assigned to control channel ', m%LineList( TempIDnums(J) )%CtrlChan, ' so cannot also be assigned to channel ', Itemp + CALL WrScr('Error: Line '//TRIM(Int2LStr(TempIDnums(J)))//' already is assigned to control channel '//TRIM(Int2LStr(m%LineList( TempIDnums(J) )%CtrlChan))//' so cannot also be assigned to channel '//TRIM(Int2LStr(Itemp))) end if else - print *, 'Error: Line ID ', TempIDnums(J), ' of CtrlChan ', Itemp, ' is out of range' + CALL WrScr('Error: Line ID '//TRIM(Int2LStr(TempIDnums(J)))//' of CtrlChan '//TRIM(Int2LStr(Itemp))//' is out of range') end if END DO @@ -2107,7 +2108,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er END DO IF (ErrStat == ErrID_Fatal) THEN - print *, "NaN detected at time ", t, " during MoorDyn's dynamic relaxation process." + CALL WrScr("NaN detected at time "//TRIM(Num2LStr(t))//" during MoorDyn's dynamic relaxation process.") IF (wordy > 1) THEN print *, "Here is the state vector: " print *, x%states @@ -2373,7 +2374,7 @@ SUBROUTINE MD_UpdateStates( t, n, u, t_array, p, x, xd, z, other, m, ErrStat, Er END DO IF (ErrStat == ErrID_Fatal) THEN - print *, "NaN detected at time ", t2, " in MoorDyn." + CALL WrScr("NaN detected at time "//TRIM(Num2LStr(t2))//" in MoorDyn.") IF (wordy > 1) THEN print *, ". Here is the state vector: " print *, x%states @@ -2404,7 +2405,7 @@ SUBROUTINE MD_UpdateStates( t, n, u, t_array, p, x, xd, z, other, m, ErrStat, Er END DO IF (ErrStat == ErrID_Fatal) THEN - print *, "NaN detected at time ", t2, " in MoorDyn." + CALL WrScr("NaN detected at time "//TRIM(Num2LStr(t2))//" in MoorDyn.") IF (wordy > 1) THEN print *, ". Here is the state vector: " print *, x%states @@ -3621,6 +3622,11 @@ SUBROUTINE MD_Init_Jacobian(Init, p, u, y, m, InitOut, ErrStat, ErrMsg) CHARACTER(*), PARAMETER :: RoutineName = 'SD_Init_Jacobian' real(ReKi) :: dx, dy, dz, maxDim + INTEGER(IntKi) :: l, I + real(ReKi) :: dl_slack ! how much a given line segment is stretched [m] + real(ReKi) :: dl_slack_min ! minimum change in a node position for the least-strained segment in the simulation to go slack [m] + + ! local variables: ErrStat = ErrID_None ErrMsg = "" @@ -3631,6 +3637,24 @@ SUBROUTINE MD_Init_Jacobian(Init, p, u, y, m, InitOut, ErrStat, ErrMsg) !dz = maxval(Init%Nodes(:,4))- minval(Init%Nodes(:,4)) !maxDim = max(dx, dy, dz) + + ! Figure out appropriate transverse perturbation size to avoid slack segments + dl_slack_min = 0.1_ReKi ! start at 0.1 m + + do l = 1,p%nLines + do I = 1, m%LineList(l)%N + dl_slack = m%LineList(l)%lstr(I) - m%LineList(l)%l(I) + + ! store the smallest positive length margin to a segment going slack + if (( dl_slack > 0.0_ReKi) .and. (dl_slack < dl_slack_min)) then + dl_slack_min = dl_slack + end if + end do + end do + + !TODO: consider attachment radii to also produce a rotational perturbation size from the above + + ! --- System dimension call Init_Jacobian_y(); if (Failed()) return call Init_Jacobian_x(); if (Failed()) return @@ -3696,7 +3720,7 @@ SUBROUTINE Init_Jacobian_x() idx = 0 ! Free bodies DO l = 1,p%nFreeBodies ! Body m%BodyList(m%FreeBodyIs(l)) - p%dx(idx+1:idx+3) = 0.2 ! body displacement [m] + p%dx(idx+1:idx+3) = dl_slack_min ! body displacement [m] p%dx(idx+4:idx+6) = 0.02 ! body rotation [rad] ! corresponds to state indices: (m%BodyStateIs1(l)+6:m%BodyStateIs1(l)+11) InitOut%LinNames_x(idx+1) = 'Body '//trim(num2lstr(m%FreeBodyIs(l)))//' Px, m' @@ -3717,7 +3741,7 @@ SUBROUTINE Init_Jacobian_x() ! Rods DO l = 1,p%nFreeRods ! Rod m%RodList(m%FreeRodIs(l)) if (m%RodList(m%FreeRodIs(l))%typeNum == 1) then ! pinned rod - p%dx(idx+1:idx+3) = 0.02 ! body rotation [rad] + p%dx(idx+1:idx+3) = 0.02 ! rod rotation [rad] ! corresponds to state indices: (m%RodStateIs1(l)+3:m%RodStateIs1(l)+5) InitOut%LinNames_x(idx+1) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_x, rad' InitOut%LinNames_x(idx+2) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' rot_y, rad' @@ -3727,8 +3751,8 @@ SUBROUTINE Init_Jacobian_x() p%dxIdx_map2_xStateIdx(idx+6) = m%RodStateIs1(l)+5 ! x%state index for rot_z idx = idx + 3 else ! free rod - p%dx(idx+1:idx+3) = 0.1 ! body displacement [m] - p%dx(idx+4:idx+6) = 0.02 ! body rotation [rad] + p%dx(idx+1:idx+3) = dl_slack_min ! rod displacement [m] + p%dx(idx+4:idx+6) = 0.02 ! rod rotation [rad] ! corresponds to state indices: (m%RodStateIs1(l)+6:m%RodStateIs1(l)+11) InitOut%LinNames_x(idx+1) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Px, m' InitOut%LinNames_x(idx+2) = 'Rod '//trim(num2lstr(m%FreeRodIs(l)))//' Py, m' @@ -3749,7 +3773,7 @@ SUBROUTINE Init_Jacobian_x() ! Free Connnections DO l = 1,p%nFreeCons ! Point m%ConnectList(m%FreeConIs(l)) ! corresponds to state indices: (m%ConStateIs1(l)+3:m%ConStateIs1(l)+5) - p%dx(idx+1:idx+3) = 0.1 ! point displacement [m] + p%dx(idx+1:idx+3) = dl_slack_min ! point displacement [m] InitOut%LinNames_x(idx+1) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Px, m' InitOut%LinNames_x(idx+2) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Py, m' InitOut%LinNames_x(idx+3) = 'Point '//trim(num2lstr(m%FreeConIs(l)))//' Pz, m' @@ -3764,7 +3788,7 @@ SUBROUTINE Init_Jacobian_x() ! corresponds to state indices: (m%LineStateIs1(l)+3*N-3:m%LineStateIs1(l)+6*N-7) -- NOTE: end nodes not included N = m%LineList(l)%N ! number of segments in the line DO i = 0,N-2 - p%dx(idx+1:idx+3) = 0.1 ! line internal node displacement [m] + p%dx(idx+1:idx+3) = dl_slack_min ! line internal node displacement [m] InitOut%LinNames_x(idx+1) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Px, m' InitOut%LinNames_x(idx+2) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Py, m' InitOut%LinNames_x(idx+3) = 'Line '//trim(num2lstr(l))//' node '//trim(num2lstr(i+1))//' Pz, m' @@ -3873,7 +3897,6 @@ SUBROUTINE Init_Jacobian_x() END SUBROUTINE Init_Jacobian_x SUBROUTINE Init_Jacobian_u() - REAL(R8Ki) :: perturb INTEGER(IntKi) :: i, j, idx, nu, i_meshField character(10) :: LinStr ! for noting which line a DeltaL control is attached to logical :: LinCtrl ! Is the current DeltaL channel associated with a line? @@ -3949,14 +3972,13 @@ SUBROUTINE Init_Jacobian_u() ! --- Default perturbations, p%du: call allocAry( p%du, 11, 'p%du', ErrStat2, ErrMsg2); if(ErrStat2/=ErrID_None) return - perturb = 2.0_R8Ki*D2R_D - p%du( 1) = perturb ! u%CoupledKinematics(1)%TranslationDisp = 1; - p%du( 2) = perturb ! u%CoupledKinematics(1)%Orientation = 2; - p%du( 3) = perturb ! u%CoupledKinematics(1)%TranslationVel = 3; - p%du( 4) = perturb ! u%CoupledKinematics(1)%RotationVel = 4; - p%du( 5) = perturb ! u%CoupledKinematics(1)%TranslationAcc = 5; - p%du( 6) = perturb ! u%CoupledKinematics(1)%RotationAcc = 6; - p%du(10) = 0.2_ReKi ! deltaL [m] + p%du( 1) = dl_slack_min ! u%CoupledKinematics(1)%TranslationDisp = 1; + p%du( 2) = 0.1_ReKi ! u%CoupledKinematics(1)%Orientation = 2; + p%du( 3) = 0.1_ReKi ! u%CoupledKinematics(1)%TranslationVel = 3; + p%du( 4) = 0.1_ReKi ! u%CoupledKinematics(1)%RotationVel = 4; + p%du( 5) = 0.1_ReKi ! u%CoupledKinematics(1)%TranslationAcc = 5; + p%du( 6) = 0.1_ReKi ! u%CoupledKinematics(1)%RotationAcc = 6; + p%du(10) = dl_slack_min ! deltaL [m] p%du(11) = 0.2_ReKi ! deltaLdot [m/s] END SUBROUTINE Init_Jacobian_u diff --git a/modules/moordyn/src/MoorDyn_Body.f90 b/modules/moordyn/src/MoorDyn_Body.f90 index aa6385d8bc..56d9eba8a5 100644 --- a/modules/moordyn/src/MoorDyn_Body.f90 +++ b/modules/moordyn/src/MoorDyn_Body.f90 @@ -358,7 +358,7 @@ SUBROUTINE Body_GetStateDeriv(Body, Xd, m, p) ! check for NaNs (should check all state derivatives, not just first 6) DO J = 1, 6 IF (Is_NaN(Xd(J))) THEN - print *, "NaN detected at time ", Body%time, " in Body ",Body%IdNum, "in MoorDyn," + CALL WrScr("NaN detected at time "//trim(Num2LStr(Body%time))//" in Body "//trim(Int2LStr(Body%IdNum))//"in MoorDyn,") IF (wordy > 0) print *, "state derivatives:" IF (wordy > 0) print *, Xd EXIT diff --git a/modules/moordyn/src/MoorDyn_IO.f90 b/modules/moordyn/src/MoorDyn_IO.f90 index 16391b3592..577e6117f2 100644 --- a/modules/moordyn/src/MoorDyn_IO.f90 +++ b/modules/moordyn/src/MoorDyn_IO.f90 @@ -159,7 +159,7 @@ SUBROUTINE setupBathymetry(inputString, defaultDepth, BathGrid, BathGrid_Xs, Bat BathGrid_Ys(1) = 0.0_DbKi ELSE ! otherwise interpret the input as a file name to load the bathymetry lookup data from - PRINT *, " The depth input contains letters so will load a bathymetry file." + CALL WrScr(" The depth input contains letters so will load a bathymetry file.") ! load lookup table data from file CALL GetNewUnit( UnCoef ) ! unit number for coefficient input file @@ -232,7 +232,7 @@ SUBROUTINE getCoefficientOrCurve(inputString, LineProp_c, LineProp_npoints, Line else ! otherwise interpet the input as a file name to load stress-strain lookup data from - print *, "found A letter in the line coefficient value so will try to load the filename." + CALL WrScr("found A letter in the line coefficient value so will try to load the filename.") LineProp_c = 0.0 @@ -250,10 +250,10 @@ SUBROUTINE getCoefficientOrCurve(inputString, LineProp_c, LineProp_npoints, Line READ(UnCoef,'(A)',IOSTAT=ErrStat4) Line2 !read into a line IF (ErrStat4 > 0) then - print *, "Error while reading lookup table file" + CALL WrScr("Error while reading lookup table file") EXIT ELSE IF (ErrStat4 < 0) then - print *, "Read ", I-1, " data lines from lookup table file" + CALL WrScr("Read "//trim(Int2LStr(I-1))//" data lines from lookup table file") EXIT ELSE READ(Line2,*,IOSTAT=ErrStat4) LineProp_Xs(I), LineProp_Ys(I) @@ -297,7 +297,7 @@ SUBROUTINE SplitByBars(instring, n, outstrings) END IF n = n + 1 if (n > 6) then - print *, "ERROR - SplitByBars cannot do more than 6 entries" + CALL WrScr("ERROR - SplitByBars cannot do more than 6 entries") end if outstrings(n) = instring(pos1:pos1+pos2-2) pos1 = pos2+pos1 diff --git a/modules/moordyn/src/MoorDyn_Misc.f90 b/modules/moordyn/src/MoorDyn_Misc.f90 index 012f185fe7..0272a04ef6 100644 --- a/modules/moordyn/src/MoorDyn_Misc.f90 +++ b/modules/moordyn/src/MoorDyn_Misc.f90 @@ -903,7 +903,8 @@ END SUBROUTINE getDepthFromBathymetry ! master function to get wave/water kinematics at a given point -- called by each object from grid-based data SUBROUTINE getWaterKin(p, x, y, z, t, tindex, U, Ud, zeta, PDyn) - ! note, this whole approach assuems that px, py, and pz are in increasing order + ! This whole approach assuems that px, py, and pz are in increasing order. + ! Wheeler stretching is now built in. TYPE(MD_ParameterType),INTENT (IN ) :: p ! MoorDyn parameters (contains the wave info for now) Real(DbKi), INTENT (IN ) :: x @@ -921,24 +922,32 @@ SUBROUTINE getWaterKin(p, x, y, z, t, tindex, U, Ud, zeta, PDyn) INTEGER(IntKi) :: iz0, iz1 ! special indices for currrent interpolation INTEGER(IntKi) :: N ! number of rod elements for convenience Real(SiKi) :: fx, fy, fz, ft ! interpolation fractions - !Real(DbKi) :: qt ! used in time step interpolation + Real(DbKi) :: zp ! zprime coordinate used for Wheeler stretching ! if wave kinematics enabled, get interpolated values from grid if (p%WaveKin > 0) then - - CALL getInterpNumsSiKi(p%pxWave , REAL(x,SiKi), 1, ix, fx) - CALL getInterpNumsSiKi(p%pyWave , REAL(y,SiKi), 1, iy, fy) - CALL getInterpNumsSiKi(p%pzWave , REAL(z,SiKi), 1, iz, fz) + + ! find time interpolation indices and coefficients !CALL getInterpNums(p%tWave, t, tindex, it, ft) it = floor(t/ p%dtWave) + 1 ! add 1 because Fortran indexing starts at 1 ft = (t - (it-1)*p%dtWave)/p%dtWave - tindex = it + tindex = it + + ! find x-y interpolation indices and coefficients + CALL getInterpNumsSiKi(p%pxWave , REAL(x,SiKi), 1, ix, fx) + CALL getInterpNumsSiKi(p%pyWave , REAL(y,SiKi), 1, iy, fy) - CALL calculate3Dinterpolation(p%zeta, ix, iy, it, fx, fy, ft, zeta) + ! interpolate wave elevation + CALL calculate3Dinterpolation(p%zeta, ix, iy, it, fx, fy, ft, zeta) - CALL calculate4Dinterpolation(p%PDyn, ix, iy, iz, it, fx, fy, fz, ft, PDyn) + ! compute modified z coordinate to be used for interpolating velocities and accelerations with Wheeler stretching + zp = ( z - zeta ) * p%WtrDpth/( p%WtrDpth + zeta ) + CALL getInterpNumsSiKi(p%pzWave , REAL(zp,SiKi), 1, iz, fz) + + ! interpolate everything else + CALL calculate4Dinterpolation(p%PDyn , ix, iy, iz, it, fx, fy, fz, ft, PDyn) CALL calculate4Dinterpolation(p%uxWave, ix, iy, iz, it, fx, fy, fz, ft, U(1) ) CALL calculate4Dinterpolation(p%uyWave, ix, iy, iz, it, fx, fy, fz, ft, U(2) ) CALL calculate4Dinterpolation(p%uzWave, ix, iy, iz, it, fx, fy, fz, ft, U(3) ) diff --git a/modules/moordyn/src/MoorDyn_Point.f90 b/modules/moordyn/src/MoorDyn_Point.f90 index 976af8539b..fce8aab12f 100644 --- a/modules/moordyn/src/MoorDyn_Point.f90 +++ b/modules/moordyn/src/MoorDyn_Point.f90 @@ -73,7 +73,7 @@ SUBROUTINE Connect_Initialize(Connect, states, m) IF (wordy > 0) print *, "Initialized Connection ", Connect%IdNum else - print *," Error: wrong Point type given to Connect_Initialize for number ", Connect%idNum + CALL WrScr(" Error: wrong Point type given to Connect_Initialize for number "//trim(Int2Lstr(Connect%idNum))) end if END SUBROUTINE Connect_Initialize @@ -195,7 +195,7 @@ SUBROUTINE Connect_GetStateDeriv(Connect, Xd, m, p) ! check for NaNs DO J = 1, 6 IF (Is_NaN(Xd(J))) THEN - print *, "NaN detected at time ", Connect%time, " in Point ",Connect%IdNum, " in MoorDyn." + CALL WrScr("NaN detected at time "//trim(Num2LStr(Connect%time))//" in Point "//trim(Int2LStr(Connect%IdNum))//" in MoorDyn.") IF (wordy > 1) print *, "state derivatives:" IF (wordy > 1) print *, Xd EXIT @@ -312,7 +312,7 @@ SUBROUTINE Connect_GetCoupledForce(Connect, Fnet_out, m, p) Fnet_out = Connect%Fnet + F_iner ! add inertial loads ELSE - print *, "Connect_GetCoupledForce called for wrong (uncoupled) Point type in MoorDyn!" + CALL WrScr("Connect_GetCoupledForce called for wrong (uncoupled) Point type in MoorDyn!") END IF END SUBROUTINE Connect_GetCoupledForce diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 63f808db29..f859336fa8 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -383,8 +383,13 @@ typedef ^ ^ CHARACTER(1) Delim - typedef ^ ^ IntKi MDUnOut - - - "Unit number of main output file" typedef ^ ^ CHARACTER(1024) PriPath - - - "The path to the primary MoorDyn input file, used if looking for additional input files" typedef ^ ^ IntKi writeLog - - - "Switch for level of log file output" +<<<<<<< HEAD #NOTE: there may be an issue with start/restart with the UnLog stored in parameters. We'll ignore this for now -- ADP typedef ^ ^ IntKi UnLog - -1 - "Unit number of log file" +======= +# Note from Andy: there could be a potential start/restart issue with UnLog. May need to revisit. +typedef ^ ^ IntKi UnLog - -1 - "Unit number of log file" +>>>>>>> 58d0febd (MoorDyn updates: linearization, wave stretching, buoyancy, print:) typedef ^ ^ IntKi WaveKin - - - "Flag for whether or how to consider water kinematics" typedef ^ ^ IntKi Current - - - "Flag for whether or how to consider water kinematics" typedef ^ ^ IntKi nTurbines - - - "Number of turbines if MoorDyn is performing an array-level simulation with FAST.Farm, otherwise 0" diff --git a/modules/moordyn/src/MoorDyn_Rod.f90 b/modules/moordyn/src/MoorDyn_Rod.f90 index 15bab8760e..8bc092c9c8 100644 --- a/modules/moordyn/src/MoorDyn_Rod.f90 +++ b/modules/moordyn/src/MoorDyn_Rod.f90 @@ -486,7 +486,8 @@ SUBROUTINE Rod_GetStateDeriv(Rod, Xd, m, p) ! check for NaNs (should check all state derivatives, not just first 6) DO J = 1, 6 IF (Is_NaN(Xd(J))) THEN - print *, "NaN detected at time ", Rod%time, " in Rod ",Rod%IdNum + CALL WrScr("NaN detected at time "//trim(Num2LStr(Rod%time))//" in Rod "//trim(Int2LStr(Rod%IdNum))) + IF (wordy > 1) THEN print *, " state derivatives:" print *, Xd @@ -542,6 +543,7 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) Real(DbKi) :: z1hi ! highest elevation of cross section at node [m] Real(DbKi) :: z1lo ! lowest elevation of cross section at node [m] Real(DbKi) :: G ! distance normal to axis from bottom edge of cross section to waterplane [m] + Real(DbKi) :: al ! angle involved in circular segment buoyancy calc [rad] Real(DbKi) :: A ! area of cross section at node that is below the waterline [m2] Real(DbKi) :: zA ! crude approximation to z value of centroid of submerged cross section at node [m] @@ -608,10 +610,8 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ! <<<< currently F is not being used and instead a VOF variable is used within the node loop END DO - ! >>> remember to check for violated conditions, if there are any... <<< - - zeta = Rod%zeta(N)! just use the wave elevation computed at the location of the top node for now - + ! Calculated h0 (note this should be deprecated/replced) + zeta = Rod%zeta(N) ! temporary ! get approximate location of waterline crossing along Rod axis (note: negative h0 indicates end A is above end B, and measures -distance from end A to waterline crossing) if ((Rod%r(3,0) < zeta) .and. (Rod%r(3,N) < zeta)) then ! fully submerged case Rod%h0 = Rod%UnstrLen @@ -670,20 +670,23 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) z1hi = Rod%r(3,I) + 0.5*Rod%d*abs(sinPhi) ! highest elevation of cross section at node z1lo = Rod%r(3,I) - 0.5*Rod%d*abs(sinPhi) ! lowest elevation of cross section at node - if (z1lo > zeta) then ! fully out of water + if (z1lo > Rod%zeta(I)) then ! fully out of water A = 0.0 ! area zA = 0 ! centroid depth - else if (z1hi < zeta) then ! fully submerged + else if (z1hi < Rod%zeta(I)) then ! fully submerged A = Pi*0.25*Rod%d**2 zA = Rod%r(3,I) else ! if z1hi*z1lo < 0.0: # if cross section crosses waterplane - if (abs(sinPhi) < 0.001) then - A = 0.5_DbKi + if (abs(sinPhi) < 0.001) then ! if cylinder is near vertical, i.e. end is horizontal + A = 0.5_DbKi ! <<< shouldn't this just be zero? <<< zA = 0.0_DbKi else - G = (-z1lo+zeta)/abs(sinPhi) ! distance from node to waterline cross at same axial location [m] - A = 0.25*Rod%d**2*acos((Rod%d - 2.0*G)/Rod%d) - (0.5*Rod%d-G)*sqrt(Rod%d*G-G**2) ! area of circular cross section that is below waterline [m^2] - zA = (z1lo-zeta)/2 ! very crude approximation of centroid for now... <<< need to double check zeta bit <<< + G = (Rod%r(3,I)-Rod%zeta(I))/abs(sinPhi) !(-z1lo+Rod%zeta(I))/abs(sinPhi) ! distance from node to waterline cross at same axial location [m] + !A = 0.25*Rod%d**2*acos((Rod%d - 2.0*G)/Rod%d) - (0.5*Rod%d-G)*sqrt(Rod%d*G-G**2) ! area of circular cross section that is below waterline [m^2] + !zA = (z1lo-Rod%zeta(I))/2 ! very crude approximation of centroid for now... <<< need to double check zeta bit <<< + al = acos(2.0*G/Rod%d) + A = Rod%d*Rod%d/8.0 * (2.0*al - sin(2.0*al)) + zA = Rod%r(3,I) - 0.6666666666 * Rod%d* (sin(al))**3 / (2.0*al - sin(2.0*al)) end if end if @@ -782,15 +785,14 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ! ------ now add forces, moments, and added mass from Rod end effects (these can exist even if N==0) ------- - ! end A - IF ((I==0) .and. (Rod%h0 > 0.0_ReKi)) THEN ! if this is end A and it is submerged + IF ((I==0) .and. (z1lo < Rod%zeta(I))) THEN ! if this is end A and it is at least partially submerged ! >>> eventually should consider a VOF approach for the ends hTilt = 0.5*Rod%d/cosPhi <<< ! buoyancy force Ftemp = -VOF * 0.25*Pi*Rod%d*Rod%d * p%rhoW*p%g* zA Rod%Bo(:,I) = Rod%Bo(:,I) + (/ Ftemp*cosBeta*sinPhi, Ftemp*sinBeta*sinPhi, Ftemp*cosPhi /) - + ! buoyancy moment Mtemp = -VOF * 1.0/64.0*Pi*Rod%d**4 * p%rhoW*p%g * sinPhi Rod%Mext = Rod%Mext + (/ Mtemp*sinBeta, -Mtemp*cosBeta, 0.0_DbKi /) @@ -815,7 +817,7 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) END IF - IF ((I==N) .and. ((Rod%h0 >= Rod%UnstrLen) .or. (Rod%h0 < 0.0_DbKi))) THEN ! if this end B and it is submerged (note, if N=0, both this and previous if statement are true) + IF ((I==N) .and. (z1lo < Rod%zeta(I))) THEN ! if this end B and it is at least partially submerged (note, if N=0, both this and previous if statement are true) ! buoyancy force Ftemp = VOF * 0.25*Pi*Rod%d*Rod%d * p%rhoW*p%g* zA @@ -844,7 +846,6 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) END IF - ! ---------------------------- total forces for this node ----------------------------- Rod%Fnet(:,I) = Rod%W(:,I) + Rod%Bo(:,I) + Rod%Dp(:,I) + Rod%Dq(:,I) & @@ -855,7 +856,7 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ! ----- add waterplane moment of inertia moment if applicable ----- - IF ((Rod%r(3,0) < zeta) .and. (Rod%r(3,N) > zeta)) then ! check if it's crossing the water plane + IF ((Rod%r(3,0) < zeta) .and. (Rod%r(3,N) > zeta)) then ! check if it's crossing the water plane <<< may need updating ! >>> could scale the below based on whether part of the end cap is crossing the water plane... !Mtemp = 1.0/16.0 *Pi*Rod%d**4 * p%rhoW*p%g * sinPhi * (1.0 + 0.5* tanPhi**2) ! original (goes to infinity at 90 deg) Mtemp = 1.0/16.0 *Pi*Rod%d**4 * p%rhoW*p%g * sinPhi * cosPhi ! simple alternative that goes to 0 at 90 deg then reverses sign beyond that From 6f8dc6fb575514c0aea1d14e3a02e12dc3bad8d1 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Mon, 2 May 2022 14:48:16 -0600 Subject: [PATCH 131/242] MDv2: merge issue in MoorDyn registry -- registry.exe could not build it --- modules/moordyn/src/MoorDyn_Registry.txt | 5 ----- modules/moordyn/src/MoorDyn_Types.f90 | 2 +- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index f859336fa8..63f808db29 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -383,13 +383,8 @@ typedef ^ ^ CHARACTER(1) Delim - typedef ^ ^ IntKi MDUnOut - - - "Unit number of main output file" typedef ^ ^ CHARACTER(1024) PriPath - - - "The path to the primary MoorDyn input file, used if looking for additional input files" typedef ^ ^ IntKi writeLog - - - "Switch for level of log file output" -<<<<<<< HEAD #NOTE: there may be an issue with start/restart with the UnLog stored in parameters. We'll ignore this for now -- ADP typedef ^ ^ IntKi UnLog - -1 - "Unit number of log file" -======= -# Note from Andy: there could be a potential start/restart issue with UnLog. May need to revisit. -typedef ^ ^ IntKi UnLog - -1 - "Unit number of log file" ->>>>>>> 58d0febd (MoorDyn updates: linearization, wave stretching, buoyancy, print:) typedef ^ ^ IntKi WaveKin - - - "Flag for whether or how to consider water kinematics" typedef ^ ^ IntKi Current - - - "Flag for whether or how to consider water kinematics" typedef ^ ^ IntKi nTurbines - - - "Number of turbines if MoorDyn is performing an array-level simulation with FAST.Farm, otherwise 0" diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index f306dc104a..c607018556 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -415,7 +415,7 @@ MODULE MoorDyn_Types INTEGER(IntKi) :: MDUnOut !< Unit number of main output file [-] CHARACTER(1024) :: PriPath !< The path to the primary MoorDyn input file, used if looking for additional input files [-] INTEGER(IntKi) :: writeLog !< Switch for level of log file output [-] - INTEGER(IntKi) :: UnLog !< Unit number of log file [-] + INTEGER(IntKi) :: UnLog = -1 !< Unit number of log file [-] INTEGER(IntKi) :: WaveKin !< Flag for whether or how to consider water kinematics [-] INTEGER(IntKi) :: Current !< Flag for whether or how to consider water kinematics [-] INTEGER(IntKi) :: nTurbines !< Number of turbines if MoorDyn is performing an array-level simulation with FAST.Farm, otherwise 0 [-] From e96a0861bb8d58901f1716057e3bdbf3551ac49f Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Tue, 3 May 2022 11:19:45 -0600 Subject: [PATCH 132/242] MDv2: [BugFix] segfault in logging option. update input files in r-test --- modules/moordyn/src/MoorDyn_Registry.txt | 2 +- modules/moordyn/src/MoorDyn_Types.f90 | 2 +- reg_tests/r-test | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/moordyn/src/MoorDyn_Registry.txt b/modules/moordyn/src/MoorDyn_Registry.txt index 63f808db29..a3ed6ef2b9 100644 --- a/modules/moordyn/src/MoorDyn_Registry.txt +++ b/modules/moordyn/src/MoorDyn_Registry.txt @@ -382,7 +382,7 @@ typedef ^ ^ MD_OutParmType OutParam {:} typedef ^ ^ CHARACTER(1) Delim - - - "Column delimiter for output text files" - typedef ^ ^ IntKi MDUnOut - - - "Unit number of main output file" typedef ^ ^ CHARACTER(1024) PriPath - - - "The path to the primary MoorDyn input file, used if looking for additional input files" -typedef ^ ^ IntKi writeLog - - - "Switch for level of log file output" +typedef ^ ^ IntKi writeLog - -1 - "Switch for level of log file output" #NOTE: there may be an issue with start/restart with the UnLog stored in parameters. We'll ignore this for now -- ADP typedef ^ ^ IntKi UnLog - -1 - "Unit number of log file" typedef ^ ^ IntKi WaveKin - - - "Flag for whether or how to consider water kinematics" diff --git a/modules/moordyn/src/MoorDyn_Types.f90 b/modules/moordyn/src/MoorDyn_Types.f90 index c607018556..3dc43291fc 100644 --- a/modules/moordyn/src/MoorDyn_Types.f90 +++ b/modules/moordyn/src/MoorDyn_Types.f90 @@ -414,7 +414,7 @@ MODULE MoorDyn_Types CHARACTER(1) :: Delim !< Column delimiter for output text files [-] INTEGER(IntKi) :: MDUnOut !< Unit number of main output file [-] CHARACTER(1024) :: PriPath !< The path to the primary MoorDyn input file, used if looking for additional input files [-] - INTEGER(IntKi) :: writeLog !< Switch for level of log file output [-] + INTEGER(IntKi) :: writeLog = -1 !< Switch for level of log file output [-] INTEGER(IntKi) :: UnLog = -1 !< Unit number of log file [-] INTEGER(IntKi) :: WaveKin !< Flag for whether or how to consider water kinematics [-] INTEGER(IntKi) :: Current !< Flag for whether or how to consider water kinematics [-] diff --git a/reg_tests/r-test b/reg_tests/r-test index 19eba75fc1..994c656fa3 160000 --- a/reg_tests/r-test +++ b/reg_tests/r-test @@ -1 +1 @@ -Subproject commit 19eba75fc1871a914e65820fa49789ad34ce185a +Subproject commit 994c656fa372feb1f5cd31e115f6532a72eb5d87 From 4ac90265cee62ad0848b55586c19e4cbf6d28b12 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Tue, 3 May 2022 15:30:47 -0600 Subject: [PATCH 133/242] MDv2: update input file format in r-test --- reg_tests/r-test | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/reg_tests/r-test b/reg_tests/r-test index 994c656fa3..f55474e90f 160000 --- a/reg_tests/r-test +++ b/reg_tests/r-test @@ -1 +1 @@ -Subproject commit 994c656fa372feb1f5cd31e115f6532a72eb5d87 +Subproject commit f55474e90f025e89ab317d2e524665da5746d594 From 7844b9350cdd463cf5afbcd1c778002577301c5d Mon Sep 17 00:00:00 2001 From: Matt Hall <5151457+mattEhall@users.noreply.github.com> Date: Wed, 4 May 2022 08:53:54 -0600 Subject: [PATCH 134/242] MoorDyn: add slack-segment 0.5 safety factor for linearization perturbation size --- modules/moordyn/src/MoorDyn.f90 | 2 ++ 1 file changed, 2 insertions(+) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index f4c7daf1d5..bfad9a3f9f 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -3652,6 +3652,8 @@ SUBROUTINE MD_Init_Jacobian(Init, p, u, y, m, InitOut, ErrStat, ErrMsg) end do end do + dl_slack_min = 0.5*dl_slack_min ! apply 0.5 safety factor + !TODO: consider attachment radii to also produce a rotational perturbation size from the above From 670375e156e9a453b87988778ed9af0b77b4f977 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Fri, 6 May 2022 06:57:38 -0600 Subject: [PATCH 135/242] MD: fix issue with compiling double with Intel This almost seems like a compiler bug. Not sure why Double would complain about this, but single would not. --- modules/moordyn/src/MoorDyn_Misc.f90 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/moordyn/src/MoorDyn_Misc.f90 b/modules/moordyn/src/MoorDyn_Misc.f90 index 0272a04ef6..a9d66a6516 100644 --- a/modules/moordyn/src/MoorDyn_Misc.f90 +++ b/modules/moordyn/src/MoorDyn_Misc.f90 @@ -101,7 +101,7 @@ SUBROUTINE ScaleVector( u_in, newlength, u_out ) if (length_squared > 0) then scaler = newlength/sqrt(length_squared) else ! if original vector is zero, return zero - scaler = 0_DbKi + scaler = 0.0_DbKi end if DO J=1,3 From eca8387023b6826560596ecab20836574d156db5 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Fri, 6 May 2022 11:52:09 -0600 Subject: [PATCH 136/242] SD lin: [BugFix] indexing to SD_y%Y3Mesh start in Indx_y_SD_Y3Mesh_Start --- modules/openfast-library/src/FAST_Lin.f90 | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/openfast-library/src/FAST_Lin.f90 b/modules/openfast-library/src/FAST_Lin.f90 index 2ce92b2ee1..f982af34ae 100644 --- a/modules/openfast-library/src/FAST_Lin.f90 +++ b/modules/openfast-library/src/FAST_Lin.f90 @@ -5167,7 +5167,7 @@ FUNCTION Indx_y_SD_Y1Mesh_Start(y_SD, y_FAST) RESULT(SD_Out_Start) TYPE(FAST_OutputFileType), INTENT(IN ) :: y_FAST !< FAST output file data (for linearization) TYPE(SD_OutputType), INTENT(IN ) :: y_SD !< SD outputs at t - INTEGER :: SD_Out_Start !< starting index of this mesh in ElastoDyn outputs + INTEGER :: SD_Out_Start !< starting index of this mesh in SubDyn outputs SD_Out_Start = y_FAST%Lin%Modules(MODULE_SD)%Instance(1)%LinStartIndx(LIN_OUTPUT_COL) END FUNCTION Indx_y_SD_Y1Mesh_Start @@ -5177,7 +5177,7 @@ FUNCTION Indx_y_SD_Y2Mesh_Start(y_SD, y_FAST) RESULT(SD_Out_Start) TYPE(FAST_OutputFileType), INTENT(IN ) :: y_FAST !< FAST output file data (for linearization) TYPE(SD_OutputType), INTENT(IN ) :: y_SD !< SD outputs at t - INTEGER :: SD_Out_Start !< starting index of this mesh in ElastoDyn outputs + INTEGER :: SD_Out_Start !< starting index of this mesh in SubDyn outputs SD_Out_Start = Indx_y_SD_Y1Mesh_Start(y_SD, y_FAST) + y_SD%Y1Mesh%NNodes * 6 ! 3 forces + 3 moments at each node! skip all of the Y1Mesh data and get to the beginning of END FUNCTION Indx_y_SD_Y2Mesh_Start @@ -5186,9 +5186,9 @@ FUNCTION Indx_y_SD_Y3Mesh_Start(y_SD, y_FAST) RESULT(SD_Out_Start) TYPE(FAST_OutputFileType), INTENT(IN ) :: y_FAST !< FAST output file data (for linearization) TYPE(SD_OutputType), INTENT(IN ) :: y_SD !< SD outputs at t - INTEGER :: SD_Out_Start !< starting index of this mesh in ElastoDyn outputs + INTEGER :: SD_Out_Start !< starting index of this mesh in SubDyn outputs - SD_Out_Start = Indx_y_SD_Y2Mesh_Start(y_SD, y_FAST) + y_SD%Y2Mesh%NNodes * 6 ! 3 forces + 3 moments at each node! skip all of the Y1Mesh data and get to the beginning of + SD_Out_Start = Indx_y_SD_Y2Mesh_Start(y_SD, y_FAST) + y_SD%Y2Mesh%NNodes * 18 ! 6 fields (TranslationDisp, MASKID_Orientation, TranslationVel, RotationVel, TranslationAcc, RotationAcc) with 3 components END FUNCTION Indx_y_SD_Y3Mesh_Start !---------------------------------------------------------------------------------------------------------------------------------- From 729a2afd3808abe85a39c1b2e9aca4762f8aef18 Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Thu, 16 Jun 2022 10:46:21 -0600 Subject: [PATCH 137/242] UA/DBEMT: update of documentation and static equation references in code --- docs/source/user/aerodyn/bibliography.bib | 12 +++ docs/source/user/aerodyn/input.rst | 33 ++++---- docs/source/user/aerodyn/theory.rst | 92 +++++++++++++++++++++++ docs/source/user/aerodyn/theory_ua.rst | 23 ++++-- modules/aerodyn/src/DBEMT.f90 | 2 +- modules/aerodyn/src/UnsteadyAero.f90 | 55 +++++++++----- 6 files changed, 177 insertions(+), 40 deletions(-) diff --git a/docs/source/user/aerodyn/bibliography.bib b/docs/source/user/aerodyn/bibliography.bib index 248a77a523..cd9265b2dc 100644 --- a/docs/source/user/aerodyn/bibliography.bib +++ b/docs/source/user/aerodyn/bibliography.bib @@ -26,6 +26,18 @@ @book{ad-Branlard:book } +@article{ad-Branlard:2022, + author = {E Branlard and B Jonkman and G R Pirrung and K Dixon and J Jonkman}, + title = {Dynamic inflow and unsteady aerodynamics models for modal and stability analyses in {OpenFAST}}, + doi = {10.1088/1742-6596/2265/3/032044}, + year = 2022, + publisher = {{IOP} Publishing}, + volume = {2265}, + number = {3}, + pages = {032044}, + journal = {Journal of Physics: Conference Series} +} + @article{ad-Hansen:book, author = {Hansen, M. O. L. and S{\o}rensen, J. N. and Voutsinas, S. and S{\o}rensen, N. and Madsen, H. Aa.}, doi = {10.1016/j.paerosci.2006.10.002}, diff --git a/docs/source/user/aerodyn/input.rst b/docs/source/user/aerodyn/input.rst index a49686c455..31e55d99d7 100644 --- a/docs/source/user/aerodyn/input.rst +++ b/docs/source/user/aerodyn/input.rst @@ -67,7 +67,7 @@ program). Set ``WakeMod`` to 0 if you want to disable rotor wake/induction effects or 1 to include these effects using the (quasi-steady) BEM theory model. When ``WakeMod`` is set to 2, a dynamic BEM theory model (DBEMT) is used (also -referred to as dynamic inflow or dynamic wake model). When ``WakeMod`` is set +referred to as dynamic inflow or dynamic wake model, see :numref:`AD_DBEMT`). When ``WakeMod`` is set to 3, the free vortex wake model is used, also referred to as OLAF (see :numref:`OLAF`). ``WakeMod`` cannot be set to 2 or 3 during linearization analyses. @@ -179,13 +179,20 @@ Dynamic Blade-Element/Momentum Theory Options ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ The input parameters in this section are used only when ``WakeMod = 2``. +The theory is described in :numref:`AD_DBEMT`. + +There are three options available for ``DBEMT_Mod``: + +- ``1``: discrete-time Oye's model, with constant :math:`\tau_1` +- ``2``: discrete-time Oye's model, with varying :math:`\tau_1`, automatically adjusted based on inflow. (recommended for time-domain simulations) +- ``3``: continuous-time Oye's model, with constant :math:`\tau_1` (recommended for linearization) + +For ``DBEMT_Mod=1`` or ``DBEMT_Mod=3`` it is the user responsability to set the value of :math:`\tau_1` (i.e. ``tau1_const``) according to the expression given in :numref:`AD_DBEMT`, using an estimate of what the mean axial induction (:math:`\overline{a}`) and the mean relative wind velocity across the rotor (:math:`\overline{U_0}`) are for a given simulation. + +The option ``DBEMT_Mod=3`` is the only one that can be used for linearization. + -Set ``DBEMT_Mod`` to 1 for the constant-tau1 model, set ``DBEMT_Mod`` to 2 -to use a model where tau1 varies with time, or set ``DBEMT_Mod`` to 3 -to use a continuous-state model with constant tau1. -If ``DBEMT_Mod=1`` (constant-tau1 model) or ``DBEMT_Mod=3`` (continuous-state constant-tau1 model), -set ``tau1_const`` to the time constant to use for DBEMT. OLAF -- cOnvecting LAgrangian Filaments (Free Vortex Wake) Theory Options ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -207,13 +214,13 @@ The input parameters in this section are used only when ``AFAeroMod ``UAMod`` determines the UA model. It has the following options: -- ``1``: the original theoretical developments of B-L (**not currently functional**), -- ``2``: the extensions to B-L developed by González -- ``3``: the extensions to B-L developed by Minnema/Pierce -- ``4``: a continuous-state model developed by Hansen, Gaunna, and Madsen (HGM) -- ``5``: a model similar to HGM with an additional state for vortex generation -- ``6``: Oye's dynamic stall model -- ``7``: Boeing-Vertol model +- ``1``: the discrete-time model of Beddoes-Leishman (B-L) (**not currently functional**), +- ``2``: the extensions to B-L developed by González (changes in Cn, Cc, Cm) +- ``3``: the extensions to B-L developed by Minnema/Pierce (changes in Cc and Cm) +- ``4``: 4-states continuous-time B-L model developed by Hansen, Gaunna, and Madsen (HGM). NOTE: might require smaller time steps until a stiff integrator is implemented. +- ``5``: 5-states continuous-time B-L model similar to HGM with an additional state for vortex generation +- ``6``: 1-state continuous-time developed by Oye +- ``7``: discrete-time Boeing-Vertol (BV) model The models are described in :numref:`AD_UA`. diff --git a/docs/source/user/aerodyn/theory.rst b/docs/source/user/aerodyn/theory.rst index dfa632f1c9..16d095bc91 100644 --- a/docs/source/user/aerodyn/theory.rst +++ b/docs/source/user/aerodyn/theory.rst @@ -14,6 +14,98 @@ Steady BEM The steady blade element momentum (BEM) equations are solved as a constrained equation, and the formulation follows the description from Ning :cite:`ad-Ning:2014`. + +.. _AD_DBEMT: + +Dynamic BEM Theory (DBEMT) +~~~~~~~~~~~~~~~~~~~~~~~~~~ + + + +Two equivalent versions of Oye's dynamic inflow model are implemented in AeroDyn. +The first one uses discrete time, it can be used with the constant-tau1 model +(``DBEMT_Mod=1``) or the varying-tau1 model (``DBEMT_Mod=2``), but it cannot be used for linearization. +The second version uses a continuous-time state-space formulation (``DBEMT_Mod=1``), it assumes a constant-tau1, and can be used for linearization. +For a same value of :math:`\tau_1`, the discrete-time and continuous-time formulations returns exactly the same results. + + + + + +Oye's dynamic inflow model consists of two first-order differential equations (see :cite:`ad-Branlard:book`): + +.. math:: + \begin{align} + \boldsymbol{W}_\text{int}+\tau_1 \boldsymbol{\dot{W}}_\text{int} + &= + \boldsymbol{W}_\text{qs} + k \tau_1 \boldsymbol{\dot{W}}_\text{qs} \\ + \boldsymbol{W}+\tau_2 \boldsymbol{\dot{W}} + &= + \boldsymbol{W}_\text{int} + \end{align} + +where +:math:`\boldsymbol{W}` is the dynamic induction vector at the rotor (at a given blade position and radial position), +:math:`\boldsymbol{W}_\text{qs}` is the quasi-steady induction, +:math:`\boldsymbol{W}_\text{int}` is an intermediate value coupling the quasi-steady and the actual inductions (may be discontinuous if the quasi-steady indution is discontinuous). +and +:math:`(\dot{\ })` represents the time derivative. +The coupling constant :math:`k`, with values between 0 and 1, is usually chosen as :math:`k=0.6`. +Oye's dynamic inflow model relies on two time constants, :math:`\tau_1` and :math:`\tau_2` : + +.. math:: + \tau_1=\frac{1.1}{1-1.3 \min(\overline{a},0.5)} \frac{R}{\overline{U}_0} + , \qquad + \tau_2 =\left[ 0.39-0.26\left(\frac{r}{R}\right)^2\right] \tau_1 + +where :math:`R` is the rotor radius, :math:`\overline{U}_0` is the average wind speed over the rotor, :math:`\overline{a}` is the average axial induction over the rotor, and :math:`r` is the radial position along the blade. +For ``DBEMT_Mod=1`` or ``DBEMT_Mod=3``, the user needs to provide the value of :math:`\tau_1`. + + + + +The continuous-time state-space formulation of the dynamic inflow model (``DBEMT_Mod=3``) was derived in :cite:`ad-Branlard:2022`. + +.. math:: + \begin{align} + \begin{bmatrix} + \boldsymbol{\dot{W}}_\text{red}\\ + \boldsymbol{\dot{W}}\\ + \end{bmatrix} + = + \begin{bmatrix} + -\frac{1}{\tau_1}\boldsymbol{I}_2 & \boldsymbol{0} \\ + \frac{1}{\tau_2}\boldsymbol{I}_2 & + -\frac{1}{\tau_2}\boldsymbol{I}_2 \\ + \end{bmatrix} + \begin{bmatrix} + \boldsymbol{W}_\text{red}\\ + \boldsymbol{W}\\ + \end{bmatrix} + + + \begin{bmatrix} + \frac{1-k}{\tau_1} \\ + \frac{k}{\tau_2}\\ + \end{bmatrix} + \boldsymbol{W}_\text{qs} + \end{align} + +where +:math:`\boldsymbol{I}_2` is the 2x2 identity matrix, +:math:`\boldsymbol{W}_\text{red}` is the reduced induction which is a continuous, scaled, and lagged version of the quasi-steady induction, defined as: + +.. math:: + \boldsymbol{W}_\text{int} = \boldsymbol{W}_\text{red} + k \boldsymbol{W}_\text{qs} + + +The discrete-time version of the model is documented in the unpublished manual of DBEMT. +The current discrete-time formulation is complex and in the future it can be simplified by using :math:`\boldsymbol{W}_\text{red}`. + + + + + + .. _AD_twr_shadow: Tower shadow models diff --git a/docs/source/user/aerodyn/theory_ua.rst b/docs/source/user/aerodyn/theory_ua.rst index 7c66917bb8..64097e65e7 100644 --- a/docs/source/user/aerodyn/theory_ua.rst +++ b/docs/source/user/aerodyn/theory_ua.rst @@ -197,8 +197,12 @@ Two variants are implemented in the Unsteady Aerodynamic module. These two (comp Beddoes-Leishman 4-states model (UAMod=4) ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -The 4-states (incompressible) dynamic stall model from Hansen-Gaunaa-Madsen (HGM) is described in :cite:`ad-Hansen:2004` and enabled using ``UAMod=4``. The model uses :math:`C_l` as main physical quantity. -Linearization of the model will be available in the future. +The 4-states (incompressible) dynamic stall model as implemented in OpenFAST is described in :cite:`ad-Branlard:2022`. +The original formulation from Hansen-Gaunaa-Madsen (HGM) is described in :cite:`ad-Hansen:2004`. +This model is enabled using ``UAMod=4``. The model uses :math:`C_l` as main physical quantity. +Linearization of the model is available. + +NOTE: this model might require smaller time steps until a stiff integrator is implemented in AeroDyn-UA. **State equation:** @@ -225,6 +229,9 @@ with \end{aligned} + + + **Output equation:** The unsteady airfoil coefficients :math:`C_{l,\text{dyn}}`, :math:`C_{d,\text{dyn}}`, @@ -233,8 +240,9 @@ The unsteady airfoil coefficients .. math:: \begin{aligned} - C_{l,\text{dyn}}(t) &= x_4 (\alpha_E-\alpha_0) C_{l,\alpha} + (1-x_4) C_{l,{fs}}(\alpha_E)+ \pi T_u \omega \\ - C_{d,\text{dyn}}(t) &= C_d(\alpha_E) + (\alpha_{ac}-\alpha_E) C_{l,\text{dyn}} + \left[ C_d(\alpha_E)-C_d(\alpha_0)\right ] \Delta C_{d,f}'' \\ + C_{l,\text{dyn}}(t) &= C_{l,\text{circ}} + \pi T_u \omega \\ + % C_{d,\text{dyn}}(t) &= C_d(\alpha_E) + (\alpha_{ac}-\alpha_E) C_{l,\text{dyn}} + \left[ C_d(\alpha_E)-C_d(\alpha_0)\right ] \Delta C_{d,f}'' \\ + C_{d,\text{dyn}}(t) &= C_d(\alpha_E) + \left[(\alpha_{ac}-\alpha_E) +T_u \omega \right]C_{l,\text{circ}} + \left[ C_d(\alpha_E)-C_d(\alpha_0)\right ] \Delta C_{d,f}'' \\ % C_{m,\text{dyn}}(t) &= C_m(\alpha_E) + C_{l,\text{dyn}} \Delta C_{m,f}'' - \frac{\pi}{2} T_u \omega\\ C_{m,\text{dyn}}(t) &= C_m(\alpha_E) - \frac{\pi}{2} T_u \omega\\ \end{aligned} @@ -245,7 +253,8 @@ with: \begin{aligned} \Delta C_{d,f}'' &= \frac{\sqrt{f_s^{st}(\alpha_E)}-\sqrt{x_4}}{2} - \frac{f_s^{st}(\alpha_E)-x_4}{4} ,\qquad - x_4\ge 0 + x_4\ge 0 \\ + C_{l,\text{circ}}&= x_4 (\alpha_E-\alpha_0) C_{l,\alpha} + (1-x_4) C_{l,{\text{fs}}}(\alpha_E) \end{aligned} @@ -258,7 +267,7 @@ Beddoes-Leishman 5-states model (UAMod=5) The 5-states (incompressible) dynamic stall model is similar to the Beddoes-Leishman 4-states model (UAMod=4), but adds a 5th state to represent vortex generation. It is enabled using ``UAMod=5``. The model uses :math:`C_n` and :math:`C_c` as main physical quantities. -Linearization of the model will be available in the future. +Linearization of the model is available. @@ -272,7 +281,7 @@ Oye model (UAMod=6) Oye's dynamic stall model is a one-state (continuous) model, formulated in :cite:`ad-Oye:1991` and described e.g. in :cite:`ad-Branlard:book`. The model attempts to capture trailing edge stall. -Linearization of the model will be available in the future. +Linearization of the model is available. **State equation:** diff --git a/modules/aerodyn/src/DBEMT.f90 b/modules/aerodyn/src/DBEMT.f90 index 31834f5d7a..3c12e5c3b4 100644 --- a/modules/aerodyn/src/DBEMT.f90 +++ b/modules/aerodyn/src/DBEMT.f90 @@ -21,7 +21,7 @@ ! References: ! [1] E. Branlard, B. Jonkman, G.R. Pirrung, K. Dixon, J. Jonkman (2022) ! Dynamic inflow and unsteady aerodynamics models for modal and stability analyses in OpenFAST, -! Journal of Physics: conference series +! Journal of Physics: Conference Series, doi:10.1088/1742-6596/2265/3/032044 ! [2] R. Damiani, J.Jonkman ! DBEMT Theory Rev. 3 ! Unpublished diff --git a/modules/aerodyn/src/UnsteadyAero.f90 b/modules/aerodyn/src/UnsteadyAero.f90 index 79740d8910..02c5aeb1c3 100644 --- a/modules/aerodyn/src/UnsteadyAero.f90 +++ b/modules/aerodyn/src/UnsteadyAero.f90 @@ -18,6 +18,23 @@ ! limitations under the License. ! !********************************************************************************************************************************** +! References: +! +! [40] E. Branlard, B. Jonkman, G.R. Pirrung, K. Dixon, J. Jonkman (2022) +! Dynamic inflow and unsteady aerodynamics models for modal and stability analyses in OpenFAST, +! Journal of Physics: Conference Series, doi:10.1088/1742-6596/2265/3/032044 +! +! [41] E. Branlard, J. Jonkman, B.Jonkman (2020) +! Development plan for the aerodynamic linearization in OpenFAST +! Unpublished +! +! [70] User Documentation / AeroDyn / Unsteady Aerodynamics / Boing-Vertol model +! https://openfast.readthedocs.io/ +! +! [other] R. Damiani and G. Hayman (2017) +! The Unsteady Aerodynamics Module for FAST 8 +! NOTE: equations for this reference are labeled as x.y [n] where n is the number of the equation when several equations are given. + module UnsteadyAero ! This module uses equations defined in the document "The Unsteady Aerodynamics Module for FAST 8" by Rick Damiani and Greg Hayman, 28-Feb-2017 @@ -1650,7 +1667,7 @@ end function Failed end subroutine UA_UpdateDiscOtherState_BV !============================================================================== -!> Calculate angle of attacks using Boeing-Vertol model +!> Calculate angle of attacks using Boeing-Vertol model, see [70] !! Drag effective angle of attack needs extra computation subroutine BV_getAlphas(i, j, u, p, xd, BL_p, tc, alpha_34, alphaE_L, alphaLag_D, adotnorm) integer, intent(in ) :: i !< node index within a blade @@ -1709,7 +1726,7 @@ subroutine BV_getAlphas(i, j, u, p, xd, BL_p, tc, alpha_34, alphaE_L, alphaLag_D alphaLag_D = alpha_34 - dalphaD*isgn ! NOTE: not effective alpha yet for drag end subroutine BV_getAlphas !============================================================================== -!> Calculate gamma for lift and drag based rel thickness. See CACTUS BV_DynStall.f95 +!> Calculate gamma for lift and drag based rel thickness. See CACTUS BV_DynStall.f95 subroutine BV_getGammas(tc, umach, gammaL, gammaD) real(ReKi), intent(in) :: tc !< Relative thickness of airfoil real(ReKi), intent(in) :: umach !< Mach number of Urel, = Urel*MinfMinf (freestrem Mach), 0 for incompressible @@ -1752,7 +1769,7 @@ real(ReKi) function BV_TransA(BL_p) BV_TransA = .5_ReKi*dalphaMax ! transition region for fairing lagged AOA in pure lag model end function BV_TransA !============================================================================== -!> Calculate deltas to negative and postivive stall angle +!> Calculate deltas to negative and postivive stall angle, see [70] subroutine BV_delNP(adotnorm, alpha, alphaLag_D, BL_p, activeD, delN, delP) real(ReKi), intent(in) :: adotnorm !< alphadot * Tu real(ReKi), intent(in) :: alpha !< alpha (3/4) @@ -1784,7 +1801,7 @@ subroutine BV_delNP(adotnorm, alpha, alphaLag_D, BL_p, activeD, delN, delP) end if end subroutine BV_delNP !============================================================================== -!> Calculate effective angle of attack for drag coefficient, based on lagged angle of attack +!> Calculate effective angle of attack for drag coefficient, based on lagged angle of attack, see [70] real(ReKi) function BV_alphaE_D(adotnorm, alpha, alphaLag_D, BL_p, activeD) real(ReKi), intent(in) :: adotnorm !< alphadot * Tu real(ReKi), intent(in) :: alpha !< alpha (3/4) @@ -1813,7 +1830,7 @@ real(ReKi) function BV_alphaE_D(adotnorm, alpha, alphaLag_D, BL_p, activeD) end if end function BV_alphaE_D !============================================================================== -!> Activate dynamic stall for lift or drag +!> Activate dynamic stall for lift or drag, see [70] subroutine BV_UpdateActiveStates(adotnorm, alpha, alphaLag_D, alphaE_L, BL_p, activeL, activeD) real(ReKi), intent(in) :: adotnorm !< alphadot * Tu real(ReKi), intent(in) :: alpha !< alpha (3/4) @@ -2414,7 +2431,7 @@ SUBROUTINE HGM_Steady( i, j, u, p, x, AFInfo, ErrStat, ErrMsg ) x%x(3) = AFI_interp%Cl ! Not used elseif (p%UAMod==UA_HGM) then - x%x(3) = BL_p%c_lalpha * (alphaE-BL_p%alpha0) + x%x(3) = BL_p%c_lalpha * (alphaE-BL_p%alpha0) ! Clp ! calculate x%x(4) = fs_aF = f_st(alphaF): !alphaF = x%x(3)/BL_p%c_lalpha + BL_p%alpha0 ! p. 13 @@ -2513,7 +2530,7 @@ subroutine UA_CalcContStateDeriv( i, j, t, u_in, p, x, OtherState, AFInfo, m, dx ! find alphaF where FullyAttached(alphaF) = x(3) if (p%UAMod == UA_HGM) then !note: BL_p%c_lalpha cannot be zero. UA is turned off at initialization if this occurs. - alphaF = x%x(3)/BL_p%c_lalpha + BL_p%alpha0 ! p. 13 + alphaF = x%x(3)/BL_p%c_lalpha + BL_p%alpha0 ! Eq. 15 [40] else if (p%UAMod == UA_OYE) then alphaF = alpha_34 @@ -2550,8 +2567,8 @@ subroutine UA_CalcContStateDeriv( i, j, t, u_in, p, x, OtherState, AFInfo, m, dx call AddOrSub2Pi(real(x%x(1),ReKi), alpha_34) ! make sure we use the same alpha_34 for both x1 and x2 equations. if (p%ShedEffect) then - dxdt%x(1) = -1.0_R8Ki / Tu * (BL_p%b1 + p%c(i,j) * U_dot/(2*u%u**2)) * x%x(1) + BL_p%b1 * BL_p%A1 / Tu * alpha_34 - dxdt%x(2) = -1.0_R8Ki / Tu * (BL_p%b2 + p%c(i,j) * U_dot/(2*u%u**2)) * x%x(2) + BL_p%b2 * BL_p%A2 / Tu * alpha_34 + dxdt%x(1) = -1.0_R8Ki / Tu * (BL_p%b1 + p%c(i,j) * U_dot/(2*u%u**2)) * x%x(1) + BL_p%b1 * BL_p%A1 / Tu * alpha_34 ! Eq. 8 [40] + dxdt%x(2) = -1.0_R8Ki / Tu * (BL_p%b2 + p%c(i,j) * U_dot/(2*u%u**2)) * x%x(2) + BL_p%b2 * BL_p%A2 / Tu * alpha_34 ! Eq. 9 [40] else dxdt%x(1) = 0.0_ReKi dxdt%x(2) = 0.0_ReKi @@ -2560,8 +2577,8 @@ subroutine UA_CalcContStateDeriv( i, j, t, u_in, p, x, OtherState, AFInfo, m, dx if (p%UAMod == UA_HGM) then call AddOrSub2Pi(BL_p%alpha0, alphaE) Clp = BL_p%c_lalpha * (alphaE - BL_p%alpha0) + pi * Tu * u%omega ! Eq. 13 - dxdt%x(3) = -1.0_R8Ki / BL_p%T_p * x%x(3) + 1.0_ReKi / BL_p%T_p * Clp - dxdt%x(4) = -1.0_R8Ki / BL_p%T_f0 * x4 + 1.0_ReKi / BL_p%T_f0 * AFI_AlphaF%f_st + dxdt%x(3) = -1.0_R8Ki / BL_p%T_p * x%x(3) + 1.0_ReKi / BL_p%T_p * Clp ! Eq. 10 [40] + dxdt%x(4) = -1.0_R8Ki / BL_p%T_f0 * x4 + 1.0_ReKi / BL_p%T_f0 * AFI_AlphaF%f_st ! Eq. 11 [40] dxdt%x(5) = 0.0_R8Ki elseif (p%UAMod == UA_OYE) then @@ -3139,20 +3156,20 @@ subroutine UA_CalcOutput( i, j, t, u_in, p, x, xd, OtherState, AFInfo, y, misc, call AddOrSub2Pi(BL_p%alpha0, alphaE) cl_fa = (alphaE - BL_p%alpha0) * BL_p%c_lalpha - delta_c_df_primeprime = 0.5_ReKi * (sqrt(fs_aE) - sqrt(x4)) - 0.25_ReKi * (fs_aE - x4) ! Eq. 81 + delta_c_df_primeprime = 0.5_ReKi * (sqrt(fs_aE) - sqrt(x4)) - 0.25_ReKi * (fs_aE - x4) ! Eq. 20 [40] ! bjj: do we need to check that u%alpha is between -pi and + pi? - cl_circ = x4 * cl_fa + (1.0_ReKi - x4) * cl_fs - y%Cl = cl_circ + pi * Tu * u%omega ! Eq. 78 + cl_circ = x4 * cl_fa + (1.0_ReKi - x4) * cl_fs ! Eq. 19 [40] + y%Cl = cl_circ + pi * Tu * u%omega ! Eq. 16 [40] call AddOrSub2Pi(u%alpha, alphaE) cd_tors = cl_circ * Tu * u%omega - y%Cd = AFI_interp%Cd + (alpha_34 - alphaE) * cl_circ + (AFI_interp%Cd - BL_p%Cd0) * delta_c_df_primeprime + cd_tors ! Eq. 79 + y%Cd = AFI_interp%Cd + (alpha_34 - alphaE) * cl_circ + (AFI_interp%Cd - BL_p%Cd0) * delta_c_df_primeprime + cd_tors ! Eq. 17 [40] if (AFInfo%ColCm == 0) then ! we don't have a cm column, so make everything 0 y%Cm = 0.0_ReKi else - y%Cm = AFI_interp%Cm + y%Cl * delta_c_mf_primeprime - piBy2 * Tu * u%omega ! Eq. 80 + y%Cm = AFI_interp%Cm + y%Cl * delta_c_mf_primeprime - piBy2 * Tu * u%omega ! Eq. 18 [40] end if y%Cn = y%Cl*cos(u%alpha) + y%Cd*sin(u%alpha) @@ -3173,10 +3190,10 @@ subroutine UA_CalcOutput( i, j, t, u_in, p, x, xd, OtherState, AFInfo, y, misc, tau_vl = tau_vl / Tu ! make this non-dimensional (to compare with T_VL) tV_ratio = min(1.5_ReKi, tau_vl/BL_p%T_VL) - delta_c_df_primeprime = 0.5_ReKi * (sqrt(fs_aE) - sqrt(x4)) - 0.25_ReKi * (fs_aE - x4) ! Eq. 81 + delta_c_df_primeprime = 0.5_ReKi * (sqrt(fs_aE) - sqrt(x4)) - 0.25_ReKi * (fs_aE - x4) ! Eq. 20 [40] call AddOrSub2Pi(u%alpha, alphaE) - y%Cd = AFI_interp%Cd + (u%alpha - alphaE) * y%Cn + (AFI_interp%Cd - BL_p%Cd0) * delta_c_df_primeprime ! Eq. 79 + y%Cd = AFI_interp%Cd + (u%alpha - alphaE) * y%Cn + (AFI_interp%Cd - BL_p%Cd0) * delta_c_df_primeprime ! Eq. 79 [41] if (AFInfo%ColCm == 0) then ! we don't have a cm column, so make everything 0 @@ -3456,7 +3473,7 @@ subroutine UA_CalcOutput( i, j, t, u_in, p, x, xd, OtherState, AFInfo, y, misc, contains !> Calc Outputs for Boieng-Vertol dynamic stall - !! See BV_DynStall.f95 of CACTUS, notations kept more or less consistent + !! See BV_DynStall.f95 of CACTUS, and [70], notations kept more or less consistent subroutine BV_CalcOutput() real(ReKi) :: alpha_50 real(ReKi) :: Cm25_stat From d27b0adf14c156bc4023aa7f419e2c86c99a9afc Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Thu, 16 Jun 2022 11:31:47 -0600 Subject: [PATCH 138/242] UA/DBEMT: update of r-test --- reg_tests/CTestList.cmake | 1 + reg_tests/r-test | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/reg_tests/CTestList.cmake b/reg_tests/CTestList.cmake index f1f01ce2f2..a67a44b6e5 100644 --- a/reg_tests/CTestList.cmake +++ b/reg_tests/CTestList.cmake @@ -269,6 +269,7 @@ ad_regression("ad_VerticalAxis_OLAF" "aerodyn;bem") ad_regression("ad_BAR_CombinedCases" "aerodyn;bem") # NOTE: doing BAR at the end to avoid copy errors ad_regression("ad_BAR_OLAF" "aerodyn;bem") ad_regression("ad_BAR_SineMotion" "aerodyn;bem") +ad_regression("ad_BAR_SineMotion_UA4_DBEMT3" "aerodyn;bem") ad_regression("ad_BAR_RNAMotion" "aerodyn;bem") # BeamDyn regression tests diff --git a/reg_tests/r-test b/reg_tests/r-test index d5dd4299d7..17337e1ce4 160000 --- a/reg_tests/r-test +++ b/reg_tests/r-test @@ -1 +1 @@ -Subproject commit d5dd4299d7d7d19155a383442ac5788e44b87dc1 +Subproject commit 17337e1ce4f8df989d5b019ee8315be210412b61 From fc911410154d678e967685998b498af070c445fd Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Mon, 18 Jul 2022 11:14:55 -0600 Subject: [PATCH 139/242] UA/DBEMT: update of r-tests --- reg_tests/r-test | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/reg_tests/r-test b/reg_tests/r-test index 17337e1ce4..98848440e7 160000 --- a/reg_tests/r-test +++ b/reg_tests/r-test @@ -1 +1 @@ -Subproject commit 17337e1ce4f8df989d5b019ee8315be210412b61 +Subproject commit 98848440e72847dd1b576f537b7621a529bb91e1 From 461ad532c38ff70fe2fe95635df90264d63acca9 Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Mon, 18 Jul 2022 13:26:49 -0500 Subject: [PATCH 140/242] Link to repo-hosted NWTC Programmer's Handbook --- docs/source/dev/code_style.rst | 2 +- docs/source/this_doc.rst | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/source/dev/code_style.rst b/docs/source/dev/code_style.rst index eac515713b..9e383738b9 100644 --- a/docs/source/dev/code_style.rst +++ b/docs/source/dev/code_style.rst @@ -4,7 +4,7 @@ Code Style ~~~~~~~~~~ OpenFAST and its underlying modules are mostly written in Fortran adhering to the 2003 standard, but modules can be written in C or C++. The -`NWTC Programmer's Handbook `__ +:download:`NWTC Programmer's Handbook <../../OtherSupporting/NWTC_Programmers_Handbook.pdf>` is the definitive reference for all questions related to working with the FAST Framework and adding code to OpenFAST. diff --git a/docs/source/this_doc.rst b/docs/source/this_doc.rst index 78b3ffa5df..2991b53fa4 100644 --- a/docs/source/this_doc.rst +++ b/docs/source/this_doc.rst @@ -19,7 +19,7 @@ and link to other relevant websites. While OpenFAST developer documentation is being enhanced here, developers are encouraged to consult the legacy FAST v8 -`Programmer's Handbook `_. +:download:`NWTC Programmer's Handbook <../OtherSupporting/NWTC_Programmers_Handbook.pdf>`. Instructions on obtaining and installing OpenFAST are available in :ref:`installation`, and documentation for verifying an installation with the automated tests is at :ref:`testing`. From de7f525f6390320ec26f74a140fe6ea6b0417806 Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Mon, 18 Jul 2022 13:47:57 -0500 Subject: [PATCH 141/242] Rename AeroDyn images so they get past ad blockers See https://github.com/OpenFAST/openfast/issues/912 --- docs/source/user/aerodyn/appendix.rst | 8 ++++---- docs/source/user/aerodyn/driver.rst | 2 +- .../{ad_blade_geom.png => aerodyn_blade_geom.png} | Bin ...lade_local_cs.png => aerodyn_blade_local_cs.png} | Bin .../{ad_driver_geom.png => aerodyn_driver_geom.png} | Bin .../user/aerodyn/figs/aerodyn_not_ad.README.txt | 5 +++++ ...utput_channel.pdf => aerodyn_output_channel.pdf} | Bin .../{ad_tower_geom.png => aerodyn_tower_geom.png} | Bin docs/source/user/aerodyn/input.rst | 8 ++++---- 9 files changed, 14 insertions(+), 9 deletions(-) rename docs/source/user/aerodyn/figs/{ad_blade_geom.png => aerodyn_blade_geom.png} (100%) rename docs/source/user/aerodyn/figs/{ad_blade_local_cs.png => aerodyn_blade_local_cs.png} (100%) rename docs/source/user/aerodyn/figs/{ad_driver_geom.png => aerodyn_driver_geom.png} (100%) create mode 100644 docs/source/user/aerodyn/figs/aerodyn_not_ad.README.txt rename docs/source/user/aerodyn/figs/{ad_output_channel.pdf => aerodyn_output_channel.pdf} (100%) rename docs/source/user/aerodyn/figs/{ad_tower_geom.png => aerodyn_tower_geom.png} (100%) diff --git a/docs/source/user/aerodyn/appendix.rst b/docs/source/user/aerodyn/appendix.rst index 0a92e1bb0a..97b8f43758 100644 --- a/docs/source/user/aerodyn/appendix.rst +++ b/docs/source/user/aerodyn/appendix.rst @@ -57,10 +57,10 @@ The local tower coordinate system is shown in :numref:`ad_tower_geom` and the lo .. _ad_blade_local_cs: -.. figure:: figs/ad_blade_local_cs.png +.. figure:: figs/aerodyn_blade_local_cs.png :width: 80% :align: center - :alt: ad_blade_local_cs.png + :alt: aerodyn_blade_local_cs.png AeroDyn Local Blade Coordinate System (Looking Toward the Tip, from the Root) – l: Lift, d: Drag, m: Pitching, x: Normal (to Plane), @@ -69,9 +69,9 @@ The local tower coordinate system is shown in :numref:`ad_tower_geom` and the lo .. _ad-output-channel: -.. figure:: figs/ad_output_channel.pdf +.. figure:: figs/aerodyn_output_channel.pdf :width: 500px :align: center - :alt: ad_output_channel.pdf + :alt: aerodyn_output_channel.pdf AeroDyn Output Channel List diff --git a/docs/source/user/aerodyn/driver.rst b/docs/source/user/aerodyn/driver.rst index 0c214249e8..4d8748ce5b 100644 --- a/docs/source/user/aerodyn/driver.rst +++ b/docs/source/user/aerodyn/driver.rst @@ -152,7 +152,7 @@ Two turbine input formats are supported: In this format, the turbine geometry is entirely determined by the number of blades (`NumBlades`), the hub radius (`HubRad`), the hub height (`HubHt`), the overhang (`Overhang`), the shaft tilt (`ShftTilt`), the precone (`Precone`), and the vertical distance from the tower-top to the rotor shaft (`Twr2Shft`), as shown in :numref:`fig:BasicGeometry`. The definition of each parameter follows the ElastoDyn convention. For example, `HubRad` specifies the radius from the center-of-rotation to the blade root along the (possibly preconed) blade-pitch axis and must be greater than zero. `HubHt` specifies the elevation of the hub center above the ground for land-based wind turbines, above the mean sea level (MSL) for offshore wind turbines, or above the seabed for MHK turbines. `Overhang` specifies the distance along the (possibly tilted) rotor shaft between the tower centerline and hub center and is positive downwind (use a negative number for upwind rotors). `ShftTilt` is the angle (in degrees) between the rotor shaft and the horizontal plane, and positive `ShftTilt` means that the downwind end of the shaft is the highest (upwind turbines have negative `ShftTilt` for improved tower clearance). `Precone` is the angle (in degrees) between a flat rotor disk and the cone swept by the blades, positive downwind (upwind turbines have negative `Precone` for improved tower clearance). - .. figure:: figs/ad_driver_geom.png + .. figure:: figs/aerodyn_driver_geom.png :width: 60% :name: fig:BasicGeometry diff --git a/docs/source/user/aerodyn/figs/ad_blade_geom.png b/docs/source/user/aerodyn/figs/aerodyn_blade_geom.png similarity index 100% rename from docs/source/user/aerodyn/figs/ad_blade_geom.png rename to docs/source/user/aerodyn/figs/aerodyn_blade_geom.png diff --git a/docs/source/user/aerodyn/figs/ad_blade_local_cs.png b/docs/source/user/aerodyn/figs/aerodyn_blade_local_cs.png similarity index 100% rename from docs/source/user/aerodyn/figs/ad_blade_local_cs.png rename to docs/source/user/aerodyn/figs/aerodyn_blade_local_cs.png diff --git a/docs/source/user/aerodyn/figs/ad_driver_geom.png b/docs/source/user/aerodyn/figs/aerodyn_driver_geom.png similarity index 100% rename from docs/source/user/aerodyn/figs/ad_driver_geom.png rename to docs/source/user/aerodyn/figs/aerodyn_driver_geom.png diff --git a/docs/source/user/aerodyn/figs/aerodyn_not_ad.README.txt b/docs/source/user/aerodyn/figs/aerodyn_not_ad.README.txt new file mode 100644 index 0000000000..4e929b42df --- /dev/null +++ b/docs/source/user/aerodyn/figs/aerodyn_not_ad.README.txt @@ -0,0 +1,5 @@ + +The AeroDyn documentation should not reference images that are prefixed with "ad_". +This can lead to ad-blockers in browsers blocking these images. Instead, simply +use the prefix "aerodyn_". +See https://github.com/OpenFAST/openfast/issues/912 for details. diff --git a/docs/source/user/aerodyn/figs/ad_output_channel.pdf b/docs/source/user/aerodyn/figs/aerodyn_output_channel.pdf similarity index 100% rename from docs/source/user/aerodyn/figs/ad_output_channel.pdf rename to docs/source/user/aerodyn/figs/aerodyn_output_channel.pdf diff --git a/docs/source/user/aerodyn/figs/ad_tower_geom.png b/docs/source/user/aerodyn/figs/aerodyn_tower_geom.png similarity index 100% rename from docs/source/user/aerodyn/figs/ad_tower_geom.png rename to docs/source/user/aerodyn/figs/aerodyn_tower_geom.png diff --git a/docs/source/user/aerodyn/input.rst b/docs/source/user/aerodyn/input.rst index a49686c455..f99fc1216c 100644 --- a/docs/source/user/aerodyn/input.rst +++ b/docs/source/user/aerodyn/input.rst @@ -371,10 +371,10 @@ quantities are actually output at these nodes. .. _ad_tower_geom: -.. figure:: figs/ad_tower_geom.png +.. figure:: figs/aerodyn_tower_geom.png :width: 60% :align: center - :alt: ad_tower_geom.png + :alt: aerodyn_tower_geom.png AeroDyn Tower Geometry @@ -740,10 +740,10 @@ See :numref:`ad_blade_geom`. Twist is shown in :numref:`ad_blade_local_cs` of :n .. _ad_blade_geom: -.. figure:: figs/ad_blade_geom.png +.. figure:: figs/aerodyn_blade_geom.png :width: 90% :align: center - :alt: ad_blade_geom.png + :alt: aerodyn_blade_geom.png AeroDyn Blade Geometry – Left: Side View; Right: Front View (Looking Downwind) From fdb59b326fb3617231623898d6c889384ccb6c1d Mon Sep 17 00:00:00 2001 From: Hannah Ross Date: Mon, 18 Jul 2022 13:27:50 -0600 Subject: [PATCH 142/242] Update regression tests --- reg_tests/r-test | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/reg_tests/r-test b/reg_tests/r-test index d5dd4299d7..ce455c56f2 160000 --- a/reg_tests/r-test +++ b/reg_tests/r-test @@ -1 +1 @@ -Subproject commit d5dd4299d7d7d19155a383442ac5788e44b87dc1 +Subproject commit ce455c56f29efb8b5c56bb9ed5ab04a60cc9236d From eb347694f23a2858574bd86fcf977d4fceede1df Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Mon, 18 Jul 2022 13:44:40 -0600 Subject: [PATCH 143/242] UA/DBEMT: update of documentation. UAMod=4,5,6 support linearization --- docs/source/user/aerodyn/input.rst | 2 +- docs/source/user/aerodyn/theory_ua.rst | 5 ++--- modules/aerodyn/src/AeroDyn.f90 | 4 ++-- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/docs/source/user/aerodyn/input.rst b/docs/source/user/aerodyn/input.rst index 31e55d99d7..5a36e19642 100644 --- a/docs/source/user/aerodyn/input.rst +++ b/docs/source/user/aerodyn/input.rst @@ -222,7 +222,7 @@ The input parameters in this section are used only when ``AFAeroMod - ``6``: 1-state continuous-time developed by Oye - ``7``: discrete-time Boeing-Vertol (BV) model -The models are described in :numref:`AD_UA`. +Linearization is supported with ``UAMod=4,5,6`` (which use continuous-time states) but not with the other models. The different models are described in :numref:`AD_UA`. **While all of the UA models are documented in this diff --git a/docs/source/user/aerodyn/theory_ua.rst b/docs/source/user/aerodyn/theory_ua.rst index 64097e65e7..da05583613 100644 --- a/docs/source/user/aerodyn/theory_ua.rst +++ b/docs/source/user/aerodyn/theory_ua.rst @@ -197,9 +197,8 @@ Two variants are implemented in the Unsteady Aerodynamic module. These two (comp Beddoes-Leishman 4-states model (UAMod=4) ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -The 4-states (incompressible) dynamic stall model as implemented in OpenFAST is described in :cite:`ad-Branlard:2022`. -The original formulation from Hansen-Gaunaa-Madsen (HGM) is described in :cite:`ad-Hansen:2004`. -This model is enabled using ``UAMod=4``. The model uses :math:`C_l` as main physical quantity. +The 4-states (incompressible) dynamic stall model as implemented in OpenFAST is described in :cite:`ad-Branlard:2022` (the model differs slithgly from the original formulation from Hansen-Gaunaa-Madsen (HGM) :cite:`ad-Hansen:2004`). +The model is enabled using ``UAMod=4``. The model uses :math:`C_l` as main physical quantity. Linearization of the model is available. NOTE: this model might require smaller time steps until a stiff integrator is implemented in AeroDyn-UA. diff --git a/modules/aerodyn/src/AeroDyn.f90 b/modules/aerodyn/src/AeroDyn.f90 index bd62072e16..f90f861867 100644 --- a/modules/aerodyn/src/AeroDyn.f90 +++ b/modules/aerodyn/src/AeroDyn.f90 @@ -2479,8 +2479,8 @@ SUBROUTINE ValidateInputData( InitInp, InputFileData, NumBl, ErrStat, ErrMsg ) !.................. if (InitInp%Linearize) then if (InputFileData%AFAeroMod /= AFAeroMod_Steady) then - if (InputFileData%UAMod /= UA_HGM .and. InputFileData%UAMod /= UA_OYE) then - call SetErrStat( ErrID_Fatal, 'When AFAeroMod=2, UAMod must be 4 for linearization. Set AFAeroMod=1 or UAMod=4.', ErrStat, ErrMsg, RoutineName ) + if (InputFileData%UAMod /= UA_HGM .and. InputFileData%UAMod /= UA_HGMV .and. InputFileData%UAMod /= UA_OYE) then + call SetErrStat( ErrID_Fatal, 'When AFAeroMod=2, UAMod must be 4, 5, or 6 for linearization. Set AFAeroMod=1, or, set UAMod=4, 5, or 6.', ErrStat, ErrMsg, RoutineName ) end if end if From eab09f07d196e267d8b0d7d2345ce12d497ed9e3 Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Mon, 18 Jul 2022 14:47:33 -0600 Subject: [PATCH 144/242] UA/DBEMT: linearization with UA_OYE, using 4th state only --- modules/aerodyn/src/AeroDyn.f90 | 81 ++++++++++++++++++++-------- modules/aerodyn/src/UnsteadyAero.f90 | 4 +- 2 files changed, 61 insertions(+), 24 deletions(-) diff --git a/modules/aerodyn/src/AeroDyn.f90 b/modules/aerodyn/src/AeroDyn.f90 index f90f861867..918c60d61d 100644 --- a/modules/aerodyn/src/AeroDyn.f90 +++ b/modules/aerodyn/src/AeroDyn.f90 @@ -4824,14 +4824,23 @@ SUBROUTINE RotGetOP( t, u, p, p_AD, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, end if if (p%BEMT%UA%lin_nx>0) then - do j=1,p%NumBlades ! size(x%BEMT%UA%element,2) - do i=1,p%NumBlNds ! size(x%BEMT%UA%element,1) - do k=1,4 !size(x%BEMT%UA%element(i,j)%x) !linearize only first 4 states (5th is vortex) - x_op(index) = x%BEMT%UA%element(i,j)%x(k) + if (p%BEMT%UA%UAMod==UA_OYE) then + do j=1,p%NumBlades ! size(x%BEMT%UA%element,2) + do i=1,p%NumBlNds ! size(x%BEMT%UA%element,1) + x_op(index) = x%BEMT%UA%element(i,j)%x(4) index = index + 1 end do end do - end do + else + do j=1,p%NumBlades ! size(x%BEMT%UA%element,2) + do i=1,p%NumBlNds ! size(x%BEMT%UA%element,1) + do k=1,4 !size(x%BEMT%UA%element(i,j)%x) !linearize only first 4 states (5th is vortex) + x_op(index) = x%BEMT%UA%element(i,j)%x(k) + index = index + 1 + end do + end do + end do + endif end if @@ -4877,14 +4886,23 @@ SUBROUTINE RotGetOP( t, u, p, p_AD, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, end if if (p%BEMT%UA%lin_nx>0) then - do j=1,p%NumBlades ! size(dxdt%BEMT%UA%element,2) - do i=1,p%NumBlNds ! size(dxdt%BEMT%UA%element,1) - do k=1,4 !size(dxdt%BEMT%UA%element(i,j)%x) don't linearize 5th state - dx_op(index) = dxdt%BEMT%UA%element(i,j)%x(k) + if (p%BEMT%UA%UAMod==UA_OYE) then + do j=1,p%NumBlades ! size(dxdt%BEMT%UA%element,2) + do i=1,p%NumBlNds ! size(dxdt%BEMT%UA%element,1) + dx_op(index) = dxdt%BEMT%UA%element(i,j)%x(4) index = index + 1 end do end do - end do + else + do j=1,p%NumBlades ! size(dxdt%BEMT%UA%element,2) + do i=1,p%NumBlNds ! size(dxdt%BEMT%UA%element,1) + do k=1,4 !size(dxdt%BEMT%UA%element(i,j)%x) don't linearize 5th state + dx_op(index) = dxdt%BEMT%UA%element(i,j)%x(k) + index = index + 1 + end do + end do + end do + endif end if call AD_DestroyRotContinuousStateType( dxdt, ErrStat2, ErrMsg2) @@ -5387,15 +5405,17 @@ SUBROUTINE Init_Jacobian_x( p, InitOut, ErrStat, ErrMsg) do j=1,p%NumBlades ! size(x%BEMT%DBEMT%element,2) do i=1,p%NumBlNds ! size(x%BEMT%DBEMT%element,1) NodeTxt = 'blade '//trim(num2lstr(j))//', node '//trim(num2lstr(i)) + if (p%BEMT%UA%UAMod/=UA_OYE) then - InitOut%LinNames_x(k) = 'x1 '//trim(NodeTxt)//', rad' - k = k + 1 + InitOut%LinNames_x(k) = 'x1 '//trim(NodeTxt)//', rad' + k = k + 1 - InitOut%LinNames_x(k) = 'x2 '//trim(NodeTxt)//', rad' - k = k + 1 - - InitOut%LinNames_x(k) = 'x3 '//trim(NodeTxt)//', -' - k = k + 1 + InitOut%LinNames_x(k) = 'x2 '//trim(NodeTxt)//', rad' + k = k + 1 + + InitOut%LinNames_x(k) = 'x3 '//trim(NodeTxt)//', -' + k = k + 1 + endif InitOut%LinNames_x(k) = 'x4 '//trim(NodeTxt)//', -' p%dx(k) = 0.001 ! x4 is a number between 0 and 1, so we need this to be small @@ -5574,7 +5594,13 @@ SUBROUTINE Perturb_x( p, n, perturb_sign, x, dx ) else !call GetStateIndices( n - p%BEMT%DBEMT%lin_nx, size(x%BEMT%UA%element,2), size(x%BEMT%UA%element,1), size(x%BEMT%UA%element(1,1)%x), Blade, BladeNode, StateIndex ) - call GetStateIndices( n - p%BEMT%DBEMT%lin_nx, size(x%BEMT%UA%element,2), size(x%BEMT%UA%element,1), 4, Blade, BladeNode, StateIndex ) + + if (p%BEMT%UA%UAMod==UA_OYE) then + call GetStateIndices( n - p%BEMT%DBEMT%lin_nx, size(x%BEMT%UA%element,2), size(x%BEMT%UA%element,1), 1, Blade, BladeNode, StateIndex ) + StateIndex=4 ! Always the 4th one + else + call GetStateIndices( n - p%BEMT%DBEMT%lin_nx, size(x%BEMT%UA%element,2), size(x%BEMT%UA%element,1), 4, Blade, BladeNode, StateIndex ) + endif x%BEMT%UA%element(BladeNode,Blade)%x(StateIndex) = x%BEMT%UA%element(BladeNode,Blade)%x(StateIndex) + dx * perturb_sign end if @@ -5677,12 +5703,21 @@ SUBROUTINE Compute_dX(p, x_p, x_m, delta_p, delta_m, dX) if (p%BEMT%UA%lin_nx>0) then - do j=1,size(x_p%BEMT%UA%element,2) ! number of blades - do i=1,size(x_p%BEMT%UA%element,1) ! number of nodes per blade - dX(indx_first:indx_first+3) = x_p%BEMT%UA%element(i,j)%x(1:4) - x_m%BEMT%UA%element(i,j)%x(1:4) - indx_first = indx_first + 4 ! = index_first += 4 + if (p%BEMT%UA%UAMod==UA_OYE) then + do j=1,size(x_p%BEMT%UA%element,2) ! number of blades + do i=1,size(x_p%BEMT%UA%element,1) ! number of nodes per blade + dX(indx_first) = x_p%BEMT%UA%element(i,j)%x(4) - x_m%BEMT%UA%element(i,j)%x(4) + indx_first = indx_first + 1 ! = index_first += 4 + end do end do - end do + else + do j=1,size(x_p%BEMT%UA%element,2) ! number of blades + do i=1,size(x_p%BEMT%UA%element,1) ! number of nodes per blade + dX(indx_first:indx_first+3) = x_p%BEMT%UA%element(i,j)%x(1:4) - x_m%BEMT%UA%element(i,j)%x(1:4) + indx_first = indx_first + 4 ! = index_first += 4 + end do + end do + endif end if diff --git a/modules/aerodyn/src/UnsteadyAero.f90 b/modules/aerodyn/src/UnsteadyAero.f90 index 92a30596fb..27f832bf36 100644 --- a/modules/aerodyn/src/UnsteadyAero.f90 +++ b/modules/aerodyn/src/UnsteadyAero.f90 @@ -745,8 +745,10 @@ subroutine UA_SetParameters( dt, InitInp, p, AFInfo, AFIndx, ErrStat, ErrMsg ) p%Flookup = InitInp%Flookup p%ShedEffect = InitInp%ShedEffect - if (p%UAMod==UA_HGM) then + if (p%UAMod==UA_HGM .or. p%UAMod==UA_HGMV) then p%lin_nx = p%numBlades*p%nNodesPerBlade*4 ! 4 continuous states per node per blade (5th state isn't currently linearizable) + else if (p%UAMod==UA_OYE) then + p%lin_nx = p%numBlades*p%nNodesPerBlade*1 ! continuous state per node per blade, but stored at position 4 else p%lin_nx = 0 end if From da66f9a092adcc68b10dde9116a30765cb9afa8c Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Mon, 18 Jul 2022 16:56:30 -0500 Subject: [PATCH 145/242] Make test documentation more visible in TOC See https://github.com/OpenFAST/openfast/issues/779 --- docs/source/testing/index.rst | 2 -- docs/source/testing/regression_test.rst | 4 ++-- docs/source/testing/unit_test.rst | 4 ++-- 3 files changed, 4 insertions(+), 6 deletions(-) diff --git a/docs/source/testing/index.rst b/docs/source/testing/index.rst index a8dad20a20..77fea3c218 100644 --- a/docs/source/testing/index.rst +++ b/docs/source/testing/index.rst @@ -29,8 +29,6 @@ GitHub users and is highly recommended as part of the development workflow. After enabling GitHub Actions in an OpenFAST repository, simply pushing new commits will trigger the tests. -Test specific documentation ---------------------------- .. toctree:: :maxdepth: 1 diff --git a/docs/source/testing/regression_test.rst b/docs/source/testing/regression_test.rst index 212e7dd4ca..bb134c501f 100644 --- a/docs/source/testing/regression_test.rst +++ b/docs/source/testing/regression_test.rst @@ -1,7 +1,7 @@ .. _regression_test: -Regression test -=============== +Regression tests +================ The regression test executes a series of test cases which intend to fully describe OpenFAST and its module's capabilities. Jump to one of the following sections for instructions on running the regression diff --git a/docs/source/testing/unit_test.rst b/docs/source/testing/unit_test.rst index e8bfd3f9e7..31c9cca58c 100644 --- a/docs/source/testing/unit_test.rst +++ b/docs/source/testing/unit_test.rst @@ -1,7 +1,7 @@ .. _unit_test: -Unit test -========= +Unit tests +========== In a software package as dynamic and collaborative as OpenFAST, confidence in multiple layers of code is best accomplished with a strong system of unit tests. Through robust testing practices, the entire OpenFAST community can From 70628ac7d6716558e013e76e957786cd480717cd Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Mon, 18 Jul 2022 17:31:26 -0500 Subject: [PATCH 146/242] Move IfW API changes to the correct section These were moved to the wrong section in https://github.com/OpenFAST/openfast/commit/54f2a62620d14d8c249bc4672a5a0f86f53c63d4. See https://github.com/OpenFAST/openfast/issues/752. For comparison, the files are available at 2.4: https://github.com/OpenFAST/r-test/blob/v2.4.0/glue-codes/openfast/AOC_WSt/AOC_WSt_InflowWind.dat 2.5: https://github.com/OpenFAST/r-test/blob/v2.5.0/glue-codes/openfast/AOC_WSt/AOC_WSt_InflowWind.dat --- docs/source/user/api_change.rst | 25 +++++++++++++++---------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/docs/source/user/api_change.rst b/docs/source/user/api_change.rst index 2e39de8561..552821edf6 100644 --- a/docs/source/user/api_change.rst +++ b/docs/source/user/api_change.rst @@ -303,13 +303,23 @@ Modified in OpenFAST v2.5.0 ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- Module Line Flag Name / section Example Value ============================ ====== ================================================ ==================================================================================== -MoorDyn na added CtrlChan column in LINE PROPERTIES table .. code-block:: none - - Line LineType UnstrLen NumSegs NodeAnch NodeFair Outputs CtrlChan - (-) (-) (m) (-) (-) (-) (-) (-) - 1 main 835.35 20 1 4 - 0 +MoorDyn na added CtrlChan column in LINE PROPERTIES table ============================ ====== ================================================ ==================================================================================== +============== ====== =============== ============== ============================================================================================================================================================================= +Renamed in OpenFAST v2.5.0 +---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- +Module Line Previous Name New Name Example Value +============== ====== =============== ============== ============================================================================================================================================================================= +InflowWind 17 Filename FileName_Uni "Shr11_30.wnd" FileName_Uni - Filename of time series data for uniform wind field. (-) +InflowWind 18 RefHt RefHt_Uni 90 RefHt_Uni - Reference height for horizontal wind speed (m) +InflowWind 21 Filename FileName_BTS "unused" FileName_BTS - Name of the Full field wind file to use (.bts) (-) +InflowWind 23 Filename FileNameRoot "unused" FileNameRoot - WindType=4: Rootname of the full-field wind file to use (.wnd, .sum); WindType=7: name of the intermediate file with wind scaling values +InflowWind 35 RefHt RefHt_Hawc 90 RefHt_Hawc - reference height; the height (in meters) of the vertical center of the grid (m) +InflowWind 47 PLExp PLExp_Hawc 0.2 PLExp_Hawc - Power law exponent (-) (used for PL wind profile type only) +InflowWind 49 InitPosition(x) XOffset 0 XOffset - Initial offset in +x direction (shift of wind box) +============== ====== =============== ============== ============================================================================================================================================================================= + OpenFAST v2.3.0 to OpenFAST v2.4.0 @@ -343,11 +353,6 @@ Modified in OpenFAST v2.4.0 Module Line New Flag Name Example Value Previous Flag Name/Value ============== ==== ================== ======================================================================================================================================================= ========================= AirFoilTables 40\* filtCutOff "DEFAULT" filtCutOff - Reduced frequency cut-off for low-pass filtering the AoA input to UA, as well as the 1st and 2nd deriv (-) [default = 0.5] [default = 20] -InflowWind 17 Filename_Uni "unused" Filename_Uni - Filename of time series data for uniform wind field. (-) Filename -InflowWind 18 RefHt_Uni 90 RefHt_Uni - Reference height for horizontal wind speed (m) RefHt -InflowWind 35 RefHt_Hawc 90 RefHt_Hawc - reference height; the height (in meters) of the vertical center of the grid (m) RefHt -InflowWind 47 PLExp_Hawc 0.2 PLExp_Hawc - Power law exponent (-) (used for PL wind profile type only) PLExp -InflowWind 49 XOffset 0 XOffset - Initial offset in +x direction (shift of wind box) InitPosition(x) ============== ==== ================== ======================================================================================================================================================= ========================= \*non-comment line count, excluding lines contained if NumCoords is not 0. From a21da3b90cb2ded71a39daaa245d9e74fba70057 Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Mon, 18 Jul 2022 17:55:27 -0500 Subject: [PATCH 147/242] Fix name of InflowWind_Driver in syntax help docs See https://github.com/OpenFAST/openfast/issues/665 --- docs/source/user/inflowwind/driver.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/source/user/inflowwind/driver.rst b/docs/source/user/inflowwind/driver.rst index b0ef7a0170..da2783c243 100644 --- a/docs/source/user/inflowwind/driver.rst +++ b/docs/source/user/inflowwind/driver.rst @@ -7,7 +7,7 @@ Command-line syntax for InflowWind driver: :: - InlowWind_Driver [options] + InflowWind_Driver [options] where: -- Name of driver input file to use options: /ifw -- treat as name of InflowWind input file (no driver input file) From 75879ae502c977f1f693b49a3386d78fae6dfce1 Mon Sep 17 00:00:00 2001 From: Andy Platt Date: Mon, 18 Jul 2022 17:11:12 -0600 Subject: [PATCH 148/242] Simulink: add documentation of channels in FAST_Library.h (#1176) Also add note in the Simulink glue-code directory about where Simulink channel information can be found. --- glue-codes/simulink/README.md | 5 +++++ modules/openfast-library/src/FAST_Library.h | 12 +++++++++++- 2 files changed, 16 insertions(+), 1 deletion(-) create mode 100644 glue-codes/simulink/README.md diff --git a/glue-codes/simulink/README.md b/glue-codes/simulink/README.md new file mode 100644 index 0000000000..a6509128b9 --- /dev/null +++ b/glue-codes/simulink/README.md @@ -0,0 +1,5 @@ +OpenFAST expects a set of channels passed from Simulink to the library. The number channels may change between OpenFAST releases. + +For list of control channels, see comments at end of source file modules/openfast-library/src/FAST_Library.h + +The examples included here inclue all the channels listed in the FAST_Library.h file. diff --git a/modules/openfast-library/src/FAST_Library.h b/modules/openfast-library/src/FAST_Library.h index 61b3db54a1..a30495c5b5 100644 --- a/modules/openfast-library/src/FAST_Library.h +++ b/modules/openfast-library/src/FAST_Library.h @@ -54,6 +54,16 @@ EXTERNAL_ROUTINE void FAST_CreateCheckpoint(int * iTurb, const char *CheckpointR #define MAXInitINPUTS 53 #define NumFixedInputs 2 + 2 + MAXIMUM_BLADES + 1 + MAXIMUM_AFCTRL + MAXIMUM_CABLE_DELTAL + MAXIMUM_CABLE_DELTALDOT - +/* Fixed inputs list: + 1 Generator Torque (N-m) + 2 Electrical Power (W) + 3 Yaw pos (rad) + 4 Yaw rate (rad/s) + 5-7 Blade 1-3 pitch angle (rad) + 8 High speed shaft brake fraction (-) + 9-11 Blade 1-3 Airfoil control (-) + 12-31 Cable control channel 1-20 DeltaL (m) + 32-51 Cable control channel 1-20 DeltaLDot (m/s) +*/ #endif From 924241a59853c10c16ebb06724aefee3f51cfd78 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Tue, 19 Jul 2022 09:57:18 -0600 Subject: [PATCH 149/242] MAP: allow keyword `fixed` and `fix` This allows the same word used in MD to be used in MAP (nasty seg-fault otherwise). --- modules/map/src/mapinit.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/map/src/mapinit.c b/modules/map/src/mapinit.c index 4def89c440..3afe39f3a4 100644 --- a/modules/map/src/mapinit.c +++ b/modules/map/src/mapinit.c @@ -1777,7 +1777,7 @@ MAP_ERROR_CODE allocate_types_for_nodes(MAP_InputType_t* u_type, MAP_ConstraintS while (i_parsedqty-1) { /* iterating through all strings */ if (parsed->entry[i_parsed]->slen) { /* if the string length is not 0 */ if (next==1) { - if (biseqcstrcaseless(parsed->entry[i_parsed],"FIX")) { + if (biseqcstrcaseless(parsed->entry[i_parsed],"FIX") || biseqcstrcaseless(parsed->entry[i_parsed],"FIXED")) { fix_num++; break; /* break the while-loop because the agenda is reached */ } else if (biseqcstrcaseless(parsed->entry[i_parsed],"CONNECT")) { @@ -1898,7 +1898,7 @@ MAP_ERROR_CODE set_node_list(const MAP_ParameterType_t* p_type, MAP_InputType_t if (next==0) { next++; } else if (next==1) { - if (biseqcstrcaseless(parsed->entry[i_parsed],"FIX")) { + if (biseqcstrcaseless(parsed->entry[i_parsed],"FIX") || biseqcstrcaseless(parsed->entry[i_parsed],"FIXED")) { node_iter->type = FIX; fix_num++; /* VarTypePtr FAST derived array index */ success = associate_vartype_ptr(&node_iter->position_ptr.x, other_type->x, fix_num); From 3a12ac60d39c989036c6a80a4f7066eec9462810 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Wed, 20 Jul 2022 09:51:28 -0600 Subject: [PATCH 150/242] AD15: add WrVTK_Type to driver for outputting line mesh info --- .../aerodyn/src/AeroDyn_Driver_Registry.txt | 3 +- modules/aerodyn/src/AeroDyn_Driver_Subs.f90 | 76 +++++++++++++++++-- modules/aerodyn/src/AeroDyn_Driver_Types.f90 | 9 ++- reg_tests/r-test | 2 +- 4 files changed, 79 insertions(+), 11 deletions(-) diff --git a/modules/aerodyn/src/AeroDyn_Driver_Registry.txt b/modules/aerodyn/src/AeroDyn_Driver_Registry.txt index 5debd28f0f..ca84d6e9e8 100644 --- a/modules/aerodyn/src/AeroDyn_Driver_Registry.txt +++ b/modules/aerodyn/src/AeroDyn_Driver_Registry.txt @@ -45,7 +45,8 @@ typedef ^ ^ character(25) Fmt_a typedef ^ ^ character(1) delim - - - "column delimiter" "-" typedef ^ ^ character(20) outFmt - - - "Format specifier" "-" typedef ^ ^ IntKi fileFmt - - - "Output format 1=Text, 2=Binary, 3=Both" "-" -typedef ^ ^ IntKi wrVTK - - - "0= no vtk, 1=animation" "-" +typedef ^ ^ IntKi wrVTK - - - "0= no vtk, 1=init only, 2=animation" "-" +typedef ^ ^ IntKi WrVTK_Type - - - "Flag for VTK output type (1=surface, 2=line, 3=both)" - typedef ^ ^ character(1024) Root - - - "Output file rootname" "-" typedef ^ ^ character(1024) VTK_OutFileRoot - - - "Output file rootname for vtk" "-" typedef ^ ^ character(ChanLen) WriteOutputHdr {:} - - "Channel headers" "-" diff --git a/modules/aerodyn/src/AeroDyn_Driver_Subs.f90 b/modules/aerodyn/src/AeroDyn_Driver_Subs.f90 index 1993dbcd60..cb2801e995 100644 --- a/modules/aerodyn/src/AeroDyn_Driver_Subs.f90 +++ b/modules/aerodyn/src/AeroDyn_Driver_Subs.f90 @@ -277,12 +277,17 @@ subroutine Dvr_TimeStep(nt, dvr, AD, IW, errStat, errMsg) ! VTK outputs - if (dvr%out%WrVTK==1 .and. nt==1) then + if ((dvr%out%WrVTK>=1 .and. nt==1) .or. (dvr%out%WrVTK==2)) then ! Init only - call WrVTK_Surfaces(time, dvr, dvr%out, nt-1, AD) - else if (dvr%out%WrVTK==2) then - ! Animation - call WrVTK_Surfaces(time, dvr, dvr%out, nt-1, AD) + select case (dvr%out%WrVTK_Type) + case (1) ! surfaces + call WrVTK_Surfaces(time, dvr, dvr%out, nt-1, AD) + case (2) ! lines + call WrVTK_Lines( time, dvr, dvr%out, nt-1, AD) + case (3) ! both + call WrVTK_Surfaces(time, dvr, dvr%out, nt-1, AD) + call WrVTK_Lines( time, dvr, dvr%out, nt-1, AD) + end select endif ! Get state variables at next step: INPUT at step nt - 1, OUTPUT at step nt @@ -1587,6 +1592,7 @@ subroutine Dvr_ReadInputFile(fileName, dvr, errStat, errMsg ) call ParseVar(FileInfo_In, CurLine, 'outFmt' , dvr%out%outFmt , errStat2, errMsg2, unEc); if(Failed()) return call ParseVar(FileInfo_In, CurLine, 'outFileFmt' , dvr%out%fileFmt , errStat2, errMsg2, unEc); if(Failed()) return call ParseVar(FileInfo_In, CurLine, 'WrVTK' , dvr%out%WrVTK , errStat2, errMsg2, unEc); if(Failed()) return + call ParseVar(FileInfo_In, CurLine, 'WrVTK_Type' , dvr%out%WrVTK_Type , errStat2, errMsg2, unEc); if(Failed()) return call ParseVar(FileInfo_In, CurLine, 'VTKHubRad' , hubRad_ReKi , errStat2, errMsg2, unEc); if(Failed()) return call ParseAry(FileInfo_In, CurLine, 'VTKNacDim' , dvr%out%VTKNacDim, 6, errStat2, errMsg2, unEc); if(Failed()) return dvr%out%VTKHubRad = real(hubRad_ReKi,SiKi) @@ -2108,6 +2114,7 @@ SUBROUTINE SetVTKParameters(p_FAST, dvr, InitOutData_AD, AD, ErrStat, ErrMsg) ErrStat = ErrID_None ErrMsg = "" +!FIXME: include any info needed for line mesh vtk writing ! get the name of the output directory for vtk files (in a subdirectory called "vtk" of the output directory), and ! create the VTK directory if it does not exist call GetPath ( p_FAST%root, p_FAST%VTK_OutFileRoot, vtkroot ) ! the returned p_FAST%VTK_OutFileRoot includes a file separator character at the end @@ -2124,8 +2131,8 @@ SUBROUTINE SetVTKParameters(p_FAST, dvr, InitOutData_AD, AD, ErrStat, ErrMsg) allocate(p_FAST%VTK_Surface(dvr%numTurbines)) ! --- Find dimensions for all objects to determine "Ground" and typical dimensions - WorldBoxMax(2) =-HUGE(1.0_SiKi) - WorldBoxMin(2) = HUGE(1.0_SiKi) + WorldBoxMax =-HUGE(1.0_SiKi) + WorldBoxMin = HUGE(1.0_SiKi) MaxBladeLength=0 MaxTwrLength=0 do iWT=1,dvr%numTurbines @@ -2300,7 +2307,7 @@ SUBROUTINE WrVTK_Surfaces(t_global, dvr, p_FAST, VTK_count, AD) end do if (p_FAST%WrVTK>1) then - ! --- Debug outputs + ! --- animations ! Tower base call MeshWrVTK_PointSurface (p_FAST%VTKRefPoint, wt%twr%ptMesh, trim(p_FAST%VTK_OutFileRoot)//trim(sWT)//'.TwrBaseSurface', & VTK_count, OutputFields, ErrStat2, ErrMsg2, p_FAST%VTK_tWidth , & @@ -2324,6 +2331,59 @@ SUBROUTINE WrVTK_Surfaces(t_global, dvr, p_FAST, VTK_count, AD) end if END SUBROUTINE WrVTK_Surfaces !---------------------------------------------------------------------------------------------------------------------------------- +!> This routine writes a minimal subset of meshes with surfaces to VTK-formatted files. It doesn't bother with +!! returning an error code. +SUBROUTINE WrVTK_Lines(t_global, dvr, p_FAST, VTK_count, AD) + use FVW_IO, only: WrVTK_FVW + REAL(DbKi), INTENT(IN ) :: t_global !< Current global time + type(Dvr_SimData), target, intent(inout) :: dvr ! intent(out) only so that we can save FmtWidth in dvr%out%ActualChanLen + TYPE(Dvr_Outputs), INTENT(IN ) :: p_FAST !< Parameters for the glue code + INTEGER(IntKi) , INTENT(IN ) :: VTK_count + TYPE(AeroDyn_Data), INTENT(IN ) :: AD !< AeroDyn data + logical, parameter :: OutputFields = .FALSE. ! due to confusion about what fields mean on a surface, we are going to just output the basic meshes if people ask for fields + INTEGER(IntKi) :: k + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMSg2 + CHARACTER(*), PARAMETER :: RoutineName = 'WrVTK_Lines' + integer(IntKi) :: iWT + type(WTData), pointer :: wt ! Alias to shorten notation + character(10) :: sWT + + do iWT = 1, size(dvr%WT) + sWT = '.T'//trim(num2lstr(iWT)) + wt=>dvr%WT(iWT) + + ! Tower motions + if (AD%u(2)%rotors(iWT)%TowerMotion%nNodes>0) then + call MeshWrVTK(p_FAST%VTKRefPoint, AD%u(2)%rotors(iWT)%TowerMotion, trim(p_FAST%VTK_OutFileRoot)//trim(sWT)//'.Tower', & + VTK_count, OutputFields, ErrStat2, ErrMsg2, p_FAST%VTK_tWidth ) + endif + + if (wt%numBlades>0) then + ! Nacelle + call MeshWrVTK(p_FAST%VTKRefPoint, wt%nac%ptMesh, trim(p_FAST%VTK_OutFileRoot)//trim(sWT)//'.Nacelle', & + VTK_count, OutputFields, ErrStat2, ErrMsg2, p_FAST%VTK_tWidth ) + + ! Hub + call MeshWrVTK(p_FAST%VTKRefPoint, AD%u(2)%rotors(iWT)%HubMotion, trim(p_FAST%VTK_OutFileRoot)//trim(sWT)//'.Hub', & + VTK_count, OutputFields, ErrStat2, ErrMsg2, p_FAST%VTK_tWidth ) + endif + + ! Blades + do K=1,wt%numBlades + call MeshWrVTK(p_FAST%VTKRefPoint, AD%u(2)%rotors(iWT)%BladeMotion(K), trim(p_FAST%VTK_OutFileRoot)//trim(sWT)//'.Blade'//trim(num2lstr(k)), & + VTK_count, OutputFields, ErrStat2, ErrMsg2, p_FAST%VTK_tWidth, Sib=AD%y%rotors(iWT)%BladeLoad(k) ) + end do + enddo + + ! Free wake (only write this here if doing line meshes only -- FVW is written with surface outputs) + if (allocated(AD%m%FVW_u) .and. dvr%out%WrVTK_Type==2) then + if (allocated(AD%m%FVW_u(1)%WingsMesh)) then + call WrVTK_FVW(AD%p%FVW, AD%x%FVW, AD%z%FVW, AD%m%FVW, trim(p_FAST%VTK_OutFileRoot)//'.FVW', VTK_count, p_FAST%VTK_tWidth, bladeFrame=.FALSE.) ! bladeFrame==.FALSE. to output in global coords + end if + end if +END SUBROUTINE WrVTK_Lines +!---------------------------------------------------------------------------------------------------------------------------------- !> This routine writes the ground or seabed reference surface information in VTK format. !! see VTK file information format for XML, here: http://www.vtk.org/wp-content/uploads/2015/04/file-formats.pdf SUBROUTINE WrVTK_Ground ( RefPoint, HalfLengths, FileRootName, ErrStat, ErrMsg ) diff --git a/modules/aerodyn/src/AeroDyn_Driver_Types.f90 b/modules/aerodyn/src/AeroDyn_Driver_Types.f90 index 61f020d76d..61b1087629 100644 --- a/modules/aerodyn/src/AeroDyn_Driver_Types.f90 +++ b/modules/aerodyn/src/AeroDyn_Driver_Types.f90 @@ -76,7 +76,8 @@ MODULE AeroDyn_Driver_Types character(1) :: delim !< column delimiter [-] character(20) :: outFmt !< Format specifier [-] INTEGER(IntKi) :: fileFmt !< Output format 1=Text, 2=Binary, 3=Both [-] - INTEGER(IntKi) :: wrVTK !< 0= no vtk, 1=animation [-] + INTEGER(IntKi) :: wrVTK !< 0= no vtk, 1=init only, 2=animation [-] + INTEGER(IntKi) :: WrVTK_Type !< Flag for VTK output type (1=surface, 2=line, 3=both) [-] character(1024) :: Root !< Output file rootname [-] character(1024) :: VTK_OutFileRoot !< Output file rootname for vtk [-] character(ChanLen) , DIMENSION(:), ALLOCATABLE :: WriteOutputHdr !< Channel headers [-] @@ -1036,6 +1037,7 @@ SUBROUTINE AD_Dvr_CopyDvr_Outputs( SrcDvr_OutputsData, DstDvr_OutputsData, CtrlC DstDvr_OutputsData%outFmt = SrcDvr_OutputsData%outFmt DstDvr_OutputsData%fileFmt = SrcDvr_OutputsData%fileFmt DstDvr_OutputsData%wrVTK = SrcDvr_OutputsData%wrVTK + DstDvr_OutputsData%WrVTK_Type = SrcDvr_OutputsData%WrVTK_Type DstDvr_OutputsData%Root = SrcDvr_OutputsData%Root DstDvr_OutputsData%VTK_OutFileRoot = SrcDvr_OutputsData%VTK_OutFileRoot IF (ALLOCATED(SrcDvr_OutputsData%WriteOutputHdr)) THEN @@ -1212,6 +1214,7 @@ SUBROUTINE AD_Dvr_PackDvr_Outputs( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, Int_BufSz = Int_BufSz + 1*LEN(InData%outFmt) ! outFmt Int_BufSz = Int_BufSz + 1 ! fileFmt Int_BufSz = Int_BufSz + 1 ! wrVTK + Int_BufSz = Int_BufSz + 1 ! WrVTK_Type Int_BufSz = Int_BufSz + 1*LEN(InData%Root) ! Root Int_BufSz = Int_BufSz + 1*LEN(InData%VTK_OutFileRoot) ! VTK_OutFileRoot Int_BufSz = Int_BufSz + 1 ! WriteOutputHdr allocated yes/no @@ -1356,6 +1359,8 @@ SUBROUTINE AD_Dvr_PackDvr_Outputs( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, Int_Xferred = Int_Xferred + 1 IntKiBuf(Int_Xferred) = InData%wrVTK Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%WrVTK_Type + Int_Xferred = Int_Xferred + 1 DO I = 1, LEN(InData%Root) IntKiBuf(Int_Xferred) = ICHAR(InData%Root(I:I), IntKi) Int_Xferred = Int_Xferred + 1 @@ -1606,6 +1611,8 @@ SUBROUTINE AD_Dvr_UnPackDvr_Outputs( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrSta Int_Xferred = Int_Xferred + 1 OutData%wrVTK = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 + OutData%WrVTK_Type = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 DO I = 1, LEN(OutData%Root) OutData%Root(I:I) = CHAR(IntKiBuf(Int_Xferred)) Int_Xferred = Int_Xferred + 1 diff --git a/reg_tests/r-test b/reg_tests/r-test index ce455c56f2..e8efa89461 160000 --- a/reg_tests/r-test +++ b/reg_tests/r-test @@ -1 +1 @@ -Subproject commit ce455c56f29efb8b5c56bb9ed5ab04a60cc9236d +Subproject commit e8efa89461f6a7de4ffe3e9416d2ff5dabfb566d From 9e1f8f976451a376ecef4ea75ee226dc2d6bb066 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Wed, 20 Jul 2022 10:10:53 -0600 Subject: [PATCH 151/242] AD15: update documentation for WrVTK_Type in the driver --- .../user/aerodyn/examples/ad_driver_example.dvr | 9 +++++---- .../user/aerodyn/examples/ad_driver_multiple.dvr | 13 +++++++------ docs/source/user/api_change.rst | 9 ++++++++- 3 files changed, 20 insertions(+), 11 deletions(-) diff --git a/docs/source/user/aerodyn/examples/ad_driver_example.dvr b/docs/source/user/aerodyn/examples/ad_driver_example.dvr index d390e38561..ea92c3edd1 100644 --- a/docs/source/user/aerodyn/examples/ad_driver_example.dvr +++ b/docs/source/user/aerodyn/examples/ad_driver_example.dvr @@ -34,9 +34,9 @@ False Echo - Echo input parameters to ".ech"? 3.09343 Twr2Shft(1) - Vertical distance from the tower-top to the rotor shaft (m) ----- Turbine(1) Motion [used only when AnalysisType=1] --------------------------------- 1 BaseMotionType(1) - Type of motion prescribed for this base {0: fixed, 1: Sinusoidal motion, 2: arbitrary motion} (flag) -1 DegreeOfFreedom(1) - {1:xg, 2:yg, 3:zg, 4:theta_xg, 5:theta_yg, 6:theta_zg} [used only when BaseMotionType=1] (flag) -5.0 Amplitude(1) - Amplitude of sinusoidal motion [used only when BaseMotionType=1] -0.1 Frequency(1) - Frequency of sinusoidal motion [used only when BaseMotionType=1] +1 DegreeOfFreedom(1) - {1:xt, 2:yt, 3:zt, 4:theta_xt, 5:theta_yt, 6:theta_zt} [used only when BaseMotionType=1] (flag) +5.0 Amplitude(1) - Amplitude of sinusoidal motion [used only when BaseMotionType=1] (m or rad) +0.1 Frequency(1) - Frequency of sinusoidal motion [used only when BaseMotionType=1] (Hz) "" BaseMotionFileName(1) - Filename containing arbitrary base motion (19 columns: Time, x, y, z, theta_x, ..., alpha_z) [used only when BaseMotionType=2] 0 NacYaw(1) - Yaw angle (about z_t) of the nacelle (deg) 7 RotSpeed(1) - Rotational speed of rotor in rotor coordinates (rpm) @@ -54,6 +54,7 @@ HWndSpeed PLExp RotSpd Pitch Yaw dT Tmax DOF Amplitude Frequency ----- Output Settings ------------------------------------------------------------------- "ES15.8E2" OutFmt - Format used for text tabular output, excluding the time channel. Resulting field should be 10 characters. (quoted string) 2 OutFileFmt - Format for tabular (time-marching) output file (switch) {1: text file [.out], 2: binary file [.outb], 3: both} -0 WrVTK - VTK visualization data output: (switch) {0=none; 1=animation} +0 WrVTK - VTK visualization data output: (switch) {0=none; 1=init; 2=animation} +1 WrVTK_Type - VTK visualization data type: (switch) {1=surfaces; 2=lines; 3=both} 2 VTKHubRad - HubRadius for VTK visualization (m) -1,-1,-1,2,2,2 VTKNacDim - Nacelle Dimension for VTK visualization x0,y0,z0,Lx,Ly,Lz (m) diff --git a/docs/source/user/aerodyn/examples/ad_driver_multiple.dvr b/docs/source/user/aerodyn/examples/ad_driver_multiple.dvr index 43e601ae87..00cf198871 100644 --- a/docs/source/user/aerodyn/examples/ad_driver_multiple.dvr +++ b/docs/source/user/aerodyn/examples/ad_driver_multiple.dvr @@ -46,8 +46,8 @@ True HAWTprojection(1) - True if turbine is a horizontal axis tu ----- Turbine(1) Motion [used only when AnalysisType=1] --------------------------------- 0 BaseMotionType(1) - Type of motion prescribed for this base {0: fixed, 1: Sinusoidal motion, 2: arbitrary motion} (flag) 1 DegreeOfFreedom(1) - {1:xt, 2:yt, 3:zt, 4:theta_xt, 5:theta_yt, 6:theta_zt} [used only when BaseMotionType=1] (flag) -0 Amplitude(1) - Amplitude of sinusoidal motion [used only when BaseMotionType=1] -0 Frequency(1) - Frequency of sinusoidal motion [used only when BaseMotionType=1] +0 Amplitude(1) - Amplitude of sinusoidal motion [used only when BaseMotionType=1] (m or rad) +0 Frequency(1) - Frequency of sinusoidal motion [used only when BaseMotionType=1] (Hz) "unused" BaseMotionFileName(1) - Filename containing arbitrary base motion (19 columns: Time, x, y, z, theta_x, ..., alpha_z) [used only when BaseMotionType=2] 0 NacMotionType(1) - Type of motion prescribed for the nacelle {0: fixed yaw, 1: time varying yaw angle} (flag) 0 NacYaw(1) - Yaw angle (about z_t) of the nacelle [user only when NacMotionType=0] (deg) @@ -75,8 +75,8 @@ True HAWTprojection(1) - True if turbine is a horizontal axis tu ----- Turbine(2) Motion [used only when AnalysisType=1] --------------------------------- 0 BaseMotionType(2) - Type of motion prescribed for this base {0: fixed, 1: Sinusoidal motion, 2: arbitrary motion} (flag) 0 DegreeOfFreedom(2) - {1:xt, 2:yt, 3:zt, 4:theta_xt, 5:theta_yt, 6:theta_zt} [used only when BaseMotionType=1] (flag) -0.0 Amplitude(2) - Amplitude of sinusoidal motion [used only when BaseMotionType=1] -0.0 Frequency(2) - Frequency of sinusoidal motion [used only when BaseMotionType=1] +0.0 Amplitude(2) - Amplitude of sinusoidal motion [used only when BaseMotionType=1] (m or deg) +0.0 Frequency(2) - Frequency of sinusoidal motion [used only when BaseMotionType=1] (Hz) "unused" BaseMotionFileName(2) - Filename containing arbitrary base motion (29 columns: Time, x, y, z, theta_x, ..., alpha_z) [used only when BaseMotionType=2] 0 NacYaw(2) - Yaw angle (about z_t) of the nacelle [user only when NacMotionType=0] (deg) 6 RotSpeed(2) - Rotational speed of rotor in rotor coordinates [used only when RotorMotionType=0] (rpm) @@ -86,10 +86,11 @@ True HAWTprojection(1) - True if turbine is a horizontal axis tu ----- Combined-Case Analysis [used only when AnalysisType=3, numTurbines=1] ------------- 0 NumCases - Number of cases to run HWndSpeed PLExp RotSpd Pitch Yaw dT Tmax DOF Amplitude Frequency -(m/s) (-) (rpm) (deg) (deg) (s) (s) (-) (-) (Hz) +(m/s) (-) (rpm) (deg) (deg) (s) (s) (-) (m or rad) (Hz) ----- Output Settings ------------------------------------------------------------------- "ES15.8E2" OutFmt - Format used for text tabular output, excluding the time channel. Resulting field should be 10 characters. (quoted string) 2 OutFileFmt - Format for tabular (time-marching) output file (switch) {1: text file [.out], 2: binary file [.outb], 3: both} -0 WrVTK - VTK visualization data output: (switch) {0=none; 1=animation} +0 WrVTK - VTK visualization data output: (switch) {0=none; 1=init; 2=animation} +1 WrVTK_Type - VTK visualization data type: (switch) {1=surfaces; 2=lines; 3=both} 2 VTKHubRad - HubRadius for VTK visualization (m) -7,-4,0,21,8,8 VTKNacDim - Nacelle Dimension for VTK visualization x0,y0,z0,Lx,Ly,Lz (m) diff --git a/docs/source/user/api_change.rst b/docs/source/user/api_change.rst index 2e39de8561..f460ed90e6 100644 --- a/docs/source/user/api_change.rst +++ b/docs/source/user/api_change.rst @@ -13,8 +13,15 @@ Thus, be sure to implement each in order so that subsequent line numbers are cor OpenFAST v3.2.0 to OpenFAST `dev` ---------------------------------- -None +============================================= ==== =============== ======================================================================================================================================================================================================== +Added in OpenFAST dev +--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- +Module Line Flag Name Example Value +============================================= ==== =============== ======================================================================================================================================================================================================== +AeroDyn driver 54\* WrVTK_Type 1 WrVTK_Type - VTK visualization data type: (switch) {1=surfaces; 2=lines; 3=both} +============================================= ==== =============== ======================================================================================================================================================================================================== +\*Exact line number depends on number of entries in various preceeding tables. OpenFAST v3.1.0 to OpenFAST v3.2.0 From b4c9facb0d33445e9419510a7e5140e25a859909 Mon Sep 17 00:00:00 2001 From: andrew-platt Date: Wed, 20 Jul 2022 10:22:29 -0600 Subject: [PATCH 152/242] AD15: add checks on driver inputs for WrVTK and WrVTK_Type --- modules/aerodyn/src/AeroDyn_Driver_Subs.f90 | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/modules/aerodyn/src/AeroDyn_Driver_Subs.f90 b/modules/aerodyn/src/AeroDyn_Driver_Subs.f90 index cb2801e995..184055d32b 100644 --- a/modules/aerodyn/src/AeroDyn_Driver_Subs.f90 +++ b/modules/aerodyn/src/AeroDyn_Driver_Subs.f90 @@ -1709,6 +1709,12 @@ subroutine ValidateInputs(dvr, errStat, errMsg) !if ( FmtWidth < MinChanLen ) call SetErrStat( ErrID_Warn, 'OutFmt produces a column less than '//trim(num2lstr(MinChanLen))//' characters wide ('// & ! TRIM(Num2LStr(FmtWidth))//'), which may be too small.', ErrStat, ErrMsg, RoutineName ) + if (Check((dvr%out%WrVTK<0 .or. dvr%out%WrVTK>2 ), 'WrVTK must be 0 (none), 1 (initialization only), 2 (animation), or 3 (mode shapes).')) then + return + else + if (Check((dvr%out%WrVTK_Type<1 .or. dvr%out%WrVTK_Type>3), 'VTK_type must be 1 (surfaces), 2 (lines/points), or 3 (both).')) return + endif + contains logical function Check(Condition, ErrMsg_in) @@ -2114,7 +2120,6 @@ SUBROUTINE SetVTKParameters(p_FAST, dvr, InitOutData_AD, AD, ErrStat, ErrMsg) ErrStat = ErrID_None ErrMsg = "" -!FIXME: include any info needed for line mesh vtk writing ! get the name of the output directory for vtk files (in a subdirectory called "vtk" of the output directory), and ! create the VTK directory if it does not exist call GetPath ( p_FAST%root, p_FAST%VTK_OutFileRoot, vtkroot ) ! the returned p_FAST%VTK_OutFileRoot includes a file separator character at the end From 8539e4e90f92007c0f819818f59be21ce0a7cc5c Mon Sep 17 00:00:00 2001 From: Matt Hall Date: Wed, 20 Jul 2022 18:27:36 -0600 Subject: [PATCH 153/242] MoorDyn Rod bugfixes to solve power cable issues: - Added proper setting of reference and initial orientations of coupled rods. - Updated/corrected calculations for determing orientation of zero-length rods. - Now passing Moordyn parameters data structure to object setup subroutines to facilitate log file output. --- modules/moordyn/src/MoorDyn.f90 | 27 ++++++++------ modules/moordyn/src/MoorDyn_Body.f90 | 4 +-- modules/moordyn/src/MoorDyn_Line.f90 | 54 ++++++++++++++++++++-------- modules/moordyn/src/MoorDyn_Rod.f90 | 48 ++++++++++++++++++++----- reg_tests/r-test | 2 +- 5 files changed, 99 insertions(+), 36 deletions(-) diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index bfad9a3f9f..f3f6566696 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -35,7 +35,7 @@ MODULE MoorDyn PRIVATE - TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn', 'v2.a26', '2022-04-13' ) + TYPE(ProgDesc), PARAMETER :: MD_ProgDesc = ProgDesc( 'MoorDyn', 'v2.a27', '2022-07-20' ) INTEGER(IntKi), PARAMETER :: wordy = 0 ! verbosity level. >1 = more console output @@ -87,6 +87,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er REAL(ReKi) :: rPos(3) ! array for setting fairlead reference positions in mesh REAL(ReKi) :: OrMat(3,3) ! rotation matrix for setting fairlead positions correctly if there is initial platform rotation REAL(ReKi) :: OrMat2(3,3) + REAL(R8Ki) :: OrMatRef(3,3) REAL(DbKi), ALLOCATABLE :: FairTensIC(:,:)! array of size nCpldCons, 3 to store three latest fairlead tensions of each line CHARACTER(20) :: TempString ! temporary string for incidental use INTEGER(IntKi) :: ErrStat2 ! Error status of the operation @@ -557,7 +558,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! ---------------------- now go through again and process file contents -------------------- - call Body_Setup( m%GroundBody, m%zeros6, p%rhoW, ErrStat2, ErrMsg2) + call Body_Setup( m%GroundBody, m%zeros6, p, ErrStat2, ErrMsg2) CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN @@ -863,7 +864,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! set up body - CALL Body_Setup( m%BodyList(l), tempArray, p%rhoW, ErrStat2, ErrMsg2) + CALL Body_Setup( m%BodyList(l), tempArray, p, ErrStat2, ErrMsg2) CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN @@ -1056,7 +1057,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er END IF ! set up rod - CALL Rod_Setup( m%RodList(l), m%RodTypeList(m%RodList(l)%PropsIdNum), tempArray, p%rhoW, ErrStat2, ErrMsg2) + CALL Rod_Setup( m%RodList(l), m%RodTypeList(m%RodList(l)%PropsIdNum), tempArray, p, ErrStat2, ErrMsg2) CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN @@ -1398,7 +1399,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! setup line - CALL SetupLine( m%LineList(l), m%LineTypeList(m%LineList(l)%PropsIdNum), p%rhoW , ErrStat2, ErrMsg2) + CALL SetupLine( m%LineList(l), m%LineTypeList(m%LineList(l)%PropsIdNum), p, ErrStat2, ErrMsg2) CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN @@ -1731,7 +1732,7 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er ! Go through each turbine and set up its mesh and initial positions of coupled objects DO iTurb = 1,p%nTurbines - ! calculate rotation matrix for the initial orientation provided for this turbine + ! calculate rotation matrix OrMat for the initial orientation provided for this turbine CALL SmllRotTrans('PtfmInit', InitInp%PtfmInit(4,iTurb),InitInp%PtfmInit(5,iTurb),InitInp%PtfmInit(6,iTurb), OrMat, '', ErrStat2, ErrMsg2) CALL CheckError( ErrStat2, ErrMsg2 ) IF (ErrStat >= AbortErrLev) RETURN @@ -1764,12 +1765,13 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er !OrMatRef = CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef(1:3), ErrStat2, ErrMsg2) ! defaults to identity orientation matrix + !TODO: >>> should also maybe set reference orientation (which might make part of a couple lines down redundant) <<< ! calculate initial point relative position, adjusted due to initial platform translations u%CoupledKinematics(iTurb)%TranslationDisp(:,J) = InitInp%PtfmInit(1:3,iTurb) - rRef(1:3) - OrMat2 = MATMUL(OrMat, TRANSPOSE( EulerConstruct( rRef(4:6)))) - u%CoupledKinematics(iTurb)%Orientation(:,:,J) = OrMat2 + OrMat2 = MATMUL(OrMat, TRANSPOSE( EulerConstruct( rRef(4:6)))) ! combine the Body's relative orientation with the turbine's initial orientation + u%CoupledKinematics(iTurb)%Orientation(:,:,J) = OrMat2 ! set the result as the current orientation of the body <<< ! set absolute initial positions in MoorDyn m%BodyList(m%CpldBodyIs(l,iTurb))%r6(1:3) = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + p%TurbineRefPos(:,iTurb) @@ -1785,16 +1787,21 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er DO l = 1,p%nCpldRods(iTurb) ! keeping this one simple for now, positioning at whatever is specified in input file <<<<< should change to glue code! J = J + 1 - rRef = m%RodList(m%CpldRodIs(l,iTurb))%r6 ! for now set reference position as per input file <<< - CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef(1:3), ErrStat2, ErrMsg2) ! defaults to identity orientation matrix + rRef = m%RodList(m%CpldRodIs(l,iTurb))%r6 ! for now set reference position as per input file <<< + OrMatRef = TRANSPOSE( m%RodList(m%CpldRodIs(l,iTurb))%OrMat ) ! for now set reference orientation as per input file <<< + CALL MeshPositionNode(u%CoupledKinematics(iTurb), J, rRef(1:3), ErrStat2, ErrMsg2, OrMatRef) ! assign the reference position and orientation ! calculate initial point relative position, adjusted due to initial platform rotations and translations <<< could convert to array math u%CoupledKinematics(iTurb)%TranslationDisp(1,J) = InitInp%PtfmInit(1,iTurb) + OrMat(1,1)*rRef(1) + OrMat(2,1)*rRef(2) + OrMat(3,1)*rRef(3) - rRef(1) u%CoupledKinematics(iTurb)%TranslationDisp(2,J) = InitInp%PtfmInit(2,iTurb) + OrMat(1,2)*rRef(1) + OrMat(2,2)*rRef(2) + OrMat(3,2)*rRef(3) - rRef(2) u%CoupledKinematics(iTurb)%TranslationDisp(3,J) = InitInp%PtfmInit(3,iTurb) + OrMat(1,3)*rRef(1) + OrMat(2,3)*rRef(2) + OrMat(3,3)*rRef(3) - rRef(3) + OrMat2 = MATMUL(OrMat, TRANSPOSE( EulerConstruct( rRef(4:6)))) ! combine the Rod's relative orientation with the turbine's initial orientation + u%CoupledKinematics(iTurb)%Orientation(:,:,J) = OrMat2 ! set the result as the current orientation of the rod <<< + ! set absolute initial positions in MoorDyn m%RodList(m%CpldRodIs(l,iTurb))%r6(1:3) = u%CoupledKinematics(iTurb)%Position(:,J) + u%CoupledKinematics(iTurb)%TranslationDisp(:,J) + p%TurbineRefPos(:,iTurb) + m%RodList(m%CpldRodIs(l,iTurb))%r6(4:6) = EulerExtract(MATMUL(OrMat, OrMatRef)) ! apply rotation from PtfmInit onto input file's rod orientation to get its true initial orientation ! >>> still need to set Rod initial orientations accounting for PtfmInit rotation <<< diff --git a/modules/moordyn/src/MoorDyn_Body.f90 b/modules/moordyn/src/MoorDyn_Body.f90 index 56d9eba8a5..c34e21c162 100644 --- a/modules/moordyn/src/MoorDyn_Body.f90 +++ b/modules/moordyn/src/MoorDyn_Body.f90 @@ -51,11 +51,11 @@ MODULE MoorDyn_Body CONTAINS - SUBROUTINE Body_Setup( Body, tempArray, rhoW, ErrStat, ErrMsg) + SUBROUTINE Body_Setup( Body, tempArray, p, ErrStat, ErrMsg) TYPE(MD_Body), INTENT(INOUT) :: Body ! the single body object of interest REAL(DbKi), INTENT(IN) :: tempArray(6) ! initial pose of body - REAL(DbKi), INTENT(IN) :: rhoW + TYPE(MD_ParameterType), INTENT(IN ) :: p ! Parameters INTEGER, INTENT(INOUT ) :: ErrStat ! returns a non-zero value when an error occurs CHARACTER(*), INTENT(INOUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None diff --git a/modules/moordyn/src/MoorDyn_Line.f90 b/modules/moordyn/src/MoorDyn_Line.f90 index 8efe941133..0974a2c092 100644 --- a/modules/moordyn/src/MoorDyn_Line.f90 +++ b/modules/moordyn/src/MoorDyn_Line.f90 @@ -47,12 +47,12 @@ MODULE MoorDyn_Line !----------------------------------------------------------------------- ! >>>>>>>>>>>>>> rename/reorganize this subroutine >>>>>>>>>>>>> - SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) + SUBROUTINE SetupLine (Line, LineProp, p, ErrStat, ErrMsg) ! allocate arrays in line object TYPE(MD_Line), INTENT(INOUT) :: Line ! the single line object of interest TYPE(MD_LineProp), INTENT(INOUT) :: LineProp ! the single line property set for the line of interest - REAL(DbKi), INTENT(IN) :: rhoW + TYPE(MD_ParameterType), INTENT(IN ) :: p ! Parameters INTEGER, INTENT( INOUT ) :: ErrStat ! returns a non-zero value when an error occurs CHARACTER(*), INTENT( INOUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None @@ -212,6 +212,23 @@ SUBROUTINE SetupLine (Line, LineProp, rhoW, ErrStat, ErrMsg) RETURN END IF + + if (p%writeLog > 1) then + write(p%UnLog, '(A)') " - Line"//trim(num2lstr(Line%IdNum)) + write(p%UnLog, '(A)') " ID: "//trim(num2lstr(Line%IdNum)) + write(p%UnLog, '(A)') " UnstrLen: "//trim(num2lstr(Line%UnstrLen)) + write(p%UnLog, '(A)') " N : "//trim(num2lstr(Line%N )) + write(p%UnLog, '(A)') " d : "//trim(num2lstr(Line%d )) + write(p%UnLog, '(A)') " rho : "//trim(num2lstr(Line%rho )) + write(p%UnLog, '(A)') " E : "//trim(num2lstr(Line%EA )) + write(p%UnLog, '(A)') " EI : "//trim(num2lstr(Line%EI )) + !write(p%UnLog, '(A)') " BAin: "//trim(num2lstr(Line%BAin)) + write(p%UnLog, '(A)') " Can : "//trim(num2lstr(Line%Can )) + write(p%UnLog, '(A)') " Cat : "//trim(num2lstr(Line%Cat )) + write(p%UnLog, '(A)') " Cdn : "//trim(num2lstr(Line%Cdn )) + write(p%UnLog, '(A)') " Cdt : "//trim(num2lstr(Line%Cdt )) + !write(p%UnLog, '(A)') " ww_l: " << ( (rho - env->rho_w)*(pi/4.*d*d) )*9.81 << endl; + end if ! need to add cleanup sub <<< @@ -1548,25 +1565,34 @@ SUBROUTINE Line_GetEndStuff(Line, Fnet_out, Moment_out, M_out, topOfLine) END SUBROUTINE Line_GetEndStuff !-------------------------------------------------------------- - - - ! set end kinematics of a line that's attached to a Rod, and this includes rotational information + + ! Get bending stiffness vector from line end for use in computing orientation of zero-length rods !-------------------------------------------------------------- - SUBROUTINE Line_GetEndSegmentInfo(Line, qEnd, EIout, dlOut, topOfLine) + SUBROUTINE Line_GetEndSegmentInfo(Line, q_EI_dl, topOfLine, rodEndB) TYPE(MD_Line), INTENT(INOUT) :: Line ! label for the current line, for convenience - REAL(DbKi), INTENT( OUT) :: qEnd(3) - REAL(DbKi), INTENT( OUT) :: EIout - REAL(DbKi), INTENT( OUT) :: dlOut + REAL(DbKi), INTENT( OUT) :: q_EI_dl(3) ! EI/dl of the line end segment multiplied by the axis unit vector with the correct sign INTEGER(IntKi), INTENT(IN ) :: topOfLine ! 0 for end A (Node 0), 1 for end B (node N) + INTEGER(IntKi), INTENT(IN ) :: rodEndB ! rodEndB=0 means the line is attached to Rod end A, =1 means attached to Rod end B (implication for unit vector sign) - EIout = Line%EI; - + REAL(DbKi) :: qEnd(3) + REAL(DbKi) :: dlEnd + if (topOfLine==1) then - CALL UnitVector(Line%r(:,Line%N-1), Line%r(:,Line%N), qEnd, dlOut) ! unit vector of last line segment + CALL UnitVector(Line%r(:,Line%N-1), Line%r(:,Line%N), qEnd, dlEnd) ! unit vector of last line segment + if (rodEndB == 0) then + q_EI_dl = qEnd*Line%EI/dlEnd ! -----line----->[A==ROD==>B] + else + q_EI_dl = -qEnd*Line%EI/dlEnd ! -----line----->[B==ROD==>A] + end if else - CALL UnitVector(Line%r(:,0 ), Line%r(:,1 ), qEnd, dlOut) ! unit vector of first line segment - END IF + CALL UnitVector(Line%r(:,0 ), Line%r(:,1 ), qEnd, dlEnd) ! unit vector of first line segment + if (rodEndB == 0) then + q_EI_dl = -qEnd*Line%EI/dlEnd ! <----line-----[A==ROD==>B] + else + q_EI_dl = qEnd*Line%EI/dlEnd ! <----line-----[B==ROD==>A] + end if + end if END SUBROUTINE Line_GetEndSegmentInfo !-------------------------------------------------------------- diff --git a/modules/moordyn/src/MoorDyn_Rod.f90 b/modules/moordyn/src/MoorDyn_Rod.f90 index 8bc092c9c8..26bd00c96b 100644 --- a/modules/moordyn/src/MoorDyn_Rod.f90 +++ b/modules/moordyn/src/MoorDyn_Rod.f90 @@ -49,12 +49,12 @@ MODULE MoorDyn_Rod !----------------------------------------------------------------------- - SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) + SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, p, ErrStat, ErrMsg) TYPE(MD_Rod), INTENT(INOUT) :: Rod ! the single rod object of interest TYPE(MD_RodProp), INTENT(INOUT) :: RodProp ! the single rod property set for the line of interest REAL(DbKi), INTENT(IN) :: endCoords(6) - REAL(DbKi), INTENT(IN) :: rhoW + TYPE(MD_ParameterType), INTENT(IN ) :: p ! Parameters INTEGER, INTENT( INOUT ) :: ErrStat ! returns a non-zero value when an error occurs CHARACTER(*), INTENT( INOUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None @@ -62,6 +62,9 @@ SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) INTEGER(4) :: K ! Generic index INTEGER(IntKi) :: N + Real(DbKi) :: phi, beta, sinPhi, cosPhi, tanPhi, sinBeta, cosBeta ! various orientation things + Real(DbKi) :: k_hat(3) ! unit vector (redundant, not used) <<<< + INTEGER :: ErrStat2 N = Rod%N ! number of segments in this line (for code readability) @@ -138,9 +141,32 @@ SUBROUTINE Rod_Setup(Rod, RodProp, endCoords, rhoW, ErrStat, ErrMsg) ! set gravity and bottom contact forces to zero initially (because the horizontal components should remain at zero) Rod%W = 0.0_DbKi Rod%B = 0.0_DbKi + + ! calculate some orientation items to be used for mesh setup + call GetOrientationAngles(Rod%q, phi, sinPhi, cosPhi, tanPhi, beta, sinBeta, cosBeta, k_hat) ! calculate some orientation information for the Rod as a whole + Rod%OrMat = CalcOrientation(phi, beta, 0.0_DbKi) ! get rotation matrix to put things in global rather than rod-axis orientations + IF (wordy > 0) print *, "Set up Rod ",Rod%IdNum, ", type ", Rod%typeNum + + if (p%writeLog > 1) then + write(p%UnLog, '(A)') " - Rod "//trim(num2lstr(Rod%IdNum)) + write(p%UnLog, '(A)') " ID: "//trim(num2lstr(Rod%IdNum)) + write(p%UnLog, '(A)') " UnstrLen: "//trim(num2lstr(Rod%UnstrLen)) + write(p%UnLog, '(A)') " N : "//trim(num2lstr(Rod%N )) + write(p%UnLog, '(A)') " d : "//trim(num2lstr(Rod%d )) + write(p%UnLog, '(A)') " rho : "//trim(num2lstr(Rod%rho )) + write(p%UnLog, '(A)') " Can : "//trim(num2lstr(Rod%Can )) + write(p%UnLog, '(A)') " Cat : "//trim(num2lstr(Rod%Cat )) + write(p%UnLog, '(A)') " CaEnd: "//trim(num2lstr(Rod%CaEnd )) + write(p%UnLog, '(A)') " Cdn : "//trim(num2lstr(Rod%Cdn )) + write(p%UnLog, '(A)') " Cdt : "//trim(num2lstr(Rod%Cdt )) + write(p%UnLog, '(A)') " CdEnd: "//trim(num2lstr(Rod%CdEnd )) + !write(p%UnLog, '(A)') " ww_l: " << ( (rho - env->rho_w)*(pi/4.*d*d) )*9.81 << endl; + end if + + ! need to add cleanup sub <<< @@ -339,6 +365,7 @@ SUBROUTINE Rod_SetDependentKin(Rod, t, m, initial) INTEGER(IntKi) :: N ! number of segments REAL(DbKi) :: qEnd(3) ! unit vector of attached line end segment, following same direction convention as Rod's q vector + REAL(DbKi) :: q_EI_dl(3) ! <<<< add description REAL(DbKi) :: EIend ! bending stiffness of attached line end segment REAL(DbKi) :: dlEnd ! stretched length of attached line end segment REAL(DbKi) :: qMomentSum(3) ! summation of qEnd*EI/dl_stretched (with correct sign) for each attached line @@ -377,22 +404,24 @@ SUBROUTINE Rod_SetDependentKin(Rod, t, m, initial) DO l=1,Rod%nAttachedA - CALL Line_GetEndSegmentInfo(m%LineList(Rod%attachedA(l)), qEnd, EIend, dlEnd, Rod%TopA(l)) + CALL Line_GetEndSegmentInfo(m%LineList(Rod%attachedA(l)), q_EI_dl, Rod%TopA(l), 0) - qMomentSum = qMomentSum + qEnd*EIend/dlEnd ! add each component to the summation vector + qMomentSum = qMomentSum + q_EI_dl ! add each component to the summation vector END DO DO l=1,Rod%nAttachedB - CALL Line_GetEndSegmentInfo(m%LineList(Rod%attachedB(l)), qEnd, EIend, dlEnd, Rod%TopB(l)) + CALL Line_GetEndSegmentInfo(m%LineList(Rod%attachedB(l)), q_EI_dl, Rod%TopB(l), 1) - qMomentSum = qMomentSum + qEnd*EIend/dlEnd ! add each component to the summation vector + qMomentSum = qMomentSum + q_EI_dl ! add each component to the summation vector END DO ! solve for line unit vector that balances all moments (unit vector of summation of qEnd*EI/dl_stretched over each line) CALL ScaleVector(qMomentSum, 1.0_DbKi, Rod%q) + + Rod%r6(4:6) = Rod%q ! set orientation angles END IF ! pass Rod orientation to any attached lines (this is just like what a Connection does, except for both ends) @@ -558,7 +587,7 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) ! used in lumped 6DOF calculations: Real(DbKi) :: rRel( 3) ! relative position of each node i from rRef - Real(DbKi) :: OrMat(3,3) ! rotation matrix to rotate global z to rod's axis + !Real(DbKi) :: OrMat(3,3) ! rotation matrix to rotate global z to rod's axis Real(DbKi) :: F6_i(6) ! a node's contribution to the total force vector Real(DbKi) :: M6_i(6,6) ! a node's contribution to the total mass matrix Real(DbKi) :: I_l ! axial inertia of rod @@ -936,9 +965,10 @@ SUBROUTINE Rod_DoRHS(Rod, m, p) Imat_l(3,3) = I_l end if - OrMat = CalcOrientation(phi, beta, 0.0_DbKi) ! get rotation matrix to put things in global rather than rod-axis orientations + ! >>> some of the kinematics parts of this could potentially be moved to a different routine <<< + Rod%OrMat = CalcOrientation(phi, beta, 0.0_DbKi) ! get rotation matrix to put things in global rather than rod-axis orientations - Imat = RotateM3(Imat_l, OrMat) ! rotate to give inertia matrix about CG in global frame + Imat = RotateM3(Imat_l, Rod%OrMat) ! rotate to give inertia matrix about CG in global frame ! these supplementary inertias can then be added the matrix (these are the terms ASIDE from the parallel axis terms) Rod%M6net(4:6,4:6) = Rod%M6net(4:6,4:6) + Imat diff --git a/reg_tests/r-test b/reg_tests/r-test index f55474e90f..19eba75fc1 160000 --- a/reg_tests/r-test +++ b/reg_tests/r-test @@ -1 +1 @@ -Subproject commit f55474e90f025e89ab317d2e524665da5746d594 +Subproject commit 19eba75fc1871a914e65820fa49789ad34ce185a From 3d415d3ef9cbb0a188c91bfae7b5ff57805bab61 Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Mon, 25 Jul 2022 15:17:51 -0500 Subject: [PATCH 154/242] Add overview presentation from NAWEA 2019 Using git-lfs since this PDF is about 70MB. See https://git-lfs.github.com for more info. --- .gitattributes | 3 +-- .../OpenFAST_Overview_NAWEAWindTech2019.pdf | Bin 0 -> 133 bytes docs/source/user/index.rst | 1 + 3 files changed, 2 insertions(+), 2 deletions(-) create mode 100644 docs/OtherSupporting/OpenFAST_Overview_NAWEAWindTech2019.pdf diff --git a/.gitattributes b/.gitattributes index 096b1c29a1..4f8ec66db2 100644 --- a/.gitattributes +++ b/.gitattributes @@ -1,6 +1,5 @@ # add (semi-useful) version info to git archive CreateGitVersion.bat ident export-subst - # Declare files that will always have CRLF line endings on checkout. *.bat text eol=crlf - +docs/OtherSupporting/OpenFAST_Overview_NAWEAWindTech2019.pdf filter=lfs diff=lfs merge=lfs -text diff --git a/docs/OtherSupporting/OpenFAST_Overview_NAWEAWindTech2019.pdf b/docs/OtherSupporting/OpenFAST_Overview_NAWEAWindTech2019.pdf new file mode 100644 index 0000000000000000000000000000000000000000..b44124835e2c4cf34368dec2607abe07ec34b8d3 GIT binary patch literal 133 zcmWN?NfN>!5CFhCuiyg)W>|*gH*6zRsiXvR@b%i4zUq7S@se$=a~?|F`?@`NZvWdS zZFxM^JZn}Lh|!CjEu!-WY34!*h(37IzyKa0V%ZZUqKFx+(P*JsmoYN%p1~MxT_CpE Oh}pkY66t{6Yw-gg(k93N literal 0 HcmV?d00001 diff --git a/docs/source/user/index.rst b/docs/source/user/index.rst index 59736b2fe6..6da79a7d63 100644 --- a/docs/source/user/index.rst +++ b/docs/source/user/index.rst @@ -24,6 +24,7 @@ General Workshop material, legacy documentation, and other resources are listed below. +- :download:`Overview of OpenFAST at NAWEA WindTech 2019 <../../OtherSupporting/OpenFAST_Overview_NAWEAWindTech2019.pdf>` - `Workshop Presentations `_ - :download:`Old FAST v6 User's Guide <../../OtherSupporting/Old_FAST6_UsersGuide.pdf>` - :download:`FAST v8 README <../../OtherSupporting/FAST8_README.pdf>` From 23d2330e1581b32f68301776d4f65fa4ed456c49 Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Wed, 27 Jul 2022 22:41:04 -0600 Subject: [PATCH 155/242] Lin: create VTK directory on restart (user might have deleted it) --- modules/openfast-library/src/FAST_Subs.f90 | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/modules/openfast-library/src/FAST_Subs.f90 b/modules/openfast-library/src/FAST_Subs.f90 index 3a6fb654d4..95c8529100 100644 --- a/modules/openfast-library/src/FAST_Subs.f90 +++ b/modules/openfast-library/src/FAST_Subs.f90 @@ -7263,6 +7263,8 @@ SUBROUTINE FAST_RestoreForVTKModeShape_T(t_initial, p_FAST, y_FAST, m_FAST, ED, CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'FAST_RestoreForVTKModeShape_T' CHARACTER(1024) :: VTK_RootName + CHARACTER(1024) :: VTK_RootDir + CHARACTER(1024) :: vtkroot ErrStat = ErrID_None @@ -7283,6 +7285,13 @@ SUBROUTINE FAST_RestoreForVTKModeShape_T(t_initial, p_FAST, y_FAST, m_FAST, ED, VTK_RootName = p_FAST%VTK_OutFileRoot + ! Creating VTK folder in case user deleted it. + ! We have to extract the vtk root dir again because p_FAST%VTK_OutFileRoot contains the full basename + call GetPath ( p_FAST%OutFileRoot, VTK_RootDir, vtkroot ) + VTK_RootDir = trim(VTK_RootDir) // 'vtk' + call MKDIR( trim(VTK_RootDir) ) + + select case (p_FAST%VTK_modes%VTKLinTim) case (1) From 157cfac8d98afaeca6f12bec02a60583ccf2bb1a Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Wed, 27 Jul 2022 22:43:37 -0600 Subject: [PATCH 156/242] Lin: unifying some code for VTKLinTim=1,2 and adding screen output --- modules/openfast-library/src/FAST_Subs.f90 | 40 ++++++++++------------ 1 file changed, 19 insertions(+), 21 deletions(-) diff --git a/modules/openfast-library/src/FAST_Subs.f90 b/modules/openfast-library/src/FAST_Subs.f90 index 95c8529100..f568934895 100644 --- a/modules/openfast-library/src/FAST_Subs.f90 +++ b/modules/openfast-library/src/FAST_Subs.f90 @@ -7265,6 +7265,7 @@ SUBROUTINE FAST_RestoreForVTKModeShape_T(t_initial, p_FAST, y_FAST, m_FAST, ED, CHARACTER(1024) :: VTK_RootName CHARACTER(1024) :: VTK_RootDir CHARACTER(1024) :: vtkroot + CHARACTER(1024) :: sInfo !< String used for formatted screen output ErrStat = ErrID_None @@ -7292,15 +7293,18 @@ SUBROUTINE FAST_RestoreForVTKModeShape_T(t_initial, p_FAST, y_FAST, m_FAST, ED, call MKDIR( trim(VTK_RootDir) ) - select case (p_FAST%VTK_modes%VTKLinTim) - case (1) - - do iMode = 1,p_FAST%VTK_modes%VTKLinModes - ModeNo = p_FAST%VTK_modes%VTKModes(iMode) - - call GetTimeConstants(p_FAST%VTK_modes%DampedFreq_Hz(ModeNo), p_FAST%VTK_fps, nt, dt, p_FAST%VTK_tWidth ) - if (nt > 500) cycle + do iMode = 1,p_FAST%VTK_modes%VTKLinModes + ModeNo = p_FAST%VTK_modes%VTKModes(iMode) + call GetTimeConstants(p_FAST%VTK_modes%DampedFreq_Hz(ModeNo), p_FAST%VTK_fps, nt, dt, p_FAST%VTK_tWidth ) + write(sInfo, '(A,I4,A,F12.4,A,I4,A,I0)') 'Mode',ModeNo,', Freq=', p_FAST%VTK_modes%DampedFreq_Hz(ModeNo),'Hz, NLinTimes=',NLinTimes,', nt=',nt + call WrScr(trim(sInfo)) + if (nt > 500) then + call WrScr(' Skipping mode '//trim(num2lstr(ModeNo))//' due to low frequency.') + cycle + endif + select case (p_FAST%VTK_modes%VTKLinTim) + case (1) p_FAST%VTK_OutFileRoot = trim(VTK_RootName)//'.Mode'//trim(num2lstr(ModeNo)) y_FAST%VTK_count = 1 ! we are skipping the reference meshes by starting at 1 do iLinTime = 1,NLinTimes @@ -7329,15 +7333,7 @@ SUBROUTINE FAST_RestoreForVTKModeShape_T(t_initial, p_FAST, y_FAST, m_FAST, ED, call WriteVTK(m_FAST%Lin%LinTimes(iLinTime), p_FAST, y_FAST, MeshMapData, ED, BD, AD, IfW, OpFM, HD, SD, ExtPtfm, SrvD, MAPp, FEAM, MD, Orca, IceF, IceD) end do ! iLinTime - end do ! iMode - - case (2) - - do iMode = 1,p_FAST%VTK_modes%VTKLinModes - ModeNo = p_FAST%VTK_modes%VTKModes(iMode) - - call GetTimeConstants(p_FAST%VTK_modes%DampedFreq_Hz(ModeNo), p_FAST%VTK_fps, nt, dt, p_FAST%VTK_tWidth ) - if (nt > 500) cycle + case (2) do iLinTime = 1,NLinTimes p_FAST%VTK_OutFileRoot = trim(VTK_RootName)//'.Mode'//trim(num2lstr(ModeNo))//'.LinTime'//trim(num2lstr(iLinTime)) @@ -7368,13 +7364,15 @@ SUBROUTINE FAST_RestoreForVTKModeShape_T(t_initial, p_FAST, y_FAST, m_FAST, ED, call WriteVTK(m_FAST%Lin%LinTimes(iLinTime)+tprime, p_FAST, y_FAST, MeshMapData, ED, BD, AD, IfW, OpFM, HD, SD, ExtPtfm, SrvD, MAPp, FEAM, MD, Orca, IceF, IceD) - end do + end do ! it + end do ! iLinTime + + end select ! VTKLinTim=1 or 2 + + end do ! iMode - end do ! iLinTime - end do ! iMode - end select END SUBROUTINE FAST_RestoreForVTKModeShape_T !---------------------------------------------------------------------------------------------------------------------------------- From 2ee8b4ae3dfdab3916342f415908fb136b22d16e Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Wed, 27 Jul 2022 22:44:41 -0600 Subject: [PATCH 157/242] Lin: surface mesh outputs without AD using basic geometries, adding circle/rectangle as options --- modules/openfast-library/src/FAST_Subs.f90 | 88 ++++++++++++++++------ 1 file changed, 66 insertions(+), 22 deletions(-) diff --git a/modules/openfast-library/src/FAST_Subs.f90 b/modules/openfast-library/src/FAST_Subs.f90 index f568934895..2617e96b92 100644 --- a/modules/openfast-library/src/FAST_Subs.f90 +++ b/modules/openfast-library/src/FAST_Subs.f90 @@ -3308,6 +3308,7 @@ SUBROUTINE SetVTKParameters(p_FAST, InitOutData_ED, InitOutData_AD, InitInData_H INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'SetVTKParameters' + INTEGER(IntKi) :: rootNode, cylNode, tipNode ErrStat = ErrID_None @@ -3417,16 +3418,8 @@ SUBROUTINE SetVTKParameters(p_FAST, InitOutData_ED, InitOutData_AD, InitInData_H call move_alloc( InitOutData_AD%rotors(1)%BladeShape(k)%AirfoilCoords, p_FAST%VTK_Surface%BladeShape(k)%AirfoilCoords ) end do ELSE -#ifndef USE_DEFAULT_BLADE_SURFACE - call setErrStat(ErrID_Fatal,'Cannot do surface visualization without airfoil coordinates defined in AeroDyn.',ErrStat,ErrMsg,RoutineName) - return - END IF - ELSE - call setErrStat(ErrID_Fatal,'Cannot do surface visualization without using AeroDyn.',ErrStat,ErrMsg,RoutineName) - return - END IF -#else ! AD used without airfoil coordinates specified + call WrScr('Using generic blade surfaces for AeroDyn (S809 airfoil, assumed chord, twist, AC). ') rootNode = 1 @@ -3434,19 +3427,20 @@ SUBROUTINE SetVTKParameters(p_FAST, InitOutData_ED, InitOutData_AD, InitInData_H tipNode = AD%Input(1)%rotors(1)%BladeMotion(K)%NNodes cylNode = min(3,AD%Input(1)%rotors(1)%BladeMotion(K)%Nnodes) - call SetVTKDefaultBladeParams(AD%Input(1)%rotors(1)%BladeMotion(K), p_FAST%VTK_Surface%BladeShape(K), tipNode, rootNode, cylNode, ErrStat2, ErrMsg2) + call SetVTKDefaultBladeParams(AD%Input(1)%rotors(1)%BladeMotion(K), p_FAST%VTK_Surface%BladeShape(K), tipNode, rootNode, cylNode, 1, ErrStat2, ErrMsg2) CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) IF (ErrStat >= AbortErrLev) RETURN END DO END IF ELSE IF ( p_FAST%CompElast == Module_BD ) THEN + call WrScr('Using generic blade surfaces for BeamDyn (circular airfoil, constant chord). ') ! TODO make this an option rootNode = 1 DO K=1,NumBl tipNode = BD%y(k)%BldMotion%NNodes cylNode = min(3,BD%y(k)%BldMotion%NNodes) - call SetVTKDefaultBladeParams(BD%y(k)%BldMotion, p_FAST%VTK_Surface%BladeShape(K), tipNode, rootNode, cylNode, ErrStat2, ErrMsg2) + call SetVTKDefaultBladeParams(BD%y(k)%BldMotion, p_FAST%VTK_Surface%BladeShape(K), tipNode, rootNode, cylNode, 2, ErrStat2, ErrMsg2) CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) IF (ErrStat >= AbortErrLev) RETURN END DO @@ -3456,12 +3450,11 @@ SUBROUTINE SetVTKParameters(p_FAST, InitOutData_ED, InitOutData_AD, InitInData_H tipNode = ED%y%BladeLn2Mesh(K)%NNodes-1 cylNode = min(2,ED%y%BladeLn2Mesh(K)%NNodes) - call SetVTKDefaultBladeParams(ED%y%BladeLn2Mesh(K), p_FAST%VTK_Surface%BladeShape(K), tipNode, rootNode, cylNode, ErrStat2, ErrMsg2) + call SetVTKDefaultBladeParams(ED%y%BladeLn2Mesh(K), p_FAST%VTK_Surface%BladeShape(K), tipNode, rootNode, cylNode, 1, ErrStat2, ErrMsg2) CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) IF (ErrStat >= AbortErrLev) RETURN END DO END IF -#endif !....................... @@ -3498,13 +3491,14 @@ SUBROUTINE SetVTKParameters(p_FAST, InitOutData_ED, InitOutData_AD, InitInData_H END SUBROUTINE SetVTKParameters !---------------------------------------------------------------------------------------------------------------------------------- !> This subroutine comes up with some default airfoils for blade surfaces for a given blade mesh, M. -SUBROUTINE SetVTKDefaultBladeParams(M, BladeShape, tipNode, rootNode, cylNode, ErrStat, ErrMsg) +SUBROUTINE SetVTKDefaultBladeParams(M, BladeShape, tipNode, rootNode, cylNode, iShape, ErrStat, ErrMsg) TYPE(MeshType), INTENT(IN ) :: M !< The Mesh the defaults should be calculated for TYPE(FAST_VTK_BLSurfaceType), INTENT(INOUT) :: BladeShape !< BladeShape to set to default values INTEGER(IntKi), INTENT(IN ) :: rootNode !< Index of root node (innermost node) for this mesh INTEGER(IntKi), INTENT(IN ) :: tipNode !< Index of tip node (outermost node) for this mesh INTEGER(IntKi), INTENT(IN ) :: cylNode !< Index of last node to have a cylinder shape + INTEGER(IntKi), INTENT(IN ) :: iShape !< 1: S809, 2: circle, 3: square, 4: rectangle INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status of the operation CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message if ErrStat /= ErrID_None @@ -3516,15 +3510,46 @@ SUBROUTINE SetVTKDefaultBladeParams(M, BladeShape, tipNode, rootNode, cylNode, E INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'SetVTKDefaultBladeParams' - - !Note: jmj does not like this default option - - integer, parameter :: N = 66 + integer :: N ! Number of points for airfoil + real, allocatable, dimension(:) :: xc, yc ! Coordinate of airfoil + + if (iShape==1) then + N=66 + call AllocAry(xc, N, 'xc', Errstat2, ErrMsg2) + call AllocAry(yc, N, 'yc', Errstat2, ErrMsg2) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName); if (ErrStat >= AbortErrLev) return + xc=(/ 1.0,0.996203,0.98519,0.967844,0.945073,0.917488,0.885293,0.848455,0.80747,0.763042,0.715952,0.667064,0.617331,0.56783,0.519832,0.474243,0.428461,0.382612,0.33726,0.29297,0.250247,0.209576,0.171409,0.136174,0.104263,0.076035,0.051823,0.03191,0.01659,0.006026,0.000658,0.000204,0.0,0.000213,0.001045,0.001208,0.002398,0.009313,0.02323,0.04232,0.065877,0.093426,0.124111,0.157653,0.193738,0.231914,0.271438,0.311968,0.35337,0.395329,0.438273,0.48192,0.527928,0.576211,0.626092,0.676744,0.727211,0.776432,0.823285,0.86663,0.905365,0.938474,0.965086,0.984478,0.996141,1.0 /) + yc=(/ 0.0,0.000487,0.002373,0.00596,0.011024,0.017033,0.023458,0.03028,0.037766,0.045974,0.054872,0.064353,0.074214,0.084095,0.093268,0.099392,0.10176,0.10184,0.10007,0.096703,0.091908,0.085851,0.078687,0.07058,0.061697,0.052224,0.042352,0.032299,0.02229,0.012615,0.003723,0.001942,-0.00002,-0.001794,-0.003477,-0.003724,-0.005266,-0.011499,-0.020399,-0.030269,-0.040821,-0.051923,-0.063082,-0.07373,-0.083567,-0.092442,-0.099905,-0.105281,-0.108181,-0.108011,-0.104552,-0.097347,-0.086571,-0.073979,-0.060644,-0.047441,-0.0351,-0.024204,-0.015163,-0.008204,-0.003363,-0.000487,0.000743,0.000775,0.00029,0.0 /) + else if (iShape==2) then + ! Circle + N=21 + call AllocAry(xc, N, 'xc', Errstat2, ErrMsg2) + call AllocAry(yc, N, 'yc', Errstat2, ErrMsg2) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName); if (ErrStat >= AbortErrLev) return + do i=1,N + angle = (i-1)*TwoPi/N + xc(i) = (cos(angle)+1)/2 ! between 0 and 1, 0.5 substracted later + yc(i) = (sin(angle)+1)/2-0.5 ! between -0.5 and 0.5 + enddo + else if (iShape==3) then + ! Square + N=5 + call AllocAry(xc, N, 'xc', Errstat2, ErrMsg2) + call AllocAry(yc, N, 'yc', Errstat2, ErrMsg2) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName); if (ErrStat >= AbortErrLev) return + xc = (/1.0 , 0.0 , 0.0 , 1.0 , 1.0/) ! between 0 and 1, 0.5 substracted later + yc = (/-0.5 , -0.5 , 0.5 , 0.5 , -0.5/) ! between -0.5 and 0.5 + else if (iShape==4) then + ! Rectangle + N=5 + call AllocAry(xc, N, 'xc', Errstat2, ErrMsg2) + call AllocAry(yc, N, 'yc', Errstat2, ErrMsg2) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName); if (ErrStat >= AbortErrLev) return + xc = (/1.0 , 0.0 , 0.0 , 1.0 , 1.0/) ! between 0 and 1, 0.5 substracted later + yc = (/-0.25 , -0.25 , 0.25 , 0.25 , 0.0/) ! between 0.25 and 0.25 + endif ! default airfoil shape coordinates; uses S809 values from http://wind.nrel.gov/airfoils/Shapes/S809_Shape.html: - real, parameter, dimension(N) :: xc=(/ 1.0,0.996203,0.98519,0.967844,0.945073,0.917488,0.885293,0.848455,0.80747,0.763042,0.715952,0.667064,0.617331,0.56783,0.519832,0.474243,0.428461,0.382612,0.33726,0.29297,0.250247,0.209576,0.171409,0.136174,0.104263,0.076035,0.051823,0.03191,0.01659,0.006026,0.000658,0.000204,0.0,0.000213,0.001045,0.001208,0.002398,0.009313,0.02323,0.04232,0.065877,0.093426,0.124111,0.157653,0.193738,0.231914,0.271438,0.311968,0.35337,0.395329,0.438273,0.48192,0.527928,0.576211,0.626092,0.676744,0.727211,0.776432,0.823285,0.86663,0.905365,0.938474,0.965086,0.984478,0.996141,1.0 /) - real, parameter, dimension(N) :: yc=(/ 0.0,0.000487,0.002373,0.00596,0.011024,0.017033,0.023458,0.03028,0.037766,0.045974,0.054872,0.064353,0.074214,0.084095,0.093268,0.099392,0.10176,0.10184,0.10007,0.096703,0.091908,0.085851,0.078687,0.07058,0.061697,0.052224,0.042352,0.032299,0.02229,0.012615,0.003723,0.001942,-0.00002,-0.001794,-0.003477,-0.003724,-0.005266,-0.011499,-0.020399,-0.030269,-0.040821,-0.051923,-0.063082,-0.07373,-0.083567,-0.092442,-0.099905,-0.105281,-0.108181,-0.108011,-0.104552,-0.097347,-0.086571,-0.073979,-0.060644,-0.047441,-0.0351,-0.024204,-0.015163,-0.008204,-0.003363,-0.000487,0.000743,0.000775,0.00029,0.0 /) - call AllocAry(BladeShape%AirfoilCoords, 2, N, M%NNodes, 'BladeShape%AirfoilCoords', ErrStat2, ErrMsg2) CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) IF (ErrStat >= AbortErrLev) RETURN @@ -3535,6 +3560,25 @@ SUBROUTINE SetVTKDefaultBladeParams(M, BladeShape, tipNode, rootNode, cylNode, E bladeLengthFract = 0.22*bladeLength bladeLengthFract2 = bladeLength-bladeLengthFract != 0.78*bladeLength + + ! Circle, square or rectangle, constant chord + if (iShape>1) then + chord = bladeLength*0.04 ! chord set to 4% of blade length + DO i=1,M%Nnodes + posLength = TwoNorm( M%Position(:,i) - M%Position(:,rootNode) ) + DO j=1,N + ! normalized x,y coordinates for airfoil + x = yc(j) + y = xc(j) - 0.5 + ! x,y coordinates for cylinder + BladeShape%AirfoilCoords(1,j,i) = chord*x + BladeShape%AirfoilCoords(2,j,i) = chord*y + END DO + enddo + return ! We exit this routine + endif + + ! Assumed chord/twist/AC distribution for a blade DO i=1,M%Nnodes posLength = TwoNorm( M%Position(:,i) - M%Position(:,rootNode) ) @@ -7649,7 +7693,7 @@ SUBROUTINE ReadModeShapeFile(p_FAST, InputFile, ErrStat, ErrMsg, checkpointOnly) ! overwrite these based on inputs: if (p_FAST%VTK_modes%VTKLinTim == 2) then - p_FAST%VTK_modes%VTKLinPhase = 0 ! "Phase when making one animation for all LinTimes together (used only when VTKLinTim=1)" - + !p_FAST%VTK_modes%VTKLinPhase = 0 ! "Phase when making one animation for all LinTimes together (used only when VTKLinTim=1)" - if (VTKLinTimes1) then p_FAST%VTK_modes%VTKNLinTimes = 1 From 71940d87a41430a84d051f9d804210ea551c9e2d Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Thu, 28 Jul 2022 18:53:09 -0600 Subject: [PATCH 158/242] Lin: default rectangular cross section for BD and ED. Using FPS for VTKLinTim=2 --- modules/openfast-library/src/FAST_Subs.f90 | 34 +++++++++++++--------- 1 file changed, 21 insertions(+), 13 deletions(-) diff --git a/modules/openfast-library/src/FAST_Subs.f90 b/modules/openfast-library/src/FAST_Subs.f90 index 2617e96b92..621bdef31a 100644 --- a/modules/openfast-library/src/FAST_Subs.f90 +++ b/modules/openfast-library/src/FAST_Subs.f90 @@ -3434,23 +3434,24 @@ SUBROUTINE SetVTKParameters(p_FAST, InitOutData_ED, InitOutData_AD, InitInData_H END IF ELSE IF ( p_FAST%CompElast == Module_BD ) THEN - call WrScr('Using generic blade surfaces for BeamDyn (circular airfoil, constant chord). ') ! TODO make this an option + call WrScr('Using generic blade surfaces for BeamDyn (rectangular airfoil, constant chord). ') ! TODO make this an option rootNode = 1 DO K=1,NumBl tipNode = BD%y(k)%BldMotion%NNodes cylNode = min(3,BD%y(k)%BldMotion%NNodes) - call SetVTKDefaultBladeParams(BD%y(k)%BldMotion, p_FAST%VTK_Surface%BladeShape(K), tipNode, rootNode, cylNode, 2, ErrStat2, ErrMsg2) + call SetVTKDefaultBladeParams(BD%y(k)%BldMotion, p_FAST%VTK_Surface%BladeShape(K), tipNode, rootNode, cylNode, 4, ErrStat2, ErrMsg2) CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) IF (ErrStat >= AbortErrLev) RETURN END DO ELSE + call WrScr('Using generic blade surfaces for ElastoDyn (rectangular airfoil, constant chord). ') ! TODO make this an option DO K=1,NumBl rootNode = ED%y%BladeLn2Mesh(K)%NNodes tipNode = ED%y%BladeLn2Mesh(K)%NNodes-1 cylNode = min(2,ED%y%BladeLn2Mesh(K)%NNodes) - call SetVTKDefaultBladeParams(ED%y%BladeLn2Mesh(K), p_FAST%VTK_Surface%BladeShape(K), tipNode, rootNode, cylNode, 1, ErrStat2, ErrMsg2) + call SetVTKDefaultBladeParams(ED%y%BladeLn2Mesh(K), p_FAST%VTK_Surface%BladeShape(K), tipNode, rootNode, cylNode, 4, ErrStat2, ErrMsg2) CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) IF (ErrStat >= AbortErrLev) RETURN END DO @@ -7339,7 +7340,7 @@ SUBROUTINE FAST_RestoreForVTKModeShape_T(t_initial, p_FAST, y_FAST, m_FAST, ED, do iMode = 1,p_FAST%VTK_modes%VTKLinModes ModeNo = p_FAST%VTK_modes%VTKModes(iMode) - call GetTimeConstants(p_FAST%VTK_modes%DampedFreq_Hz(ModeNo), p_FAST%VTK_fps, nt, dt, p_FAST%VTK_tWidth ) + call GetTimeConstants(p_FAST%VTK_modes%DampedFreq_Hz(ModeNo), p_FAST%VTK_fps, p_FAST%VTK_modes%VTKLinTim, nt, dt, p_FAST%VTK_tWidth ) write(sInfo, '(A,I4,A,F12.4,A,I4,A,I0)') 'Mode',ModeNo,', Freq=', p_FAST%VTK_modes%DampedFreq_Hz(ModeNo),'Hz, NLinTimes=',NLinTimes,', nt=',nt call WrScr(trim(sInfo)) if (nt > 500) then @@ -7420,9 +7421,10 @@ SUBROUTINE FAST_RestoreForVTKModeShape_T(t_initial, p_FAST, y_FAST, m_FAST, ED, END SUBROUTINE FAST_RestoreForVTKModeShape_T !---------------------------------------------------------------------------------------------------------------------------------- -SUBROUTINE GetTimeConstants(DampedFreq_Hz, VTK_fps, nt, dt, VTK_tWidth ) +SUBROUTINE GetTimeConstants(DampedFreq_Hz, VTK_fps, VTKLinTim, nt, dt, VTK_tWidth) REAL(R8Ki), INTENT(IN ) :: DampedFreq_Hz REAL(DbKi), INTENT(IN ) :: VTK_fps + INTEGER(IntKi), INTENT(IN ) :: VTKLinTim INTEGER(IntKi), INTENT( OUT) :: nt !< number of steps REAL(DbKi), INTENT( OUT) :: dt !< time step INTEGER(IntKi), INTENT( OUT) :: VTK_tWidth @@ -7431,21 +7433,27 @@ SUBROUTINE GetTimeConstants(DampedFreq_Hz, VTK_fps, nt, dt, VTK_tWidth ) INTEGER(IntKi) :: NCycles INTEGER(IntKi), PARAMETER :: MinFrames = 5 - if (DampedFreq_Hz <= 0.0_DbKi) then + if (DampedFreq_Hz <= 1e-4_DbKi) then nt = huge(nt) dt = epsilon(dt) VTK_tWidth = 1 return end if - nt = 1 - NCycles = 0 - do while (nt Date: Fri, 29 Jul 2022 10:31:34 -0500 Subject: [PATCH 159/242] Option to disable variable tracking with GNU compiler (#1198) * Allow disabling variable tracking via CMake to improve compile time * Disable variable tracking in GH Actions Improves compile time * Disable variable tracking for FAST Farm wrapper * Add a CMake warning when variable tracking is off but debug is on --- .github/workflows/automated-dev-tests.yml | 3 +++ CMakeLists.txt | 7 +++++++ glue-codes/fast-farm/CMakeLists.txt | 9 +++++---- glue-codes/openfast/CMakeLists.txt | 8 ++++---- modules/openfast-library/CMakeLists.txt | 7 ++++--- 5 files changed, 23 insertions(+), 11 deletions(-) diff --git a/.github/workflows/automated-dev-tests.yml b/.github/workflows/automated-dev-tests.yml index a04fc80a2c..fd63edb92c 100644 --- a/.github/workflows/automated-dev-tests.yml +++ b/.github/workflows/automated-dev-tests.yml @@ -103,6 +103,7 @@ jobs: -DCMAKE_BUILD_TYPE:STRING=RelWithDebInfo \ -DBUILD_TESTING:BOOL=ON \ -DCTEST_PLOT_ERRORS:BOOL=ON \ + -DVARIABLE_TRACKING=OFF \ ${GITHUB_WORKSPACE} - name: Build OpenFAST # if: contains(github.event.head_commit.message, 'Action - Test All') || contains(github.event.pull_request.labels.*.name, 'Action - Test All') @@ -275,6 +276,7 @@ jobs: -DCMAKE_BUILD_TYPE:STRING=RelWithDebInfo \ -DBUILD_TESTING:BOOL=ON \ -DCTEST_PLOT_ERRORS:BOOL=ON \ + -DVARIABLE_TRACKING=OFF \ ${GITHUB_WORKSPACE} - name: Build FAST.Farm # if: contains(github.event.head_commit.message, 'Action - Test All') || contains(github.event.pull_request.labels.*.name, 'Action - Test All') @@ -447,6 +449,7 @@ jobs: -DBUILD_SHARED_LIBS:BOOL=ON \ -DBUILD_TESTING:BOOL=ON \ -DCTEST_PLOT_ERRORS:BOOL=ON \ + -DVARIABLE_TRACKING=OFF \ ${GITHUB_WORKSPACE} - name: Build OpenFAST C-Interfaces diff --git a/CMakeLists.txt b/CMakeLists.txt index 36f0a0ecc3..9d1d3dd61a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,6 +28,7 @@ if (NOT CMAKE_BUILD_TYPE) "Choose the build type: Debug Release" FORCE) endif (NOT CMAKE_BUILD_TYPE) +option(VARIABLE_TRACKING "Enables variable tracking for better runtime debugging output. May increase compile time. Valid only for GNU." on) option(GENERATE_TYPES "Use the openfast-regsitry to autogenerate types modules" off) option(BUILD_SHARED_LIBS "Enable building shared libraries" off) option(DOUBLE_PRECISION "Treat REAL as double precision" on) @@ -45,6 +46,12 @@ if(APPLE) option(CMAKE_MACOSX_RPATH "Use RPATH runtime linking" on) endif() +# Warn if atypical configuration for variable tracking and build type +string(TOUPPER ${CMAKE_BUILD_TYPE} _build_type) +if(NOT ${VARIABLE_TRACKING} AND (${_build_type} STREQUAL "DEBUG" OR ${_build_type} STREQUAL "RELWITHDEBINFO") ) + message(WARNING "Variable tracking is disabled and build type includes debug symbols. This may reduce the ability to debug.") +endif() + # Precompiler/preprocessor flag configuration # Do this before configuring modules so that the flags are included option(BUILD_TESTING "Build the testing tree." OFF) diff --git a/glue-codes/fast-farm/CMakeLists.txt b/glue-codes/fast-farm/CMakeLists.txt index 40507fc30c..f404db7fab 100644 --- a/glue-codes/fast-farm/CMakeLists.txt +++ b/glue-codes/fast-farm/CMakeLists.txt @@ -44,12 +44,13 @@ set_property(TARGET FAST.Farm PROPERTY LINKER_LANGUAGE Fortran) string(TOUPPER ${CMAKE_Fortran_COMPILER_ID} _compiler_id) string(TOUPPER ${CMAKE_BUILD_TYPE} _build_type) -if (${_compiler_id} STREQUAL "GNU" AND ${_build_type} STREQUAL "RELEASE") +if (${_compiler_id} STREQUAL "GNU" AND NOT ${VARIABLE_TRACKING}) # With variable tracking enabled, the compile step frequently aborts on large modules and - # restarts with this option off. Disabling in Release mode avoids this problem when compiling with - # full optimizations, but leaves it enabled for RelWithDebInfo which adds both -O2 and -g flags. + # restarts with this option off. Disabling avoids this problem when compiling with + # full optimizations. However, variable tracking should be enabled when actively debugging + # for better runtime debugging output. # https://gcc.gnu.org/onlinedocs/gcc/Debugging-Options.html - set_source_files_properties(src/FAST_Farm_Types.f90 src/FASTWrapper_Types.f90 PROPERTIES COMPILE_FLAGS "-fno-var-tracking -fno-var-tracking-assignments") + set_source_files_properties(src/FAST_Farm_Types.f90 src/FASTWrapper_Types.f90 src/FASTWrapper.f90 PROPERTIES COMPILE_FLAGS "-fno-var-tracking -fno-var-tracking-assignments") endif() if (${_compiler_id} MATCHES "^INTEL" AND ${_build_type} STREQUAL "RELEASE" AND NOT WIN32) # Compilation hangs on FAST_Farm_Types.f90 with -O3 on linux (on some hardware) diff --git a/glue-codes/openfast/CMakeLists.txt b/glue-codes/openfast/CMakeLists.txt index b6de649e29..5fe7c78227 100644 --- a/glue-codes/openfast/CMakeLists.txt +++ b/glue-codes/openfast/CMakeLists.txt @@ -27,11 +27,11 @@ add_executable(openfast_cpp src/FAST_Prog.cpp src/FastLibAPI.cpp) target_link_libraries(openfast_cpp openfastlib) string(TOUPPER ${CMAKE_Fortran_COMPILER_ID} _compiler_id) -string(TOUPPER ${CMAKE_BUILD_TYPE} _build_type) -if (${_compiler_id} STREQUAL "GNU" AND ${_build_type} STREQUAL "RELEASE") +if (${_compiler_id} STREQUAL "GNU" AND NOT ${VARIABLE_TRACKING}) # With variable tracking enabled, the compile step frequently aborts on large modules and - # restarts with this option off. Disabling in Release mode avoids this problem when compiling with - # full optimizations, but leaves it enabled for RelWithDebInfo which adds both -O2 and -g flags. + # restarts with this option off. Disabling avoids this problem when compiling with + # full optimizations. However, variable tracking should be enabled when actively debugging + # for better runtime debugging output. # https://gcc.gnu.org/onlinedocs/gcc/Debugging-Options.html set_source_files_properties(src/FAST_Prog.f90 PROPERTIES COMPILE_FLAGS "-fno-var-tracking -fno-var-tracking-assignments") endif() diff --git a/modules/openfast-library/CMakeLists.txt b/modules/openfast-library/CMakeLists.txt index 1a54ec8ab4..f6496a8cb1 100644 --- a/modules/openfast-library/CMakeLists.txt +++ b/modules/openfast-library/CMakeLists.txt @@ -58,10 +58,11 @@ target_link_libraries(openfastlib openfast_postlib openfast_prelib scdataexlib f string(TOUPPER ${CMAKE_Fortran_COMPILER_ID} _compiler_id) string(TOUPPER ${CMAKE_BUILD_TYPE} _build_type) -if (${_compiler_id} STREQUAL "GNU" AND ${_build_type} STREQUAL "RELEASE") +if (${_compiler_id} STREQUAL "GNU" AND NOT ${VARIABLE_TRACKING}) # With variable tracking enabled, the compile step frequently aborts on large modules and - # restarts with this option off. Disabling in Release mode avoids this problem when compiling with - # full optimizations, but leaves it enabled for RelWithDebInfo which adds both -O2 and -g flags. + # restarts with this option off. Disabling avoids this problem when compiling with + # full optimizations. However, variable tracking should be enabled when actively debugging + # for better runtime debugging output. # https://gcc.gnu.org/onlinedocs/gcc/Debugging-Options.html set_source_files_properties( src/FAST_Subs.f90 src/FAST_Types.f90 src/FAST_Library.f90 From 7ed465503c0caf4f43e8809f8d974de97149a3b2 Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Thu, 9 Jun 2022 20:52:40 -0500 Subject: [PATCH 160/242] Add parallel jobs to improve Actions reg test time --- .github/workflows/automated-dev-tests.yml | 865 ++++++++++++++-------- reg_tests/CMakeLists.txt | 20 +- 2 files changed, 574 insertions(+), 311 deletions(-) diff --git a/.github/workflows/automated-dev-tests.yml b/.github/workflows/automated-dev-tests.yml index fd63edb92c..0137d1e01b 100644 --- a/.github/workflows/automated-dev-tests.yml +++ b/.github/workflows/automated-dev-tests.yml @@ -1,4 +1,4 @@ - + name: 'Development Pipeline' on: @@ -25,26 +25,31 @@ env: jobs: - regression-tests-aerodyn-driver: + ### BUILD JOBS + + + build-all-debug: + # Tests compiling in debug mode. + # TODO: NOT ENABLED Single precision compile checks that the precision types are correctly set. + # Debug more speeds up the build. runs-on: ubuntu-20.04 steps: - name: Checkout uses: actions/checkout@main with: submodules: recursive - - name: Setup Python - uses: actions/setup-python@v2 + uses: actions/setup-python@v3 with: - python-version: '3.7' + python-version: '3.9' + cache: 'pip' - name: Install dependencies run: | python -m pip install --upgrade pip - pip install numpy Bokeh==1.4 - - - name: Setup Workspace + pip install numpy Bokeh + - name: Setup workspace run: cmake -E make_directory ${{runner.workspace}}/openfast/build - - name: Configure Build + - name: Configure build working-directory: ${{runner.workspace}}/openfast/build run: | cmake \ @@ -52,47 +57,43 @@ jobs: -DCMAKE_Fortran_COMPILER:STRING=${{env.FORTRAN_COMPILER}} \ -DCMAKE_CXX_COMPILER:STRING=${{env.CXX_COMPILER}} \ -DCMAKE_C_COMPILER:STRING=${{env.C_COMPILER}} \ - -DCMAKE_BUILD_TYPE:STRING=RelWithDebInfo \ + -DCMAKE_BUILD_TYPE:STRING=DEBUG \ + -DBUILD_SHARED_LIBS:BOOL=OFF \ + -DVARIABLE_TRACKING=OFF \ -DBUILD_TESTING:BOOL=ON \ -DCTEST_PLOT_ERRORS:BOOL=ON \ ${GITHUB_WORKSPACE} - - name: Build AeroDyn Driver + # -DDOUBLE_PRECISION=OFF \ + - name: Build all working-directory: ${{runner.workspace}}/openfast/build - run: cmake --build . --target aerodyn_driver -- -j ${{env.NUM_PROCS}} - - - name: Run AeroDyn tests - uses: ./.github/actions/tests-module-aerodyn + run: | + cmake --build . --target all -- -j ${{env.NUM_PROCS}} + - name: Cache the workspace + uses: actions/cache@v3.0.4 with: - test-target: regression + path: ${{runner.workspace}} + key: build-all-debug-${{ github.sha }} - - name: Failing test artifacts - uses: actions/upload-artifact@v2 - if: failure() - with: - name: regression-tests-aerodyn-module - path: | - ${{runner.workspace}}/openfast/build/reg_tests/modules - regression-tests-release: + build-drivers-release: runs-on: ubuntu-20.04 steps: - name: Checkout uses: actions/checkout@main with: submodules: recursive - - name: Setup Python - uses: actions/setup-python@v2 + uses: actions/setup-python@v3 with: - python-version: '3.7' + python-version: '3.9' + cache: 'pip' - name: Install dependencies run: | python -m pip install --upgrade pip - pip install numpy Bokeh==1.4 - - - name: Setup Workspace + pip install numpy Bokeh + - name: Setup workspace run: cmake -E make_directory ${{runner.workspace}}/openfast/build - - name: Configure Build + - name: Configure build working-directory: ${{runner.workspace}}/openfast/build run: | cmake \ @@ -101,403 +102,649 @@ jobs: -DCMAKE_CXX_COMPILER:STRING=${{env.CXX_COMPILER}} \ -DCMAKE_C_COMPILER:STRING=${{env.C_COMPILER}} \ -DCMAKE_BUILD_TYPE:STRING=RelWithDebInfo \ + -DVARIABLE_TRACKING=OFF \ -DBUILD_TESTING:BOOL=ON \ -DCTEST_PLOT_ERRORS:BOOL=ON \ - -DVARIABLE_TRACKING=OFF \ ${GITHUB_WORKSPACE} - - name: Build OpenFAST - # if: contains(github.event.head_commit.message, 'Action - Test All') || contains(github.event.pull_request.labels.*.name, 'Action - Test All') + - name: Build module drivers working-directory: ${{runner.workspace}}/openfast/build - run: cmake --build . --target install -- -j ${{env.NUM_PROCS}} - - # SubDyn has only regression tests - - name: Run SubDyn tests - uses: ./.github/actions/tests-module-subdyn - # - name: Run AeroDyn tests - # uses: ./.github/actions/tests-module-aerodyn - # with: - # test-target: regression - # HydroDyn has only regression tests - - name: Run HydroDyn tests - uses: ./.github/actions/tests-module-hydrodyn - - name: Run InflowWind tests - uses: ./.github/actions/tests-module-inflowwind - with: - test-target: regression - - name: Run BeamDyn tests - uses: ./.github/actions/tests-module-beamdyn + run: | + cmake --build . --target regression_test_module_drivers -- -j ${{env.NUM_PROCS}} + - name: Cache the workspace + uses: actions/cache@v3.0.4 with: - test-target: regression - - name: Run OpenFAST tests - # if: contains(github.event.head_commit.message, 'Action - Test All') || contains(github.event.pull_request.labels.*.name, 'Action - Test All') - uses: ./.github/actions/tests-gluecode-openfast + path: ${{runner.workspace}} + key: build-drivers-release-${{ github.sha }} - - name: Failing test artifacts - uses: actions/upload-artifact@v2 - if: failure() - with: - name: regression-tests-release - path: | - ${{runner.workspace}}/openfast/build/reg_tests/modules - ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast - !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/5MW_Baseline - !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AOC - !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AWT27 - !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/SWRT - !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/UAE_VI - !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/WP_Baseline - regression-tests-debug: - runs-on: ${{ matrix.os }} - strategy: - fail-fast: false - matrix: - include: - - os: macOS-11 - FORTRAN_COMPILER: gfortran-11 - install_deps: brew install gcovr - - os: ubuntu-20.04 - FORTRAN_COMPILER: gfortran-10 - install_deps: sudo apt-get update && sudo apt-get install -y gcovr - - name: regression-test-debug-${{ matrix.os }}-${{ matrix.FORTRAN_COMPILER }} + build-postlib-release: + runs-on: ubuntu-20.04 steps: - name: Checkout uses: actions/checkout@main with: submodules: recursive - - name: Setup Python - uses: actions/setup-python@v2 + uses: actions/setup-python@v3 with: - python-version: '3.7' - - name: Install Dependencies + python-version: '3.9' + cache: 'pip' + - name: Install dependencies run: | python -m pip install --upgrade pip - pip install numpy Bokeh==1.4 - ${{matrix.install_deps}} - - - name: Setup Workspace + pip install numpy Bokeh + sudo apt-get update -y + sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev # gcovr + - name: Setup workspace run: cmake -E make_directory ${{runner.workspace}}/openfast/build - - name: Configure Build + - name: Configure build working-directory: ${{runner.workspace}}/openfast/build run: | cmake \ -DCMAKE_INSTALL_PREFIX:PATH=${{runner.workspace}}/openfast/install \ - -DCMAKE_Fortran_COMPILER:STRING=${{matrix.FORTRAN_COMPILER}} \ + -DCMAKE_Fortran_COMPILER:STRING=${{env.FORTRAN_COMPILER}} \ -DCMAKE_CXX_COMPILER:STRING=${{env.CXX_COMPILER}} \ -DCMAKE_C_COMPILER:STRING=${{env.C_COMPILER}} \ - -DCMAKE_BUILD_TYPE:STRING=Debug \ + -DCMAKE_BUILD_TYPE:STRING=RELWITHDEBINFO \ + -DOPENMP:BOOL=ON \ + -DDOUBLE_PRECISION=ON \ + -DVARIABLE_TRACKING=OFF \ + -DBUILD_FASTFARM:BOOL=ON \ + -DBUILD_OPENFAST_CPP_API:BOOL=ON \ + -DBUILD_SHARED_LIBS:BOOL=OFF \ -DBUILD_TESTING:BOOL=ON \ -DCTEST_PLOT_ERRORS:BOOL=ON \ ${GITHUB_WORKSPACE} + - name: Build openfast-postlib + working-directory: ${{runner.workspace}}/openfast/build + run: cmake --build . --target openfast_postlib -- -j ${{env.NUM_PROCS}} + - name: Cache the workspace + uses: actions/cache@v3.0.4 + with: + path: ${{runner.workspace}} + key: build-postlib-release-${{ github.sha }} + - - name: Build Drivers + build-interfaces-release: + runs-on: ubuntu-20.04 + needs: build-postlib-release + steps: + - name: Cache the workspace + uses: actions/cache@v3.0.4 + with: + path: ${{runner.workspace}} + key: build-postlib-release-${{ github.sha }} + - name: Setup Python + uses: actions/setup-python@v3 + with: + python-version: '3.9' + cache: 'pip' + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install numpy Bokeh + sudo apt-get update -y + sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev + - name: Build OpenFAST C-Interfaces working-directory: ${{runner.workspace}}/openfast/build run: | - cmake --build . --target aerodyn_driver -- -j ${{env.NUM_PROCS}} - cmake --build . --target beamdyn_driver -- -j ${{env.NUM_PROCS}} - cmake --build . --target hydrodyn_driver -- -j ${{env.NUM_PROCS}} - cmake --build . --target inflowwind_driver -- -j ${{env.NUM_PROCS}} - cmake --build . --target subdyn_driver -- -j ${{env.NUM_PROCS}} + cmake --build . --target openfastlib -- -j ${{env.NUM_PROCS}} + cmake --build . --target openfast_cpp -- -j ${{env.NUM_PROCS}} + cmake --build . --target openfastcpp -- -j ${{env.NUM_PROCS}} + cmake --build . --target ifw_c_binding -- -j ${{env.NUM_PROCS}} + cmake --build . --target regression_test_controllers -- -j ${{env.NUM_PROCS}} + - name: Cache the workspace + uses: actions/cache@v3.0.4 + with: + path: ${{runner.workspace}} + key: build-interfaces-release-${{ github.sha }} - - name: Run SubDyn tests - uses: ./.github/actions/tests-module-subdyn - # - name: Run AeroDyn tests - # uses: ./.github/actions/tests-module-aerodyn - # with: - # test-target: regression - - name: Run HydroDyn tests - uses: ./.github/actions/tests-module-hydrodyn - - name: Run InflowWind tests - uses: ./.github/actions/tests-module-inflowwind + + build-openfast-release: + runs-on: ubuntu-20.04 + needs: build-postlib-release + steps: + - name: Cache the workspace + uses: actions/cache@v3.0.4 + with: + path: ${{runner.workspace}} + key: build-postlib-release-${{ github.sha }} + - name: Setup Python + uses: actions/setup-python@v3 + with: + python-version: '3.9' + cache: 'pip' + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install numpy Bokeh + sudo apt-get update -y + sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev + - name: Build OpenFAST glue-code + working-directory: ${{runner.workspace}}/openfast/build + run: | + cmake --build . --target openfast -- -j ${{env.NUM_PROCS}} + - name: Cache the workspace + uses: actions/cache@v3.0.4 + with: + path: ${{runner.workspace}} + key: build-openfast-release-${{ github.sha }} + + + build-fastfarm-release: + runs-on: ubuntu-20.04 + needs: build-postlib-release + steps: + - name: Cache the workspace + uses: actions/cache@v3.0.4 + with: + path: ${{runner.workspace}} + key: build-postlib-release-${{ github.sha }} + - name: Setup Python + uses: actions/setup-python@v3 + with: + python-version: '3.9' + cache: 'pip' + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install numpy Bokeh + sudo apt-get update -y + sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev + - name: Build FAST.Farm + working-directory: ${{runner.workspace}}/openfast/build + run: | + cmake --build . --target FAST.Farm -- -j ${{env.NUM_PROCS}} + - name: Upload build + uses: actions/upload-artifact@main + with: + name: fastfarm-build + path: build-fastfarm-${{ github.sha }}.tar + - name: Cache the workspace + uses: actions/cache@v3.0.4 + with: + path: ${{runner.workspace}} + key: build-fastfarm-release-${{ github.sha }} + + + ### TEST JOBS + + regression-tests-module-drivers: + runs-on: ubuntu-20.04 + needs: build-drivers-release + steps: + - name: Cache the workspace + uses: actions/cache@v3.0.4 + with: + path: ${{runner.workspace}} + key: build-drivers-release-${{ github.sha }} + - name: Setup Python + uses: actions/setup-python@v3 + with: + python-version: '3.9' + cache: 'pip' + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install numpy Bokeh + sudo apt-get update -y + sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev + - name: Run AeroDyn tests + uses: ./.github/actions/tests-module-aerodyn with: test-target: regression - name: Run BeamDyn tests uses: ./.github/actions/tests-module-beamdyn with: test-target: regression - - # Disabled Codecov since the dashboard and GitHub comments were buggy, - # but it may be useful to post the gcov coverage reports to GitHub Actions - # artifacts. - # Note: if reenabling Codecov, the reports must be in xml format not html. - # - name: Generate coverage report - # working-directory: ${{runner.workspace}}/openfast/build - # run: | - # find . -type f -name '*.gcno' -not -path "**tests**" -exec ${{env.GCOV_EXE}} -pb {} + - # cd .. - # gcovr -g -k -r . --html --html-details -o regressioncov.html # -v - # # cp `find . -name *.gcno` . - # # cp `find . -name *.gcda` . - # # ${{env.GCOV_EXE}} -b -l -p -c *.gcno - # - name: Success artifacts - # uses: actions/upload-artifact@v2 - # if: success() - # with: - # name: regression-tests-debug - # path: | - # ${{runner.workspace}}/openfast/regressioncov.html + - name: Run HydroDyn tests + uses: ./.github/actions/tests-module-hydrodyn + - name: Run InflowWind tests + uses: ./.github/actions/tests-module-inflowwind + with: + test-target: regression + - name: Run SubDyn tests + uses: ./.github/actions/tests-module-subdyn - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() with: - name: regression-tests-debug + name: regression-tests-aerodyn-module path: | ${{runner.workspace}}/openfast/build/reg_tests/modules - fastfarm-regression-test: + + regression-tests-interfaces: runs-on: ubuntu-20.04 + needs: build-interfaces-release steps: - - name: Checkout - uses: actions/checkout@main + - name: Cache the workspace + uses: actions/cache@v3.0.4 with: - submodules: recursive - + path: ${{runner.workspace}} + key: build-interfaces-release-${{ github.sha }} - name: Setup Python - uses: actions/setup-python@v2 + uses: actions/setup-python@v3 with: - python-version: '3.7' + python-version: '3.9' + cache: 'pip' - name: Install dependencies run: | python -m pip install --upgrade pip - pip install numpy Bokeh==1.4 + pip install numpy Bokeh + sudo apt-get update -y + sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev + - name: Run Interface / API tests + working-directory: ${{runner.workspace}}/openfast/build + run: | + ctest -VV -L "cpp|python" -j ${{env.NUM_PROCS}} + - name: Failing test artifacts + uses: actions/upload-artifact@v2 + if: failure() + with: + name: c-interface-reg-tests + path: | + ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast-cpp + ${{runner.workspace}}/openfast/build/reg_tests/modules/inflowwind + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast-cpp/5MW_Baseline - - name: Setup Workspace - run: cmake -E make_directory ${{runner.workspace}}/openfast/build - - name: Configure Build + + regression-tests-openfast-5MW: + runs-on: ubuntu-20.04 + needs: build-openfast-release + steps: + - name: Cache the workspace + uses: actions/cache@v3.0.4 + with: + path: ${{runner.workspace}} + key: build-openfast-release-${{ github.sha }} + - name: Setup Python + uses: actions/setup-python@v3 + with: + python-version: '3.9' + cache: 'pip' + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install numpy Bokeh + sudo apt-get update -y + sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev + - name: Configure Tests working-directory: ${{runner.workspace}}/openfast/build run: | - cmake \ - -DCMAKE_INSTALL_PREFIX:PATH=${{runner.workspace}}/openfast/install \ - -DCMAKE_Fortran_COMPILER:STRING=${{env.FORTRAN_COMPILER}} \ - -DOPENMP:BOOL=ON \ - -DBUILD_FASTFARM:BOOL=ON \ - -DCMAKE_BUILD_TYPE:STRING=RelWithDebInfo \ - -DBUILD_TESTING:BOOL=ON \ - -DCTEST_PLOT_ERRORS:BOOL=ON \ - -DVARIABLE_TRACKING=OFF \ - ${GITHUB_WORKSPACE} - - name: Build FAST.Farm - # if: contains(github.event.head_commit.message, 'Action - Test All') || contains(github.event.pull_request.labels.*.name, 'Action - Test All') + cmake --build . --target regression_test_controllers -- -j ${{env.NUM_PROCS}} + - name: Run 5MW tests working-directory: ${{runner.workspace}}/openfast/build run: | - cmake --build . --target FAST.Farm -- -j ${{env.NUM_PROCS}} - cmake --build . --target regression_tests -- -j ${{env.NUM_PROCS}} + ctest -VV -j8 -L openfast -LE "cpp|linear|python" -R 5MW_ -E "5MW_Land_BD_DLL_WTurb|5MW_OC3Mnpl_DLL_WTurb_WavesIrr|5MW_OC4Semi_WSt_WavesWN|5MW_OC3Trpd_DLL_WSt_WavesReg|5MW_OC4Jckt_DLL_WTurb_WavesIrr_MGrowth" + - name: Failing test artifacts + uses: actions/upload-artifact@v2 + if: failure() + with: + name: regression-tests-5MW + path: | + ${{runner.workspace}}/openfast/build/reg_tests/modules + ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/5MW_Baseline + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AOC + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AWT27 + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/SWRT + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/UAE_VI + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/WP_Baseline - - name: Run FAST.Farm tests + + regression-tests-openfast-5MW_OC4Semi_WSt_WavesWN: + runs-on: ubuntu-20.04 + needs: build-openfast-release + steps: + - name: Cache the workspace + uses: actions/cache@v3.0.4 + with: + path: ${{runner.workspace}} + key: build-openfast-release-${{ github.sha }} + - name: Setup Python + uses: actions/setup-python@v3 + with: + python-version: '3.9' + cache: 'pip' + - name: Install dependencies run: | - ctest -VV -L fastfarm -j ${{env.NUM_PROCS}} + python -m pip install --upgrade pip + pip install numpy Bokeh + sudo apt-get update -y + sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev + - name: Configure Tests working-directory: ${{runner.workspace}}/openfast/build - shell: bash - + run: | + cmake --build . --target regression_test_controllers -- -j ${{env.NUM_PROCS}} + - name: Run 5MW tests + working-directory: ${{runner.workspace}}/openfast/build + run: | + ctest -VV -L openfast -LE "linear|python" -R 5MW_OC4Semi_WSt_WavesWN - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() with: - name: test-results + name: regression-tests-5MW_OC4Semi_WSt_WavesWN path: | - ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/fastfarm + ${{runner.workspace}}/openfast/build/reg_tests/modules + ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/5MW_Baseline + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AOC + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AWT27 + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/SWRT + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/UAE_VI + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/WP_Baseline + - unit-test: + regression-tests-openfast-5MW_OC3Mnpl_DLL_WTurb_WavesIrr: runs-on: ubuntu-20.04 + needs: build-openfast-release steps: - - name: Checkout - uses: actions/checkout@main + - name: Cache the workspace + uses: actions/cache@v3.0.4 with: - submodules: recursive - + path: ${{runner.workspace}} + key: build-openfast-release-${{ github.sha }} - name: Setup Python - uses: actions/setup-python@v2 + uses: actions/setup-python@v3 with: - python-version: '3.7' + python-version: '3.9' + cache: 'pip' - name: Install dependencies run: | python -m pip install --upgrade pip - pip install numpy Bokeh==1.4 - + pip install numpy Bokeh sudo apt-get update -y - sudo apt-get install -y gcovr - - - name: Setup Workspace - run: cmake -E make_directory ${{runner.workspace}}/openfast/build - - name: Configure Build + sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev + - name: Configure Tests working-directory: ${{runner.workspace}}/openfast/build run: | - cmake \ - -DCMAKE_INSTALL_PREFIX:PATH=${{runner.workspace}}/openfast/install \ - -DCMAKE_Fortran_COMPILER:STRING=${{env.FORTRAN_COMPILER}} \ - -DCMAKE_CXX_COMPILER:STRING=${{env.CXX_COMPILER}} \ - -DCMAKE_C_COMPILER:STRING=${{env.C_COMPILER}} \ - -DCMAKE_BUILD_TYPE:STRING=Debug \ - -DBUILD_TESTING:BOOL=ON \ - ${GITHUB_WORKSPACE} - - - name: Build unit tests + cmake --build . --target regression_test_controllers -- -j ${{env.NUM_PROCS}} + - name: Run 5MW tests working-directory: ${{runner.workspace}}/openfast/build - run: cmake --build . --target unit_tests -- -j ${{env.NUM_PROCS}} + run: | + ctest -VV -L openfast -LE "linear|python" -R 5MW_OC3Mnpl_DLL_WTurb_WavesIrr + - name: Failing test artifacts + uses: actions/upload-artifact@v2 + if: failure() + with: + name: regression-tests-5MW_OC3Mnpl_DLL_WTurb_WavesIrr + path: | + ${{runner.workspace}}/openfast/build/reg_tests/modules + ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/5MW_Baseline + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AOC + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AWT27 + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/SWRT + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/UAE_VI + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/WP_Baseline - # NWTC Library has only unit tests - - name: Run NWTC Library tests - uses: ./.github/actions/tests-module-nwtclibrary - # VersionInfo has only unit tests - - name: Run VersionInfo tests - uses: ./.github/actions/tests-module-version - - name: Run AeroDyn tests - uses: ./.github/actions/tests-module-aerodyn + + regression-tests-openfast-5MW_OC4Jckt_DLL_WTurb_WavesIrr_MGrowth: + runs-on: ubuntu-20.04 + needs: build-openfast-release + steps: + - name: Cache the workspace + uses: actions/cache@v3.0.4 with: - test-target: unit - - name: Run BeamDyn tests - uses: ./.github/actions/tests-module-beamdyn + path: ${{runner.workspace}} + key: build-openfast-release-${{ github.sha }} + - name: Setup Python + uses: actions/setup-python@v3 with: - test-target: unit - - name: Run InflowWind tests - uses: ./.github/actions/tests-module-inflowwind + python-version: '3.9' + cache: 'pip' + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install numpy Bokeh + sudo apt-get update -y + sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev + - name: Configure Tests + working-directory: ${{runner.workspace}}/openfast/build + run: | + cmake --build . --target regression_test_controllers -- -j ${{env.NUM_PROCS}} + - name: Run 5MW tests + working-directory: ${{runner.workspace}}/openfast/build + run: | + ctest -VV -L openfast -LE "linear|python" -R 5MW_OC4Jckt_DLL_WTurb_WavesIrr_MGrowth + - name: Failing test artifacts + uses: actions/upload-artifact@v2 + if: failure() with: - test-target: unit + name: regression-tests-5MW_OC4Jckt_DLL_WTurb_WavesIrr_MGrowth + path: | + ${{runner.workspace}}/openfast/build/reg_tests/modules + ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/5MW_Baseline + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AOC + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AWT27 + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/SWRT + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/UAE_VI + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/WP_Baseline - # Disabled Codecov since the dashboard and GitHub comments were buggy, - # but it may be useful to post the gcov coverage reports to GitHub Actions - # artifacts. - # Note: if reenabling Codecov, the reports must be in xml format not html. - # - name: Generate coverage report - # working-directory: ${{runner.workspace}}/openfast/build - # run: | - # find . -type f -name '*.gcno' -not -path "**tests**" -exec ${{env.GCOV_EXE}} -pb {} + - # cd .. - # gcovr -g -k -r . --html --html-details unitcov.html - # - name: Success artifacts - # uses: actions/upload-artifact@v2 - # if: success() - # with: - # name: unit-tests - # path: | - # ${{runner.workspace}}/openfast/unitcov.html + regression-tests-openfast-5MW_OC3Trpd_DLL_WSt_WavesReg: + runs-on: ubuntu-20.04 + needs: build-openfast-release + steps: + - name: Cache the workspace + uses: actions/cache@v3.0.4 + with: + path: ${{runner.workspace}} + key: build-openfast-release-${{ github.sha }} + - name: Setup Python + uses: actions/setup-python@v3 + with: + python-version: '3.9' + cache: 'pip' + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install numpy Bokeh + sudo apt-get update -y + sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev + - name: Configure Tests + working-directory: ${{runner.workspace}}/openfast/build + run: | + cmake --build . --target regression_test_controllers -- -j ${{env.NUM_PROCS}} + - name: Run 5MW tests + working-directory: ${{runner.workspace}}/openfast/build + run: | + ctest -VV -L openfast -LE "linear|python" -R 5MW_OC3Trpd_DLL_WSt_WavesReg - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() with: - name: unit-tests + name: regression-tests-5MW_OC3Trpd_DLL_WSt_WavesReg path: | - ${{runner.workspace}}/openfast/build/unit_tests + ${{runner.workspace}}/openfast/build/reg_tests/modules + ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/5MW_Baseline + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AOC + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AWT27 + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/SWRT + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/UAE_VI + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/WP_Baseline - compile-all-single-precision: - # Test if single precision compile completes. - # Compiles all targets excluding tests. - # Run with the OpenFAST registry generating the types files. - # Do not run the test suite. + regression-tests-openfast-5MW_Land_BD_DLL_WTurb: runs-on: ubuntu-20.04 + needs: build-openfast-release steps: - - name: Checkout - uses: actions/checkout@main + - name: Cache the workspace + uses: actions/cache@v3.0.4 with: - submodules: recursive - - name: Setup - run: cmake -E make_directory ${{runner.workspace}}/openfast/build - - name: Configure - working-directory: ${{runner.workspace}}/openfast/build + path: ${{runner.workspace}} + key: build-openfast-release-${{ github.sha }} + - name: Setup Python + uses: actions/setup-python@v3 + with: + python-version: '3.9' + cache: 'pip' + - name: Install dependencies run: | - cmake \ - -DCMAKE_INSTALL_PREFIX:PATH=${{runner.workspace}}/openfast/install \ - -DCMAKE_Fortran_COMPILER:STRING=${{env.FORTRAN_COMPILER}} \ - -DCMAKE_BUILD_TYPE:STRING=Debug \ - -DDOUBLE_PRECISION:BOOL=OFF \ - -DGENERATE_TYPES:BOOL=ON \ - ${GITHUB_WORKSPACE} - - name: Build all + python -m pip install --upgrade pip + pip install numpy Bokeh + sudo apt-get update -y + sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev + - name: Configure Tests working-directory: ${{runner.workspace}}/openfast/build - run: cmake --build . --target all -- -j ${{env.NUM_PROCS}} - - name: Test + run: | + cmake --build . --target regression_test_controllers -- -j ${{env.NUM_PROCS}} + - name: Run 5MW tests working-directory: ${{runner.workspace}}/openfast/build - run: ./glue-codes/openfast/openfast -v + run: | + ctest -VV -L openfast -LE "linear|python" -R 5MW_Land_BD_DLL_WTurb + - name: Failing test artifacts + uses: actions/upload-artifact@v2 + if: failure() + with: + name: regression-tests-5MW_Land_BD_DLL_WTurb + path: | + ${{runner.workspace}}/openfast/build/reg_tests/modules + ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/5MW_Baseline + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AOC + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AWT27 + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/SWRT + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/UAE_VI + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/WP_Baseline + - interface-tests: + regression-tests-openfast-not-5MW: runs-on: ubuntu-20.04 + needs: build-openfast-release steps: - - name: Checkout - uses: actions/checkout@main + - name: Cache the workspace + uses: actions/cache@v3.0.4 with: - submodules: recursive - + path: ${{runner.workspace}} + key: build-openfast-release-${{ github.sha }} - name: Setup Python - uses: actions/setup-python@v2 + uses: actions/setup-python@v3 with: - python-version: '3.7' + python-version: '3.9' + cache: 'pip' - name: Install dependencies run: | python -m pip install --upgrade pip - pip install numpy Bokeh==1.4 - + pip install numpy Bokeh sudo apt-get update -y - sudo apt-get install -y gcovr sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev - - - name: Setup Workspace - run: cmake -E make_directory ${{runner.workspace}}/openfast/build - - name: Configure Build + - name: Configure Tests working-directory: ${{runner.workspace}}/openfast/build run: | - cmake \ - -DCMAKE_INSTALL_PREFIX:PATH=${{runner.workspace}}/openfast/install \ - -DCMAKE_Fortran_COMPILER:STRING=${{env.FORTRAN_COMPILER}} \ - -DCMAKE_CXX_COMPILER:STRING=${{env.CXX_COMPILER}} \ - -DCMAKE_C_COMPILER:STRING=${{env.C_COMPILER}} \ - -DCMAKE_BUILD_TYPE:STRING=RelWithDebInfo \ - -DBUILD_OPENFAST_CPP_API:BOOL=ON \ - -DBUILD_SHARED_LIBS:BOOL=ON \ - -DBUILD_TESTING:BOOL=ON \ - -DCTEST_PLOT_ERRORS:BOOL=ON \ - -DVARIABLE_TRACKING=OFF \ - ${GITHUB_WORKSPACE} - - - name: Build OpenFAST C-Interfaces + cmake --build . --target regression_test_controllers -- -j ${{env.NUM_PROCS}} + - name: Run non-5MW tests working-directory: ${{runner.workspace}}/openfast/build run: | - cmake --build . --target openfastlib -- -j ${{env.NUM_PROCS}} - cmake --build . --target openfast_cpp -- -j ${{env.NUM_PROCS}} - cmake --build . --target openfastcpp -- -j ${{env.NUM_PROCS}} - cmake --build . --target hydrodyn_c_binding -- -j ${{env.NUM_PROCS}} - cmake --build . --target ifw_c_binding -- -j ${{env.NUM_PROCS}} - cmake --build . --target regression_tests -- -j ${{env.NUM_PROCS}} + ctest -VV -j8 -E 5MW_ -L openfast -LE "python|cpp" + - name: Failing test artifacts + uses: actions/upload-artifact@v2 + if: failure() + with: + name: regression-tests-not-5MW + path: | + ${{runner.workspace}}/openfast/build/reg_tests/modules + ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/5MW_Baseline + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AOC + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AWT27 + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/SWRT + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/UAE_VI + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/WP_Baseline + - - name: Run C++ API tests + regression-tests-fastfarm: + runs-on: ubuntu-20.04 + needs: build-fastfarm-release + steps: + - name: Cache the workspace + uses: actions/cache@v3.0.4 + with: + path: ${{runner.workspace}} + key: build-fastfarm-release-${{ github.sha }} + - name: Setup Python + uses: actions/setup-python@v3 + with: + python-version: '3.9' + cache: 'pip' + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install numpy Bokeh + sudo apt-get update -y + sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev + - name: Configure Tests working-directory: ${{runner.workspace}}/openfast/build run: | - ctest -VV -L cpp - - - name: Run Python API tests + cmake --build . --target regression_test_controllers -- -j ${{env.NUM_PROCS}} + - name: Run FAST.Farm tests working-directory: ${{runner.workspace}}/openfast/build + shell: bash run: | - ctest -VV -L python - - # Disabled Codecov since the dashboard and GitHub comments were buggy, - # but it may be useful to post the gcov coverage reports to GitHub Actions - # artifacts. - # Note: if reenabling Codecov, the reports must be in xml format not html. - # - name: Generate coverage report - # working-directory: ${{runner.workspace}}/openfast/build - # run: | - # find . -type f -name '*.gcno' -not -path "**tests**" -exec ${{env.GCOV_EXE}} -pb {} + - # cd .. - # gcovr -g -k -r . --html --html-details regressioncov.html - # - name: Success artifacts - # uses: actions/upload-artifact@v2 - # if: success() - # with: - # name: c-interface-reg-tests - # path: | - # ${{runner.workspace}}/openfast/regressioncov.html + ctest -VV -L fastfarm -j ${{env.NUM_PROCS}} + - name: Failing test artifacts + uses: actions/upload-artifact@v2 + if: failure() + with: + name: fastfarm-tests-release + path: | + ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/fastfarm + + module-tests-debug: + runs-on: ubuntu-20.04 + needs: build-all-debug + steps: + - name: Cache the workspace + uses: actions/cache@v3.0.4 + with: + path: ${{runner.workspace}} + key: build-all-debug-${{ github.sha }} + - name: Setup Python + uses: actions/setup-python@v3 + with: + python-version: '3.9' + cache: 'pip' + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install numpy Bokeh + sudo apt-get update -y + sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev + - name: Configure Tests + working-directory: ${{runner.workspace}}/openfast/build + run: | + cmake \ + -DBUILD_TESTING:BOOL=ON \ + -DCTEST_PLOT_ERRORS:BOOL=ON \ + ${GITHUB_WORKSPACE} + cmake --build . --target regression_test_controllers -- -j ${{env.NUM_PROCS}} + - name: Run AeroDyn tests + uses: ./.github/actions/tests-module-aerodyn + with: + # Don't run regression tests here since they currently fail inconsistently + test-target: unit + - name: Run BeamDyn tests + uses: ./.github/actions/tests-module-beamdyn + - name: Run HydroDyn tests + uses: ./.github/actions/tests-module-hydrodyn + - name: Run InflowWind tests + uses: ./.github/actions/tests-module-inflowwind + - name: Run NWTC Library tests + uses: ./.github/actions/tests-module-nwtclibrary + - name: Run SubDyn tests + uses: ./.github/actions/tests-module-subdyn + - name: Run VersionInfo tests + uses: ./.github/actions/tests-module-version - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() with: - name: interface-reg-tests + name: debug-tests-release path: | - ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast-cpp - ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/python - ${{runner.workspace}}/openfast/build/reg_tests/modules/hydrodyn - ${{runner.workspace}}/openfast/build/reg_tests/modules/inflowwind - !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast-cpp/5MW_Baseline + ${{runner.workspace}}/openfast/build/reg_tests/modules + ${{runner.workspace}}/openfast/build/unit_tests diff --git a/reg_tests/CMakeLists.txt b/reg_tests/CMakeLists.txt index 3587cd56d2..38451a4247 100644 --- a/reg_tests/CMakeLists.txt +++ b/reg_tests/CMakeLists.txt @@ -163,9 +163,8 @@ add_custom_command( ) add_custom_target( - regression_tests + regression_test_controllers DEPENDS - openfast "${of_dest}/DISCON.dll" "${of_dest}/DISCON_ITIBarge.dll" "${of_dest}/DISCON_OC3Hywind.dll" @@ -176,3 +175,20 @@ add_custom_target( "${ff_dest}/DISCON_WT2.dll" "${ff_dest}/DISCON_WT3.dll" ) + +add_custom_target( + regression_tests + DEPENDS + openfast + regression_test_controllers +) + +add_custom_target( + regression_test_module_drivers + DEPENDS + aerodyn_driver + beamdyn_driver + hydrodyn_driver + inflowwind_driver + subdyn_driver +) \ No newline at end of file From 5a8ec2669aed688d99a76315783f69a5a0cac1e6 Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Sun, 10 Jul 2022 10:25:44 -0500 Subject: [PATCH 161/242] Configure OpenFAST Library as shared library --- modules/openfast-library/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/modules/openfast-library/CMakeLists.txt b/modules/openfast-library/CMakeLists.txt index f6496a8cb1..b838662c2d 100644 --- a/modules/openfast-library/CMakeLists.txt +++ b/modules/openfast-library/CMakeLists.txt @@ -53,8 +53,9 @@ add_library(openfast_postlib ) target_link_libraries(openfast_postlib openfast_prelib scdataexlib foamfastlib versioninfolib) -add_library(openfastlib src/FAST_Library.f90) +add_library(openfastlib SHARED src/FAST_Library.f90) target_link_libraries(openfastlib openfast_postlib openfast_prelib scdataexlib foamfastlib) +set_property(TARGET openfastlib PROPERTY LINKER_LANGUAGE Fortran) string(TOUPPER ${CMAKE_Fortran_COMPILER_ID} _compiler_id) string(TOUPPER ${CMAKE_BUILD_TYPE} _build_type) From c4d2ca8f48f2b930407f4f54e7bd8b271af59326 Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Mon, 11 Jul 2022 17:22:03 -0500 Subject: [PATCH 162/242] Add fPIC flag for MAP++ This is required on unix to link with openfastlib shared library. For the difference on fpic and fPIC, see https://stackoverflow.com/questions/3544035/what-is-the-difference-between-fpic-and-fpic-gcc-parameters https://tldp.org/HOWTO/Program-Library-HOWTO/shared-libraries.html https://gcc.gnu.org/onlinedocs/gcc/Code-Gen-Options.html --- modules/map/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/modules/map/CMakeLists.txt b/modules/map/CMakeLists.txt index 2bf1de3f4c..2589d5a893 100644 --- a/modules/map/CMakeLists.txt +++ b/modules/map/CMakeLists.txt @@ -19,6 +19,11 @@ if(WIN32 OR CYGWIN OR MINGW) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAP_DLL_EXPORTS -DCMINPACK_NO_DLL -DNDEBUG -D_WINDOWS -D_USRDLL") endif() +if (${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU" AND NOT WIN32) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") +endif() + if (GENERATE_TYPES) generate_f90_types(src/MAP_Fortran_Registry.txt ${CMAKE_CURRENT_LIST_DIR}/src/MAP_Fortran_Types.f90 -noextrap) generate_f90_types(src/MAP_Registry.txt ${CMAKE_CURRENT_LIST_DIR}/src/MAP_Types.f90 -ccode) From 7693fa1fe86785d94dbc1a06c763e4285acdb552 Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Thu, 14 Jul 2022 09:18:37 -0500 Subject: [PATCH 163/242] Add a branch for linearization tests --- .github/workflows/automated-dev-tests.yml | 56 ++++++++++++++++++++--- 1 file changed, 50 insertions(+), 6 deletions(-) diff --git a/.github/workflows/automated-dev-tests.yml b/.github/workflows/automated-dev-tests.yml index 0137d1e01b..a2bdbd68a8 100644 --- a/.github/workflows/automated-dev-tests.yml +++ b/.github/workflows/automated-dev-tests.yml @@ -420,7 +420,7 @@ jobs: - name: Run 5MW tests working-directory: ${{runner.workspace}}/openfast/build run: | - ctest -VV -L openfast -LE "linear|python" -R 5MW_OC4Semi_WSt_WavesWN + ctest -VV -L openfast -LE "cpp|linear|python" -R 5MW_OC4Semi_WSt_WavesWN - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() @@ -464,7 +464,7 @@ jobs: - name: Run 5MW tests working-directory: ${{runner.workspace}}/openfast/build run: | - ctest -VV -L openfast -LE "linear|python" -R 5MW_OC3Mnpl_DLL_WTurb_WavesIrr + ctest -VV -L openfast -LE "cpp|linear|python" -R 5MW_OC3Mnpl_DLL_WTurb_WavesIrr - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() @@ -508,7 +508,7 @@ jobs: - name: Run 5MW tests working-directory: ${{runner.workspace}}/openfast/build run: | - ctest -VV -L openfast -LE "linear|python" -R 5MW_OC4Jckt_DLL_WTurb_WavesIrr_MGrowth + ctest -VV -L openfast -LE "cpp|linear|python" -R 5MW_OC4Jckt_DLL_WTurb_WavesIrr_MGrowth - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() @@ -552,7 +552,7 @@ jobs: - name: Run 5MW tests working-directory: ${{runner.workspace}}/openfast/build run: | - ctest -VV -L openfast -LE "linear|python" -R 5MW_OC3Trpd_DLL_WSt_WavesReg + ctest -VV -L openfast -LE "cpp|linear|python" -R 5MW_OC3Trpd_DLL_WSt_WavesReg - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() @@ -596,7 +596,7 @@ jobs: - name: Run 5MW tests working-directory: ${{runner.workspace}}/openfast/build run: | - ctest -VV -L openfast -LE "linear|python" -R 5MW_Land_BD_DLL_WTurb + ctest -VV -L openfast -LE "cpp|linear|python" -R 5MW_Land_BD_DLL_WTurb - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() @@ -640,7 +640,51 @@ jobs: - name: Run non-5MW tests working-directory: ${{runner.workspace}}/openfast/build run: | - ctest -VV -j8 -E 5MW_ -L openfast -LE "python|cpp" + ctest -VV -j8 -E 5MW_ -L openfast -LE "cpp|linear|python" + - name: Failing test artifacts + uses: actions/upload-artifact@v2 + if: failure() + with: + name: regression-tests-not-5MW + path: | + ${{runner.workspace}}/openfast/build/reg_tests/modules + ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/5MW_Baseline + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AOC + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AWT27 + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/SWRT + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/UAE_VI + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/WP_Baseline + + + regression-tests-openfast-linearization: + runs-on: ubuntu-20.04 + needs: build-openfast-release + steps: + - name: Cache the workspace + uses: actions/cache@v3.0.4 + with: + path: ${{runner.workspace}} + key: build-openfast-release-${{ github.sha }} + - name: Setup Python + uses: actions/setup-python@v3 + with: + python-version: '3.9' + cache: 'pip' + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install numpy Bokeh + sudo apt-get update -y + sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev + - name: Configure Tests + working-directory: ${{runner.workspace}}/openfast/build + run: | + cmake --build . --target regression_test_controllers -- -j ${{env.NUM_PROCS}} + - name: Run OpenFAST linearization tests + working-directory: ${{runner.workspace}}/openfast/build + run: | + ctest -VV -j8 -L linear - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() From 78fc18774b46568ff9bbfade22e88c8c7373c014 Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Thu, 14 Jul 2022 09:27:44 -0500 Subject: [PATCH 164/242] Consolidate tests and improve naming --- .github/workflows/automated-dev-tests.yml | 175 +++++++++------------- 1 file changed, 68 insertions(+), 107 deletions(-) diff --git a/.github/workflows/automated-dev-tests.yml b/.github/workflows/automated-dev-tests.yml index a2bdbd68a8..a463da8ee9 100644 --- a/.github/workflows/automated-dev-tests.yml +++ b/.github/workflows/automated-dev-tests.yml @@ -269,7 +269,7 @@ jobs: ### TEST JOBS - regression-tests-module-drivers: + rtest-module-drivers: runs-on: ubuntu-20.04 needs: build-drivers-release steps: @@ -309,20 +309,20 @@ jobs: uses: actions/upload-artifact@v2 if: failure() with: - name: regression-tests-aerodyn-module + name: rtest-module-drivers path: | ${{runner.workspace}}/openfast/build/reg_tests/modules - regression-tests-interfaces: + rtest-modules-debug: runs-on: ubuntu-20.04 - needs: build-interfaces-release + needs: build-all-debug steps: - name: Cache the workspace uses: actions/cache@v3.0.4 with: path: ${{runner.workspace}} - key: build-interfaces-release-${{ github.sha }} + key: build-all-debug-${{ github.sha }} - name: Setup Python uses: actions/setup-python@v3 with: @@ -334,30 +334,50 @@ jobs: pip install numpy Bokeh sudo apt-get update -y sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev - - name: Run Interface / API tests + - name: Configure Tests working-directory: ${{runner.workspace}}/openfast/build run: | - ctest -VV -L "cpp|python" -j ${{env.NUM_PROCS}} + cmake \ + -DBUILD_TESTING:BOOL=ON \ + -DCTEST_PLOT_ERRORS:BOOL=ON \ + ${GITHUB_WORKSPACE} + cmake --build . --target regression_test_controllers -- -j ${{env.NUM_PROCS}} + - name: Run AeroDyn tests + uses: ./.github/actions/tests-module-aerodyn + with: + # Don't run regression tests here since they currently fail inconsistently + test-target: unit + - name: Run BeamDyn tests + uses: ./.github/actions/tests-module-beamdyn + - name: Run HydroDyn tests + uses: ./.github/actions/tests-module-hydrodyn + - name: Run InflowWind tests + uses: ./.github/actions/tests-module-inflowwind + - name: Run NWTC Library tests + uses: ./.github/actions/tests-module-nwtclibrary + - name: Run SubDyn tests + uses: ./.github/actions/tests-module-subdyn + - name: Run VersionInfo tests + uses: ./.github/actions/tests-module-version - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() with: - name: c-interface-reg-tests + name: rtest-modules-debug path: | - ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast-cpp - ${{runner.workspace}}/openfast/build/reg_tests/modules/inflowwind - !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast-cpp/5MW_Baseline + ${{runner.workspace}}/openfast/build/reg_tests/modules + ${{runner.workspace}}/openfast/build/unit_tests - regression-tests-openfast-5MW: + rtest-interfaces: runs-on: ubuntu-20.04 - needs: build-openfast-release + needs: build-interfaces-release steps: - name: Cache the workspace uses: actions/cache@v3.0.4 with: path: ${{runner.workspace}} - key: build-openfast-release-${{ github.sha }} + key: build-interfaces-release-${{ github.sha }} - name: Setup Python uses: actions/setup-python@v3 with: @@ -369,31 +389,24 @@ jobs: pip install numpy Bokeh sudo apt-get update -y sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev - - name: Configure Tests - working-directory: ${{runner.workspace}}/openfast/build - run: | - cmake --build . --target regression_test_controllers -- -j ${{env.NUM_PROCS}} - - name: Run 5MW tests + - name: Run Interface / API tests working-directory: ${{runner.workspace}}/openfast/build run: | - ctest -VV -j8 -L openfast -LE "cpp|linear|python" -R 5MW_ -E "5MW_Land_BD_DLL_WTurb|5MW_OC3Mnpl_DLL_WTurb_WavesIrr|5MW_OC4Semi_WSt_WavesWN|5MW_OC3Trpd_DLL_WSt_WavesReg|5MW_OC4Jckt_DLL_WTurb_WavesIrr_MGrowth" + ctest -VV -L "cpp|python" -j ${{env.NUM_PROCS}} - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() with: - name: regression-tests-5MW + name: rtest-interfaces path: | - ${{runner.workspace}}/openfast/build/reg_tests/modules - ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast - !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/5MW_Baseline - !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AOC - !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/AWT27 - !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/SWRT - !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/UAE_VI - !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/WP_Baseline + ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast-cpp + ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/python + ${{runner.workspace}}/openfast/build/reg_tests/modules/inflowwind + ${{runner.workspace}}/openfast/build/reg_tests/modules/hydrodyn + !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast-cpp/5MW_Baseline - regression-tests-openfast-5MW_OC4Semi_WSt_WavesWN: + rtest-OF: runs-on: ubuntu-20.04 needs: build-openfast-release steps: @@ -420,12 +433,15 @@ jobs: - name: Run 5MW tests working-directory: ${{runner.workspace}}/openfast/build run: | - ctest -VV -L openfast -LE "cpp|linear|python" -R 5MW_OC4Semi_WSt_WavesWN + ctest -VV -j8 \ + -L openfast \ + -LE "cpp|linear|python" \ + -E "5MW_OC4Semi_WSt_WavesWN|5MW_OC3Mnpl_DLL_WTurb_WavesIrr|5MW_OC4Jckt_DLL_WTurb_WavesIrr_MGrowth|5MW_OC3Trpd_DLL_WSt_WavesReg|5MW_Land_BD_DLL_WTurb" - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() with: - name: regression-tests-5MW_OC4Semi_WSt_WavesWN + name: rtest-OF path: | ${{runner.workspace}}/openfast/build/reg_tests/modules ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast @@ -437,7 +453,7 @@ jobs: !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/WP_Baseline - regression-tests-openfast-5MW_OC3Mnpl_DLL_WTurb_WavesIrr: + rtest-OF-5MW_OC4Semi_WSt_WavesWN: runs-on: ubuntu-20.04 needs: build-openfast-release steps: @@ -464,12 +480,12 @@ jobs: - name: Run 5MW tests working-directory: ${{runner.workspace}}/openfast/build run: | - ctest -VV -L openfast -LE "cpp|linear|python" -R 5MW_OC3Mnpl_DLL_WTurb_WavesIrr + ctest -VV -L openfast -LE "cpp|linear|python" -R 5MW_OC4Semi_WSt_WavesWN - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() with: - name: regression-tests-5MW_OC3Mnpl_DLL_WTurb_WavesIrr + name: rtest-OF-5MW_OC4Semi_WSt_WavesWN path: | ${{runner.workspace}}/openfast/build/reg_tests/modules ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast @@ -481,7 +497,7 @@ jobs: !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/WP_Baseline - regression-tests-openfast-5MW_OC4Jckt_DLL_WTurb_WavesIrr_MGrowth: + rtest-OF-5MW_OC3Mnpl_DLL_WTurb_WavesIrr: runs-on: ubuntu-20.04 needs: build-openfast-release steps: @@ -508,12 +524,12 @@ jobs: - name: Run 5MW tests working-directory: ${{runner.workspace}}/openfast/build run: | - ctest -VV -L openfast -LE "cpp|linear|python" -R 5MW_OC4Jckt_DLL_WTurb_WavesIrr_MGrowth + ctest -VV -L openfast -LE "cpp|linear|python" -R 5MW_OC3Mnpl_DLL_WTurb_WavesIrr - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() with: - name: regression-tests-5MW_OC4Jckt_DLL_WTurb_WavesIrr_MGrowth + name: rtest-OF-5MW_OC3Mnpl_DLL_WTurb_WavesIrr path: | ${{runner.workspace}}/openfast/build/reg_tests/modules ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast @@ -525,7 +541,7 @@ jobs: !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/WP_Baseline - regression-tests-openfast-5MW_OC3Trpd_DLL_WSt_WavesReg: + rtest-OF-5MW_OC4Jckt_DLL_WTurb_WavesIrr_MGrowth: runs-on: ubuntu-20.04 needs: build-openfast-release steps: @@ -552,12 +568,12 @@ jobs: - name: Run 5MW tests working-directory: ${{runner.workspace}}/openfast/build run: | - ctest -VV -L openfast -LE "cpp|linear|python" -R 5MW_OC3Trpd_DLL_WSt_WavesReg + ctest -VV -L openfast -LE "cpp|linear|python" -R 5MW_OC4Jckt_DLL_WTurb_WavesIrr_MGrowth - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() with: - name: regression-tests-5MW_OC3Trpd_DLL_WSt_WavesReg + name: rtest-OF-5MW_OC4Jckt_DLL_WTurb_WavesIrr_MGrowth path: | ${{runner.workspace}}/openfast/build/reg_tests/modules ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast @@ -569,7 +585,7 @@ jobs: !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/WP_Baseline - regression-tests-openfast-5MW_Land_BD_DLL_WTurb: + rtest-OF-5MW_OC3Trpd_DLL_WSt_WavesReg: runs-on: ubuntu-20.04 needs: build-openfast-release steps: @@ -596,12 +612,12 @@ jobs: - name: Run 5MW tests working-directory: ${{runner.workspace}}/openfast/build run: | - ctest -VV -L openfast -LE "cpp|linear|python" -R 5MW_Land_BD_DLL_WTurb + ctest -VV -L openfast -LE "cpp|linear|python" -R 5MW_OC3Trpd_DLL_WSt_WavesReg - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() with: - name: regression-tests-5MW_Land_BD_DLL_WTurb + name: rtest-OF-5MW_OC3Trpd_DLL_WSt_WavesReg path: | ${{runner.workspace}}/openfast/build/reg_tests/modules ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast @@ -613,7 +629,7 @@ jobs: !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/WP_Baseline - regression-tests-openfast-not-5MW: + rtest-OF-5MW_Land_BD_DLL_WTurb: runs-on: ubuntu-20.04 needs: build-openfast-release steps: @@ -637,15 +653,15 @@ jobs: working-directory: ${{runner.workspace}}/openfast/build run: | cmake --build . --target regression_test_controllers -- -j ${{env.NUM_PROCS}} - - name: Run non-5MW tests + - name: Run 5MW tests working-directory: ${{runner.workspace}}/openfast/build run: | - ctest -VV -j8 -E 5MW_ -L openfast -LE "cpp|linear|python" + ctest -VV -L openfast -LE "cpp|linear|python" -R 5MW_Land_BD_DLL_WTurb - name: Failing test artifacts uses: actions/upload-artifact@v2 if: failure() with: - name: regression-tests-not-5MW + name: rtest-OF-5MW_Land_BD_DLL_WTurb path: | ${{runner.workspace}}/openfast/build/reg_tests/modules ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast @@ -657,7 +673,7 @@ jobs: !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/WP_Baseline - regression-tests-openfast-linearization: + rtest-OF-linearization: runs-on: ubuntu-20.04 needs: build-openfast-release steps: @@ -689,7 +705,7 @@ jobs: uses: actions/upload-artifact@v2 if: failure() with: - name: regression-tests-not-5MW + name: rtest-OF-linearization path: | ${{runner.workspace}}/openfast/build/reg_tests/modules ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast @@ -701,7 +717,7 @@ jobs: !${{runner.workspace}}/openfast/build/reg_tests/glue-codes/openfast/WP_Baseline - regression-tests-fastfarm: + rtest-FF: runs-on: ubuntu-20.04 needs: build-fastfarm-release steps: @@ -734,61 +750,6 @@ jobs: uses: actions/upload-artifact@v2 if: failure() with: - name: fastfarm-tests-release + name: rtest-FF path: | ${{runner.workspace}}/openfast/build/reg_tests/glue-codes/fastfarm - - - module-tests-debug: - runs-on: ubuntu-20.04 - needs: build-all-debug - steps: - - name: Cache the workspace - uses: actions/cache@v3.0.4 - with: - path: ${{runner.workspace}} - key: build-all-debug-${{ github.sha }} - - name: Setup Python - uses: actions/setup-python@v3 - with: - python-version: '3.9' - cache: 'pip' - - name: Install dependencies - run: | - python -m pip install --upgrade pip - pip install numpy Bokeh - sudo apt-get update -y - sudo apt-get install -y libhdf5-dev libopenmpi-dev libyaml-cpp-dev - - name: Configure Tests - working-directory: ${{runner.workspace}}/openfast/build - run: | - cmake \ - -DBUILD_TESTING:BOOL=ON \ - -DCTEST_PLOT_ERRORS:BOOL=ON \ - ${GITHUB_WORKSPACE} - cmake --build . --target regression_test_controllers -- -j ${{env.NUM_PROCS}} - - name: Run AeroDyn tests - uses: ./.github/actions/tests-module-aerodyn - with: - # Don't run regression tests here since they currently fail inconsistently - test-target: unit - - name: Run BeamDyn tests - uses: ./.github/actions/tests-module-beamdyn - - name: Run HydroDyn tests - uses: ./.github/actions/tests-module-hydrodyn - - name: Run InflowWind tests - uses: ./.github/actions/tests-module-inflowwind - - name: Run NWTC Library tests - uses: ./.github/actions/tests-module-nwtclibrary - - name: Run SubDyn tests - uses: ./.github/actions/tests-module-subdyn - - name: Run VersionInfo tests - uses: ./.github/actions/tests-module-version - - name: Failing test artifacts - uses: actions/upload-artifact@v2 - if: failure() - with: - name: debug-tests-release - path: | - ${{runner.workspace}}/openfast/build/reg_tests/modules - ${{runner.workspace}}/openfast/build/unit_tests From 3aa7f39bad967205b8cc31c2b6f054a267c376aa Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Thu, 14 Jul 2022 10:35:45 -0500 Subject: [PATCH 165/242] Disable variable tracking on all FF modules --- glue-codes/fast-farm/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/glue-codes/fast-farm/CMakeLists.txt b/glue-codes/fast-farm/CMakeLists.txt index f404db7fab..c6ad9ffc75 100644 --- a/glue-codes/fast-farm/CMakeLists.txt +++ b/glue-codes/fast-farm/CMakeLists.txt @@ -51,6 +51,9 @@ if (${_compiler_id} STREQUAL "GNU" AND NOT ${VARIABLE_TRACKING}) # for better runtime debugging output. # https://gcc.gnu.org/onlinedocs/gcc/Debugging-Options.html set_source_files_properties(src/FAST_Farm_Types.f90 src/FASTWrapper_Types.f90 src/FASTWrapper.f90 PROPERTIES COMPILE_FLAGS "-fno-var-tracking -fno-var-tracking-assignments") + set_source_files_properties(src/FAST_Farm_IO.f90 PROPERTIES COMPILE_FLAGS "-fno-var-tracking -fno-var-tracking-assignments") + set_source_files_properties(src/FAST_Farm_Subs.f90 PROPERTIES COMPILE_FLAGS "-fno-var-tracking -fno-var-tracking-assignments") + set_source_files_properties(src/FAST_Farm.f90 PROPERTIES COMPILE_FLAGS "-fno-var-tracking -fno-var-tracking-assignments") endif() if (${_compiler_id} MATCHES "^INTEL" AND ${_build_type} STREQUAL "RELEASE" AND NOT WIN32) # Compilation hangs on FAST_Farm_Types.f90 with -O3 on linux (on some hardware) From 6d9d1c978ca7f6a211e0d232d9e6e8f1f9b63523 Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Fri, 29 Jul 2022 13:48:29 -0500 Subject: [PATCH 166/242] Update ideal beam linearization test baselines These were not previously run in Actions so it was not caught that the baselines were out of date --- reg_tests/r-test | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/reg_tests/r-test b/reg_tests/r-test index e8efa89461..cd9a7a4f27 160000 --- a/reg_tests/r-test +++ b/reg_tests/r-test @@ -1 +1 @@ -Subproject commit e8efa89461f6a7de4ffe3e9416d2ff5dabfb566d +Subproject commit cd9a7a4f27f4197dfd3d1ba699fd24a4c7f1aea4 From 5099377d24459307890c9ffb374fb30994c64281 Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Fri, 29 Jul 2022 14:00:01 -0500 Subject: [PATCH 167/242] Build HydroDyn C-interface for interface tests --- .github/workflows/automated-dev-tests.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/automated-dev-tests.yml b/.github/workflows/automated-dev-tests.yml index a463da8ee9..06ebebcf22 100644 --- a/.github/workflows/automated-dev-tests.yml +++ b/.github/workflows/automated-dev-tests.yml @@ -192,6 +192,7 @@ jobs: cmake --build . --target openfast_cpp -- -j ${{env.NUM_PROCS}} cmake --build . --target openfastcpp -- -j ${{env.NUM_PROCS}} cmake --build . --target ifw_c_binding -- -j ${{env.NUM_PROCS}} + cmake --build . --target hydrodyn_c_binding -- -j ${{env.NUM_PROCS}} cmake --build . --target regression_test_controllers -- -j ${{env.NUM_PROCS}} - name: Cache the workspace uses: actions/cache@v3.0.4 From 84addf3ad90ed06a96bf6d8fab0284af0894b6e3 Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Tue, 17 May 2022 13:03:26 -0500 Subject: [PATCH 168/242] Clean up in rtestlib --- reg_tests/executeOpenfastRegressionCase.py | 4 +-- reg_tests/lib/pass_fail.py | 38 +++++++++++----------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/reg_tests/executeOpenfastRegressionCase.py b/reg_tests/executeOpenfastRegressionCase.py index 2bb1bb14ca..691736d8d0 100644 --- a/reg_tests/executeOpenfastRegressionCase.py +++ b/reg_tests/executeOpenfastRegressionCase.py @@ -24,7 +24,7 @@ import os import sys -basepath = os.path.dirname(os.path.abspath(__file__)) +basepath = os.path.dirname(__file__) sys.path.insert(0, os.path.sep.join([basepath, "lib"])) import argparse import shutil @@ -158,7 +158,7 @@ def ignoreBaselineItems(directory, contents): rtl.validateFileOrExit(localOutFile) rtl.validateFileOrExit(baselineOutFile) -testData, testInfo, testPack = pass_fail.readFASTOut(localOutFile) +testData, testInfo, _ = pass_fail.readFASTOut(localOutFile) baselineData, baselineInfo, _ = pass_fail.readFASTOut(baselineOutFile) performance = pass_fail.calculateNorms(testData, baselineData) normalizedNorm = performance[:, 1] diff --git a/reg_tests/lib/pass_fail.py b/reg_tests/lib/pass_fail.py index 3bfdf0062a..61030a6ceb 100644 --- a/reg_tests/lib/pass_fail.py +++ b/reg_tests/lib/pass_fail.py @@ -18,7 +18,7 @@ This library provides tools for comparing a test solution to a baseline solution for any structured output file generated within the OpenFAST framework. """ -import sys, os +import sys import numpy as np from numpy import linalg as LA from fast_io import load_output @@ -72,10 +72,14 @@ def calculateNorms(test_data, baseline_data): relative_norm = calculate_max_norm_over_range(test_data, baseline_data) max_norm = calculate_max_norm(test_data, baseline_data) relative_l2_norm = calculate_relative_norm(test_data, baseline_data) - results = np.hstack(( - relative_norm.reshape(-1, 1), relative_l2_norm.reshape(-1, 1), - max_norm.reshape(-1, 1) - )) + results = np.stack( + ( + relative_norm, + relative_l2_norm, + max_norm + ), + axis=1 + ) return results if __name__=="__main__": @@ -84,22 +88,18 @@ def calculateNorms(test_data, baseline_data): testSolution = sys.argv[1] baselineSolution = sys.argv[2] - tolerance = sys.argv[3] - - try: - tolerance = float(tolerance) - except ValueError: - rtl.exitWithError("Error: invalid tolerance given, {}".format(tolerance)) + tolerance = float(sys.argv[3]) rtl.validateFileOrExit(testSolution) rtl.validateFileOrExit(baselineSolution) testData, testInfo, testPack = readFASTOut(testSolution) - baselineData, baselineInfo, basePack = readFASTOut(baselineSolution) - - normalizedNorm, maxNorm = pass_fail.calculateNorms(testData, baselineData, tolerance) - if passRegressionTest(normalizedNorm, tolerance): - sys.exit(0) - else: - dict1, info1, pack1 = readFASTOut(testSolution) - sys.exit(1) + baselineData, baselineInfo, _ = readFASTOut(baselineSolution) + relative_norm, normalized_norm, max_norm = calculateNorms(testData, baselineData) + print(relative_norm) + print(normalized_norm) + print(max_norm) + + # if not passRegressionTest(normalizedNorm, tolerance): + # dict1, info1, pack1 = readFASTOut(testSolution) + # sys.exit(1) From fa66d5b64434848f024bb5c02304af58276bdfe1 Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Wed, 22 Jun 2022 14:59:56 -0500 Subject: [PATCH 169/242] Determine passing channels instead of norms --- reg_tests/executeOpenfastRegressionCase.py | 43 ++++++++++++---------- reg_tests/lib/errorPlotting.py | 33 +++++++++-------- reg_tests/lib/pass_fail.py | 21 ++++++++--- 3 files changed, 56 insertions(+), 41 deletions(-) diff --git a/reg_tests/executeOpenfastRegressionCase.py b/reg_tests/executeOpenfastRegressionCase.py index 691736d8d0..f245e780b7 100644 --- a/reg_tests/executeOpenfastRegressionCase.py +++ b/reg_tests/executeOpenfastRegressionCase.py @@ -27,6 +27,7 @@ basepath = os.path.dirname(__file__) sys.path.insert(0, os.path.sep.join([basepath, "lib"])) import argparse +import numpy as np import shutil import subprocess import rtestlib as rtl @@ -160,27 +161,29 @@ def ignoreBaselineItems(directory, contents): testData, testInfo, _ = pass_fail.readFASTOut(localOutFile) baselineData, baselineInfo, _ = pass_fail.readFASTOut(baselineOutFile) -performance = pass_fail.calculateNorms(testData, baselineData) -normalizedNorm = performance[:, 1] -# export all case summaries -results = list(zip(testInfo["attribute_names"], [*performance])) -results_max = performance.max(axis=0) -exportCaseSummary(testBuildDirectory, caseName, results, results_max, tolerance) +passing_channels = pass_fail.passing_channels(testData.T, baselineData.T) +passing_channels = passing_channels.T -# failing case -if not pass_fail.passRegressionTest(normalizedNorm, tolerance): - if plotError: - from errorPlotting import finalizePlotDirectory, plotOpenfastError - for channel in testInfo["attribute_names"]: - try: - plotOpenfastError(localOutFile, baselineOutFile, channel) - except: - error = sys.exc_info()[1] - print("Error generating plots: {}".format(error)) - finalizePlotDirectory(localOutFile, testInfo["attribute_names"], caseName) - - sys.exit(1) +norms = pass_fail.calculateNorms(testData, baselineData) + +# export all case summaries +channel_names = testInfo["attribute_names"] +exportCaseSummary(testBuildDirectory, caseName, channel_names, passing_channels, norms) # passing case -sys.exit(0) +if np.all(passing_channels): + sys.exit(0) + +# failing case +if plotError: + from errorPlotting import finalizePlotDirectory, plotOpenfastError + for channel in testInfo["attribute_names"]: + try: + plotOpenfastError(localOutFile, baselineOutFile, channel) + except: + error = sys.exc_info()[1] + print("Error generating plots: {}".format(error)) + finalizePlotDirectory(localOutFile, testInfo["attribute_names"], caseName) + +sys.exit(1) diff --git a/reg_tests/lib/errorPlotting.py b/reg_tests/lib/errorPlotting.py index 1778f3fe75..a68546e610 100644 --- a/reg_tests/lib/errorPlotting.py +++ b/reg_tests/lib/errorPlotting.py @@ -243,40 +243,43 @@ def exportResultsSummary(path, results): html.write( _htmlTail() ) html.close() -def exportCaseSummary(path, case, results, results_max, tolerance): +def exportCaseSummary(path, case, channel_names, passing_channels, norms): + """ + norms: first dimension is the channel and second dimension contains the norms + passing_channels: 1d boolean array for whether a channel passed the comparison + """ + with open(os.path.join(path, case+".html"), "w") as html: html.write( _htmlHead(case + " Summary") ) html.write('\n') html.write('

{}

\n'.format(case + " Summary")) - html.write('

Maximum values for each norm are highlighted and failing norms (norm >= {0}) are highlighted

\n'.format(tolerance)) html.write('
\n') - data = [ - ('{0}'.format(attribute), *norms) - for attribute, *norms in results - ] + channel_tags = [ '{0}'.format(channel) for channel in channel_names ] + cols = [ - 'Channel', 'Relative Max Norm', - 'Relative L2 Norm', 'Infinity Norm' + 'Channel', + 'Relative Max Norm', + 'Relative L2 Norm', + 'Infinity Norm' ] table = _tableHead(cols) body = ' ' + '\n' - for i, d in enumerate(data): + + for i, channel_tag in enumerate(channel_tags): body += ' ' + '\n' body += ' {}'.format(i+1) + '\n' - body += ' {0:s}'.format(d[0]) + '\n' + body += ' {0:s}'.format(channel_tag) + '\n' fmt = '{0:0.4e}' - for j, val in enumerate(d[1]): - if val == results_max[j]: - body += (' ' + fmt + '\n').format(val) - elif val > tolerance: + for val in norms[i]: + if not passing_channels[i]: body += (' ' + fmt + '\n').format(val) else: body += (' ' + fmt + '\n').format(val) - + body += ' ' + '\n' body += ' ' + '\n' table += body diff --git a/reg_tests/lib/pass_fail.py b/reg_tests/lib/pass_fail.py index 61030a6ceb..53ffd5f80a 100644 --- a/reg_tests/lib/pass_fail.py +++ b/reg_tests/lib/pass_fail.py @@ -30,12 +30,21 @@ def readFASTOut(fastoutput): except Exception as e: rtl.exitWithError("Error: {}".format(e)) -def passRegressionTest(norm, tolerance): - if np.any(np.isnan(norm)): - return False - if np.any(np.isinf(norm)): - return False - return True if max(norm) < tolerance else False +def passing_channels(test, baseline) -> np.ndarray: + """ + test, baseline: arrays containing the results from OpenFAST in the following format + [ + channels, + data + ] + So that test[0,:] are the data for the 0th channel and test[:,0] are the 0th entry in each channel. + """ + atol = 1e-4 + rtol = 1e-6 + whereclose = np.isclose( test, baseline, atol=atol, rtol=rtol ) + passing_channels = np.all(whereclose, axis=1) + + return passing_channels def maxnorm(data, axis=0): return LA.norm(data, np.inf, axis=axis) From b2b3d5c7d03f2af41c62f708653c6b0631831f26 Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Tue, 17 May 2022 13:06:41 -0500 Subject: [PATCH 170/242] Plot pass/fail boundary in error plot --- reg_tests/lib/errorPlotting.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/reg_tests/lib/errorPlotting.py b/reg_tests/lib/errorPlotting.py index a68546e610..ed686ead36 100644 --- a/reg_tests/lib/errorPlotting.py +++ b/reg_tests/lib/errorPlotting.py @@ -66,6 +66,12 @@ def _plotError(xseries, y1series, y2series, xlabel, title1, title2): p2.grid.grid_line_alpha = 0 p2.xaxis.axis_label = 'Time (s)' p2.line(xseries, abs(y2series - y1series), color='blue') + + atol = 1e-4 + rtol = 1e-6 + passfail_line = atol + rtol * abs(y2series) + p2.line(xseries, passfail_line, color='red') + p2.add_tools(HoverTool(tooltips=[('Time','@x'), ('Error', '@y')], mode='vline')) grid = gridplot([[p1, p2]], plot_width=650, plot_height=375, sizing_mode="scale_both") @@ -117,7 +123,7 @@ def plotOpenfastError(testSolution, baselineSolution, attribute): rtl.exitWithError("Error: Invalid channel name--{}".format(e)) title1 = attribute + " (" + info1["attribute_units"][channel] + ")" - title2 = "Max norm" + title2 = "ABS(Local - Baseline)" xlabel = 'Time (s)' timevec = dict1[:, 0] From 22e8395ae28fea71f7ed8195f4de8b637f3ef0c9 Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Mon, 1 Aug 2022 16:23:01 -0500 Subject: [PATCH 171/242] Unify baselines --- reg_tests/CTestList.cmake | 2 - ...ecuteOpenfastAeroAcousticRegressionCase.py | 79 +++++++------------ reg_tests/executeOpenfastRegressionCase.py | 25 +----- reg_tests/r-test | 2 +- 4 files changed, 30 insertions(+), 78 deletions(-) diff --git a/reg_tests/CTestList.cmake b/reg_tests/CTestList.cmake index 7542433367..9a0c188938 100644 --- a/reg_tests/CTestList.cmake +++ b/reg_tests/CTestList.cmake @@ -57,8 +57,6 @@ function(regression TEST_SCRIPT EXECUTABLE SOURCE_DIRECTORY BUILD_DIRECTORY TEST ${SOURCE_DIRECTORY} # openfast source directory ${BUILD_DIRECTORY} # build directory for test ${TOLERANCE} - ${CMAKE_SYSTEM_NAME} # [Darwin,Linux,Windows] - ${CMAKE_Fortran_COMPILER_ID} # [Intel,GNU] ${PLOT_FLAG} # empty or "-p" ${RUN_VERBOSE_FLAG} # empty or "-v" ) diff --git a/reg_tests/executeOpenfastAeroAcousticRegressionCase.py b/reg_tests/executeOpenfastAeroAcousticRegressionCase.py index 381879eb25..29dfcd926a 100644 --- a/reg_tests/executeOpenfastAeroAcousticRegressionCase.py +++ b/reg_tests/executeOpenfastAeroAcousticRegressionCase.py @@ -28,6 +28,7 @@ basepath = os.path.dirname(os.path.abspath(__file__)) sys.path.insert(0, os.path.sep.join([basepath, "lib"])) import argparse +import numpy as np import shutil import subprocess import rtestlib as rtl @@ -56,8 +57,6 @@ def ignoreBaselineItems(directory, contents): parser.add_argument("sourceDirectory", metavar="path/to/openfast_repo", type=str, nargs=1, help="The path to the OpenFAST repository.") parser.add_argument("buildDirectory", metavar="path/to/openfast_repo/build", type=str, nargs=1, help="The path to the OpenFAST repository build directory.") parser.add_argument("tolerance", metavar="Test-Tolerance", type=float, nargs=1, help="Tolerance defining pass or failure in the regression test.") -parser.add_argument("systemName", metavar="System-Name", type=str, nargs=1, help="The current system\'s name: [Darwin,Linux,Windows]") -parser.add_argument("compilerId", metavar="Compiler-Id", type=str, nargs=1, help="The compiler\'s id: [Intel,GNU]") parser.add_argument("-p", "-plot", dest="plot", action='store_true', help="bool to include plots in failed cases") parser.add_argument("-n", "-no-exec", dest="noExec", action='store_true', help="bool to prevent execution of the test cases") parser.add_argument("-v", "-verbose", dest="verbose", action='store_true', help="bool to include verbose system output") @@ -69,8 +68,6 @@ def ignoreBaselineItems(directory, contents): sourceDirectory = args.sourceDirectory[0] buildDirectory = args.buildDirectory[0] tolerance = args.tolerance[0] -systemName = args.systemName[0] -compilerId = args.compilerId[0] plotError = args.plot noExec = args.noExec verbose = args.verbose @@ -81,37 +78,17 @@ def ignoreBaselineItems(directory, contents): if not os.path.isdir(buildDirectory): os.makedirs(buildDirectory) -### Map the system and compiler configurations to a solution set -# Internal names -> Human readable names -systemName_map = { - "darwin": "macos", - "linux": "linux", - "windows": "windows" -} -compilerId_map = { - "gnu": "gnu", - "intel": "intel" -} -# Build the target output directory name or choose the default -supportedBaselines = ["macos-gnu", "linux-intel", "linux-gnu", "windows-intel"] -targetSystem = systemName_map.get(systemName.lower(), "") -targetCompiler = compilerId_map.get(compilerId.lower(), "") -outputType = os.path.join(targetSystem+"-"+targetCompiler) -if outputType not in supportedBaselines: - outputType = supportedBaselines[0] -print("-- Using gold standard files with machine-compiler type {}".format(outputType)) - ### Build the filesystem navigation variables for running openfast on the test case regtests = os.path.join(sourceDirectory, "reg_tests") lib = os.path.join(regtests, "lib") rtest = os.path.join(regtests, "r-test") moduleDirectory = os.path.join(rtest, "glue-codes", "openfast") inputsDirectory = os.path.join(moduleDirectory, caseName) -targetOutputDirectory = os.path.join(inputsDirectory, outputType) +targetOutputDirectory = os.path.join(inputsDirectory) testBuildDirectory = os.path.join(buildDirectory, caseName) - + # verify all the required directories exist if not os.path.isdir(rtest): rtl.exitWithError("The test data directory, {}, does not exist. If you haven't already, run `git submodule update --init --recursive`".format(rtest)) @@ -124,14 +101,14 @@ def ignoreBaselineItems(directory, contents): # and initialize it with input files for all test cases if not os.path.isdir(testBuildDirectory): shutil.copytree(inputsDirectory, testBuildDirectory, ignore=ignoreBaselineItems) - + ### Run openfast on the test case if not noExec: caseInputFile = os.path.join(testBuildDirectory, caseName + ".fst") returnCode = openfastDrivers.runOpenfastCase(caseInputFile, executable) if returnCode != 0: rtl.exitWithError("") - + ### Build the filesystem navigation variables for running the regression test # testing on file 2. Gives each observer and sweep of frequency ranges localOutFile = os.path.join(testBuildDirectory, caseName + "_2.out") @@ -139,31 +116,31 @@ def ignoreBaselineItems(directory, contents): rtl.validateFileOrExit(localOutFile) rtl.validateFileOrExit(baselineOutFile) -testData, testInfo, testPack = pass_fail.readFASTOut(localOutFile) +testData, testInfo, _ = pass_fail.readFASTOut(localOutFile) baselineData, baselineInfo, _ = pass_fail.readFASTOut(baselineOutFile) -performance = pass_fail.calculateNorms(testData, baselineData) -normalizedNorm = performance[:, 1] + +passing_channels = pass_fail.passing_channels(testData.T, baselineData.T) +passing_channels = passing_channels.T + +norms = pass_fail.calculateNorms(testData, baselineData) # export all case summaries -results = list(zip(testInfo["attribute_names"], [*performance])) -results_max = performance.max(axis=0) -exportCaseSummary(testBuildDirectory, caseName, results, results_max, tolerance) +channel_names = testInfo["attribute_names"] +exportCaseSummary(testBuildDirectory, caseName, channel_names, passing_channels, norms) -# failing case -if not pass_fail.passRegressionTest(normalizedNorm, tolerance): - if plotError: - from errorPlotting import finalizePlotDirectory, plotOpenfastError - ixFailChannels = [i for i in range(len(testInfo["attribute_names"])) if normalizedNorm[i] > tolerance] - failChannels = [channel for i, channel in enumerate(testInfo["attribute_names"]) if i in ixFailChannels] - failResults = [res for i, res in enumerate(results) if i in ixFailChannels] - for channel in failChannels: - try: - plotOpenfastError(localOutFile, baselineOutFile, channel) - except: - error = sys.exc_info()[1] - print("Error generating plots: {}".format(error.msg)) - finalizePlotDirectory(localOutFile, failChannels, caseName) - sys.exit(1) - # passing case -sys.exit(0) +if np.all(passing_channels): + sys.exit(0) + +# failing case +if plotError: + from errorPlotting import finalizePlotDirectory, plotOpenfastError + for channel in testInfo["attribute_names"]: + try: + plotOpenfastError(localOutFile, baselineOutFile, channel) + except: + error = sys.exc_info()[1] + print("Error generating plots: {}".format(error)) + finalizePlotDirectory(localOutFile, testInfo["attribute_names"], caseName) + +sys.exit(1) \ No newline at end of file diff --git a/reg_tests/executeOpenfastRegressionCase.py b/reg_tests/executeOpenfastRegressionCase.py index f245e780b7..39d393d0bd 100644 --- a/reg_tests/executeOpenfastRegressionCase.py +++ b/reg_tests/executeOpenfastRegressionCase.py @@ -56,8 +56,6 @@ def ignoreBaselineItems(directory, contents): parser.add_argument("sourceDirectory", metavar="path/to/openfast_repo", type=str, nargs=1, help="The path to the OpenFAST repository.") parser.add_argument("buildDirectory", metavar="path/to/openfast_repo/build", type=str, nargs=1, help="The path to the OpenFAST repository build directory.") parser.add_argument("tolerance", metavar="Test-Tolerance", type=float, nargs=1, help="Tolerance defining pass or failure in the regression test.") -parser.add_argument("systemName", metavar="System-Name", type=str, nargs=1, help="The current system\'s name: [Darwin,Linux,Windows]") -parser.add_argument("compilerId", metavar="Compiler-Id", type=str, nargs=1, help="The compiler\'s id: [Intel,GNU]") parser.add_argument("-p", "-plot", dest="plot", action='store_true', help="bool to include plots in failed cases") parser.add_argument("-n", "-no-exec", dest="noExec", action='store_true', help="bool to prevent execution of the test cases") parser.add_argument("-v", "-verbose", dest="verbose", action='store_true', help="bool to include verbose system output") @@ -69,8 +67,6 @@ def ignoreBaselineItems(directory, contents): sourceDirectory = args.sourceDirectory[0] buildDirectory = args.buildDirectory[0] tolerance = args.tolerance[0] -systemName = args.systemName[0] -compilerId = args.compilerId[0] plotError = args.plot noExec = args.noExec verbose = args.verbose @@ -81,25 +77,6 @@ def ignoreBaselineItems(directory, contents): if not os.path.isdir(buildDirectory): os.makedirs(buildDirectory) -### Map the system and compiler configurations to a solution set -# Internal names -> Human readable names -systemName_map = { - "darwin": "macos", - "linux": "linux", - "windows": "windows" -} -compilerId_map = { - "gnu": "gnu", - "intel": "intel" -} -# Build the target output directory name or choose the default -supportedBaselines = ["macos-gnu", "linux-intel", "linux-gnu", "windows-intel"] -targetSystem = systemName_map.get(systemName.lower(), "") -targetCompiler = compilerId_map.get(compilerId.lower(), "") -outputType = os.path.join(targetSystem+"-"+targetCompiler) -if outputType not in supportedBaselines: - outputType = supportedBaselines[0] -print("-- Using gold standard files with machine-compiler type {}".format(outputType)) ### Build the filesystem navigation variables for running openfast on the test case regtests = os.path.join(sourceDirectory, "reg_tests") @@ -107,7 +84,7 @@ def ignoreBaselineItems(directory, contents): rtest = os.path.join(regtests, "r-test") moduleDirectory = os.path.join(rtest, "glue-codes", "openfast") inputsDirectory = os.path.join(moduleDirectory, caseName) -targetOutputDirectory = os.path.join(inputsDirectory, outputType) +targetOutputDirectory = os.path.join(inputsDirectory) testBuildDirectory = os.path.join(buildDirectory, caseName) # verify all the required directories exist diff --git a/reg_tests/r-test b/reg_tests/r-test index cd9a7a4f27..71bf6ef09b 160000 --- a/reg_tests/r-test +++ b/reg_tests/r-test @@ -1 +1 @@ -Subproject commit cd9a7a4f27f4197dfd3d1ba699fd24a4c7f1aea4 +Subproject commit 71bf6ef09b2bf1961687a0fae5031528f14960ec From 68ed80ae9ecf8c8f4925f0b456cea6063d4e7c0c Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Fri, 20 May 2022 10:46:43 -0500 Subject: [PATCH 172/242] Update reg test driver scripts --- reg_tests/executeAerodynRegressionCase.py | 56 +++++++------- reg_tests/executeBeamdynRegressionCase.py | 49 ++++++------- reg_tests/executeFASTFarmRegressionCase.py | 72 +++++++----------- reg_tests/executeHydrodynRegressionCase.py | 49 ++++++------- .../executeInflowwindPyRegressionCase.py | 51 +++++++------ reg_tests/executeInflowwindRegressionCase.py | 49 ++++++------- reg_tests/executeOpenfastCppRegressionCase.py | 73 +++++++------------ .../executeOpenfastLinearRegressionCase.py | 52 ++++--------- reg_tests/executePythonRegressionCase.py | 71 +++++++----------- reg_tests/executeSubdynRegressionCase.py | 50 ++++++------- 10 files changed, 243 insertions(+), 329 deletions(-) diff --git a/reg_tests/executeAerodynRegressionCase.py b/reg_tests/executeAerodynRegressionCase.py index 1462914737..6cb1da4886 100644 --- a/reg_tests/executeAerodynRegressionCase.py +++ b/reg_tests/executeAerodynRegressionCase.py @@ -27,6 +27,7 @@ basepath = os.path.dirname(os.path.abspath(__file__)) sys.path.insert(0, os.path.sep.join([basepath, "lib"])) import argparse +import numpy as np import shutil import glob import subprocess @@ -47,8 +48,6 @@ parser.add_argument("sourceDirectory", metavar="path/to/openfast_repo", type=str, nargs=1, help="The path to the OpenFAST repository.") parser.add_argument("buildDirectory", metavar="path/to/openfast_repo/build", type=str, nargs=1, help="The path to the OpenFAST repository build directory.") parser.add_argument("tolerance", metavar="Test-Tolerance", type=float, nargs=1, help="Tolerance defining pass or failure in the regression test.") -parser.add_argument("systemName", metavar="System-Name", type=str, nargs=1, help="The current system\'s name: [Darwin,Linux,Windows]") -parser.add_argument("compilerId", metavar="Compiler-Id", type=str, nargs=1, help="The compiler\'s id: [Intel,GNU]") parser.add_argument("-p", "-plot", dest="plot", action='store_true', help="bool to include plots in failed cases") parser.add_argument("-n", "-no-exec", dest="noExec", action='store_true', help="bool to prevent execution of the test cases") parser.add_argument("-v", "-verbose", dest="verbose", action='store_true', help="bool to include verbose system output") @@ -112,44 +111,45 @@ if returnCode != 0: rtl.exitWithError("") -###Build the filesystem navigation variables for running the regression test +### Build the filesystem navigation variables for running the regression test # For multiple turbines, test turbine 2, for combined cases, test case 4 localOutFile = os.path.join(testBuildDirectory, "ad_driver.outb") localOutFileWT2 = os.path.join(testBuildDirectory, "ad_driver.T2.outb") localOutFileCase4 = os.path.join(testBuildDirectory, "ad_driver.4.outb") if os.path.exists(localOutFileWT2) : - localOutFile=localOutFileWT2 + localOutFile = localOutFileWT2 elif os.path.exists(localOutFileCase4) : - localOutFile=localOutFileCase4 + localOutFile = localOutFileCase4 baselineOutFile = os.path.join(targetOutputDirectory, os.path.basename(localOutFile)) + rtl.validateFileOrExit(localOutFile) rtl.validateFileOrExit(baselineOutFile) -testData, testInfo, testPack = pass_fail.readFASTOut(localOutFile) +testData, testInfo, _ = pass_fail.readFASTOut(localOutFile) baselineData, baselineInfo, _ = pass_fail.readFASTOut(baselineOutFile) -performance = pass_fail.calculateNorms(testData, baselineData) -normalizedNorm = performance[:, 1] + +passing_channels = pass_fail.passing_channels(testData.T, baselineData.T) +passing_channels = passing_channels.T + +norms = pass_fail.calculateNorms(testData, baselineData) # export all case summaries -results = list(zip(testInfo["attribute_names"], [*performance])) -results_max = performance.max(axis=0) -exportCaseSummary(testBuildDirectory, caseName, results, results_max, tolerance) +channel_names = testInfo["attribute_names"] +exportCaseSummary(testBuildDirectory, caseName, channel_names, passing_channels, norms) -# failing case -if not pass_fail.passRegressionTest(normalizedNorm, tolerance): - if plotError: - from errorPlotting import finalizePlotDirectory, plotOpenfastError - ixFailChannels = [i for i in range(len(testInfo["attribute_names"])) if normalizedNorm[i] > tolerance] - failChannels = [channel for i, channel in enumerate(testInfo["attribute_names"]) if i in ixFailChannels] - failResults = [res for i, res in enumerate(results) if i in ixFailChannels] - for channel in failChannels: - try: - plotOpenfastError(localOutFile, baselineOutFile, channel) - except: - error = sys.exc_info()[1] - print("Error generating plots: {}".format(error.msg)) - finalizePlotDirectory(localOutFile, failChannels, caseName) - sys.exit(1) - # passing case -sys.exit(0) +if np.all(passing_channels): + sys.exit(0) + +# failing case +if plotError: + from errorPlotting import finalizePlotDirectory, plotOpenfastError + for channel in testInfo["attribute_names"]: + try: + plotOpenfastError(localOutFile, baselineOutFile, channel) + except: + error = sys.exc_info()[1] + print("Error generating plots: {}".format(error)) + finalizePlotDirectory(localOutFile, testInfo["attribute_names"], caseName) + +sys.exit(1) diff --git a/reg_tests/executeBeamdynRegressionCase.py b/reg_tests/executeBeamdynRegressionCase.py index 93dbc01f74..0eded5f361 100644 --- a/reg_tests/executeBeamdynRegressionCase.py +++ b/reg_tests/executeBeamdynRegressionCase.py @@ -27,6 +27,7 @@ basepath = os.path.dirname(os.path.abspath(__file__)) sys.path.insert(0, os.path.sep.join([basepath, "lib"])) import argparse +import numpy as np import shutil import subprocess import rtestlib as rtl @@ -46,8 +47,6 @@ parser.add_argument("sourceDirectory", metavar="path/to/openfast_repo", type=str, nargs=1, help="The path to the OpenFAST repository.") parser.add_argument("buildDirectory", metavar="path/to/openfast_repo/build", type=str, nargs=1, help="The path to the OpenFAST repository build directory.") parser.add_argument("tolerance", metavar="Test-Tolerance", type=float, nargs=1, help="Tolerance defining pass or failure in the regression test.") -parser.add_argument("systemName", metavar="System-Name", type=str, nargs=1, help="The current system\'s name: [Darwin,Linux,Windows]") -parser.add_argument("compilerId", metavar="Compiler-Id", type=str, nargs=1, help="The compiler\'s id: [Intel,GNU]") parser.add_argument("-p", "-plot", dest="plot", action='store_true', help="bool to include plots in failed cases") parser.add_argument("-n", "-no-exec", dest="noExec", action='store_true', help="bool to prevent execution of the test cases") parser.add_argument("-v", "-verbose", dest="verbose", action='store_true', help="bool to include verbose system output") @@ -107,31 +106,31 @@ rtl.validateFileOrExit(localOutFile) rtl.validateFileOrExit(baselineOutFile) -testData, testInfo, testPack = pass_fail.readFASTOut(localOutFile) +testData, testInfo, _ = pass_fail.readFASTOut(localOutFile) baselineData, baselineInfo, _ = pass_fail.readFASTOut(baselineOutFile) -performance = pass_fail.calculateNorms(testData, baselineData) -normalizedNorm = performance[:, 1] + +passing_channels = pass_fail.passing_channels(testData.T, baselineData.T) +passing_channels = passing_channels.T + +norms = pass_fail.calculateNorms(testData, baselineData) # export all case summaries -results = list(zip(testInfo["attribute_names"], [*performance])) -results_max = performance.max(axis=0) -exportCaseSummary(testBuildDirectory, caseName, results, results_max, tolerance) +channel_names = testInfo["attribute_names"] +exportCaseSummary(testBuildDirectory, caseName, channel_names, passing_channels, norms) -# failing case -if not pass_fail.passRegressionTest(normalizedNorm, tolerance): - if plotError: - from errorPlotting import finalizePlotDirectory, plotOpenfastError - ixFailChannels = [i for i in range(len(testInfo["attribute_names"])) if normalizedNorm[i] > tolerance] - failChannels = [channel for i, channel in enumerate(testInfo["attribute_names"]) if i in ixFailChannels] - failResults = [res for i, res in enumerate(results) if i in ixFailChannels] - for channel in failChannels: - try: - plotOpenfastError(localOutFile, baselineOutFile, channel) - except: - error = sys.exc_info()[1] - print("Error generating plots: {}".format(error.msg)) - finalizePlotDirectory(localOutFile, failChannels, caseName) - sys.exit(1) - # passing case -sys.exit(0) +if np.all(passing_channels): + sys.exit(0) + +# failing case +if plotError: + from errorPlotting import finalizePlotDirectory, plotOpenfastError + for channel in testInfo["attribute_names"]: + try: + plotOpenfastError(localOutFile, baselineOutFile, channel) + except: + error = sys.exc_info()[1] + print("Error generating plots: {}".format(error)) + finalizePlotDirectory(localOutFile, testInfo["attribute_names"], caseName) + +sys.exit(1) diff --git a/reg_tests/executeFASTFarmRegressionCase.py b/reg_tests/executeFASTFarmRegressionCase.py index 273637bb46..4cb0571ee8 100644 --- a/reg_tests/executeFASTFarmRegressionCase.py +++ b/reg_tests/executeFASTFarmRegressionCase.py @@ -24,9 +24,10 @@ import os import sys -basepath = os.path.dirname(os.path.abspath(__file__)) +basepath = os.path.dirname(__file__) sys.path.insert(0, os.path.sep.join([basepath, "lib"])) import argparse +import numpy as np import shutil import subprocess import rtestlib as rtl @@ -55,8 +56,6 @@ def ignoreBaselineItems(directory, contents): parser.add_argument("sourceDirectory", metavar="path/to/openfast_repo", type=str, nargs=1, help="The path to the OpenFAST repository.") parser.add_argument("buildDirectory", metavar="path/to/openfast_repo/build", type=str, nargs=1, help="The path to the OpenFAST repository build directory.") parser.add_argument("tolerance", metavar="Test-Tolerance", type=float, nargs=1, help="Tolerance defining pass or failure in the regression test.") -parser.add_argument("systemName", metavar="System-Name", type=str, nargs=1, help="The current system\'s name: [Darwin,Linux,Windows]") -parser.add_argument("compilerId", metavar="Compiler-Id", type=str, nargs=1, help="The compiler\'s id: [Intel,GNU]") parser.add_argument("-p", "-plot", dest="plot", action='store_true', help="bool to include plots in failed cases") parser.add_argument("-n", "-no-exec", dest="noExec", action='store_true', help="bool to prevent execution of the test cases") parser.add_argument("-v", "-verbose", dest="verbose", action='store_true', help="bool to include verbose system output") @@ -68,8 +67,6 @@ def ignoreBaselineItems(directory, contents): sourceDirectory = args.sourceDirectory[0] buildDirectory = args.buildDirectory[0] tolerance = args.tolerance[0] -systemName = args.systemName[0] -compilerId = args.compilerId[0] plotError = args.plot noExec = args.noExec verbose = args.verbose @@ -80,25 +77,6 @@ def ignoreBaselineItems(directory, contents): if not os.path.isdir(buildDirectory): os.makedirs(buildDirectory) -### Map the system and compiler configurations to a solution set -# Internal names -> Human readable names -systemName_map = { - "darwin": "macos", - "linux": "linux", - "windows": "windows" -} -compilerId_map = { - "gnu": "gnu", - "intel": "intel" -} -# Build the target output directory name or choose the default -supportedBaselines = ["macos-gnu", "linux-intel", "linux-gnu", "windows-intel"] -targetSystem = systemName_map.get(systemName.lower(), "") -targetCompiler = compilerId_map.get(compilerId.lower(), "") -outputType = os.path.join(targetSystem+"-"+targetCompiler) -if outputType not in supportedBaselines: - outputType = supportedBaselines[0] -print("-- Using gold standard files with machine-compiler type {}".format(outputType)) ### Build the filesystem navigation variables for running openfast on the test case regtests = os.path.join(sourceDirectory, "reg_tests") @@ -106,7 +84,7 @@ def ignoreBaselineItems(directory, contents): rtest = os.path.join(regtests, "r-test") moduleDirectory = os.path.join(rtest, "glue-codes", "fast-farm") inputsDirectory = os.path.join(moduleDirectory, caseName) -targetOutputDirectory = os.path.join(inputsDirectory) #, outputType) +targetOutputDirectory = os.path.join(inputsDirectory) testBuildDirectory = os.path.join(buildDirectory, caseName) # verify all the required directories exist @@ -151,29 +129,31 @@ def ignoreBaselineItems(directory, contents): rtl.validateFileOrExit(localOutFile) rtl.validateFileOrExit(baselineOutFile) -testData, testInfo, testPack = pass_fail.readFASTOut(localOutFile) +testData, testInfo, _ = pass_fail.readFASTOut(localOutFile) baselineData, baselineInfo, _ = pass_fail.readFASTOut(baselineOutFile) -performance = pass_fail.calculateNorms(testData, baselineData) -normalizedNorm = performance[:, 1] -# export all case summaries -results = list(zip(testInfo["attribute_names"], [*performance])) -results_max = performance.max(axis=0) -exportCaseSummary(testBuildDirectory, caseName, results, results_max, tolerance) +passing_channels = pass_fail.passing_channels(testData.T, baselineData.T) +passing_channels = passing_channels.T -# failing case -if not pass_fail.passRegressionTest(normalizedNorm, tolerance): - if plotError: - from errorPlotting import finalizePlotDirectory, plotOpenfastError - for channel in testInfo["attribute_names"]: - try: - plotOpenfastError(localOutFile, baselineOutFile, channel) - except: - error = sys.exc_info()[1] - print("Error generating plots: {}".format(error)) - finalizePlotDirectory(localOutFile, testInfo["attribute_names"], caseName) - - sys.exit(1) +norms = pass_fail.calculateNorms(testData, baselineData) + +# export all case summaries +channel_names = testInfo["attribute_names"] +exportCaseSummary(testBuildDirectory, caseName, channel_names, passing_channels, norms) # passing case -sys.exit(0) +if np.all(passing_channels): + sys.exit(0) + +# failing case +if plotError: + from errorPlotting import finalizePlotDirectory, plotOpenfastError + for channel in testInfo["attribute_names"]: + try: + plotOpenfastError(localOutFile, baselineOutFile, channel) + except: + error = sys.exc_info()[1] + print("Error generating plots: {}".format(error)) + finalizePlotDirectory(localOutFile, testInfo["attribute_names"], caseName) + +sys.exit(1) diff --git a/reg_tests/executeHydrodynRegressionCase.py b/reg_tests/executeHydrodynRegressionCase.py index 2c442eb6ba..0105d73da7 100644 --- a/reg_tests/executeHydrodynRegressionCase.py +++ b/reg_tests/executeHydrodynRegressionCase.py @@ -27,6 +27,7 @@ basepath = os.path.dirname(os.path.abspath(__file__)) sys.path.insert(0, os.path.sep.join([basepath, "lib"])) import argparse +import numpy as np import shutil import glob import subprocess @@ -47,8 +48,6 @@ parser.add_argument("sourceDirectory", metavar="path/to/openfast_repo", type=str, nargs=1, help="The path to the OpenFAST repository.") parser.add_argument("buildDirectory", metavar="path/to/openfast_repo/build", type=str, nargs=1, help="The path to the OpenFAST repository build directory.") parser.add_argument("tolerance", metavar="Test-Tolerance", type=float, nargs=1, help="Tolerance defining pass or failure in the regression test.") -parser.add_argument("systemName", metavar="System-Name", type=str, nargs=1, help="The current system\'s name: [Darwin,Linux,Windows]") -parser.add_argument("compilerId", metavar="Compiler-Id", type=str, nargs=1, help="The compiler\'s id: [Intel,GNU]") parser.add_argument("-p", "-plot", dest="plot", default=False, metavar="Plotting-Flag", type=bool, nargs="?", help="bool to include matplotlib plots in failed cases") parser.add_argument("-n", "-no-exec", dest="noExec", default=False, metavar="No-Execution", type=bool, nargs="?", help="bool to prevent execution of the test cases") parser.add_argument("-v", "-verbose", dest="verbose", default=False, metavar="Verbose-Flag", type=bool, nargs="?", help="bool to include verbose system output") @@ -111,31 +110,31 @@ rtl.validateFileOrExit(localOutFile) rtl.validateFileOrExit(baselineOutFile) -testData, testInfo, testPack = pass_fail.readFASTOut(localOutFile) +testData, testInfo, _ = pass_fail.readFASTOut(localOutFile) baselineData, baselineInfo, _ = pass_fail.readFASTOut(baselineOutFile) -performance = pass_fail.calculateNorms(testData, baselineData) -normalizedNorm = performance[:, 1] + +passing_channels = pass_fail.passing_channels(testData.T, baselineData.T) +passing_channels = passing_channels.T + +norms = pass_fail.calculateNorms(testData, baselineData) # export all case summaries -results = list(zip(testInfo["attribute_names"], [*performance])) -results_max = performance.max(axis=0) -exportCaseSummary(testBuildDirectory, caseName, results, results_max, tolerance) +channel_names = testInfo["attribute_names"] +exportCaseSummary(testBuildDirectory, caseName, channel_names, passing_channels, norms) -# failing case -if not pass_fail.passRegressionTest(normalizedNorm, tolerance): - if plotError: - from errorPlotting import finalizePlotDirectory, plotOpenfastError - ixFailChannels = [i for i in range(len(testInfo["attribute_names"])) if normalizedNorm[i] > tolerance] - failChannels = [channel for i, channel in enumerate(testInfo["attribute_names"]) if i in ixFailChannels] - failResults = [res for i, res in enumerate(results) if i in ixFailChannels] - for channel in failChannels: - try: - plotOpenfastError(localOutFile, baselineOutFile, channel) - except: - error = sys.exc_info()[1] - print("Error generating plots: {}".format(error.msg)) - finalizePlotDirectory(localOutFile, failChannels, caseName) - sys.exit(1) - # passing case -sys.exit(0) +if np.all(passing_channels): + sys.exit(0) + +# failing case +if plotError: + from errorPlotting import finalizePlotDirectory, plotOpenfastError + for channel in testInfo["attribute_names"]: + try: + plotOpenfastError(localOutFile, baselineOutFile, channel) + except: + error = sys.exc_info()[1] + print("Error generating plots: {}".format(error)) + finalizePlotDirectory(localOutFile, testInfo["attribute_names"], caseName) + +sys.exit(1) diff --git a/reg_tests/executeInflowwindPyRegressionCase.py b/reg_tests/executeInflowwindPyRegressionCase.py index 7d88591ba9..f7b30771c2 100644 --- a/reg_tests/executeInflowwindPyRegressionCase.py +++ b/reg_tests/executeInflowwindPyRegressionCase.py @@ -27,6 +27,7 @@ basepath = os.path.dirname(os.path.abspath(__file__)) sys.path.insert(0, os.path.sep.join([basepath, "lib"])) import argparse +import numpy as np import shutil import glob import subprocess @@ -47,8 +48,6 @@ parser.add_argument("sourceDirectory", metavar="path/to/openfast_repo", type=str, nargs=1, help="The path to the OpenFAST repository.") parser.add_argument("buildDirectory", metavar="path/to/openfast_repo/build", type=str, nargs=1, help="The path to the OpenFAST repository build directory.") parser.add_argument("tolerance", metavar="Test-Tolerance", type=float, nargs=1, help="Tolerance defining pass or failure in the regression test.") -parser.add_argument("systemName", metavar="System-Name", type=str, nargs=1, help="The current system\'s name: [Darwin,Linux,Windows]") -parser.add_argument("compilerId", metavar="Compiler-Id", type=str, nargs=1, help="The compiler\'s id: [Intel,GNU]") parser.add_argument("-p", "-plot", dest="plot", action='store_true', help="bool to include plots in failed cases") parser.add_argument("-n", "-no-exec", dest="noExec", action='store_true', help="bool to prevent execution of the test cases") parser.add_argument("-v", "-verbose", dest="verbose", action='store_true', help="bool to include verbose system output") @@ -104,38 +103,38 @@ returnCode = openfastDrivers.runInflowwindDriverCase(caseInputFile, executable) if returnCode != 0: rtl.exitWithError("") - + ### Build the filesystem navigation variables for running the regression test localOutFile = os.path.join(testBuildDirectory, "Points.Velocity.dat") baselineOutFile = os.path.join(targetOutputDirectory, "Points.Velocity.dat") rtl.validateFileOrExit(localOutFile) rtl.validateFileOrExit(baselineOutFile) -testData, testInfo, testPack = pass_fail.readFASTOut(localOutFile) +testData, testInfo, _ = pass_fail.readFASTOut(localOutFile) baselineData, baselineInfo, _ = pass_fail.readFASTOut(baselineOutFile) -performance = pass_fail.calculateNorms(testData, baselineData) -normalizedNorm = performance[:, 1] + +passing_channels = pass_fail.passing_channels(testData.T, baselineData.T) +passing_channels = passing_channels.T + +norms = pass_fail.calculateNorms(testData, baselineData) # export all case summaries -results = list(zip(testInfo["attribute_names"], [*performance])) -results_max = performance.max(axis=0) -exportCaseSummary(testBuildDirectory, caseName, results, results_max, tolerance) +channel_names = testInfo["attribute_names"] +exportCaseSummary(testBuildDirectory, caseName, channel_names, passing_channels, norms) -# failing case -if not pass_fail.passRegressionTest(normalizedNorm, tolerance): - if plotError: - from errorPlotting import finalizePlotDirectory, plotOpenfastError - ixFailChannels = [i for i in range(len(testInfo["attribute_names"])) if normalizedNorm[i] > tolerance] - failChannels = [channel for i, channel in enumerate(testInfo["attribute_names"]) if i in ixFailChannels] - failResults = [res for i, res in enumerate(results) if i in ixFailChannels] - for channel in failChannels: - try: - plotOpenfastError(localOutFile, baselineOutFile, channel) - except: - error = sys.exc_info()[1] - print("Error generating plots: {}".format(error.msg)) - finalizePlotDirectory(localOutFile, failChannels, caseName) - sys.exit(1) - # passing case -sys.exit(0) +if np.all(passing_channels): + sys.exit(0) + +# failing case +if plotError: + from errorPlotting import finalizePlotDirectory, plotOpenfastError + for channel in testInfo["attribute_names"]: + try: + plotOpenfastError(localOutFile, baselineOutFile, channel) + except: + error = sys.exc_info()[1] + print("Error generating plots: {}".format(error)) + finalizePlotDirectory(localOutFile, testInfo["attribute_names"], caseName) + +sys.exit(1) diff --git a/reg_tests/executeInflowwindRegressionCase.py b/reg_tests/executeInflowwindRegressionCase.py index 42c99d6289..521211ac2d 100644 --- a/reg_tests/executeInflowwindRegressionCase.py +++ b/reg_tests/executeInflowwindRegressionCase.py @@ -27,6 +27,7 @@ basepath = os.path.dirname(os.path.abspath(__file__)) sys.path.insert(0, os.path.sep.join([basepath, "lib"])) import argparse +import numpy as np import shutil import glob import subprocess @@ -47,8 +48,6 @@ parser.add_argument("sourceDirectory", metavar="path/to/openfast_repo", type=str, nargs=1, help="The path to the OpenFAST repository.") parser.add_argument("buildDirectory", metavar="path/to/openfast_repo/build", type=str, nargs=1, help="The path to the OpenFAST repository build directory.") parser.add_argument("tolerance", metavar="Test-Tolerance", type=float, nargs=1, help="Tolerance defining pass or failure in the regression test.") -parser.add_argument("systemName", metavar="System-Name", type=str, nargs=1, help="The current system\'s name: [Darwin,Linux,Windows]") -parser.add_argument("compilerId", metavar="Compiler-Id", type=str, nargs=1, help="The compiler\'s id: [Intel,GNU]") parser.add_argument("-p", "-plot", dest="plot", action='store_true', help="bool to include plots in failed cases") parser.add_argument("-n", "-no-exec", dest="noExec", action='store_true', help="bool to prevent execution of the test cases") parser.add_argument("-v", "-verbose", dest="verbose", action='store_true', help="bool to include verbose system output") @@ -108,31 +107,31 @@ rtl.validateFileOrExit(localOutFile) rtl.validateFileOrExit(baselineOutFile) -testData, testInfo, testPack = pass_fail.readFASTOut(localOutFile) +testData, testInfo, _ = pass_fail.readFASTOut(localOutFile) baselineData, baselineInfo, _ = pass_fail.readFASTOut(baselineOutFile) -performance = pass_fail.calculateNorms(testData, baselineData) -normalizedNorm = performance[:, 1] + +passing_channels = pass_fail.passing_channels(testData.T, baselineData.T) +passing_channels = passing_channels.T + +norms = pass_fail.calculateNorms(testData, baselineData) # export all case summaries -results = list(zip(testInfo["attribute_names"], [*performance])) -results_max = performance.max(axis=0) -exportCaseSummary(testBuildDirectory, caseName, results, results_max, tolerance) +channel_names = testInfo["attribute_names"] +exportCaseSummary(testBuildDirectory, caseName, channel_names, passing_channels, norms) -# failing case -if not pass_fail.passRegressionTest(normalizedNorm, tolerance): - if plotError: - from errorPlotting import finalizePlotDirectory, plotOpenfastError - ixFailChannels = [i for i in range(len(testInfo["attribute_names"])) if normalizedNorm[i] > tolerance] - failChannels = [channel for i, channel in enumerate(testInfo["attribute_names"]) if i in ixFailChannels] - failResults = [res for i, res in enumerate(results) if i in ixFailChannels] - for channel in failChannels: - try: - plotOpenfastError(localOutFile, baselineOutFile, channel) - except: - error = sys.exc_info()[1] - print("Error generating plots: {}".format(error.msg)) - finalizePlotDirectory(localOutFile, failChannels, caseName) - sys.exit(1) - # passing case -sys.exit(0) +if np.all(passing_channels): + sys.exit(0) + +# failing case +if plotError: + from errorPlotting import finalizePlotDirectory, plotOpenfastError + for channel in testInfo["attribute_names"]: + try: + plotOpenfastError(localOutFile, baselineOutFile, channel) + except: + error = sys.exc_info()[1] + print("Error generating plots: {}".format(error)) + finalizePlotDirectory(localOutFile, testInfo["attribute_names"], caseName) + +sys.exit(1) diff --git a/reg_tests/executeOpenfastCppRegressionCase.py b/reg_tests/executeOpenfastCppRegressionCase.py index 03f9cccc2b..2ebf96b96c 100644 --- a/reg_tests/executeOpenfastCppRegressionCase.py +++ b/reg_tests/executeOpenfastCppRegressionCase.py @@ -16,9 +16,10 @@ import os import sys -basepath = os.path.dirname(os.path.abspath(__file__)) +basepath = os.path.dirname(__file__) sys.path.insert(0, os.path.sep.join([basepath, "lib"])) import argparse +import numpy as np import shutil import subprocess import rtestlib as rtl @@ -47,8 +48,6 @@ def ignoreBaselineItems(directory, contents): parser.add_argument("sourceDirectory", metavar="path/to/openfast_repo", type=str, nargs=1, help="The path to the OpenFAST repository.") parser.add_argument("buildDirectory", metavar="path/to/openfast_repo/build", type=str, nargs=1, help="The path to the OpenFAST repository build directory.") parser.add_argument("tolerance", metavar="Test-Tolerance", type=float, nargs=1, help="Tolerance defining pass or failure in the regression test.") -parser.add_argument("systemName", metavar="System-Name", type=str, nargs=1, help="The current system\'s name: [Darwin,Linux,Windows]") -parser.add_argument("compilerId", metavar="Compiler-Id", type=str, nargs=1, help="The compiler\'s id: [Intel,GNU]") parser.add_argument("-p", "-plot", dest="plot", action='store_true', help="bool to include plots in failed cases") parser.add_argument("-n", "-no-exec", dest="noExec", action='store_true', help="bool to prevent execution of the test cases") parser.add_argument("-v", "-verbose", dest="verbose", action='store_true', help="bool to include verbose system output") @@ -60,8 +59,6 @@ def ignoreBaselineItems(directory, contents): sourceDirectory = args.sourceDirectory[0] buildDirectory = args.buildDirectory[0] tolerance = args.tolerance[0] -systemName = args.systemName[0] -compilerId = args.compilerId[0] plotError = args.plot noExec = args.noExec verbose = args.verbose @@ -72,32 +69,13 @@ def ignoreBaselineItems(directory, contents): if not os.path.isdir(buildDirectory): os.makedirs(buildDirectory) -### Map the system and compiler configurations to a solution set -# Internal names -> Human readable names -systemName_map = { - "darwin": "macos", - "linux": "linux", - "windows": "windows" -} -compilerId_map = { - "gnu": "gnu", - "intel": "intel" -} -# Build the target output directory name or choose the default -supportedBaselines = ["macos-gnu", "linux-intel", "linux-gnu", "windows-intel"] -targetSystem = systemName_map.get(systemName.lower(), "") -targetCompiler = compilerId_map.get(compilerId.lower(), "") -outputType = os.path.join(targetSystem+"-"+targetCompiler) -if outputType not in supportedBaselines: - outputType = supportedBaselines[0] -print("-- Using gold standard files with machine-compiler type {}".format(outputType)) ### Build the filesystem navigation variables for running openfast on the test case rtest = os.path.join(sourceDirectory, "reg_tests", "r-test") moduleDirectory = os.path.join(rtest, "glue-codes", "openfast-cpp") openfast_gluecode_directory = os.path.join(rtest, "glue-codes", "openfast") inputsDirectory = os.path.join(moduleDirectory, caseName) -targetOutputDirectory = os.path.join(openfast_gluecode_directory, caseName.replace('_cpp', ''), outputType) +targetOutputDirectory = os.path.join(openfast_gluecode_directory, caseName.replace('_cpp', '')) testBuildDirectory = os.path.join(buildDirectory, caseName) # verify all the required directories exist @@ -142,32 +120,35 @@ def ignoreBaselineItems(directory, contents): ### Build the filesystem navigation variables for running the regression test localOutFile = os.path.join(testBuildDirectory, caseName + ".outb") baselineOutFile = os.path.join(targetOutputDirectory, caseName.replace('_cpp', '') + ".outb") + rtl.validateFileOrExit(localOutFile) rtl.validateFileOrExit(baselineOutFile) -testData, testInfo, testPack = pass_fail.readFASTOut(localOutFile) +testData, testInfo, _ = pass_fail.readFASTOut(localOutFile) baselineData, baselineInfo, _ = pass_fail.readFASTOut(baselineOutFile) -performance = pass_fail.calculateNorms(testData, baselineData) -normalizedNorm = performance[:, 1] -# export all case summaries -results = list(zip(testInfo["attribute_names"], [*performance])) -results_max = performance.max(axis=0) -exportCaseSummary(testBuildDirectory, caseName, results, results_max, tolerance) +passing_channels = pass_fail.passing_channels(testData.T, baselineData.T) +passing_channels = passing_channels.T -# failing case -if not pass_fail.passRegressionTest(normalizedNorm, tolerance): - if plotError: - from errorPlotting import finalizePlotDirectory, plotOpenfastError - for channel in testInfo["attribute_names"]: - try: - plotOpenfastError(localOutFile, baselineOutFile, channel) - except: - error = sys.exc_info()[1] - print("Error generating plots: {}".format(error)) - finalizePlotDirectory(localOutFile, testInfo["attribute_names"], caseName) - - sys.exit(1) +norms = pass_fail.calculateNorms(testData, baselineData) + +# export all case summaries +channel_names = testInfo["attribute_names"] +exportCaseSummary(testBuildDirectory, caseName, channel_names, passing_channels, norms) # passing case -sys.exit(0) +if np.all(passing_channels): + sys.exit(0) + +# failing case +if plotError: + from errorPlotting import finalizePlotDirectory, plotOpenfastError + for channel in testInfo["attribute_names"]: + try: + plotOpenfastError(localOutFile, baselineOutFile, channel) + except: + error = sys.exc_info()[1] + print("Error generating plots: {}".format(error)) + finalizePlotDirectory(localOutFile, testInfo["attribute_names"], caseName) + +sys.exit(1) diff --git a/reg_tests/executeOpenfastLinearRegressionCase.py b/reg_tests/executeOpenfastLinearRegressionCase.py index 8e8a9b7015..7d2d9bd051 100644 --- a/reg_tests/executeOpenfastLinearRegressionCase.py +++ b/reg_tests/executeOpenfastLinearRegressionCase.py @@ -24,9 +24,10 @@ import os import sys -basepath = os.path.dirname(os.path.abspath(__file__)) +basepath = os.path.dirname(__file__) sys.path.insert(0, os.path.sep.join([basepath, "lib"])) import argparse +import numpy as np import shutil import subprocess import rtestlib as rtl @@ -65,11 +66,9 @@ def isclose(a, b, rel_tol=1e-09, abs_tol=0.0): parser.add_argument("sourceDirectory", metavar="path/to/openfast_repo", type=str, nargs=1, help="The path to the OpenFAST repository.") parser.add_argument("buildDirectory", metavar="path/to/openfast_repo/build", type=str, nargs=1, help="The path to the OpenFAST repository build directory.") parser.add_argument("tolerance", metavar="Test-Tolerance", type=float, nargs=1, help="Tolerance defining pass or failure in the regression test.") -parser.add_argument("systemName", metavar="System-Name", type=str, nargs=1, help="The current system\'s name: [Darwin,Linux,Windows]") -parser.add_argument("compilerId", metavar="Compiler-Id", type=str, nargs=1, help="The compiler\'s id: [Intel,GNU]") -parser.add_argument("-p", "-plot", dest="plot", default=False, metavar="Plotting-Flag", type=bool, nargs="?", help="bool to include plots in failed cases") -parser.add_argument("-n", "-no-exec", dest="noExec", default=False, metavar="No-Execution", type=bool, nargs="?", help="bool to prevent execution of the test cases") -parser.add_argument("-v", "-verbose", dest="verbose", default=False, metavar="Verbose-Flag", type=bool, nargs="?", help="bool to include verbose system output") +parser.add_argument("-p", "-plot", dest="plot", action='store_true', help="bool to include plots in failed cases") +parser.add_argument("-n", "-no-exec", dest="noExec", action='store_true', help="bool to prevent execution of the test cases") +parser.add_argument("-v", "-verbose", dest="verbose", action='store_true', help="bool to include verbose system output") args = parser.parse_args() @@ -78,11 +77,9 @@ def isclose(a, b, rel_tol=1e-09, abs_tol=0.0): sourceDirectory = args.sourceDirectory[0] buildDirectory = args.buildDirectory[0] tolerance = args.tolerance[0] -systemName = args.systemName[0] -compilerId = args.compilerId[0] -plotError = args.plot if args.plot is False else True -noExec = args.noExec if args.noExec is False else True -verbose = args.verbose if args.verbose is False else True +plotError = args.plot +noExec = args.noExec +verbose = args.verbose # validate inputs rtl.validateExeOrExit(executable) @@ -90,33 +87,13 @@ def isclose(a, b, rel_tol=1e-09, abs_tol=0.0): if not os.path.isdir(buildDirectory): os.makedirs(buildDirectory) -### Map the system and compiler configurations to a solution set -# Internal names -> Human readable names -systemName_map = { - "darwin": "macos", - "linux": "linux", - "windows": "windows" -} -compilerId_map = { - "gnu": "gnu", - "intel": "intel" -} -# Build the target output directory name or choose the default -supportedBaselines = ["macos-gnu", "linux-intel", "linux-gnu", "windows-intel"] -targetSystem = systemName_map.get(systemName.lower(), "") -targetCompiler = compilerId_map.get(compilerId.lower(), "") -outputType = os.path.join(targetSystem+"-"+targetCompiler) -if outputType not in supportedBaselines: - outputType = supportedBaselines[0] -print("-- Using gold standard files with machine-compiler type {}".format(outputType)) - ### Build the filesystem navigation variables for running openfast on the test case regtests = os.path.join(sourceDirectory, "reg_tests") lib = os.path.join(regtests, "lib") rtest = os.path.join(regtests, "r-test") moduleDirectory = os.path.join(rtest, "glue-codes", "openfast") inputsDirectory = os.path.join(moduleDirectory, caseName) -targetOutputDirectory = os.path.join(inputsDirectory, outputType) +targetOutputDirectory = os.path.join(inputsDirectory) testBuildDirectory = os.path.join(buildDirectory, caseName) # verify all the required directories exist @@ -162,11 +139,8 @@ def isclose(a, b, rel_tol=1e-09, abs_tol=0.0): if returnCode != 0: rtl.exitWithError("") -### Get a list of all the files in the baseline directory -baselineOutFiles = os.listdir(targetOutputDirectory) -# Drop the log file, if its listed -if caseName + '.log' in baselineOutFiles: - baselineOutFiles.remove(caseName + '.log') +### Get a all the .lin files in the baseline directory +baselineOutFiles = [f for f in os.listdir(targetOutputDirectory) if '.lin' in f] # these should all exist in the local outputs directory localFiles = os.listdir(testBuildDirectory) @@ -240,7 +214,9 @@ def isclose(a, b, rel_tol=1e-09, abs_tol=0.0): l_float = float(l_element) b_float = float(b_elements[j]) if not isclose(l_float, b_float, tolerance, tolerance): - print(f"Failed in Jacobian matrix comparison: {l_float} and {b_float}") + print(f"Failed in Jacobian matrix comparison:") + print(f"{l_float} in {local_file}") + print(f"{b_float} in {baseline_file}") sys.exit(1) # skip 2 empty/header lines diff --git a/reg_tests/executePythonRegressionCase.py b/reg_tests/executePythonRegressionCase.py index 58c2599290..753b5c0457 100644 --- a/reg_tests/executePythonRegressionCase.py +++ b/reg_tests/executePythonRegressionCase.py @@ -25,11 +25,12 @@ import os import sys -basepath = os.path.sep.join(sys.argv[0].split(os.path.sep)[:-1]) if os.path.sep in sys.argv[0] else "." +basepath = os.path.dirname(__file__) sys.path.insert(0, os.path.sep.join([basepath, "lib"])) sys.path.insert(0, os.path.sep.join([basepath, "..", "glue-codes", "python"])) import platform import argparse +import numpy as np import shutil import subprocess import rtestlib as rtl @@ -59,8 +60,6 @@ def ignoreBaselineItems(directory, contents): parser.add_argument("sourceDirectory", metavar="path/to/openfast_repo", type=str, nargs=1, help="The path to the OpenFAST repository.") parser.add_argument("buildDirectory", metavar="path/to/openfast_repo/build", type=str, nargs=1, help="The path to the OpenFAST repository build directory.") parser.add_argument("tolerance", metavar="Test-Tolerance", type=float, nargs=1, help="Tolerance defining pass or failure in the regression test.") -parser.add_argument("systemName", metavar="System-Name", type=str, nargs=1, help="The current system\'s name: [Darwin,Linux,Windows]") -parser.add_argument("compilerId", metavar="Compiler-Id", type=str, nargs=1, help="The compiler\'s id: [Intel,GNU]") parser.add_argument("-p", "-plot", dest="plot", action='store_true', help="bool to include plots in failed cases") parser.add_argument("-n", "-no-exec", dest="noExec", action='store_true', help="bool to prevent execution of the test cases") parser.add_argument("-v", "-verbose", dest="verbose", action='store_true', help="bool to include verbose system output") @@ -71,8 +70,6 @@ def ignoreBaselineItems(directory, contents): sourceDirectory = args.sourceDirectory[0] buildDirectory = args.buildDirectory[0] tolerance = args.tolerance[0] -systemName = args.systemName[0] -compilerId = args.compilerId[0] plotError = args.plot noExec = args.noExec verbose = args.verbose @@ -82,25 +79,6 @@ def ignoreBaselineItems(directory, contents): if not os.path.isdir(buildDirectory): os.makedirs(buildDirectory) -### Map the system and compiler configurations to a solution set -# Internal names -> Human readable names -systemName_map = { - "darwin": "macos", - "linux": "linux", - "windows": "windows" -} -compilerId_map = { - "gnu": "gnu", - "intel": "intel" -} -# Build the target output directory name or choose the default -supportedBaselines = ["macos-gnu", "linux-intel", "linux-gnu", "windows-intel"] -targetSystem = systemName_map.get(systemName.lower(), "") -targetCompiler = compilerId_map.get(compilerId.lower(), "") -outputType = os.path.join(targetSystem+"-"+targetCompiler) -if outputType not in supportedBaselines: - outputType = supportedBaselines[0] -print("-- Using gold standard files with machine-compiler type {}".format(outputType)) ### Build the filesystem navigation variables for running openfast on the test case regtests = os.path.join(sourceDirectory, "reg_tests") @@ -108,7 +86,7 @@ def ignoreBaselineItems(directory, contents): rtest = os.path.join(regtests, "r-test") moduleDirectory = os.path.join(rtest, "glue-codes", "openfast") inputsDirectory = os.path.join(moduleDirectory, caseName) -targetOutputDirectory = os.path.join(inputsDirectory, outputType) +targetOutputDirectory = os.path.join(inputsDirectory) testBuildDirectory = os.path.join(buildDirectory, caseName) # verify all the required directories exist @@ -126,6 +104,7 @@ def ignoreBaselineItems(directory, contents): if not os.path.isdir(dataDir): shutil.copytree(os.path.join(moduleDirectory, data), dataDir) +# Special copy for the 5MW_Baseline folder because the Windows python-only workflow may have already created data in the subfolder ServoData dst = os.path.join(buildDirectory, "5MW_Baseline") src = os.path.join(moduleDirectory, "5MW_Baseline") if not os.path.isdir(dst): @@ -173,27 +152,29 @@ def ignoreBaselineItems(directory, contents): } testData = openfastlib.output_values baselineData, baselineInfo, _ = pass_fail.readFASTOut(baselineOutFile) -performance = pass_fail.calculateNorms(testData, baselineData) -normalizedNorm = performance[:, 1] -# export all case summaries -results = list(zip(testInfo["attribute_names"], [*performance])) -results_max = performance.max(axis=0) -exportCaseSummary(testBuildDirectory, caseName, results, results_max, tolerance) +passing_channels = pass_fail.passing_channels(testData.T, baselineData.T) +passing_channels = passing_channels.T -# failing case -if not pass_fail.passRegressionTest(normalizedNorm, tolerance): - if plotError: - from errorPlotting import finalizePlotDirectory, plotOpenfastError - for channel in testInfo["attribute_names"]: - try: - plotOpenfastError(localOutFile, baselineOutFile, channel) - except: - error = sys.exc_info()[1] - print("Error generating plots: {}".format(error)) - finalizePlotDirectory(localOutFile, testInfo["attribute_names"], caseName) - - sys.exit(1) +norms = pass_fail.calculateNorms(testData, baselineData) + +# export all case summaries +channel_names = testInfo["attribute_names"] +exportCaseSummary(testBuildDirectory, caseName, channel_names, passing_channels, norms) # passing case -sys.exit(0) +if np.all(passing_channels): + sys.exit(0) + +# failing case +if plotError: + from errorPlotting import finalizePlotDirectory, plotOpenfastError + for channel in testInfo["attribute_names"]: + try: + plotOpenfastError(localOutFile, baselineOutFile, channel) + except: + error = sys.exc_info()[1] + print("Error generating plots: {}".format(error)) + finalizePlotDirectory(localOutFile, testInfo["attribute_names"], caseName) + +sys.exit(1) diff --git a/reg_tests/executeSubdynRegressionCase.py b/reg_tests/executeSubdynRegressionCase.py index 94e04a710f..6efa5cbb78 100644 --- a/reg_tests/executeSubdynRegressionCase.py +++ b/reg_tests/executeSubdynRegressionCase.py @@ -27,6 +27,7 @@ basepath = os.path.dirname(os.path.abspath(__file__)) sys.path.insert(0, os.path.sep.join([basepath, "lib"])) import argparse +import numpy as np import shutil import glob import subprocess @@ -47,8 +48,6 @@ parser.add_argument("sourceDirectory", metavar="path/to/openfast_repo", type=str, nargs=1, help="The path to the OpenFAST repository.") parser.add_argument("buildDirectory", metavar="path/to/openfast_repo/build", type=str, nargs=1, help="The path to the OpenFAST repository build directory.") parser.add_argument("tolerance", metavar="Test-Tolerance", type=float, nargs=1, help="Tolerance defining pass or failure in the regression test.") -parser.add_argument("systemName", metavar="System-Name", type=str, nargs=1, help="The current system\'s name: [Darwin,Linux,Windows]") -parser.add_argument("compilerId", metavar="Compiler-Id", type=str, nargs=1, help="The compiler\'s id: [Intel,GNU]") parser.add_argument("-p", "-plot", dest="plot", action='store_true', default=False, help="bool to include matplotlib plots in failed cases") parser.add_argument("-n", "-no-exec", dest="noExec", action='store_true', default=False, help="bool to prevent execution of the test cases") parser.add_argument("-v", "-verbose", dest="verbose", action='store_true', default=False, help="bool to include verbose system output") @@ -104,34 +103,35 @@ ### Build the filesystem navigation variables for running the regression test localOutFile = os.path.join(testBuildDirectory, caseName+".SD.out") baselineOutFile = os.path.join(targetOutputDirectory, caseName+".SD.out") + rtl.validateFileOrExit(localOutFile) rtl.validateFileOrExit(baselineOutFile) -testData, testInfo, testPack = pass_fail.readFASTOut(localOutFile) +testData, testInfo, _ = pass_fail.readFASTOut(localOutFile) baselineData, baselineInfo, _ = pass_fail.readFASTOut(baselineOutFile) -performance = pass_fail.calculateNorms(testData, baselineData) -normalizedNorm = performance[:, 1] + +passing_channels = pass_fail.passing_channels(testData.T, baselineData.T) +passing_channels = passing_channels.T + +norms = pass_fail.calculateNorms(testData, baselineData) # export all case summaries -results = list(zip(testInfo["attribute_names"], [*performance])) -results_max = performance.max(axis=0) -exportCaseSummary(testBuildDirectory, caseName, results, results_max, tolerance) +channel_names = testInfo["attribute_names"] +exportCaseSummary(testBuildDirectory, caseName, channel_names, passing_channels, norms) -# failing case -if not pass_fail.passRegressionTest(normalizedNorm, tolerance): - if plotError: - from errorPlotting import finalizePlotDirectory, plotOpenfastError - ixFailChannels = [i for i in range(len(testInfo["attribute_names"])) if normalizedNorm[i] > tolerance] - failChannels = [channel for i, channel in enumerate(testInfo["attribute_names"]) if i in ixFailChannels] - failResults = [res for i, res in enumerate(results) if i in ixFailChannels] - for channel in failChannels: - try: - plotOpenfastError(localOutFile, baselineOutFile, channel) - except: - error = sys.exc_info()[1] - print("Error generating plots: {}".format(error.msg)) - finalizePlotDirectory(localOutFile, failChannels, caseName) - sys.exit(1) - # passing case -sys.exit(0) +if np.all(passing_channels): + sys.exit(0) + +# failing case +if plotError: + from errorPlotting import finalizePlotDirectory, plotOpenfastError + for channel in testInfo["attribute_names"]: + try: + plotOpenfastError(localOutFile, baselineOutFile, channel) + except: + error = sys.exc_info()[1] + print("Error generating plots: {}".format(error)) + finalizePlotDirectory(localOutFile, testInfo["attribute_names"], caseName) + +sys.exit(1) From acf2c89a012d116389a05f241cca40d99c0cc536 Mon Sep 17 00:00:00 2001 From: Rafael M Mudafort Date: Wed, 22 Jun 2022 15:02:03 -0500 Subject: [PATCH 173/242] Update reg test plotting - bug fixes, Bokeh 2.4 --- reg_tests/lib/errorPlotting.py | 30 +++++++++++++++++++++--------- 1 file changed, 21 insertions(+), 9 deletions(-) diff --git a/reg_tests/lib/errorPlotting.py b/reg_tests/lib/errorPlotting.py index ed686ead36..998d2937b9 100644 --- a/reg_tests/lib/errorPlotting.py +++ b/reg_tests/lib/errorPlotting.py @@ -51,7 +51,7 @@ def _plotError(xseries, y1series, y2series, xlabel, title1, title2): from bokeh.embed import components from bokeh.layouts import gridplot from bokeh.plotting import figure - from bokeh.models.tools import HoverTool, BoxZoomTool + from bokeh.models.tools import HoverTool p1 = figure(title=title1) p1.title.align = 'center' @@ -74,7 +74,7 @@ def _plotError(xseries, y1series, y2series, xlabel, title1, title2): p2.add_tools(HoverTool(tooltips=[('Time','@x'), ('Error', '@y')], mode='vline')) - grid = gridplot([[p1, p2]], plot_width=650, plot_height=375, sizing_mode="scale_both") + grid = gridplot([[p1, p2]], width=650, height=375, sizing_mode="scale_both") script, div = components(grid) return script, div @@ -86,7 +86,7 @@ def _replace_id_div(html_string, plot): return html_string def _replace_id_script(html_string, plot): - id_start = html_string.find('var render_items') + id_start = html_string.find('const render_items') id_start += html_string[id_start:].find('roots') id_start += html_string[id_start:].find('":"') + 3 id_end = html_string[id_start:].find('"') + id_start @@ -98,7 +98,8 @@ def _save_plot(script, div, path, attribute): file_name = "_script".join((attribute, ".txt")) with open(os.path.join(path, file_name), 'w') as f: - script = _replace_id_script(script.replace('\n', '\n '), attribute) + script = script.replace('\n', '\n ') + script = _replace_id_script(script, attribute) f.write(script) file_name = "_div".join((attribute, ".txt")) @@ -137,19 +138,25 @@ def plotOpenfastError(testSolution, baselineSolution, attribute): _save_plot(script, div, plotPath, attribute) def _htmlHead(title): + from bokeh.resources import CDN + head = '' + '\n' head += '' + '\n' head += '' + '\n' head += ' {}'.format(title) + '\n' - + + # CSS head += ' ' + '\n' head += ' ' + '\n' head += ' ' + '\n' - + + # JS head += ' ' + '\n' head += ' ' + '\n' - head += ' ' + '\n' - head += ' ' + '\n' + + # JS - Bokeh + head += f' ' + '\n' + head += f' ' + '\n' head += ' ' + '\n' head += '