diff --git a/modules/aerodyn/src/AeroDyn.f90 b/modules/aerodyn/src/AeroDyn.f90 index c4e7f79f84..762778d081 100644 --- a/modules/aerodyn/src/AeroDyn.f90 +++ b/modules/aerodyn/src/AeroDyn.f90 @@ -7433,7 +7433,7 @@ SUBROUTINE Perturb_u( p, n, perturb_sign, u, du ) ! Module/Mesh/Field: u%TowerMotion%TranslationVel = 11; ! Module/Mesh/Field: u%TowerMotion%TranslationAcc = 12; case( 9); u%TowerMotion%TranslationDisp(fieldIndx,node) = u%TowerMotion%TranslationDisp( fieldIndx,node) + du * perturb_sign - case(10); CALL PerturbOrientationMatrix( u%TowerMotion%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. ) + case(10); CALL PerturbOrientationMatrix( u%TowerMotion%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.false. ) case(11); u%TowerMotion%TranslationVel( fieldIndx,node) = u%TowerMotion%TranslationVel( fieldIndx,node) + du * perturb_sign case(12); u%TowerMotion%TranslationAcc( fieldIndx,node) = u%TowerMotion%TranslationAcc(fieldIndx,node) + du * perturb_sign diff --git a/modules/elastodyn/src/ElastoDyn.f90 b/modules/elastodyn/src/ElastoDyn.f90 index 3ada99d44f..297db86846 100644 --- a/modules/elastodyn/src/ElastoDyn.f90 +++ b/modules/elastodyn/src/ElastoDyn.f90 @@ -293,7 +293,9 @@ SUBROUTINE ED_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, InitOut ! Platform reference point wrt to global origin (0,0,0) InitOut%PlatformPos = x%QT(1:6) - CALL SmllRotTrans('initial platform rotation', x%QT(4), x%QT(5), x%QT(6), TransMat, '', ErrStat2, ErrMsg2) + ! CALL SmllRotTrans('initial platform rotation', x%QT(4), x%QT(5), x%QT(6), TransMat, '', ErrStat2, ErrMsg2) + TransMat = EulerConstructZYX((/x%QT(4),x%QT(5),x%QT(6)/)) + InitOut%PlatformPos(1) = InitOut%PlatformPos(1) - TransMat(3,1)*p%PtfmRefzt InitOut%PlatformPos(2) = InitOut%PlatformPos(2) - TransMat(3,2)*p%PtfmRefzt InitOut%PlatformPos(3) = InitOut%PlatformPos(3) - TransMat(3,3)*p%PtfmRefzt + p%PtfmRefzt @@ -490,6 +492,8 @@ SUBROUTINE ED_UpdateStates( t, n, u, utimes, p, x, xd, z, OtherState, m, ErrStat ErrStat = ErrID_None ErrMsg = "" + ! Passing in u(1) as a dummy. ED_UpdateDiscState does not require input, u, only the continuous state, x. + CALL ED_UpdateDiscState( t, n, u(1), p, x, xd, z, OtherState, m, ErrStat, ErrMsg ) SELECT CASE ( p%method ) @@ -522,7 +526,6 @@ SUBROUTINE ED_UpdateStates( t, n, u, utimes, p, x, xd, z, OtherState, m, ErrStat IF ( ( x%QT(DOF_GeAz) + x%QT(DOF_DrTr) ) >= TwoPi_D ) x%QT(DOF_GeAz) = x%QT(DOF_GeAz) - TwoPi_D - END SUBROUTINE ED_UpdateStates !---------------------------------------------------------------------------------------------------------------------------------- !> Routine for computing outputs, used in both loose and tight coupling. @@ -667,9 +670,13 @@ SUBROUTINE ED_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) FrcT0Trb = m%RtHS%FrcT0Trbt ! was FZHydro = m%RtHS%FZHydrot - FZHydro = u%PlatformPtMesh%Force(DOF_Sg,1)*m%RtHS%PLinVelEZ(DOF_Sg,0,:) & - + u%PlatformPtMesh%Force(DOF_Sw,1)*m%RtHS%PLinVelEZ(DOF_Sw,0,:) & - + u%PlatformPtMesh%Force(DOF_Hv,1)*m%RtHS%PLinVelEZ(DOF_Hv,0,:) + ! FZHydro = u%PlatformPtMesh%Force(DOF_Sg,1)*m%RtHS%PLinVelEZ(DOF_Sg,0,:) & + ! + u%PlatformPtMesh%Force(DOF_Sw,1)*m%RtHS%PLinVelEZ(DOF_Sw,0,:) & + ! + u%PlatformPtMesh%Force(DOF_Hv,1)*m%RtHS%PLinVelEZ(DOF_Hv,0,:) + + FZHydro = u%PlatformPtMesh%Force(DOF_Sg,1)*m%CoordSys%z1 & + - u%PlatformPtMesh%Force(DOF_Sw,1)*m%CoordSys%z3 & + + u%PlatformPtMesh%Force(DOF_Hv,1)*m%CoordSys%z2 MomBNcRt = m%RtHS%MomBNcRtt MomLPRot = m%RtHS%MomLPRott @@ -678,9 +685,13 @@ SUBROUTINE ED_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) MomX0Trb = m%RtHS%MomX0Trbt ! was MXHydro = m%RtHS%MXHydrot - MXHydro = u%PlatformPtMesh%Moment(DOF_R-3,1)*m%RtHS%PAngVelEX(DOF_R ,0,:) & - + u%PlatformPtMesh%Moment(DOF_P-3,1)*m%RtHS%PAngVelEX(DOF_P ,0,:) & - + u%PlatformPtMesh%Moment(DOF_Y-3,1)*m%RtHS%PAngVelEX(DOF_Y ,0,:) + ! MXHydro = u%PlatformPtMesh%Moment(DOF_R-3,1)*m%RtHS%PAngVelEX(DOF_R ,0,:) & + ! + u%PlatformPtMesh%Moment(DOF_P-3,1)*m%RtHS%PAngVelEX(DOF_P ,0,:) & + ! + u%PlatformPtMesh%Moment(DOF_Y-3,1)*m%RtHS%PAngVelEX(DOF_Y ,0,:) + + MXHydro = u%PlatformPtMesh%Moment(DOF_R-3,1)*m%CoordSys%z1 & + - u%PlatformPtMesh%Moment(DOF_P-3,1)*m%CoordSys%z3 & + + u%PlatformPtMesh%Moment(DOF_Y-3,1)*m%CoordSys%z2 DO I = 1,p%DOFs%NActvDOF ! Loop through all active (enabled) DOFs AngAccEB = AngAccEB + m%RtHS%PAngVelEB (p%DOFs%SrtPS(I),0,:)*m%QD2T(p%DOFs%SrtPS(I)) @@ -1008,9 +1019,13 @@ SUBROUTINE ED_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) m%AllOuts( TwHtTPyi(I) ) = -1.0*m%RtHS%rT(3,p%TwrGagNd(I)) m%AllOuts( TwHtTPzi(I) ) = m%RtHS%rT(2,p%TwrGagNd(I)) + p%PtfmRefzt - m%AllOuts( TwHtRPxi(I) ) = m%RtHS%AngPosEF(1,p%TwrGagNd(I))*R2D - m%AllOuts( TwHtRPyi(I) ) = -m%RtHS%AngPosEF(3,p%TwrGagNd(I))*R2D - m%AllOuts( TwHtRPzi(I) ) = m%RtHS%AngPosEF(2,p%TwrGagNd(I))*R2D + ! m%AllOuts( TwHtRPxi(I) ) = m%RtHS%AngPosEF(1,p%TwrGagNd(I))*R2D + ! m%AllOuts( TwHtRPyi(I) ) = -m%RtHS%AngPosEF(3,p%TwrGagNd(I))*R2D + ! m%AllOuts( TwHtRPzi(I) ) = m%RtHS%AngPosEF(2,p%TwrGagNd(I))*R2D + + m%AllOuts( TwHtRPxi(I) ) = m%RtHS%AngPosEF(1,p%TwrGagNd(I))*R2D ! <- AngPosEF is now simply the roll, pitch, and yaw angles (possibly large) of each tower section + m%AllOuts( TwHtRPyi(I) ) = m%RtHS%AngPosEF(2,p%TwrGagNd(I))*R2D + m%AllOuts( TwHtRPzi(I) ) = m%RtHS%AngPosEF(3,p%TwrGagNd(I))*R2D END DO !I @@ -1046,15 +1061,21 @@ SUBROUTINE ED_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) m%AllOuts( PtfmRVxt) = DOT_PRODUCT( m%RtHS%AngVelEX, m%CoordSys%a1 )*R2D m%AllOuts( PtfmRVyt) = -DOT_PRODUCT( m%RtHS%AngVelEX, m%CoordSys%a3 )*R2D m%AllOuts( PtfmRVzt) = DOT_PRODUCT( m%RtHS%AngVelEX, m%CoordSys%a2 )*R2D - m%AllOuts( PtfmRVxi) = x%QDT (DOF_R )*R2D - m%AllOuts( PtfmRVyi) = x%QDT (DOF_P )*R2D - m%AllOuts( PtfmRVzi) = x%QDT (DOF_Y )*R2D + ! m%AllOuts( PtfmRVxi) = x%QDT (DOF_R )*R2D + ! m%AllOuts( PtfmRVyi) = x%QDT (DOF_P )*R2D + ! m%AllOuts( PtfmRVzi) = x%QDT (DOF_Y )*R2D + m%AllOuts( PtfmRVxi) = m%RtHS%AngVelEX(1)*R2D + m%AllOuts( PtfmRVyi) = -m%RtHS%AngVelEX(3)*R2D + m%AllOuts( PtfmRVzi) = m%RtHS%AngVelEX(2)*R2D m%AllOuts( PtfmRAxt) = DOT_PRODUCT( AngAccEX, m%CoordSys%a1 )*R2D m%AllOuts( PtfmRAyt) = -DOT_PRODUCT( AngAccEX, m%CoordSys%a3 )*R2D m%AllOuts( PtfmRAzt) = DOT_PRODUCT( AngAccEX, m%CoordSys%a2 )*R2D - m%AllOuts( PtfmRAxi) = m%QD2T(DOF_R )*R2D - m%AllOuts( PtfmRAyi) = m%QD2T(DOF_P )*R2D - m%AllOuts( PtfmRAzi) = m%QD2T(DOF_Y )*R2D + ! m%AllOuts( PtfmRAxi) = m%QD2T(DOF_R )*R2D + ! m%AllOuts( PtfmRAyi) = m%QD2T(DOF_P )*R2D + ! m%AllOuts( PtfmRAzi) = m%QD2T(DOF_Y )*R2D + m%AllOuts( PtfmRAxi) = AngAccEX(1)*R2D + m%AllOuts( PtfmRAyi) = -AngAccEX(3)*R2D + m%AllOuts( PtfmRAzi) = AngAccEX(2)*R2D @@ -1638,28 +1659,37 @@ SUBROUTINE ED_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) y%PlatformPtMesh%TranslationDisp(2,1) = x%QT(DOF_Sw) y%PlatformPtMesh%TranslationDisp(3,1) = x%QT(DOF_Hv) - y%PlatformPtMesh%RotationVel(1,1) = x%QDT(DOF_R ) - y%PlatformPtMesh%RotationVel(2,1) = x%QDT(DOF_P ) - y%PlatformPtMesh%RotationVel(3,1) = x%QDT(DOF_Y ) + ! y%PlatformPtMesh%RotationVel(1,1) = x%QDT(DOF_R ) + ! y%PlatformPtMesh%RotationVel(2,1) = x%QDT(DOF_P ) + ! y%PlatformPtMesh%RotationVel(3,1) = x%QDT(DOF_Y ) + + y%PlatformPtMesh%RotationVel(1,1) = m%RtHS%AngVelEX(1) + y%PlatformPtMesh%RotationVel(2,1) = -1.*m%RtHS%AngVelEX(3) + y%PlatformPtMesh%RotationVel(3,1) = m%RtHS%AngVelEX(2) y%PlatformPtMesh%TranslationVel(1,1) = x%QDT(DOF_Sg) y%PlatformPtMesh%TranslationVel(2,1) = x%QDT(DOF_Sw) y%PlatformPtMesh%TranslationVel(3,1) = x%QDT(DOF_Hv) + ! CALL SmllRotTrans( 'platform displacement (ED_CalcOutput)', x%QT(DOF_R ),x%QT(DOF_P ),x%QT(DOF_Y ), & + ! y%PlatformPtMesh%Orientation(:,:,1), errstat=ErrStat, errmsg=ErrMsg ) + ! IF (ErrStat /= ErrID_None) ErrMsg = TRIM(ErrMsg)//' (occurred at '//TRIM(Num2LStr(t))//' s)' + ! !IF (ErrStat >= AbortErrLev) RETURN + + y%PlatformPtMesh%Orientation(:,:,1) = EulerConstructZYX((/x%QT(DOF_R ),x%QT(DOF_P ),x%QT(DOF_Y )/)) - CALL SmllRotTrans( 'platform displacement (ED_CalcOutput)', x%QT(DOF_R ),x%QT(DOF_P ),x%QT(DOF_Y ), & - y%PlatformPtMesh%Orientation(:,:,1), errstat=ErrStat, errmsg=ErrMsg ) - IF (ErrStat /= ErrID_None) ErrMsg = TRIM(ErrMsg)//' (occurred at '//TRIM(Num2LStr(t))//' s)' - !IF (ErrStat >= AbortErrLev) RETURN + ! y%PlatformPtMesh%RotationAcc(1,1) = m%QD2T(DOF_R ) + ! y%PlatformPtMesh%RotationAcc(2,1) = m%QD2T(DOF_P ) + ! y%PlatformPtMesh%RotationAcc(3,1) = m%QD2T(DOF_Y ) - y%PlatformPtMesh%RotationAcc(1,1) = m%QD2T(DOF_R ) - y%PlatformPtMesh%RotationAcc(2,1) = m%QD2T(DOF_P ) - y%PlatformPtMesh%RotationAcc(3,1) = m%QD2T(DOF_Y ) + y%PlatformPtMesh%RotationAcc(1,1) = AngAccEX(1) + y%PlatformPtMesh%RotationAcc(2,1) = -1.*AngAccEX(3) + y%PlatformPtMesh%RotationAcc(3,1) = AngAccEX(2) y%PlatformPtMesh%TranslationAcc(1,1) = m%QD2T(DOF_Sg) y%PlatformPtMesh%TranslationAcc(2,1) = m%QD2T(DOF_Sw) y%PlatformPtMesh%TranslationAcc(3,1) = m%QD2T(DOF_Hv) - + !............................................................................................................................... ! Outputs required for external tower loads !............................................................................................................................... @@ -1868,10 +1898,12 @@ SUBROUTINE ED_CalcContStateDeriv( t, u, p, x, xd, z, OtherState, m, dxdt, ErrSta IF (ErrStat >= AbortErrLev) RETURN CALL CalculatePositions( p, x, m%CoordSys, m%RtHS ) ! calculate positions - CALL CalculateAngularPosVelPAcc(p, x, m%CoordSys, m%RtHS ) ! calculate angular positions, velocities, and partial accelerations, including partial angular quantities + CALL CalculateAngularPosVelPAcc(p, x, m%CoordSys, m%RtHS, ErrStat2, ErrMsg2 ) ! calculate angular positions, velocities, and partial accelerations, including partial angular quantities CALL CalculateLinearVelPAcc( p, x, m%CoordSys, m%RtHS ) ! calculate linear velocities and partial accelerations CALL CalculateForcesMoments( p, x, m%CoordSys, u, m%RtHS ) ! calculate the forces and moments (requires AeroBladeForces and AeroBladeMoments) - + call setErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + END IF !..................................... @@ -3418,7 +3450,6 @@ SUBROUTINE SetPrimaryParameters( InitInp, p, InputFileData, ErrStat, ErrMsg ) !p%NcIMUyn = InputFileData%NcIMUyn !p%NcIMUzn = InputFileData%NcIMUzn - ! plus everything else from FAST_Initialize @@ -5943,14 +5974,28 @@ SUBROUTINE SetCoordSy( t, CoordSys, RtHSdat, BlPitch, p, x, ErrStat, ErrMsg ) ! Tower base / platform coordinate system: - CALL SmllRotTrans( 'platform displacement (ElastoDyn SetCoordSy)', x%QT(DOF_R), x%QT(DOF_Y), -x%QT(DOF_P), TransMat, TRIM(Num2LStr(t))//' s', ErrStat2, ErrMsg2 ) ! Get the transformation matrix, TransMat, from inertial frame to tower base / platform coordinate systems. - CALL CheckError( ErrStat2, ErrMsg2 ) - IF (ErrStat >= AbortErrLev) RETURN + ! CALL SmllRotTrans( 'platform displacement (ElastoDyn SetCoordSy)', x%QT(DOF_R), x%QT(DOF_Y), -x%QT(DOF_P), TransMat, TRIM(Num2LStr(t))//' s', ErrStat2, ErrMsg2 ) ! Get the transformation matrix, TransMat, from inertial frame to tower base / platform coordinate systems. + ! CALL CheckError( ErrStat2, ErrMsg2 ) + ! IF (ErrStat >= AbortErrLev) RETURN + + ! CoordSys%a1 = TransMat(1,1)*CoordSys%z1 + TransMat(1,2)*CoordSys%z2 + TransMat(1,3)*CoordSys%z3 ! Vector / direction a1 (= xt from the IEC coord. system). + ! CoordSys%a2 = TransMat(2,1)*CoordSys%z1 + TransMat(2,2)*CoordSys%z2 + TransMat(2,3)*CoordSys%z3 ! Vector / direction a2 (= zt from the IEC coord. system). + ! CoordSys%a3 = TransMat(3,1)*CoordSys%z1 + TransMat(3,2)*CoordSys%z2 + TransMat(3,3)*CoordSys%z3 ! Vector / direction a3 (= -yt from the IEC coord. system). - CoordSys%a1 = TransMat(1,1)*CoordSys%z1 + TransMat(1,2)*CoordSys%z2 + TransMat(1,3)*CoordSys%z3 ! Vector / direction a1 (= xt from the IEC coord. system). - CoordSys%a2 = TransMat(2,1)*CoordSys%z1 + TransMat(2,2)*CoordSys%z2 + TransMat(2,3)*CoordSys%z3 ! Vector / direction a2 (= zt from the IEC coord. system). - CoordSys%a3 = TransMat(3,1)*CoordSys%z1 + TransMat(3,2)*CoordSys%z2 + TransMat(3,3)*CoordSys%z3 ! Vector / direction a3 (= -yt from the IEC coord. system). + ! Platform orientation after yaw + CoordSys%alpha1 = cos(x%QT(DOF_Y))*CoordSys%z1 - sin(x%QT(DOF_Y))*CoordSys%z3 + CoordSys%alpha2 = CoordSys%z2 + CoordSys%alpha3 = sin(x%QT(DOF_Y))*CoordSys%z1 + cos(x%QT(DOF_Y))*CoordSys%z3 + ! Platform orientation after pitch + CoordSys%beta1 = cos(x%QT(DOF_P))*CoordSys%alpha1 - sin(x%QT(DOF_P))*CoordSys%alpha2 + CoordSys%beta2 = sin(x%QT(DOF_P))*CoordSys%alpha1 + cos(x%QT(DOF_P))*CoordSys%alpha2 + CoordSys%beta3 = CoordSys%alpha3 + + ! Platform orientation after roll + CoordSys%a1 = CoordSys%beta1 + CoordSys%a2 = cos(x%QT(DOF_R))*CoordSys%beta2 + sin(x%QT(DOF_R))*CoordSys%beta3 + CoordSys%a3 = -sin(x%QT(DOF_R))*CoordSys%beta2 + cos(x%QT(DOF_R))*CoordSys%beta3 DO J = 1,p%TwrNodes ! Loop through the tower nodes / elements @@ -6639,7 +6684,7 @@ END SUBROUTINE CalculatePositions !---------------------------------------------------------------------------------------------------------------------------------- !> This routine is used to calculate the angular positions, velocities, and partial accelerations stored in other states that are used in !! both the CalcOutput and CalcContStateDeriv routines. -SUBROUTINE CalculateAngularPosVelPAcc( p, x, CoordSys, RtHSdat ) +SUBROUTINE CalculateAngularPosVelPAcc( p, x, CoordSys, RtHSdat, ErrStat, ErrMsg ) !.................................................................................................................................. ! Passed variables @@ -6648,6 +6693,9 @@ SUBROUTINE CalculateAngularPosVelPAcc( p, x, CoordSys, RtHSdat ) TYPE(ED_CoordSys), INTENT(IN ) :: CoordSys !< The coordinate systems that have been set for these states/time TYPE(ED_RtHndSide), INTENT(INOUT) :: RtHSdat !< data from the RtHndSid module (contains positions to be set) + INTEGER(IntKi), INTENT(OUT) :: ErrStat !< Error status + CHARACTER(*), INTENT(OUT) :: ErrMsg !< Error message + !Local variables REAL(ReKi) :: AngVelHM (3) ! Angular velocity of eleMent J of blade K (body M) in the hub (body H). @@ -6655,6 +6703,15 @@ SUBROUTINE CalculateAngularPosVelPAcc( p, x, CoordSys, RtHSdat ) REAL(ReKi) :: AngAccELt (3) ! Portion of the angular acceleration of the low-speed shaft (body L) in the inertia frame (body E for earth) associated with everything but the QD2T()'s. INTEGER(IntKi) :: J ! Counter for elements INTEGER(IntKi) :: K ! Counter for blades + REAL(R8Ki) :: PtfmOrientation (3,3) ! Orientation matrix for the platform (-). + REAL(R8Ki) :: TransMat (3,3) ! Orientation matrix for the platform (-). + REAL(R8Ki) :: ThetaFA ! Tower fore-aft tilt deflection angle. + REAL(R8Ki) :: ThetaSS ! Tower side-to-side tilt deflection angle. + + INTEGER(IntKi) :: ErrStat2 ! Temporary error status + CHARACTER(ErrMsgLen) :: ErrMsg2 ! Temporary error message + + CHARACTER(*), PARAMETER :: RoutineName = "CalculateAngularPosVelPAcc" !------------------------------------------------------------------------------------------------- ! Angular and partial angular velocities @@ -6664,15 +6721,19 @@ SUBROUTINE CalculateAngularPosVelPAcc( p, x, CoordSys, RtHSdat ) ! NOTE: PAngVelEN(I,D,:) = the Dth-derivative of the partial angular velocity of DOF I for body N in body E. RtHSdat%PAngVelEX( :,0,:) = 0.0 - RtHSdat%PAngVelEX(DOF_R ,0,:) = CoordSys%z1 - RtHSdat%PAngVelEX(DOF_P ,0,:) = -CoordSys%z3 + ! RtHSdat%PAngVelEX(DOF_R ,0,:) = CoordSys%z1 + ! RtHSdat%PAngVelEX(DOF_P ,0,:) = -CoordSys%z3 + ! RtHSdat%PAngVelEX(DOF_Y ,0,:) = CoordSys%z2 + RtHSdat%PAngVelEX(DOF_R ,0,:) = CoordSys%beta1 + RtHSdat%PAngVelEX(DOF_P ,0,:) = -CoordSys%alpha3 RtHSdat%PAngVelEX(DOF_Y ,0,:) = CoordSys%z2 RtHSdat%AngVelEX = x%QDT(DOF_R )*RtHSdat%PAngVelEX(DOF_R ,0,:) & + x%QDT(DOF_P )*RtHSdat%PAngVelEX(DOF_P ,0,:) & + x%QDT(DOF_Y )*RtHSdat%PAngVelEX(DOF_Y ,0,:) - RtHSdat%AngPosEX = x%QT (DOF_R )*RtHSdat%PAngVelEX(DOF_R ,0,:) & - + x%QT (DOF_P )*RtHSdat%PAngVelEX(DOF_P ,0,:) & - + x%QT (DOF_Y )*RtHSdat%PAngVelEX(DOF_Y ,0,:) + ! RtHSdat%AngPosEX = x%QT (DOF_R )*RtHSdat%PAngVelEX(DOF_R ,0,:) & ! <- LW: Doesn't work for large rotation. Impacts AngPosEF (TwHtRP*i output) + ! + x%QT (DOF_P )*RtHSdat%PAngVelEX(DOF_P ,0,:) & + ! + x%QT (DOF_Y )*RtHSdat%PAngVelEX(DOF_Y ,0,:) + PtfmOrientation = EulerConstructZYX((/x%QT(DOF_R),x%QT(DOF_P),x%QT(DOF_Y)/)) RtHSdat%PAngVelEB( :,0,:) = RtHSdat%PAngVelEX(:,0,:) RtHSdat%PAngVelEB(DOF_TFA1,0,:) = -p%TwrFASF(1,p%TTopNode,1)*CoordSys%a3 @@ -6725,7 +6786,10 @@ SUBROUTINE CalculateAngularPosVelPAcc( p, x, CoordSys, RtHSdat ) ! everything but the QD2T()'s: RtHSdat%PAngVelEX( :,1,:) = 0.0 - RtHSdat%AngAccEXt = 0.0 + RtHSdat%PAngVelEX(DOF_R ,1,:) = CROSS_PRODUCT( RtHSdat%AngVelEX, RtHSdat%PAngVelEX(DOF_R ,0,:) ) + RtHSdat%PAngVelEX(DOF_P ,1,:) = CROSS_PRODUCT( x%QDT(DOF_Y)*CoordSys%z2, RtHSdat%PAngVelEX(DOF_P ,0,:) ) + RtHSdat%AngAccEXt = x%QDT(DOF_R)*RtHSdat%PAngVelEX(DOF_R ,1,:) & + + x%QDT(DOF_P)*RtHSdat%PAngVelEX(DOF_P ,1,:) RtHSdat%PAngVelEB( :,1,:) = RtHSdat%PAngVelEX(:,1,:) RtHSdat%PAngVelEB(DOF_TFA1,1,:) = CROSS_PRODUCT( RtHSdat%AngVelEX, RtHSdat%PAngVelEB(DOF_TFA1,0,:) ) @@ -6847,7 +6911,17 @@ SUBROUTINE CalculateAngularPosVelPAcc( p, x, CoordSys, RtHSdat ) + x%QT (DOF_TSS1)*RtHSdat%PAngVelEF(J,DOF_TSS1,0,:) & + x%QT (DOF_TFA2)*RtHSdat%PAngVelEF(J,DOF_TFA2,0,:) & + x%QT (DOF_TSS2)*RtHSdat%PAngVelEF(J,DOF_TSS2,0,:) - RtHSdat%AngPosEF (:,J) = RtHSdat%AngPosEX + RtHSdat%AngPosXF(:,J) + + !RtHSdat%AngPosEF (:,J) = RtHSdat%AngPosEX + RtHSdat%AngPosXF(:,J) ! LW: This is no longer right with large Ptfm Rotation + ThetaSS = p%TwrSSSF(1,J,1)*x%QT(DOF_TSS1) + p%TwrSSSF(2,J,1)*x%QT(DOF_TSS2) + ThetaFA = -p%TwrFASF(1,J,1)*x%QT(DOF_TFA1) - p%TwrFASF(2,J,1)*x%QT(DOF_TFA2) + CALL SmllRotTrans('tower element rotation',ThetaSS,-ThetaFA,0.0_R8Ki,TransMat,'',ErrStat2,ErrMsg2) + CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + if (ErrStat>=AbortErrLev) then + return + end if + RtHSdat%AngPosEF (:,J) = EulerExtractZYX(MatMul(TransMat,PtfmOrientation)) ! Extract tower element yaw, pitch, and roll angles from the combined platform and tower element rotation + RtHSdat%AngAccEFt(:,J) = RtHSdat%AngAccEXt + x%QDT(DOF_TFA1)*RtHSdat%PAngVelEF(J,DOF_TFA1,1,:) & + x%QDT(DOF_TSS1)*RtHSdat%PAngVelEF(J,DOF_TSS1,1,:) & + x%QDT(DOF_TFA2)*RtHSdat%PAngVelEF(J,DOF_TFA2,1,:) & @@ -6964,9 +7038,10 @@ SUBROUTINE CalculateLinearVelPAcc( p, x, CoordSys, RtHSdat ) TmpVec0 = CROSS_PRODUCT( RtHSdat%PAngVelEX(PX(I) ,0,:), RtHSdat%rZY ) TmpVec1 = CROSS_PRODUCT( RtHSdat%PAngVelEX(PX(I) ,0,:), EwXXrZY ) + TmpVec2 = CROSS_PRODUCT( RtHSdat%PAngVelEX(PX(I) ,1,:), RtHSdat%rZY ) RtHSdat%PLinVelEY(PX(I),0,:) = TmpVec0 + RtHSdat%PLinVelEY(PX(I) ,0,:) - RtHSdat%PLinVelEY(PX(I),1,:) = TmpVec1 + RtHSdat%PLinVelEY(PX(I) ,1,:) + RtHSdat%PLinVelEY(PX(I),1,:) = TmpVec1 + TmpVec2 + RtHSdat%PLinVelEY(PX(I) ,1,:) RtHSdat%LinAccEYt = RtHSdat%LinAccEYt + x%QDT(PX(I) )*RtHSdat%PLinVelEY(PX(I) ,1,:) @@ -7010,10 +7085,11 @@ SUBROUTINE CalculateLinearVelPAcc( p, x, CoordSys, RtHSdat ) DO I = 1,NPX ! Loop through all DOFs associated with the angular motion of the platform (body X) TmpVec0 = CROSS_PRODUCT( RtHSdat%PAngVelEX(PX(I) ,0,:), RtHSdat%rZO ) - TmpVec1 = CROSS_PRODUCT( RtHSdat%PAngVelEX(PX(I) ,0,:), EwXXrZO + LinVelXO ) + TmpVec1 = CROSS_PRODUCT( RtHSdat%PAngVelEX(PX(I) ,0,:), EwXXrZO + LinVelXO ) + TmpVec2 = CROSS_PRODUCT( RtHSdat%PAngVelEX(PX(I) ,1,:), RtHSdat%rZO ) RtHSdat%PLinVelEO(PX(I),0,:) = TmpVec0 + RtHSdat%PLinVelEO(PX(I) ,0,:) - RtHSdat%PLinVelEO(PX(I),1,:) = TmpVec1 + RtHSdat%PLinVelEO(PX(I) ,1,:) + RtHSdat%PLinVelEO(PX(I),1,:) = TmpVec1 + TmpVec2 + RtHSdat%PLinVelEO(PX(I) ,1,:) RtHSdat%LinVelEO = RtHSdat%LinVelEO + x%QDT(PX(I) )*RtHSdat%PLinVelEO(PX(I) ,0,:) RtHSdat%LinAccEOt = RtHSdat%LinAccEOt + x%QDT(PX(I) )*RtHSdat%PLinVelEO(PX(I) ,1,:) @@ -7306,11 +7382,12 @@ SUBROUTINE CalculateLinearVelPAcc( p, x, CoordSys, RtHSdat ) RtHSdat%LinVelET(:,J) = LinVelXT + RtHSdat%LinVelEZ DO I = 1,NPX ! Loop through all DOFs associated with the angular motion of the platform (body X) - TmpVec0 = CROSS_PRODUCT( RtHSdat%PAngVelEX(PX(I),0,:), RtHSdat%rZT(:,J) ) + TmpVec0 = CROSS_PRODUCT( RtHSdat%PAngVelEX(PX(I),0,:), RtHSdat%rZT(:,J) ) TmpVec1 = CROSS_PRODUCT( RtHSdat%PAngVelEX(PX(I),0,:), EwXXrZT + LinVelXT ) + TmpVec2 = CROSS_PRODUCT( RtHSdat%PAngVelEX(PX(I),1,:), RtHSdat%rZT(:,J) ) RtHSdat%PLinVelET(J,PX(I),0,:) = RtHSdat%PLinVelET(J,PX(I),0,:) + TmpVec0 - RtHSdat%PLinVelET(J,PX(I),1,:) = RtHSdat%PLinVelET(J,PX(I),1,:) + TmpVec1 + RtHSdat%PLinVelET(J,PX(I),1,:) = RtHSdat%PLinVelET(J,PX(I),1,:) + TmpVec1 + TmpVec2 RtHSdat%LinVelET( :, J) = RtHSdat%LinVelET( :, J) + x%QDT(PX(I))*RtHSdat%PLinVelET(J,PX(I),0,:) RtHSdat%LinAccETt(:, J) = RtHSdat%LinAccETt(:, J) + x%QDT(PX(I))*RtHSdat%PLinVelET(J,PX(I),1,:) @@ -7882,22 +7959,36 @@ SUBROUTINE CalculateForcesMoments( p, x, CoordSys, u, RtHSdat ) RtHSdat%PMXHydro = 0.0 DO I = 1,p%DOFs%NPYE ! Loop through all active (enabled) DOFs that contribute to the QD2T-related linear accelerations of the platform center of mass (point Y) - RtHSdat%PFZHydro(p%DOFs%PYE(I),:) = - u%PtfmAddedMass(DOF_Sg,p%DOFs%PYE(I))*RtHSdat%PLinVelEZ(DOF_Sg,0,:) & - - u%PtfmAddedMass(DOF_Sw,p%DOFs%PYE(I))*RtHSdat%PLinVelEZ(DOF_Sw,0,:) & - - u%PtfmAddedMass(DOF_Hv,p%DOFs%PYE(I))*RtHSdat%PLinVelEZ(DOF_Hv,0,:) - RtHSdat%PMXHydro(p%DOFs%PYE(I),:) = - u%PtfmAddedMass(DOF_R ,p%DOFs%PYE(I))*RtHSdat%PAngVelEX(DOF_R ,0,:) & - - u%PtfmAddedMass(DOF_P ,p%DOFs%PYE(I))*RtHSdat%PAngVelEX(DOF_P ,0,:) & - - u%PtfmAddedMass(DOF_Y ,p%DOFs%PYE(I))*RtHSdat%PAngVelEX(DOF_Y ,0,:) + ! RtHSdat%PFZHydro(p%DOFs%PYE(I),:) = - u%PtfmAddedMass(DOF_Sg,p%DOFs%PYE(I))*RtHSdat%PLinVelEZ(DOF_Sg,0,:) & + ! - u%PtfmAddedMass(DOF_Sw,p%DOFs%PYE(I))*RtHSdat%PLinVelEZ(DOF_Sw,0,:) & + ! - u%PtfmAddedMass(DOF_Hv,p%DOFs%PYE(I))*RtHSdat%PLinVelEZ(DOF_Hv,0,:) + ! RtHSdat%PMXHydro(p%DOFs%PYE(I),:) = - u%PtfmAddedMass(DOF_R ,p%DOFs%PYE(I))*RtHSdat%PAngVelEX(DOF_R ,0,:) & + ! - u%PtfmAddedMass(DOF_P ,p%DOFs%PYE(I))*RtHSdat%PAngVelEX(DOF_P ,0,:) & + ! - u%PtfmAddedMass(DOF_Y ,p%DOFs%PYE(I))*RtHSdat%PAngVelEX(DOF_Y ,0,:) + + RtHSdat%PFZHydro(p%DOFs%PYE(I),:) = - u%PtfmAddedMass(DOF_Sg,p%DOFs%PYE(I))*CoordSys%z1 & + + u%PtfmAddedMass(DOF_Sw,p%DOFs%PYE(I))*CoordSys%z3 & + - u%PtfmAddedMass(DOF_Hv,p%DOFs%PYE(I))*CoordSys%z2 + RtHSdat%PMXHydro(p%DOFs%PYE(I),:) = - u%PtfmAddedMass(DOF_R ,p%DOFs%PYE(I))*CoordSys%z1 & + + u%PtfmAddedMass(DOF_P ,p%DOFs%PYE(I))*CoordSys%z3 & + - u%PtfmAddedMass(DOF_Y ,p%DOFs%PYE(I))*CoordSys%z2 ENDDO ! I - All active (enabled) DOFs that contribute to the QD2T-related linear accelerations of the platform center of mass (point Y) - RtHSdat%FZHydrot = u%PlatformPtMesh%Force(DOF_Sg,1)*RtHSdat%PLinVelEZ(DOF_Sg,0,:) & - + u%PlatformPtMesh%Force(DOF_Sw,1)*RtHSdat%PLinVelEZ(DOF_Sw,0,:) & - + u%PlatformPtMesh%Force(DOF_Hv,1)*RtHSdat%PLinVelEZ(DOF_Hv,0,:) - RtHSdat%MXHydrot = u%PlatformPtMesh%Moment(DOF_R-3,1)*RtHSdat%PAngVelEX(DOF_R ,0,:) & - + u%PlatformPtMesh%Moment(DOF_P-3,1)*RtHSdat%PAngVelEX(DOF_P ,0,:) & - + u%PlatformPtMesh%Moment(DOF_Y-3,1)*RtHSdat%PAngVelEX(DOF_Y ,0,:) - + !RtHSdat%FZHydrot = u%PlatformPtMesh%Force(DOF_Sg,1)*RtHSdat%PLinVelEZ(DOF_Sg,0,:) & + ! + u%PlatformPtMesh%Force(DOF_Sw,1)*RtHSdat%PLinVelEZ(DOF_Sw,0,:) & + ! + u%PlatformPtMesh%Force(DOF_Hv,1)*RtHSdat%PLinVelEZ(DOF_Hv,0,:) + !RtHSdat%MXHydrot = u%PlatformPtMesh%Moment(DOF_R-3,1)*RtHSdat%PAngVelEX(DOF_R ,0,:) & + ! + u%PlatformPtMesh%Moment(DOF_P-3,1)*RtHSdat%PAngVelEX(DOF_P ,0,:) & + ! + u%PlatformPtMesh%Moment(DOF_Y-3,1)*RtHSdat%PAngVelEX(DOF_Y ,0,:) + + RtHSdat%FZHydrot = u%PlatformPtMesh%Force(DOF_Sg,1)*CoordSys%z1 & + - u%PlatformPtMesh%Force(DOF_Sw,1)*CoordSys%z3 & + + u%PlatformPtMesh%Force(DOF_Hv,1)*CoordSys%z2 + RtHSdat%MXHydrot = u%PlatformPtMesh%Moment(DOF_R-3,1)*CoordSys%z1 & + - u%PlatformPtMesh%Moment(DOF_P-3,1)*CoordSys%z3 & + + u%PlatformPtMesh%Moment(DOF_Y-3,1)*CoordSys%z2 + !..................................... ! PFrcZAll and PMomXAll ! (requires PFrcT0Trb, PMomX0Trb, PFZHydro, PMXHydro ) @@ -7955,8 +8046,16 @@ SUBROUTINE CalculateForcesMoments( p, x, CoordSys, u, RtHSdat ) TmpVec4 = CROSS_PRODUCT( -RtHSdat%AngVelEX, TmpVec ) ! = ( -angular velocity of platform in the inertia frame ) cross ( TmpVec ) RtHSdat%FrcZAllt = RtHSdat%FrcT0Trbt + RtHSdat%FZHydrot + TmpVec1 - RtHSdat%MomXAllt = RtHSdat%MomX0Trbt + RtHSdat%MXHydrot + TmpVec2 + TmpVec3 + TmpVec4 - + RtHSdat%MomXAllt = RtHSdat%MomX0Trbt + RtHSdat%MXHydrot + TmpVec2 + TmpVec3 + TmpVec4 & + - p%PtfmRIner*CoordSys%a1*DOT_PRODUCT( CoordSys%a1, RtHSdat%AngAccEXt ) & + - p%PtfmYIner*CoordSys%a2*DOT_PRODUCT( CoordSys%a2, RtHSdat%AngAccEXt ) & + - p%PtfmPIner*CoordSys%a3*DOT_PRODUCT( CoordSys%a3, RtHSdat%AngAccEXt ) & + - p%PtfmXZIner*CoordSys%a1*DOT_PRODUCT( CoordSys%a2, RtHSdat%AngAccEXt ) & + + p%PtfmXYIner*CoordSys%a1*DOT_PRODUCT( CoordSys%a3, RtHSdat%AngAccEXt ) & + + p%PtfmYZIner*CoordSys%a2*DOT_PRODUCT( CoordSys%a3, RtHSdat%AngAccEXt ) & + - p%PtfmXZIner*CoordSys%a2*DOT_PRODUCT( CoordSys%a1, RtHSdat%AngAccEXt ) & + + p%PtfmXYIner*CoordSys%a3*DOT_PRODUCT( CoordSys%a1, RtHSdat%AngAccEXt ) & + + p%PtfmYZIner*CoordSys%a3*DOT_PRODUCT( CoordSys%a2, RtHSdat%AngAccEXt ) END SUBROUTINE CalculateForcesMoments !---------------------------------------------------------------------------------------------------------------------------------- @@ -11473,8 +11572,8 @@ SUBROUTINE Compute_dY(p, y_p, y_m, delta, dY) end if if (.not. p%CompAeroMaps) then - call PackMotionMesh_dY(y_p%PlatformPtMesh, y_m%PlatformPtMesh, dY, indx_first, UseSmlAngle=.true.) ! all fields - call PackMotionMesh_dY(y_p%TowerLn2Mesh, y_m%TowerLn2Mesh, dY, indx_first, UseSmlAngle=.true.) ! all fields + call PackMotionMesh_dY(y_p%PlatformPtMesh, y_m%PlatformPtMesh, dY, indx_first, UseSmlAngle=.false.) ! all fields + call PackMotionMesh_dY(y_p%TowerLn2Mesh, y_m%TowerLn2Mesh, dY, indx_first, UseSmlAngle=.false.) ! all fields Mask = .false. Mask(MASKID_TRANSLATIONDISP) = .true. diff --git a/modules/elastodyn/src/ElastoDyn_IO.f90 b/modules/elastodyn/src/ElastoDyn_IO.f90 index 78f1f19ba3..23054a48c1 100644 --- a/modules/elastodyn/src/ElastoDyn_IO.f90 +++ b/modules/elastodyn/src/ElastoDyn_IO.f90 @@ -4276,22 +4276,21 @@ SUBROUTINE ValidatePrimaryData( InputFileData, BD4Blades, Linearize, MHK, ErrSta CALL SetErrStat(ErrID_Fatal,'ShftTilt must be between -pi/2 and pi/2 radians (i.e., in the range [-90, 90] degrees).',ErrStat,ErrMsg,RoutineName) END IF - ! Check for violations of the small-angle assumption (15-degree limit, using radians): - IF ( ABS( InputFileData%PtfmRoll ) > SmallAngleLimit_Rad ) THEN - CALL SetErrStat( ErrID_Fatal, 'PtfmRoll must be between -'//TRIM(Num2LStr(SmallAngleLimit_Rad))//' and ' & - //TRIM(Num2LStr(SmallAngleLimit_Rad))//' radians.',ErrStat,ErrMsg,RoutineName) - END IF - - IF ( ABS( InputFileData%PtfmPitch ) > SmallAngleLimit_Rad ) THEN - CALL SetErrStat( ErrID_Fatal, 'PtfmPitch must be between -'//TRIM(Num2LStr(SmallAngleLimit_Rad))//' and ' & - //TRIM(Num2LStr(SmallAngleLimit_Rad))//' radians.',ErrStat,ErrMsg,RoutineName) - END IF - - IF ( ABS( InputFileData%PtfmYaw ) > SmallAngleLimit_Rad ) THEN - CALL SetErrStat( ErrID_Fatal, 'PtfmYaw must be between -'//TRIM(Num2LStr(SmallAngleLimit_Rad))//' and ' & - //TRIM(Num2LStr(SmallAngleLimit_Rad))//' radians.',ErrStat,ErrMsg,RoutineName) - END IF + ! IF ( ABS( InputFileData%PtfmRoll ) > SmallAngleLimit_Rad ) THEN + ! CALL SetErrStat( ErrID_Fatal, 'PtfmRoll must be between -'//TRIM(Num2LStr(SmallAngleLimit_Rad))//' and ' & + ! //TRIM(Num2LStr(SmallAngleLimit_Rad))//' radians.',ErrStat,ErrMsg,RoutineName) + ! END IF + + ! IF ( ABS( InputFileData%PtfmPitch ) > SmallAngleLimit_Rad ) THEN + ! CALL SetErrStat( ErrID_Fatal, 'PtfmPitch must be between -'//TRIM(Num2LStr(SmallAngleLimit_Rad))//' and ' & + ! //TRIM(Num2LStr(SmallAngleLimit_Rad))//' radians.',ErrStat,ErrMsg,RoutineName) + ! END IF + + ! IF ( ABS( InputFileData%PtfmYaw ) > SmallAngleLimit_Rad ) THEN + ! CALL SetErrStat( ErrID_Fatal, 'PtfmYaw must be between -'//TRIM(Num2LStr(SmallAngleLimit_Rad))//' and ' & + ! //TRIM(Num2LStr(SmallAngleLimit_Rad))//' radians.',ErrStat,ErrMsg,RoutineName) + ! END IF ! Check the output parameters: IF ( InputFileData%DecFact < 1_IntKi ) CALL SetErrStat( ErrID_Fatal, 'DecFact must be greater than 0.',ErrStat,ErrMsg,RoutineName ) diff --git a/modules/elastodyn/src/ElastoDyn_Registry.txt b/modules/elastodyn/src/ElastoDyn_Registry.txt index 65f45797a0..0ffcb4130a 100644 --- a/modules/elastodyn/src/ElastoDyn_Registry.txt +++ b/modules/elastodyn/src/ElastoDyn_Registry.txt @@ -262,6 +262,12 @@ typedef ^ ED_InputFile IntKi BldNd_BladesOut - - - "The blades to output (ED # ..... Internal data types: Coordinate Systems ................................................................................... # This type defines coordinate sytems used internally by FAST. The 3 components of each vector correspond to the z1, z2, and z3 components of the individual vectors. # NOTE: the orientations of most of these coordinate systems will change every time step. +typedef ^ ED_CoordSys R8Ki alpha1 3 - - "Vector / direction alpha1 after ptfm yaw rotation" - +typedef ^ ED_CoordSys R8Ki alpha2 3 - - "Vector / direction alpha2 after ptfm yaw rotation" - +typedef ^ ED_CoordSys R8Ki alpha3 3 - - "Vector / direction alpha3 after ptfm yaw rotation" - +typedef ^ ED_CoordSys R8Ki beta1 3 - - "Vector / direction beta1 after ptfm yaw and pitch rotation" - +typedef ^ ED_CoordSys R8Ki beta2 3 - - "Vector / direction beta2 after ptfm yaw and pitch rotation" - +typedef ^ ED_CoordSys R8Ki beta3 3 - - "Vector / direction beta3 after ptfm yaw and pitch rotation" - typedef ^ ED_CoordSys R8Ki a1 3 - - "Vector / direction a1 (= xt from the IEC coord. system)" - typedef ^ ED_CoordSys R8Ki a2 3 - - "Vector / direction a2 (= zt from the IEC coord. system)" - typedef ^ ED_CoordSys R8Ki a3 3 - - "Vector / direction a3 (= -yt from the IEC coord. system)" - diff --git a/modules/elastodyn/src/ElastoDyn_Types.f90 b/modules/elastodyn/src/ElastoDyn_Types.f90 index 886da1125f..64046a0f0f 100644 --- a/modules/elastodyn/src/ElastoDyn_Types.f90 +++ b/modules/elastodyn/src/ElastoDyn_Types.f90 @@ -275,6 +275,12 @@ MODULE ElastoDyn_Types ! ======================= ! ========= ED_CoordSys ======= TYPE, PUBLIC :: ED_CoordSys + REAL(R8Ki) , DIMENSION(1:3) :: alpha1 = 0.0_R8Ki !< Vector / direction alpha1 after ptfm yaw rotation [-] + REAL(R8Ki) , DIMENSION(1:3) :: alpha2 = 0.0_R8Ki !< Vector / direction alpha2 after ptfm yaw rotation [-] + REAL(R8Ki) , DIMENSION(1:3) :: alpha3 = 0.0_R8Ki !< Vector / direction alpha3 after ptfm yaw rotation [-] + REAL(R8Ki) , DIMENSION(1:3) :: beta1 = 0.0_R8Ki !< Vector / direction beta1 after ptfm yaw and pitch rotation [-] + REAL(R8Ki) , DIMENSION(1:3) :: beta2 = 0.0_R8Ki !< Vector / direction beta2 after ptfm yaw and pitch rotation [-] + REAL(R8Ki) , DIMENSION(1:3) :: beta3 = 0.0_R8Ki !< Vector / direction beta3 after ptfm yaw and pitch rotation [-] REAL(R8Ki) , DIMENSION(1:3) :: a1 = 0.0_R8Ki !< Vector / direction a1 (= xt from the IEC coord. system) [-] REAL(R8Ki) , DIMENSION(1:3) :: a2 = 0.0_R8Ki !< Vector / direction a2 (= zt from the IEC coord. system) [-] REAL(R8Ki) , DIMENSION(1:3) :: a3 = 0.0_R8Ki !< Vector / direction a3 (= -yt from the IEC coord. system) [-] @@ -2365,6 +2371,12 @@ subroutine ED_CopyCoordSys(SrcCoordSysData, DstCoordSysData, CtrlCode, ErrStat, character(*), parameter :: RoutineName = 'ED_CopyCoordSys' ErrStat = ErrID_None ErrMsg = '' + DstCoordSysData%alpha1 = SrcCoordSysData%alpha1 + DstCoordSysData%alpha2 = SrcCoordSysData%alpha2 + DstCoordSysData%alpha3 = SrcCoordSysData%alpha3 + DstCoordSysData%beta1 = SrcCoordSysData%beta1 + DstCoordSysData%beta2 = SrcCoordSysData%beta2 + DstCoordSysData%beta3 = SrcCoordSysData%beta3 DstCoordSysData%a1 = SrcCoordSysData%a1 DstCoordSysData%a2 = SrcCoordSysData%a2 DstCoordSysData%a3 = SrcCoordSysData%a3 @@ -2683,6 +2695,12 @@ subroutine ED_PackCoordSys(RF, Indata) type(ED_CoordSys), intent(in) :: InData character(*), parameter :: RoutineName = 'ED_PackCoordSys' if (RF%ErrStat >= AbortErrLev) return + call RegPack(RF, InData%alpha1) + call RegPack(RF, InData%alpha2) + call RegPack(RF, InData%alpha3) + call RegPack(RF, InData%beta1) + call RegPack(RF, InData%beta2) + call RegPack(RF, InData%beta3) call RegPack(RF, InData%a1) call RegPack(RF, InData%a2) call RegPack(RF, InData%a3) @@ -2744,6 +2762,12 @@ subroutine ED_UnPackCoordSys(RF, OutData) integer(IntKi) :: stat logical :: IsAllocAssoc if (RF%ErrStat /= ErrID_None) return + call RegUnpack(RF, OutData%alpha1); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%alpha2); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%alpha3); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%beta1); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%beta2); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%beta3); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%a1); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%a2); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%a3); if (RegCheckErr(RF, RoutineName)) return diff --git a/modules/hydrodyn/CMakeLists.txt b/modules/hydrodyn/CMakeLists.txt index 7ef738647b..3cee15f647 100644 --- a/modules/hydrodyn/CMakeLists.txt +++ b/modules/hydrodyn/CMakeLists.txt @@ -43,6 +43,7 @@ add_library(hydrodynlib STATIC src/SS_Excitation_Types.f90 src/WAMIT_Types.f90 src/WAMIT2_Types.f90 + src/YawOffset.f90 ) target_link_libraries(hydrodynlib seastlib nwtclibs) diff --git a/modules/hydrodyn/src/HydroDyn.f90 b/modules/hydrodyn/src/HydroDyn.f90 index fcb538e2fb..2bd7a8ab6e 100644 --- a/modules/hydrodyn/src/HydroDyn.f90 +++ b/modules/hydrodyn/src/HydroDyn.f90 @@ -33,6 +33,7 @@ MODULE HydroDyn USE SeaState USE HydroDyn_Input USE HydroDyn_Output + USE YawOffset #ifdef USE_FIT USE FIT_MODULES @@ -144,9 +145,6 @@ SUBROUTINE HydroDyn_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, I ErrMsg = "" p%UnOutFile = -1 - p%WaveField => InitInp%WaveField - - ! Initialize the NWTC Subroutine Library CALL NWTC_Init( ) @@ -185,13 +183,19 @@ SUBROUTINE HydroDyn_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, I CALL CleanUp() RETURN END IF - + + p%WaveField => InitInp%WaveField + p%PtfmYMod = InputFileData%PtfmYMod InputFileData%Morison%WaveField => InitInp%WaveField InputFileData%WAMIT%WaveField => InitInp%WaveField InputFileData%WAMIT2%WaveField => InitInp%WaveField - + InputFileData%Morison%PtfmYMod = InputFileData%PtfmYMod + InputFileData%WAMIT%PtfmYMod = InputFileData%PtfmYMod + InputFileData%WAMIT%PtfmRefY = InputFileData%PtfmRefY + InputFileData%WAMIT2%PtfmYMod = InputFileData%PtfmYMod + InputFileData%WAMIT2%PtfmRefY = InputFileData%PtfmRefY ! Verify all the necessary initialization data. Do this at the HydroDynInput module-level ! because the HydroDynInput module is also responsible for parsing all this @@ -234,6 +238,17 @@ SUBROUTINE HydroDyn_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, I p%DT = Interval END IF + ! Low-pass filter constant for PtfmRefY + p%CYawFilt = exp(-TwoPi*Interval*InputFileData%PtfmYCutoff) + ! Allocate and initialize discrete states for the filtered PRP yaw position + ALLOCATE ( xd%PtfmRefY(1:3) , STAT=ErrStat2 ) + IF ( ErrStat2 /= 0 ) THEN + CALL SetErrStat( ErrID_Fatal, 'Error allocating memory for the PtfmRefY array.', ErrStat, ErrMsg, RoutineName) + CALL Cleanup() + RETURN + END IF + xd%PtfmRefY = InputFileData%PtfmRefY + ! Open a summary of the HydroDyn Initialization. Note: OutRootName must be set by the caller because there may not be an input file to obtain this rootname from. IF ( InputFileData%HDSum ) THEN @@ -946,9 +961,12 @@ SUBROUTINE HydroDyn_UpdateStates( t, n, Inputs, InputTimes, p, x, xd, z, OtherSt CHARACTER(ErrMsgLen) :: ErrMsg2 ! Error message if ErrStat2 /= ErrID_None INTEGER :: nTime ! number of inputs + REAL(R8Ki) :: PRPRotation(3) + TYPE(WAMIT_InputType), ALLOCATABLE :: Inputs_WAMIT(:) TYPE(Morison_InputType), ALLOCATABLE :: Inputs_Morison(:) TYPE(Morison_InputType) :: u_Morison + TYPE(HydroDyn_InputType) :: u CHARACTER(*), PARAMETER :: RoutineName = 'HydroDyn_UpdateStates' ! Create dummy variables required by framework but which are not used by the module @@ -965,6 +983,29 @@ SUBROUTINE HydroDyn_UpdateStates( t, n, Inputs, InputTimes, p, x, xd, z, OtherSt ErrMsg = "" nTime = size(Inputs) + ! Update PtfmRefY + IF (p%PtfmYMod .EQ. 1) THEN + ! Inefficient. Only need to interp PRPMesh below. Fix later. + CALL HydroDyn_CopyInput(Inputs(1), u, MESH_NEWCOPY, ErrStat2, ErrMsg2) + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + CALL HydroDyn_Input_ExtrapInterp(Inputs, InputTimes, u, t, ErrStat2, ErrMsg2) + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + PRPRotation = EulerExtractZYX(u%PRPMesh%Orientation(:,:,1)) + ! Yaw angle from EulerExtractZYX might not be continous in time and can contain jumps of TwoPi + ! Adjust past xd%PtfmRefY to follow. + IF ( ABS(PRPRotation(3)-xd%PtfmRefY(1)) > ABS(PRPRotation(3)-(xd%PtfmRefY(1)-TwoPi)) ) THEN + xd%PtfmRefY = xd%PtfmRefY - TwoPi + ELSE IF ( ABS(PRPRotation(3)-xd%PtfmRefY(1)) > ABS(PRPRotation(3)-(xd%PtfmRefY(1)+TwoPi)) ) THEN + xd%PtfmRefY = xd%PtfmRefY + TwoPi + END IF + ! Update PtfmRefY states + xd%PtfmRefY(3) = xd%PtfmRefY(2) + xd%PtfmRefY(2) = xd%PtfmRefY(1) + xd%PtfmRefY(1) = p%CYawFilt * xd%PtfmRefY(1) + (1.0-p%CYawFilt) * PRPRotation(3) + CALL HydroDyn_DestroyInput(u, ErrStat2, ErrMsg2) + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + END IF + IF (INPUTS(1)%Morison%Mesh%Committed) THEN ALLOCATE( Inputs_Morison(nTime), STAT = ErrStat2 ) @@ -976,13 +1017,17 @@ SUBROUTINE HydroDyn_UpdateStates( t, n, Inputs, InputTimes, p, x, xd, z, OtherSt DO i=1,nTime CALL Morison_CopyInput(Inputs(i)%Morison, Inputs_Morison(i), MESH_NEWCOPY, ErrStat2, ErrMsg2) call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + ! Inputs_Morison(i)%PtfmRefY = Inputs(i)%PtfmRefY + Inputs_Morison(i)%PtfmRefY = xd%PtfmRefY(i) END DO CALL Morison_CopyInput(Inputs(1)%Morison, u_Morison, MESH_NEWCOPY, ErrStat2, ErrMsg2) call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - + ! u_Morison%PtfmRefY = Inputs(1)%PtfmRefY + u_Morison%PtfmRefY = xd%PtfmRefY(1) + CALL Morison_Input_ExtrapInterp(Inputs_Morison, InputTimes, u_Morison, t, ErrStat2, ErrMsg2) ! get inputs at time t call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) - + IF (ErrStat < AbortErrLev) THEN ! Update the discrete states of Morison - The state of the high-pass velocity filter CALL Morison_UpdateDiscState( t, u_Morison, p%Morison, x%Morison, xd%Morison, & @@ -1018,6 +1063,8 @@ SUBROUTINE HydroDyn_UpdateStates( t, n, Inputs, InputTimes, p, x, xd, z, OtherSt ! Copy the inputs from the HD mesh into the WAMIT mesh call MeshCopy( Inputs(I)%WAMITMesh, Inputs_WAMIT(I)%Mesh, MESH_NEWCOPY, ErrStat2, ErrMsg2 ) call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + ! Inputs_WAMIT(I)%PtfmRefY = Inputs(I)%PtfmRefY + Inputs_WAMIT(I)%PtfmRefY = xd%PtfmRefY(I) end do if (ErrStat < AbortErrLev) then ! if there was an error copying the input meshes, we'll skip this step and then cleanup the temporary input meshes @@ -1039,6 +1086,8 @@ SUBROUTINE HydroDyn_UpdateStates( t, n, Inputs, InputTimes, p, x, xd, z, OtherSt ! We need to create to valid mesh data structures in our Inputs_WAMIT(I)%Mesh using the miscvar version as a template, but the actually data will be generated below call MeshCopy( m%u_WAMIT(iWAMIT)%Mesh, Inputs_WAMIT(I)%Mesh, MESH_NEWCOPY, ErrStat2, ErrMsg2 ) call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + ! Inputs_WAMIT(I)%PtfmRefY = Inputs(I)%PtfmRefY + Inputs_WAMIT(I)%PtfmRefY = xd%PtfmRefY(I) end do if (ErrStat > AbortErrLev) exit @@ -1171,7 +1220,14 @@ SUBROUTINE HydroDyn_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, ErrStat, REAL(ReKi) :: q(6*p%NBody), qdot(6*p%NBody), qdotsq(6*p%NBody), qdotdot(6*p%NBody) REAL(ReKi) :: rotdisp(3) ! small angle rotational displacements integer(IntKi) :: iBody, indxStart, indxEnd ! Counters - + REAL(ReKi), ALLOCATABLE :: RRg2b(:,:), RRb2g(:,:) + REAL(ReKi) :: PtfmRefY + REAL(R8Ki) :: PRPRotation(3) + + CHARACTER(*), PARAMETER :: RoutineName = 'HydroDyn_CalcOutput' + REAL(ReKi), PARAMETER :: LrgAngle = 0.261799387799149 ! Threshold for platform roll and pitch rotation (15 deg). This is consistent with the ElastoDyn check. + LOGICAL, SAVE :: FrstWarn_LrgY = .TRUE. + ! Initialize ErrStat ErrStat = ErrID_None @@ -1184,12 +1240,30 @@ SUBROUTINE HydroDyn_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, ErrStat, IF ( (p%OutSwtch == 1 .OR. p%OutSwtch == 3) .AND. ( Time > m%LastOutTime ) ) THEN CALL HDOut_WriteOutputs( m%LastOutTime, y, p, m%Decimate, ErrStat2, ErrMsg2 ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'HydroDyn_CalcOutput' ) + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) END IF m%LastOutTime = Time ! time associated with the next values of y%WriteOutput - - + ! Compute the filtered platform reference yaw position here - both WAMIT and Morison modules need this + PRPRotation = EulerExtractZYX(u%PRPMesh%Orientation(:,:,1)) + IF (p%PtfmYMod .EQ. 0_IntKi) THEN + PtfmRefY = xd%PtfmRefY(1) + ELSE IF (p%PtfmYMod .EQ. 1_IntKi) THEN + IF ( ABS(PRPRotation(3)-xd%PtfmRefY(1)) > ABS(PRPRotation(3)-(xd%PtfmRefY(1)-TwoPi)) ) THEN + PtfmRefY = p%CYawFilt * (xd%PtfmRefY(1) - TwoPi) + (1.0-p%CYawFilt) * PRPRotation(3) + ELSE IF ( ABS(PRPRotation(3)-xd%PtfmRefY(1)) > ABS(PRPRotation(3)-(xd%PtfmRefY(1)+TwoPi)) ) THEN + PtfmRefY = p%CYawFilt * (xd%PtfmRefY(1) + TwoPi) + (1.0-p%CYawFilt) * PRPRotation(3) + ELSE + PtfmRefY = p%CYawFilt * xd%PtfmRefY(1) + (1.0-p%CYawFilt) * PRPRotation(3) + END IF + END IF + IF ( (ABS( WrapToPi(PRPRotation(3)-PtfmRefY) ) > LrgAngle) .AND. FrstWarn_LrgY ) THEN + ErrStat2 = ErrID_Severe + ErrMsg2 = 'Yaw angle at PRP relative to the reference yaw position (PtfmRefY) violated the small angle assumption. The solution might be inaccurate. Consider using PtfmYMod=1 and adjust PtfmYCutoff in ElastoDyn. Simulation continuing, but future warnings will be suppressed.' + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + FrstWarn_LrgY = .FALSE. + END IF + !------------------------------------------------------------------- ! Additional stiffness, damping forces. These need to be placed on a point mesh which is located at the WAMIT reference point (WRP). ! This mesh will need to get mapped by the glue code for use by either ElastoDyn or SubDyn. @@ -1198,22 +1272,35 @@ SUBROUTINE HydroDyn_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, ErrStat, if ( p%PotMod == 1 ) then + ! Transformation matrices between global and PRP frame + ALLOCATE(RRb2g(6*p%NBody,6*p%NBody),STAT=ErrStat2) + ALLOCATE(RRg2b(6*p%NBody,6*p%NBody),STAT=ErrStat2) + RRg2b(:,:) = 0.0_ReKi do iBody = 1, p%NBody ! Determine the rotational angles from the direction-cosine matrix - rotdisp = GetSmllRotAngs ( u%WAMITMesh%Orientation(:,:,iBody), ErrStat2, ErrMsg2 ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'HydroDyn_CalcOutput' ) + ! rotdisp = GetRotAngs ( u%PtfmRefY, u%WAMITMesh%Orientation(:,:,iBody), ErrStat2, ErrMsg2 ) + ! CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + rotdisp = EulerExtractZYX(u%WAMITMesh%Orientation(:,:,iBody)) indxStart = (iBody-1)*6+1 indxEnd = indxStart+5 q (indxStart:indxEnd) = reshape((/real(u%WAMITMesh%TranslationDisp(:,iBody),ReKi),rotdisp(:)/),(/6/)) qdot (indxStart:indxEnd) = reshape((/u%WAMITMesh%TranslationVel(:,iBody),u%WAMITMesh%RotationVel(:,iBody)/),(/6/)) - qdotsq (indxStart:indxEnd) = abs(qdot(indxStart:indxEnd))*qdot(indxStart:indxEnd) qdotdot(indxStart:indxEnd) = reshape((/u%WAMITMesh%TranslationAcc(:,iBody),u%WAMITMesh%RotationAcc(:,iBody)/),(/6/)) + RRg2b(indxStart:(indxStart+2),indxStart:(indxStart+2)) = u%WAMITMesh%Orientation(:,:,iBody) + RRg2b((indxEnd-2):indxEnd,(indxEnd-2):indxEnd) = u%WAMITMesh%Orientation(:,:,iBody) + ! qdotsq is only used to compute the quadratic damping load, so convert to body frame here + qdotsq (indxStart:indxEnd) = matmul(RRg2b(indxStart:indxEnd,indxStart:indxEnd),qdot(indxStart:indxEnd)) + qdotsq (indxStart:indxEnd) = abs( qdotsq (indxStart:indxEnd) ) * qdotsq (indxStart:indxEnd) end do + RRb2g = transpose(RRg2b) + !FIXME: Error handling appears to be broken here. if ( p%NBodyMod == 1 ) then ! Compute the load contirbution from user-supplied added stiffness and damping - m%F_PtfmAdd = p%AddF0(:,1) - matmul(p%AddCLin(:,:,1), q) - matmul(p%AddBLin(:,:,1), qdot) - matmul(p%AddBQuad(:,:,1), qdotsq) + m%F_PtfmAdd = p%AddF0(:,1) - matmul(p%AddCLin(:,:,1), q) & + - matmul( matmul(RRb2g,p%AddBLin(:,:,1) ), matmul(RRg2b,qdot) ) & + - matmul( matmul(RRb2g,p%AddBQuad(:,:,1)), qdotsq) ! Note: qdotsq is already in body frame, see above do iBody = 1, p%NBody indxStart = (iBody-1)*6+1 indxEnd = indxStart+5 @@ -1227,7 +1314,10 @@ SUBROUTINE HydroDyn_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, ErrStat, indxStart = (iBody-1)*6+1 indxEnd = indxStart+5 - m%F_PtfmAdd(indxStart:indxEnd) = p%AddF0(:,iBody) - matmul(p%AddCLin(:,:,iBody), q(indxStart:indxEnd)) - matmul(p%AddBLin(:,:,iBody), qdot(indxStart:indxEnd)) - matmul(p%AddBQuad(:,:,iBody), qdotsq(indxStart:indxEnd)) + m%F_PtfmAdd(indxStart:indxEnd) = p%AddF0(:,iBody) - matmul(p%AddCLin(:,:,iBody), q(indxStart:indxEnd)) & + - matmul( matmul(RRb2g(indxStart:indxEnd,indxStart:indxEnd),p%AddBLin(:,:,iBody)), & + matmul(RRg2b(indxStart:indxEnd,indxStart:indxEnd),qdot(indxStart:indxEnd)) ) & + - matmul( matmul(RRb2g(indxStart:indxEnd,indxStart:indxEnd),p%AddBQuad(:,:,iBody)), qdotsq(indxStart:indxEnd)) ! Attach to the output point mesh y%WAMITMesh%Force (:,iBody) = m%F_PtfmAdd(indxStart:indxStart+2) @@ -1244,12 +1334,14 @@ SUBROUTINE HydroDyn_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, ErrStat, if ( p%NBodyMod == 1 .or. p%NBody == 1 ) then ! Copy the inputs from the HD mesh into the WAMIT mesh call MeshCopy( u%WAMITMesh, m%u_WAMIT(1)%Mesh, MESH_UPDATECOPY, ErrStat2, ErrMsg2 ) - call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'HydroDyn_CalcOutput' ) + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) if ( ErrStat >= AbortErrLev ) return - + + ! m%u_WAMIT(1)%PtfmRefY = u%PtfmRefY + m%u_WAMIT(1)%PtfmRefY = PtfmRefY call WAMIT_CalcOutput( Time, m%u_WAMIT(1), p%WAMIT(1), x%WAMIT(1), xd%WAMIT(1), & z%WAMIT, OtherState%WAMIT(1), y%WAMIT(1), m%WAMIT(1), ErrStat2, ErrMsg2 ) - call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'HydroDyn_CalcOutput' ) + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) do iBody=1,p%NBody y%WAMITMesh%Force (:,iBody) = y%WAMITMesh%Force (:,iBody) + y%WAMIT(1)%Mesh%Force (:,iBody) y%WAMITMesh%Moment(:,iBody) = y%WAMITMesh%Moment(:,iBody) + y%WAMIT(1)%Mesh%Moment(:,iBody) @@ -1268,9 +1360,10 @@ SUBROUTINE HydroDyn_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, ErrStat, m%u_WAMIT(iBody)%Mesh%TranslationAcc (:,1) = u%WAMITMesh%TranslationAcc (:,iBody) m%u_WAMIT(iBody)%Mesh%RotationAcc (:,1) = u%WAMITMesh%RotationAcc (:,iBody) + m%u_WAMIT(iBody)%PtfmRefY = PtfmRefY call WAMIT_CalcOutput( Time, m%u_WAMIT(iBody), p%WAMIT(iBody), x%WAMIT(iBody), xd%WAMIT(iBody), & z%WAMIT, OtherState%WAMIT(iBody), y%WAMIT(iBody), m%WAMIT(iBody), ErrStat2, ErrMsg2 ) - call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'HydroDyn_CalcOutput' ) + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) y%WAMITMesh%Force (:,iBody) = y%WAMITMesh%Force (:,iBody) + y%WAMIT(iBody)%Mesh%Force (:,1) y%WAMITMesh%Moment(:,iBody) = y%WAMITMesh%Moment(:,iBody) + y%WAMIT(iBody)%Mesh%Moment(:,1) @@ -1289,8 +1382,8 @@ SUBROUTINE HydroDyn_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, ErrStat, if (p%WAMIT2used) then if ( p%NBodyMod == 1 .or. p%NBody == 1 ) then - call WAMIT2_CalcOutput( Time, p%WaveField, p%WAMIT2(1), y%WAMIT2(1), m%WAMIT2(1), ErrStat2, ErrMsg2 ) - call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'HydroDyn_CalcOutput' ) + call WAMIT2_CalcOutput( Time, PtfmRefY, p%WaveField, p%WAMIT2(1), y%WAMIT2(1), m%WAMIT2(1), ErrStat2, ErrMsg2 ) + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) do iBody=1,p%NBody y%WAMITMesh%Force (:,iBody) = y%WAMITMesh%Force (:,iBody) + y%WAMIT2(1)%Mesh%Force (:,iBody) y%WAMITMesh%Moment(:,iBody) = y%WAMITMesh%Moment(:,iBody) + y%WAMIT2(1)%Mesh%Moment(:,iBody) @@ -1300,8 +1393,8 @@ SUBROUTINE HydroDyn_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, ErrStat, else do iBody=1,p%NBody - call WAMIT2_CalcOutput( Time, p%WaveField, p%WAMIT2(iBody), y%WAMIT2(iBody), m%WAMIT2(iBody), ErrStat2, ErrMsg2 ) - call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'HydroDyn_CalcOutput' ) + call WAMIT2_CalcOutput( Time, PtfmRefY, p%WaveField, p%WAMIT2(iBody), y%WAMIT2(iBody), m%WAMIT2(iBody), ErrStat2, ErrMsg2 ) + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) y%WAMITMesh%Force (:,iBody) = y%WAMITMesh%Force (:,iBody) + y%WAMIT2(iBody)%Mesh%Force (:,1) y%WAMITMesh%Moment(:,iBody) = y%WAMITMesh%Moment(:,iBody) + y%WAMIT2(iBody)%Mesh%Moment(:,1) @@ -1332,22 +1425,20 @@ SUBROUTINE HydroDyn_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, ErrStat, IF ( u%Morison%Mesh%Committed ) THEN ! Make sure we are using Morison / there is a valid mesh + u%Morison%PtfmRefY = PtfmRefY CALL Morison_CalcOutput( Time, u%Morison, p%Morison, x%Morison, xd%Morison, & z%Morison, OtherState%Morison, y%Morison, m%Morison, ErrStat2, ErrMsg2 ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'HydroDyn_CalcOutput' ) + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) END IF - + ! Integrate all the mesh loads onto the platfrom reference Point (PRP) at (0,0,0) m%F_Hydro = CalcLoadsAtWRP( y, u, m%AllHdroOrigin, m%HD_MeshMap, ErrStat2, ErrMsg2 ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'HydroDyn_CalcOutput' ) - - - + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + ! Map calculated results into the first p%NumOuts values of the y%WriteOutput Array - CALL HDOut_MapOutputs( p, y, m%WAMIT, m%WAMIT2, m%F_PtfmAdd, m%F_Waves, m%F_Hydro, u%PRPMesh, q, qdot, qdotdot, ErrStat2, ErrMsg2 ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'HydroDyn_CalcOutput' ) - - + CALL HDOut_MapOutputs( p, y, m%WAMIT, m%WAMIT2, m%F_PtfmAdd, m%F_Waves, m%F_Hydro, u%PRPMesh, PtfmRefY, q, qdot, qdotdot, ErrStat2, ErrMsg2 ) + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + ! Aggregate the sub-module outputs IF (p%Morison%NumOuts > 0) THEN J = p%NumOuts + 1 @@ -1359,6 +1450,9 @@ SUBROUTINE HydroDyn_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, ErrStat, m%LastOutTime = Time + IF (ALLOCATED(RRb2g)) DEALLOCATE(RRb2g) + IF (ALLOCATED(RRg2b)) DEALLOCATE(RRg2b) + END SUBROUTINE HydroDyn_CalcOutput @@ -2518,7 +2612,7 @@ SUBROUTINE HD_Perturb_u( p, n, perturb_sign, u, du ) CASE ( 1) !Module/Mesh/Field: u%Morison%Mesh%TranslationDisp = 1 u%Morison%Mesh%TranslationDisp (fieldIndx,node) = u%Morison%Mesh%TranslationDisp (fieldIndx,node) + du * perturb_sign CASE ( 2) !Module/Mesh/Field: u%Morison%Mesh%Orientation = 2 - CALL PerturbOrientationMatrix( u%Morison%Mesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. ) + CALL PerturbOrientationMatrix( u%Morison%Mesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.false. ) CASE ( 3) !Module/Mesh/Field: u%Morison%Mesh%TranslationVel = 3 u%Morison%Mesh%TranslationVel( fieldIndx,node) = u%Morison%Mesh%TranslationVel( fieldIndx,node) + du * perturb_sign CASE ( 4) !Module/Mesh/Field: u%Morison%Mesh%RotationVel = 4 @@ -2533,7 +2627,7 @@ SUBROUTINE HD_Perturb_u( p, n, perturb_sign, u, du ) CASE ( 7) !Module/Mesh/Field: u%WAMITMesh%TranslationDisp = 7 u%WAMITMesh%TranslationDisp (fieldIndx,node) = u%WAMITMesh%TranslationDisp (fieldIndx,node) + du * perturb_sign CASE ( 8) !Module/Mesh/Field: u%WAMITMesh%Orientation = 8 - CALL PerturbOrientationMatrix( u%WAMITMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. ) + CALL PerturbOrientationMatrix( u%WAMITMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.false. ) CASE ( 9) !Module/Mesh/Field: u%WAMITMesh%TranslationVel = 9 u%WAMITMesh%TranslationVel( fieldIndx,node) = u%WAMITMesh%TranslationVel( fieldIndx,node) + du * perturb_sign CASE (10) !Module/Mesh/Field: u%WAMITMesh%RotationVel = 10 @@ -2547,7 +2641,7 @@ SUBROUTINE HD_Perturb_u( p, n, perturb_sign, u, du ) CASE (13) !Module/Mesh/Field: u%PRPMesh%TranslationDisp = 13 u%PRPMesh%TranslationDisp (fieldIndx,node) = u%PRPMesh%TranslationDisp (fieldIndx,node) + du * perturb_sign CASE (14) !Module/Mesh/Field: u%PRPMesh%Orientation = 14 - CALL PerturbOrientationMatrix( u%PRPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. ) + CALL PerturbOrientationMatrix( u%PRPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.false. ) CASE (15) !Module/Mesh/Field: u%PRPMesh%TranslationVel = 15 u%PRPMesh%TranslationVel( fieldIndx,node) = u%PRPMesh%TranslationVel( fieldIndx,node) + du * perturb_sign CASE (16) !Module/Mesh/Field: u%PRPMesh%RotationVel = 16 @@ -2562,7 +2656,7 @@ SUBROUTINE HD_Perturb_u( p, n, perturb_sign, u, du ) CASE ( 7) !Module/Mesh/Field: u%PRPMesh%TranslationDisp = 7 u%PRPMesh%TranslationDisp (fieldIndx,node) = u%PRPMesh%TranslationDisp (fieldIndx,node) + du * perturb_sign CASE ( 8) !Module/Mesh/Field: u%PRPMesh%Orientation = 8 - CALL PerturbOrientationMatrix( u%PRPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. ) + CALL PerturbOrientationMatrix( u%PRPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.false. ) CASE ( 9) !Module/Mesh/Field: u%PRPMesh%TranslationVel = 9 u%PRPMesh%TranslationVel( fieldIndx,node) = u%PRPMesh%TranslationVel( fieldIndx,node) + du * perturb_sign CASE (10) !Module/Mesh/Field: u%PRPMesh%RotationVel = 10 @@ -2578,7 +2672,7 @@ SUBROUTINE HD_Perturb_u( p, n, perturb_sign, u, du ) CASE (1) !Module/Mesh/Field: u%WAMITMesh%TranslationDisp = 1 u%WAMITMesh%TranslationDisp (fieldIndx,node) = u%WAMITMesh%TranslationDisp (fieldIndx,node) + du * perturb_sign CASE (2) !Module/Mesh/Field: u%WAMITMesh%Orientation = 2 - CALL PerturbOrientationMatrix( u%WAMITMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. ) + CALL PerturbOrientationMatrix( u%WAMITMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.false. ) CASE (3) !Module/Mesh/Field: u%WAMITMesh%TranslationVel = 3 u%WAMITMesh%TranslationVel( fieldIndx,node) = u%WAMITMesh%TranslationVel( fieldIndx,node) + du * perturb_sign CASE (4) !Module/Mesh/Field: u%WAMITMesh%RotationVel = 4 @@ -2592,7 +2686,7 @@ SUBROUTINE HD_Perturb_u( p, n, perturb_sign, u, du ) CASE ( 7) !Module/Mesh/Field: u%PRPMesh%TranslationDisp = 7 u%PRPMesh%TranslationDisp (fieldIndx,node) = u%PRPMesh%TranslationDisp (fieldIndx,node) + du * perturb_sign CASE ( 8) !Module/Mesh/Field: u%PRPMesh%Orientation = 8 - CALL PerturbOrientationMatrix( u%PRPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. ) + CALL PerturbOrientationMatrix( u%PRPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.false. ) CASE ( 9) !Module/Mesh/Field: u%PRPMesh%TranslationVel = 9 u%PRPMesh%TranslationVel( fieldIndx,node) = u%PRPMesh%TranslationVel( fieldIndx,node) + du * perturb_sign CASE (10) !Module/Mesh/Field: u%PRPMesh%RotationVel = 10 @@ -2607,7 +2701,7 @@ SUBROUTINE HD_Perturb_u( p, n, perturb_sign, u, du ) CASE ( 1) !Module/Mesh/Field: u%PRPMesh%TranslationDisp = 1 u%PRPMesh%TranslationDisp (fieldIndx,node) = u%PRPMesh%TranslationDisp (fieldIndx,node) + du * perturb_sign CASE ( 2) !Module/Mesh/Field: u%PRPMesh%Orientation = 2 - CALL PerturbOrientationMatrix( u%PRPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. ) + CALL PerturbOrientationMatrix( u%PRPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.false. ) CASE ( 3) !Module/Mesh/Field: u%PRPMesh%TranslationVel = 3 u%PRPMesh%TranslationVel( fieldIndx,node) = u%PRPMesh%TranslationVel( fieldIndx,node) + du * perturb_sign CASE ( 4) !Module/Mesh/Field: u%PRPMesh%RotationVel = 4 diff --git a/modules/hydrodyn/src/HydroDyn.txt b/modules/hydrodyn/src/HydroDyn.txt index 00939dcaaa..42014e9639 100644 --- a/modules/hydrodyn/src/HydroDyn.txt +++ b/modules/hydrodyn/src/HydroDyn.txt @@ -63,6 +63,9 @@ typedef ^ ^ LOGICAL typedef ^ ^ INTEGER UnSum - - - "File unit for the HydroDyn summary file [-1 = no summary file]" - typedef ^ ^ CHARACTER(20) OutFmt - - - "Output format for numerical results" - typedef ^ ^ CHARACTER(20) OutSFmt - - - "Output format for header strings" - +typedef ^ ^ INTEGER PtfmYMod - - - "Large yaw model" - +typedef ^ ^ ReKi PtfmRefY - - - "Initial reference yaw offset" (rad) +typedef ^ ^ ReKi PtfmYCutoff - - - "Low-pass cutoff frequency for filtering the platform yaw motion to obtain the reference yaw offset" (Hz) # typedef HydroDyn/HydroDyn InitInputType CHARACTER(1024) InputFile - - - "Supplied by Driver: full path and filename for the HydroDyn module" - typedef ^ ^ LOGICAL UseInputFile - .TRUE. - "Supplied by Driver: .TRUE. if using a input file, .FALSE. if all inputs are being passed in by the caller" - @@ -73,8 +76,8 @@ typedef ^ ^ ReKi typedef ^ ^ DbKi TMax - - - "Supplied by Driver: The total simulation time" "(sec)" typedef ^ ^ logical VisMeshes - .false. - "Output visualization meshes" - # -typedef ^ ^ LOGICAL InvalidWithSSExctn - - - "Whether SeaState configuration is invalid with HydroDyn's state-space excitation (ExctnMod=2)" (-) -typedef ^ ^ SeaSt_WaveFieldType *WaveField - - - "Pointer to SeaState wave field" - +typedef ^ ^ LOGICAL InvalidWithSSExctn - - - "Whether SeaState configuration is invalid with HydroDyn's state-space excitation (ExctnMod=2)" (-) +typedef ^ ^ SeaSt_WaveFieldType *WaveField - - - "Pointer to SeaState wave field" - # # # Define outputs from the initialization routine here: @@ -105,6 +108,7 @@ typedef ^ ContinuousStateType Morison_Con # typedef ^ DiscreteStateType WAMIT_DiscreteStateType WAMIT {:} - - "discrete states from the wamit module" - typedef ^ DiscreteStateType Morison_DiscreteStateType Morison - - - "discrete states from the Morison module" - +typedef ^ DiscreteStateType ReKi PtfmRefY {:} - - "Reference yaw position of the PRP relative to the inertial frame - Current step and two previous steps" (radians) # # # Define constraint states here: @@ -161,12 +165,14 @@ typedef ^ ^ CHARACTER(2 typedef ^ ^ CHARACTER(ChanLen) Delim - - - "Delimiter string for outputs, defaults to tab-delimiters" - typedef ^ ^ INTEGER UnOutFile - - - "File unit for the HydroDyn outputs" - typedef ^ ^ INTEGER OutDec - - - "Write every OutDec time steps" - -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_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 ^ ^ logical VisMeshes - .false. - "Output visualization meshes" - -typedef ^ ^ SeaSt_WaveFieldType *WaveField - - - "Pointer to SeaState wave field" - +typedef ^ ^ SeaSt_WaveFieldType *WaveField - - - "Pointer to SeaState wave field" - +typedef ^ ^ INTEGER PtfmYMod - - - "Large yaw model" - +typedef ^ ^ ReKi CYawFilt - - - "Low-pass filter constant for reference platform yaw position PtfmRefY" - # # # ..... Inputs .................................................................................................................... diff --git a/modules/hydrodyn/src/HydroDyn_C_Binding.f90 b/modules/hydrodyn/src/HydroDyn_C_Binding.f90 index 00a5e67e30..644df17433 100644 --- a/modules/hydrodyn/src/HydroDyn_C_Binding.f90 +++ b/modules/hydrodyn/src/HydroDyn_C_Binding.f90 @@ -547,7 +547,7 @@ subroutine SetMotionLoadsInterfaceMeshes(ErrStat3,ErrMsg3) ! initial position and orientation of node InitPos = tmpNodePos(1:3,iNode) theta = real(tmpNodePos(4:6,iNode),DbKi) ! convert ReKi to DbKi to avoid roundoff - CALL SmllRotTrans( 'InputRotation', theta(1), theta(2), theta(3), Orient, 'Orient', ErrStat, ErrMsg ) + Orient = EulerConstructZYX(theta) call MeshPositionNode( HD_MotionMesh , & iNode , & InitPos , & ! position @@ -749,8 +749,7 @@ SUBROUTINE HydroDyn_C_CalcOutput(Time_C, NumNodePts_C, NodePos_C, NodeVel_C, Nod ! Transfer motions to input meshes - call Set_MotionMesh( ErrStat2, ErrMsg2 ) ! update motion mesh with input motion arrays - if (Failed()) return + call Set_MotionMesh() ! update motion mesh with input motion arrays call HD_SetInputMotion( HD%u(1), ErrStat2, ErrMsg2 ) ! transfer input motion mesh to u(1) meshes if (Failed()) return @@ -902,8 +901,7 @@ SUBROUTINE HydroDyn_C_UpdateStates( Time_C, TimeNext_C, NumNodePts_C, NodePos_C, tmpNodeAcc(1:6,1:NumNodePts) = reshape( real(NodeAcc_C(1:6*NumNodePts),ReKi), (/6,NumNodePts/) ) ! Transfer motions to input meshes - call Set_MotionMesh( ErrStat2, ErrMsg2 ) ! update motion mesh with input motion arrays - if (Failed()) return + call Set_MotionMesh() ! update motion mesh with input motion arrays call HD_SetInputMotion( HD%u(INPUT_PRED), ErrStat2, ErrMsg2 ) ! transfer input motion mesh to u(1) meshes if (Failed()) return @@ -1062,16 +1060,14 @@ END SUBROUTINE HydroDyn_C_End !> This routine is operating on module level data, hence few inputs -subroutine Set_MotionMesh(ErrStat3, ErrMsg3) - integer(IntKi), intent( out) :: ErrStat3 - character(ErrMsgLen), intent( out) :: ErrMsg3 +subroutine Set_MotionMesh() integer(IntKi) :: iNode real(R8Ki) :: theta(3) real(R8Ki) :: Orient(3,3) ! Set mesh corresponding to input motions do iNode=1,NumNodePts theta = real(tmpNodePos(4:6,iNode),DbKi) ! convert ReKi to DbKi to avoid roundoff - CALL SmllRotTrans( 'InputRotation', theta(1), theta(2), theta(3), Orient, 'Orient', ErrStat3, ErrMsg3 ) + Orient = EulerConstructZYX(theta) HD_MotionMesh%TranslationDisp(1:3,iNode) = tmpNodePos(1:3,iNode) - HD_MotionMesh%Position(1:3,iNode) ! relative displacement only HD_MotionMesh%Orientation(1:3,1:3,iNode) = Orient HD_MotionMesh%TranslationVel( 1:3,iNode) = tmpNodeVel(1:3,iNode) diff --git a/modules/hydrodyn/src/HydroDyn_DriverCode.f90 b/modules/hydrodyn/src/HydroDyn_DriverCode.f90 index 115a1e68bf..1502bbace3 100644 --- a/modules/hydrodyn/src/HydroDyn_DriverCode.f90 +++ b/modules/hydrodyn/src/HydroDyn_DriverCode.f90 @@ -81,7 +81,7 @@ PROGRAM HydroDynDriver real(DbKi) :: TiLstPrn ! The simulation time of the last print integer :: n_SttsTime ! Number of time steps between screen status messages (-) - integer :: i ! Loop counter + integer :: i ! Loop counter logical :: SeaState_Initialized, HydroDyn_Initialized ! For testing @@ -172,7 +172,8 @@ PROGRAM HydroDynDriver InitInData_SeaSt%OutRootName = trim(drvrData%OutRootName)//'.SEA' InitInData_SeaSt%TMax = drvrData%TMax InitInData_SeaSt%Linearize = drvrData%Linearize - + + ! Initialize the HydroDyn module Interval = drvrData%TimeInterval call SeaSt_Init( InitInData_SeaSt, u_SeaSt(1), p_SeaSt, x_SeaSt, xd_SeaSt, z_SeaSt, OtherState_SeaSt, y_SeaSt, m_SeaSt, Interval, InitOutData_SeaSt, ErrStat, ErrMsg ) @@ -322,7 +323,6 @@ PROGRAM HydroDynDriver CALL HydroDyn_UpdateStates( Time, n, u, InputTime, p, x, xd, z, OtherState, m, ErrStat, ErrMsg ); CALL CheckError() - IF ( MOD( n + 1, n_SttsTime ) == 0 ) THEN CALL SimStatus( TiLstPrn, PrevClockTime, time, drvrData%TMax ) ENDIF diff --git a/modules/hydrodyn/src/HydroDyn_DriverSubs.f90 b/modules/hydrodyn/src/HydroDyn_DriverSubs.f90 index a9cc5e2c77..0a2a5bb773 100644 --- a/modules/hydrodyn/src/HydroDyn_DriverSubs.f90 +++ b/modules/hydrodyn/src/HydroDyn_DriverSubs.f90 @@ -30,6 +30,7 @@ MODULE HydroDynDriverSubs USE HydroDyn_Output USE ModMesh_Types USE VersionInfo + USE YawOffset IMPLICIT NONE @@ -80,6 +81,7 @@ MODULE HydroDynDriverSubs REAL(DbKi) :: TimeInterval REAL(DbKi) :: TMax INTEGER :: PRPInputsMod + REAL(ReKi) :: PtfmRefzt CHARACTER(1024) :: PRPInputsFile REAL(R8Ki) :: uPRPInSteady(6) REAL(R8Ki) :: uDotPRPInSteady(6) @@ -87,10 +89,12 @@ MODULE HydroDynDriverSubs REAL(R8Ki), ALLOCATABLE :: PRPin(:,:) ! Variable for storing time, forces, and body velocities, in m/s or rad/s for PRP REAL(R8Ki), ALLOCATABLE :: PRPinTime(:) ! Variable for storing time, forces, and body velocities, in m/s or rad/s for PRP INTEGER(IntKi) :: NBody ! Number of WAMIT bodies to work with if prescribing kinematics on each body (PRPInputsMod<0) - REAL(ReKi) :: PtfmRefzt TYPE(HD_Drvr_OutputFile) :: OutData character(500) :: FTitle ! description from 2nd line of driver file + REAL(R8Ki) :: PRPHdg + REAL(ReKi) :: CYawFilt + END TYPE HD_Drvr_Data ! ----------------------------------------------------------------------------------- @@ -100,7 +104,6 @@ MODULE HydroDynDriverSubs TYPE(ProgDesc), PARAMETER :: version = ProgDesc( 'HydroDyn Driver', '', '' ) ! The version number of this program. character(*), parameter :: Delim = Tab - CONTAINS SUBROUTINE ReadDriverInputFile( FileName, drvrData, ErrStat, ErrMsg ) @@ -255,7 +258,7 @@ SUBROUTINE ReadDriverInputFile( FileName, drvrData, ErrStat, ErrMsg ) CALL ReadVar ( UnIn, FileName, drvrData%PtfmRefzt, 'PtfmRefzt', 'Vertical distance from the ground level to the platform reference point', ErrStat, ErrMsg, UnEchoLocal ) if (Failed()) return - ! PRPInputsFile + ! PRPInputsFile CALL ReadVar ( UnIn, FileName, drvrData%PRPInputsFile, 'PRPInputsFile', 'Filename for the PRP HydroDyn inputs', ErrStat2, ErrMsg2, UnEchoLocal ) if (Failed()) return IF ( PathIsRelative( drvrData%PRPInputsFile ) ) drvrData%PRPInputsFile = TRIM(PriPath)//TRIM(drvrData%PRPInputsFile) @@ -280,7 +283,6 @@ SUBROUTINE ReadDriverInputFile( FileName, drvrData, ErrStat, ErrMsg ) ! uDotDotPRPInSteady CALL ReadAry ( UnIn, FileName, drvrData%uDotDotPRPInSteady, 6, 'uDotDotPRPInSteady', 'PRP Steady-state translational and rotational accelerations.', ErrStat2, ErrMsg2, UnEchoLocal) if (Failed()) return - IF ( drvrData%PRPInputsMod /= 1 ) THEN drvrData%uPRPInSteady = 0.0 @@ -291,7 +293,6 @@ SUBROUTINE ReadDriverInputFile( FileName, drvrData, ErrStat, ErrMsg ) drvrData%WrTxtOutFile = .true. drvrData%WrBinOutFile = .false. - CALL cleanup() CONTAINS @@ -776,14 +777,18 @@ SUBROUTINE SetHDInputs_Constant(u_HD, mappingData, drvrData, ErrStat, ErrMsg) u_HD%PRPMesh%TranslationDisp(:,1) = drvrData%uPRPInSteady(1:3) ! Compute direction cosine matrix from the rotation angles - CALL SmllRotTrans( 'InputRotation', drvrData%uPRPInSteady(4), drvrData%uPRPInSteady(5), drvrData%uPRPInSteady(6), u_HD%PRPMesh%Orientation(:,:,1), 'Junk', ErrStat2, ErrMsg2 ) - call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + u_HD%PRPMesh%Orientation(:,:,1) = EulerConstructZYX(drvrData%uPRPInSteady(4:6)) + ! Translation - No transformation needed u_HD%PRPMesh%TranslationVel(:,1) = drvrData%uDotPRPInSteady(1:3) - u_HD%PRPMesh%RotationVel(:,1) = drvrData%uDotPRPInSteady(4:6) u_HD%PRPMesh%TranslationAcc(:,1) = drvrData%uDotDotPRPInSteady(1:3) - u_HD%PRPMesh%RotationAcc(:,1) = drvrData%uDotDotPRPInSteady(4:6) - + ! Rotation - Compute angular velocity and acceleration from the rotation angles and time derivatives + call EulerDerivativeToAngVelAcc(drvrData%uPRPInSteady(4:6),& + REAL(drvrData%uDotPRPInSteady(4:6),ReKi),& + REAL(drvrData%uDotDotPRPInSteady(4:6),ReKi),& + u_HD%PRPMesh%RotationVel(:,1),& + u_HD%PRPMesh%RotationAcc(:,1)) + CALL PRP_TransferToMotionInputs(u_HD, mappingData, ErrStat2, ErrMsg2 ) call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) @@ -801,9 +806,11 @@ SUBROUTINE SetHDInputs(time, n, u_HD, mappingData, drvrData, ErrStat, ErrMsg) INTEGER, INTENT( OUT ) :: ErrStat ! returns a non-zero value when an error occurs CHARACTER(*), INTENT( OUT ) :: ErrMsg ! Error message if ErrStat /= ErrID_None + REAL(ReKi) :: tmp(3),tmp2(3) + integer(IntKi) :: errStat2 ! temporary error status of the operation character(ErrMsgLen) :: errMsg2 ! temporary error message - character(*), parameter :: RoutineName = 'SetHDInputs_Constant' + character(*), parameter :: RoutineName = 'SetHDInputs' real(R8Ki) :: yInterp(size(drvrData%PRPin,2)) integer(intKi) :: indxHigh, indxMid, indxLow integer(intKi) :: i @@ -820,18 +827,23 @@ SUBROUTINE SetHDInputs(time, n, u_HD, mappingData, drvrData, ErrStat, ErrMsg) ! Compute direction cosine matrix from the rotation angles ! maxAngle = max( maxAngle, abs(yInterp(4:6)) ) - - CALL SmllRotTrans( 'InputRotation', yInterp(4), yInterp(5), yInterp(6), u_HD%PRPMesh%Orientation(:,:,1), 'Junk', ErrStat2, ErrMsg2 ) - call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + ! Obtain the orientation matrix for the small rotation part from the reference yaw orientation + u_HD%PRPMesh%Orientation(:,:,1) = EulerConstructZYX(yInterp(4:6)) + + ! Translation - No transformation needed u_HD%PRPMesh%TranslationVel(:,1) = yInterp( 7: 9) - u_HD%PRPMesh%RotationVel(:,1) = yInterp(10:12) u_HD%PRPMesh%TranslationAcc(:,1) = yInterp(13:15) - u_HD%PRPMesh%RotationAcc(:,1) = yInterp(16:18) - + ! Rotation - Compute angular velocity and acceleration from the rotation angles and time derivatives + call EulerDerivativeToAngVelAcc(yInterp(4:6),& + REAL(yInterp(10:12),ReKi),& + REAL(yInterp(16:18),ReKi),& + u_HD%PRPMesh%RotationVel(:,1),& + u_HD%PRPMesh%RotationAcc(:,1)) + CALL PRP_TransferToMotionInputs(u_HD, mappingData, ErrStat2, ErrMsg2 ) call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) - + ELSEIF ( drvrData%PRPInputsMod < 0 ) THEN !@mhall: new kinematics input for moving bodies individually @@ -851,12 +863,10 @@ SUBROUTINE SetHDInputs(time, n, u_HD, mappingData, drvrData, ErrStat, ErrMsg) END DO ! PRP and body 1-NBody orientations (skipping the maxAngle stuff) - CALL SmllRotTrans( 'InputRotation', drvrData%PRPin(n, 4), drvrData%PRPin(n, 5), drvrData%PRPin(n, 6), u_HD%PRPMesh%Orientation(:,:,1), 'PRP orientation', ErrStat2, ErrMsg2 ) - call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + u_HD%PRPMesh%Orientation(:,:,1) = EulerConstructZYX(drvrData%PRPin(n,4:6)) DO I=1, drvrData%NBody - CALL SmllRotTrans( 'InputRotation', drvrData%PRPin(n,6*I+4), drvrData%PRPin(n,6*I+5), drvrData%PRPin(n,6*I+6), u_HD%WAMITMesh%Orientation(:,:,I), 'body orientation', ErrStat2, ErrMsg2 ) - call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + u_HD%WAMITMesh%Orientation(:,:,I) = EulerConstructZYX(drvrData%PRPin(n,(6*I+4):(6*I+6))) END DO ! use finite differences for velocities and accelerations @@ -900,7 +910,8 @@ SUBROUTINE SetHDInputs(time, n, u_HD, mappingData, drvrData, ErrStat, ErrMsg) END DO END IF - + + ! TO DO: Missing the first and last step below! ! calculate accelerations based on displacements: u_HD%PRPMesh%TranslationAcc(:,1) = (drvrData%PRPin(indxHigh, 1:3) - 2*drvrData%PRPin(indxMid, 1:3) + drvrData%PRPin(indxLow, 1:3)) /(drvrData%TimeInterval**2) u_HD%PRPMesh%RotationAcc( :,1) = (drvrData%PRPin(indxHigh, 4:6) - 2*drvrData%PRPin(indxMid, 4:6) + drvrData%PRPin(indxLow, 4:6)) /(drvrData%TimeInterval**2) @@ -909,9 +920,26 @@ SUBROUTINE SetHDInputs(time, n, u_HD, mappingData, drvrData, ErrStat, ErrMsg) u_HD%WAMITMesh%TranslationAcc(:,I) = (drvrData%PRPin(indxHigh, 6*I+1:6*I+3) - 2*drvrData%PRPin(indxMid, 6*I+1:6*I+3) + drvrData%PRPin(indxLow, 6*I+1:6*I+3))/(drvrData%TimeInterval**2) u_HD%WAMITMesh%RotationAcc( :,I) = (drvrData%PRPin(indxHigh, 6*I+4:6*I+6) - 2*drvrData%PRPin(indxMid, 6*I+4:6*I+6) + drvrData%PRPin(indxLow, 6*I+4:6*I+6))/(drvrData%TimeInterval**2) END DO - + + ! Rotation - Compute angular velocity and acceleration from the rotation angles and time derivatives + call EulerDerivativeToAngVelAcc(drvrData%PRPin(n,4:6),& + u_HD%PRPMesh%RotationVel(:,1),& + u_HD%PRPMesh%RotationAcc(:,1),& + tmp,tmp2) + u_HD%PRPMesh%RotationVel(:,1) = tmp + u_HD%PRPMesh%RotationAcc(:,1) = tmp2 + + DO I=1,drvrData%NBody + call EulerDerivativeToAngVelAcc(drvrData%PRPin(n,(6*I+4):(6*I+6)),& + u_HD%WAMITMesh%RotationVel(:,I),& + u_HD%WAMITMesh%RotationAcc(:,I),& + tmp,tmp2) + u_HD%WAMITMesh%RotationVel(:,I) = tmp + u_HD%WAMITMesh%RotationAcc(:,I) = tmp2 + + END DO - ! half of the PRP_TransferToMotionInputs routine: + ! half of the PRP_TransferToMotionInputs routine: IF ( u_HD%Morison%Mesh%Initialized ) THEN ! Map kinematics to the WAMIT mesh with 1 to NBody nodes CALL Transfer_Point_to_Point( u_HD%PRPMesh, u_HD%Morison%Mesh, mappingData%HD_Ref_2_M_P, ErrStat2, ErrMsg2 ) @@ -1036,7 +1064,7 @@ SUBROUTINE PRP_Perturb_u( n, perturb_sign, p, u, EDRPMotion, du, Motion_HDRP, ma CASE ( 1) !Module/Mesh/Field: u%PRPMesh%TranslationDisp = 1 pointMesh%TranslationDisp (fieldIndx,node) = pointMesh%TranslationDisp (fieldIndx,node) + du * perturb_sign CASE ( 2) !Module/Mesh/Field: u%PRPMesh%Orientation = 2 - CALL PerturbOrientationMatrix( pointMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. ) + CALL PerturbOrientationMatrix( pointMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.false. ) CASE ( 3) !Module/Mesh/Field: u%PRPMesh%TranslationVel = 3 pointMesh%TranslationVel( fieldIndx,node) = pointMesh%TranslationVel( fieldIndx,node) + du * perturb_sign CASE ( 4) !Module/Mesh/Field: u%PRPMesh%RotationVel = 4 @@ -1255,6 +1283,40 @@ subroutine cleanup() end subroutine cleanup END SUBROUTINE LINEARIZATION + +SUBROUTINE EulerDerivativeToAngVelAcc(u,udot,uddot,AngVel,AngAcc) + REAL(DbKi), INTENT( IN ) :: u(3) ! Tait-Bryan angles following the ZYX convention + REAL(ReKi), INTENT( IN ) :: udot(3) ! First time derivatives of the Tait-Bryan angles + REAL(ReKi), INTENT( IN ) :: uddot(3) ! Second time derivatives of the Tait-Bryan angles + REAL(ReKi), INTENT( OUT ) :: AngVel(3) ! Angular velocity in the earth-fixed frame of reference + REAL(ReKi), INTENT( OUT ) :: AngAcc(3) ! Angular acceleration in the earth-fixed frame of reference + REAL(DbKi) :: R, P, Y + REAL(DbKi) :: cR, sR, cP, sP, cY, sY + REAL(DbKi) :: A(3,3) + REAL(ReKi) :: Rdot, Pdot, Ydot + + R = u(1) + P = u(2) + Y = u(3) + Rdot = udot(1) + Pdot = udot(2) + Ydot = udot(3) + cR = cos(R) + sR = sin(R) + cP = cos(P) + sP = sin(P) + cY = cos(Y) + sY = sin(Y) + A(1,:) = (/cP*cY, -sY, 0.0_DbKi/) + A(2,:) = (/cP*sY, cY, 0.0_DbKi/) + A(3,:) = (/ -sP, 0.0_DbKi, 1.0_DbKi/) + AngVel = matmul(A,udot) + AngAcc(1) = -Rdot*(Pdot*sP*cY+Ydot*cP*sY)-Pdot*Ydot*cY + AngAcc(2) = -Rdot*(Pdot*sP*sY-Ydot*cP*cY)-Pdot*Ydot*sY + AngAcc(3) = -Rdot*Pdot*cP + AngAcc = AngAcc + matmul(A,uddot) +END SUBROUTINE EulerDerivativeToAngVelAcc + !---------------------------------------------------------------------------------------------------------------------------------- END MODULE HydroDynDriverSubs diff --git a/modules/hydrodyn/src/HydroDyn_Input.f90 b/modules/hydrodyn/src/HydroDyn_Input.f90 index 4e4800c3ea..fbd48b07ff 100644 --- a/modules/hydrodyn/src/HydroDyn_Input.f90 +++ b/modules/hydrodyn/src/HydroDyn_Input.f90 @@ -144,6 +144,24 @@ SUBROUTINE HydroDyn_ParseInput( InputFileName, OutRootName, FileInfo_In, InputFi call ParseVar( FileInfo_In, CurLine, 'ExctnCutOff', InputFileData%WAMIT%ExctnCutOff, ErrStat2, ErrMsg2, UnEc ) if (Failed()) return; + ! PtfmYMod - Model for large platform yaw offset {0: Static reference yaw offset based on PtfmRefY, 1: dynamic reference yaw offset based on low-pass filtering the PRP yaw motion with cutoff frequency PtfmYCutOff} (switch) + call ParseVar( FileInfo_In, CurLine, 'PtfmYMod', InputFileData%PtfmYMod, ErrStat2, ErrMsg2, UnEc ) + if (Failed()) return + + ! PtfmRefY - Constant or initial platform reference yaw offset (deg) + call ParseVar( FileInfo_In, CurLine, 'PtfmRefY', InputFileData%PtfmRefY, ErrStat2, ErrMsg2, UnEc ) + if (Failed()) return + InputFileData%PtfmRefY = InputFileData%PtfmRefY * D2R + + ! PtfmYCutOff - Cutoff frequency for the low-pass filtering of PRP yaw motion when PtfmYMod=1 [unused when PtfmYMod=0] (Hz) + call ParseVar( FileInfo_In, CurLine, 'PtfmYCutOff', InputFileData%PtfmYCutOff, ErrStat2, ErrMsg2, UnEc ) + if (Failed()) return + + ! NExctnHdg - Number of PRP headings/yaw offset evenly distributed in the range of [-180, 180) deg to precompute [used only when PtfmYMod = 1 in the HD driver or ElastoDyn] + call ParseVar( FileInfo_In, CurLine, 'NExctnHdg', InputFileData%WAMIT%NExctnHdg, ErrStat2, ErrMsg2, UnEc ) + if (Failed()) return; + InputFileData%WAMIT2%NExctnHdg = InputFileData%WAMIT%NExctnHdg + ! RdtnMod - Radiation memory-effect model {1: convolution, 2: state-space} (switch) ! [STATE-SPACE REQUIRES *.ss INPUT FILE] call ParseVar( FileInfo_In, CurLine, 'RdtnMod', InputFileData%WAMIT%RdtnMod, ErrStat2, ErrMsg2, UnEc ) @@ -1467,7 +1485,6 @@ SUBROUTINE HydroDynInput_ProcessInitData( InitInp, Interval, InputFileData, ErrS if ( InitInp%InvalidWithSSExctn ) then call SetErrStat( ErrID_Fatal, 'Given SeaState conditions cannot be used with state-space wave excitations. In SeaState, WaveMod cannot be 6; WaveDirMod must be 0; WvDiffQTF must be FALSE; and WvSumQTF must be FALSE. Or in HydroDyn set ExctnMod to 0 or 1.', ErrStat, ErrMsg, RoutineName ) end if - if ( InputFileData%PotMod /= 1 ) then call SetErrStat( ErrID_Fatal, 'Potential-flow model via WAMIT must be used with state-space wave excitations. Set PotMod= 1.', ErrStat, ErrMsg, RoutineName ) @@ -1538,6 +1555,7 @@ SUBROUTINE HydroDynInput_ProcessInitData( InitInp, Interval, InputFileData, ErrS CALL SetErrStat( ErrID_Fatal,'AMMod must be 0 or 1',ErrStat,ErrMsg,RoutineName) RETURN END IF + !------------------------------------------------------------------------------------------------- ! Member Joints Section @@ -2196,6 +2214,35 @@ SUBROUTINE HydroDynInput_ProcessInitData( InitInp, Interval, InputFileData, ErrS END IF + !------------------------------------------------------------------------------------------------- + ! Large yaw offset + !------------------------------------------------------------------------------------------------- + if (InputFileData%PtfmYMod /= 0 .AND. InputFileData%PtfmYMod /= 1) then + call SetErrStat( ErrID_Fatal,'PtfmYMod must be 0 (static platform reference yaw offset) or 1 (dynamic platform reference yaw offset).',ErrStat,ErrMsg,RoutineName) + return + end if + IF ( InputFileData%PtfmYMod .EQ. 1_IntKi ) THEN + if ( InputFileData%PtfmYCutOff <= 0.0_ReKi ) then + CALL SetErrStat( ErrID_Fatal, 'PtfmYCutOff must be greater than 0 Hz.',ErrStat,ErrMsg,RoutineName) + end if + if ( InputFileData%Morison%WaveDisp == 0 .AND. InputFileData%Morison%NMembers > 0 ) then + call SetErrStat( ErrID_Fatal,'Dynamic reference yaw offset (PtfmYMod=1) in ElastoDyn or HydroDyn driver cannot be used with WaveDisp=0. Set WaveDisp=1.',ErrStat,ErrMsg,RoutineName) + return + end if + if ( InputFileData%PotMod > 0 .AND. InputFileData%WAMIT%ExctnMod == 2 ) then + call SetErrStat( ErrID_Fatal, 'Dynamic reference yaw offset (PtfmYMod=1) in ElastoDyn or HydroDyn driver cannot be used with State-space wave excitations. Set ExctnMod=0 or 1.', ErrStat, ErrMsg, RoutineName ) + return + end if + if ( InputFileData%PotMod > 0 .AND. InputFileData%WAMIT%NExctnHdg < 2 ) then + call SetErrStat( ErrID_Fatal, 'NExctnHdg must be greater than or equal to 2.', ErrStat, ErrMsg, RoutineName ) + return + end if + if ( InputFileData%WAMIT2%SumQTFF .OR. InputFileData%WAMIT2%DiffQTFF ) then + call SetErrStat( ErrID_Fatal, 'Dynamic reference yaw offset (PtfmYMod=1) in ElastoDyn or HydroDyn driver cannot be used with full sum-frequency or difference-frequency QTFs. Set SumQTF and DiffQTF to 0.', ErrStat, ErrMsg, RoutineName ) + return + end if + END IF + !------------------------------------------------------------------------------------------------- ! Member Output List Section !------------------------------------------------------------------------------------------------- diff --git a/modules/hydrodyn/src/HydroDyn_Output.f90 b/modules/hydrodyn/src/HydroDyn_Output.f90 index 1823f1e565..7a971b36b2 100644 --- a/modules/hydrodyn/src/HydroDyn_Output.f90 +++ b/modules/hydrodyn/src/HydroDyn_Output.f90 @@ -25,6 +25,7 @@ MODULE HydroDyn_Output USE HydroDyn_Types !USE HydroDyn_Output_Types USE Waves + USE YawOffset IMPLICIT NONE PRIVATE @@ -813,7 +814,7 @@ END SUBROUTINE HDOut_OpenSum !==================================================================================================== -SUBROUTINE HDOut_MapOutputs( p, y, m_WAMIT, m_WAMIT2, F_Add, F_Waves, F_Hydro, PRPmesh, q, qdot, qdotdot, ErrStat, ErrMsg ) +SUBROUTINE HDOut_MapOutputs( p, y, m_WAMIT, m_WAMIT2, F_Add, F_Waves, F_Hydro, PRPmesh, PtfmRefY, q, qdot, qdotdot, ErrStat, ErrMsg ) ! This subroutine writes the data stored in the y variable to the correct indexed postions in WriteOutput ! This is called by HydroDyn_CalcOutput() at each time step. !---------------------------------------------------------------------------------------------------- @@ -825,6 +826,7 @@ SUBROUTINE HDOut_MapOutputs( p, y, m_WAMIT, m_WAMIT2, F_Add, F_Waves, F_Hydro, P REAL(ReKi), ALLOCATABLE, INTENT( IN ) :: F_Waves(:) REAL(ReKi), INTENT( IN ) :: F_Hydro(:) ! All hydrodynamic loads integrated at (0,0,0) in the global coordinate system type(MeshType), INTENT( IN ) :: PRPmesh ! the PRP mesh -- for motions output + REAL(ReKi), INTENT( IN ) :: PtfmRefY ! the PRP reference yaw offset REAL(ReKi), INTENT( IN ) :: q(:) ! WAMIT body translations and rotations REAL(ReKi), INTENT( IN ) :: qdot(:) ! WAMIT body translational and rotational velocities REAL(ReKi), INTENT( IN ) :: qdotdot(:) ! WAMIT body translational and rotational accelerations @@ -843,8 +845,9 @@ SUBROUTINE HDOut_MapOutputs( p, y, m_WAMIT, m_WAMIT2, F_Add, F_Waves, F_Hydro, P ! Initialize all unused channels to zero (in case they don't get set, but are still requested) AllOuts = 0.0_ReKi - rotdisp = GetSmllRotAngs ( PRPMesh%Orientation(:,:,1), ErrStat2, ErrMsg2 ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'HDOut_MapOutputs' ) + ! rotdisp = GetRotAngs ( PtfmRefY, PRPMesh%Orientation(:,:,1), ErrStat2, ErrMsg2 ) + ! CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'HDOut_MapOutputs' ) + rotdisp = EulerExtractZYX(PRPMesh%Orientation(:,:,1)) AllOuts(PRPMotions) = reshape((/real(PRPMesh%TranslationDisp(:,1),ReKi),rotdisp(:)/),(/6/)) AllOuts(PRPVel) = reshape((/PRPMesh%TranslationVel(:,1),PRPMesh%RotationVel(:,1)/),(/6/)) AllOuts(PRPAcc) = reshape((/PRPMesh%TranslationAcc(:,1),PRPMesh%RotationAcc(:,1)/),(/6/)) diff --git a/modules/hydrodyn/src/HydroDyn_Types.f90 b/modules/hydrodyn/src/HydroDyn_Types.f90 index d0c6fac27f..bd0e1cac49 100644 --- a/modules/hydrodyn/src/HydroDyn_Types.f90 +++ b/modules/hydrodyn/src/HydroDyn_Types.f90 @@ -77,6 +77,9 @@ MODULE HydroDyn_Types INTEGER(IntKi) :: UnSum = 0_IntKi !< File unit for the HydroDyn summary file [-1 = no summary file] [-] CHARACTER(20) :: OutFmt !< Output format for numerical results [-] CHARACTER(20) :: OutSFmt !< Output format for header strings [-] + INTEGER(IntKi) :: PtfmYMod = 0_IntKi !< Large yaw model [-] + REAL(ReKi) :: PtfmRefY = 0.0_ReKi !< Initial reference yaw offset [(rad)] + REAL(ReKi) :: PtfmYCutoff = 0.0_ReKi !< Low-pass cutoff frequency for filtering the platform yaw motion to obtain the reference yaw offset [(Hz)] END TYPE HydroDyn_InputFile ! ======================= ! ========= HydroDyn_InitInputType ======= @@ -123,6 +126,7 @@ MODULE HydroDyn_Types TYPE, PUBLIC :: HydroDyn_DiscreteStateType TYPE(WAMIT_DiscreteStateType) , DIMENSION(:), ALLOCATABLE :: WAMIT !< discrete states from the wamit module [-] TYPE(Morison_DiscreteStateType) :: Morison !< discrete states from the Morison module [-] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: PtfmRefY !< Reference yaw position of the PRP relative to the inertial frame - Current step and two previous steps [(radians)] END TYPE HydroDyn_DiscreteStateType ! ======================= ! ========= HydroDyn_ConstraintStateType ======= @@ -186,6 +190,8 @@ MODULE HydroDyn_Types INTEGER(IntKi) :: Jac_ny = 0_IntKi !< number of outputs in jacobian matrix [-] LOGICAL :: VisMeshes = .false. !< Output visualization meshes [-] TYPE(SeaSt_WaveFieldType) , POINTER :: WaveField => NULL() !< Pointer to SeaState wave field [-] + INTEGER(IntKi) :: PtfmYMod = 0_IntKi !< Large yaw model [-] + REAL(ReKi) :: CYawFilt = 0.0_ReKi !< Low-pass filter constant for reference platform yaw position PtfmRefY [-] END TYPE HydroDyn_ParameterType ! ======================= ! ========= HydroDyn_InputType ======= @@ -423,6 +429,9 @@ subroutine HydroDyn_CopyInputFile(SrcInputFileData, DstInputFileData, CtrlCode, DstInputFileData%UnSum = SrcInputFileData%UnSum DstInputFileData%OutFmt = SrcInputFileData%OutFmt DstInputFileData%OutSFmt = SrcInputFileData%OutSFmt + DstInputFileData%PtfmYMod = SrcInputFileData%PtfmYMod + DstInputFileData%PtfmRefY = SrcInputFileData%PtfmRefY + DstInputFileData%PtfmYCutoff = SrcInputFileData%PtfmYCutoff end subroutine subroutine HydroDyn_DestroyInputFile(InputFileData, ErrStat, ErrMsg) @@ -526,6 +535,9 @@ subroutine HydroDyn_PackInputFile(RF, Indata) call RegPack(RF, InData%UnSum) call RegPack(RF, InData%OutFmt) call RegPack(RF, InData%OutSFmt) + call RegPack(RF, InData%PtfmYMod) + call RegPack(RF, InData%PtfmRefY) + call RegPack(RF, InData%PtfmYCutoff) if (RegCheckErr(RF, RoutineName)) return end subroutine @@ -571,6 +583,9 @@ subroutine HydroDyn_UnPackInputFile(RF, OutData) call RegUnpack(RF, OutData%UnSum); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%OutFmt); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%OutSFmt); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%PtfmYMod); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%PtfmRefY); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%PtfmYCutoff); if (RegCheckErr(RF, RoutineName)) return end subroutine subroutine HydroDyn_CopyInitInput(SrcInitInputData, DstInitInputData, CtrlCode, ErrStat, ErrMsg) @@ -1048,6 +1063,18 @@ subroutine HydroDyn_CopyDiscState(SrcDiscStateData, DstDiscStateData, CtrlCode, call Morison_CopyDiscState(SrcDiscStateData%Morison, DstDiscStateData%Morison, CtrlCode, ErrStat2, ErrMsg2) call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) if (ErrStat >= AbortErrLev) return + if (allocated(SrcDiscStateData%PtfmRefY)) then + LB(1:1) = lbound(SrcDiscStateData%PtfmRefY, kind=B8Ki) + UB(1:1) = ubound(SrcDiscStateData%PtfmRefY, kind=B8Ki) + if (.not. allocated(DstDiscStateData%PtfmRefY)) then + allocate(DstDiscStateData%PtfmRefY(LB(1):UB(1)), stat=ErrStat2) + if (ErrStat2 /= 0) then + call SetErrStat(ErrID_Fatal, 'Error allocating DstDiscStateData%PtfmRefY.', ErrStat, ErrMsg, RoutineName) + return + end if + end if + DstDiscStateData%PtfmRefY = SrcDiscStateData%PtfmRefY + end if end subroutine subroutine HydroDyn_DestroyDiscState(DiscStateData, ErrStat, ErrMsg) @@ -1072,6 +1099,9 @@ subroutine HydroDyn_DestroyDiscState(DiscStateData, ErrStat, ErrMsg) end if call Morison_DestroyDiscState(DiscStateData%Morison, ErrStat2, ErrMsg2) call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + if (allocated(DiscStateData%PtfmRefY)) then + deallocate(DiscStateData%PtfmRefY) + end if end subroutine subroutine HydroDyn_PackDiscState(RF, Indata) @@ -1091,6 +1121,7 @@ subroutine HydroDyn_PackDiscState(RF, Indata) end do end if call Morison_PackDiscState(RF, InData%Morison) + call RegPackAlloc(RF, InData%PtfmRefY) if (RegCheckErr(RF, RoutineName)) return end subroutine @@ -1117,6 +1148,7 @@ subroutine HydroDyn_UnPackDiscState(RF, OutData) end do end if call Morison_UnpackDiscState(RF, OutData%Morison) ! Morison + call RegUnpackAlloc(RF, OutData%PtfmRefY); if (RegCheckErr(RF, RoutineName)) return end subroutine subroutine HydroDyn_CopyConstrState(SrcConstrStateData, DstConstrStateData, CtrlCode, ErrStat, ErrMsg) @@ -1698,6 +1730,8 @@ subroutine HydroDyn_CopyParam(SrcParamData, DstParamData, CtrlCode, ErrStat, Err DstParamData%Jac_ny = SrcParamData%Jac_ny DstParamData%VisMeshes = SrcParamData%VisMeshes DstParamData%WaveField => SrcParamData%WaveField + DstParamData%PtfmYMod = SrcParamData%PtfmYMod + DstParamData%CYawFilt = SrcParamData%CYawFilt end subroutine subroutine HydroDyn_DestroyParam(ParamData, ErrStat, ErrMsg) @@ -1834,6 +1868,8 @@ subroutine HydroDyn_PackParam(RF, Indata) call SeaSt_WaveField_PackSeaSt_WaveFieldType(RF, InData%WaveField) end if end if + call RegPack(RF, InData%PtfmYMod) + call RegPack(RF, InData%CYawFilt) if (RegCheckErr(RF, RoutineName)) return end subroutine @@ -1933,6 +1969,8 @@ subroutine HydroDyn_UnPackParam(RF, OutData) else OutData%WaveField => null() end if + call RegUnpack(RF, OutData%PtfmYMod); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%CYawFilt); if (RegCheckErr(RF, RoutineName)) return end subroutine subroutine HydroDyn_CopyInput(SrcInputData, DstInputData, CtrlCode, ErrStat, ErrMsg) diff --git a/modules/hydrodyn/src/Morison.f90 b/modules/hydrodyn/src/Morison.f90 index b192582519..29c47031c9 100644 --- a/modules/hydrodyn/src/Morison.f90 +++ b/modules/hydrodyn/src/Morison.f90 @@ -26,6 +26,7 @@ MODULE Morison USE SeaSt_WaveField ! USE HydroDyn_Output_Types USE NWTC_Library + USE YawOffset IMPLICIT NONE @@ -1925,6 +1926,7 @@ SUBROUTINE Morison_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, In p%WaveDisp = InitInp%WaveDisp p%AMMod = InitInp%AMMod p%VisMeshes = InitInp%VisMeshes ! visualization mesh for morison elements + p%PtfmYMod = InitInp%PtfmYMod ! Pointer to SeaState WaveField p%WaveField => InitInp%WaveField @@ -2557,6 +2559,7 @@ SUBROUTINE Morison_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, errStat, REAL(ReKi) :: iArm(3), iTerm(3), Ioffset, h_c, dRdl_p, dRdl_pp, f_hydro(3), Am(3,3), lstar, deltal, deltalLeft, deltalRight REAL(ReKi) :: h, h_c_AM, deltal_AM REAL(ReKi) :: F_WMG(6), F_IMG(6), F_If(6), F_B0(6), F_B1(6), F_B2(6), F_B_End(6) + REAL(ReKi) :: AM_End(3,3), An_End(3), DP_Const_End(3), I_MG_End(3,3) ! Local variables needed for wave stretching and load smoothing/redistribution INTEGER(IntKi) :: FSElem @@ -2622,8 +2625,10 @@ SUBROUTINE Morison_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, errStat, ! Loop through each member DO im = 1, p%NMembers - N = p%Members(im)%NElements - mem = p%Members(im) !@mhall: does this have much overhead? + N = p%Members(im)%NElements + mem = p%Members(im) + call YawMember(mem, u%PtfmRefY, ErrStat2, ErrMsg2) + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) !zero member loads m%memberLoads(im)%F_B = 0.0_ReKi @@ -3478,7 +3483,6 @@ SUBROUTINE Morison_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, errStat, !TODO: Where's F_WMF_End computed? DO J = 1, p%NJoints - ! Obtain the node index because WaveVel, WaveAcc, and WaveDynP are defined in the node indexing scheme, not the markers (No longer relevant?) ! The first NJoints nodes are all the joints with the rest being the internal nodes. See Morison_GenerateSimulationNodes. @@ -3489,25 +3493,29 @@ SUBROUTINE Morison_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, errStat, ! Therefore, no need to check PropPot here. ! Effect of wave stretching already baked into m%FDynP, m%FA, and m%vrel. No additional modification needed. - + + ! Joint yaw offset + call YawJoint(J,u%PtfmRefY,AM_End,An_End,DP_Const_End,I_MG_End,ErrStat2,ErrMsg2) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + ! Lumped added mass loads qdotdot = reshape((/u%Mesh%TranslationAcc(:,J),u%Mesh%RotationAcc(:,J)/),(/6/)) - m%F_A_End(:,J) = m%nodeInWater(j) * matmul( p%AM_End(:,:,J) , ( - qdotdot(1:3)) ) + m%F_A_End(:,J) = m%nodeInWater(j) * matmul( AM_End, ( - qdotdot(1:3)) ) ! TODO: The original code did not multiply by nodeInWater, but should we? GJH ! Should be ok because m%FDynP and m%FA are both zeroed above the SWL (when WaveStMod=0) or the instantaneous free surface (when WaveStMod>0) - m%F_I_End(:,J) = (p%DP_Const_End(:,j) * m%FDynP(j) + matmul(p%AM_End(:,:,j),m%FA(:,j))) + m%F_I_End(:,J) = (DP_Const_End * m%FDynP(j) + matmul(AM_End,m%FA(:,j))) ! Marine growth inertia: ends: Section 4.2.2 ! With wave stretching, m%nodeInWater is based on the instantaneous free surface and the current body position if (WaveDisp/=0). ! This should still be ok because with wave stretching, we do not allow joints to come out of water if initially submerged or ! enter water if initially out of water. This is enforced when computing the side loads above. m%F_IMG_End(1:3,j) = -m%nodeInWater(j) * p%Mass_MG_End(j)*qdotdot(1:3) - m%F_IMG_End(4:6,j) = -m%nodeInWater(j) * (matmul(p%I_MG_End(:,:,j),qdotdot(4:6)) - cross_product(u%Mesh%RotationVel(:,J),matmul(p%I_MG_End(:,:,j),u%Mesh%RotationVel(:,J)))) + m%F_IMG_End(4:6,j) = -m%nodeInWater(j) * (matmul(I_MG_End,qdotdot(4:6)) - cross_product(u%Mesh%RotationVel(:,J),matmul(I_MG_End,u%Mesh%RotationVel(:,J)))) ! Compute the dot product of the relative velocity vector with the directional Area of the Joint ! m%nodeInWater(j) is probably not necessary because m%vrel is zeroed when the node is out of water - vmag = m%nodeInWater(j) * ( m%vrel(1,j)*p%An_End(1,J) + m%vrel(2,j)*p%An_End(2,J) + m%vrel(3,j)*p%An_End(3,J) ) + vmag = m%nodeInWater(j) * ( m%vrel(1,j)*An_End(1) + m%vrel(2,j)*An_End(2) + m%vrel(3,j)*An_End(3) ) ! High-pass filtering vmagf = p%VRelNFiltConst(J) * (vmag + xd%v_rel_n_FiltStat(J)) @@ -3520,12 +3528,12 @@ SUBROUTINE Morison_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, errStat, IF (I < 4 ) THEN ! Three force components IF ( p%DragMod_End(J) .EQ. 0_IntKi ) THEN ! Note: vmag is zero if node is not in the water - m%F_D_End(i,j) = (1.0_ReKi - p%DragLoFSc_End(j)) * p%An_End(i,j) * p%DragConst_End(j) * abs(vmagf)*vmagf & - + p%DragLoFSc_End(j) * p%An_End(i,j) * p%DragConst_End(j) * abs(vmag )*vmag + m%F_D_End(i,j) = (1.0_ReKi - p%DragLoFSc_End(j)) * An_End(i) * p%DragConst_End(j) * abs(vmagf)*vmagf & + + p%DragLoFSc_End(j) * An_End(i) * p%DragConst_End(j) * abs(vmag )*vmag ELSE IF (p%DragMod_End(J) .EQ. 1_IntKi) THEN ! Note: vmag is zero if node is not in the water - m%F_D_End(i,j) = (1.0_ReKi - p%DragLoFSc_End(j)) * p%An_End(i,j) * p%DragConst_End(j) * abs(vmagf)*max(vmagf,0.0_ReKi) & - + p%DragLoFSc_End(j) * p%An_End(i,j) * p%DragConst_End(j) * abs(vmag) *max(vmag, 0.0_ReKi) + m%F_D_End(i,j) = (1.0_ReKi - p%DragLoFSc_End(j)) * An_End(i) * p%DragConst_End(j) * abs(vmagf)*max(vmagf,0.0_ReKi) & + + p%DragLoFSc_End(j) * An_End(i) * p%DragConst_End(j) * abs(vmag) *max(vmag, 0.0_ReKi) m%F_D_End(i,j) = 2.0_ReKi * m%F_D_End(i,j) END IF @@ -3556,6 +3564,10 @@ SUBROUTINE Morison_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, errStat, SUBROUTINE GetDisplacedNodePosition( forceDisplaced, pos ) LOGICAL, INTENT( IN ) :: forceDisplaced ! Set to true to return the exact displaced position no matter WaveDisp or WaveStMod REAL(ReKi), INTENT( OUT ) :: pos(:,:) ! Displaced node positions + REAL(ReKi) :: Orient(3,3) + + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 ! Undisplaced node position pos = u%Mesh%Position @@ -3568,6 +3580,10 @@ SUBROUTINE GetDisplacedNodePosition( forceDisplaced, pos ) ! Use displaced Z position only when wave stretching is enabled pos(3,:) = pos(3,:) + u%Mesh%TranslationDisp(3,:) END IF + ELSE ! p%WaveDisp=0 implies PtfmYMod=0 + ! Rotate the structure based on PtfmRefY (constant) + call GetPtfmRefYOrient(u%PtfmRefY, Orient, ErrStat2, ErrMsg2) + pos = matmul(transpose(Orient),pos) END IF END SUBROUTINE GetDisplacedNodePosition @@ -4106,6 +4122,67 @@ SUBROUTINE getElementHstLds_Mod1( Time, pos1, pos2, Zeta1, Zeta2, k_hat, r1, r2, END IF END SUBROUTINE getElementHstLds_Mod1 + SUBROUTINE YawMember(member, PtfmRefY, ErrStat, ErrMsg) + Type(Morison_MemberType), intent(inout) :: member + Real(ReKi), intent(in ) :: PtfmRefY + Integer(IntKi), intent( out) :: ErrStat + Character(*), intent( out) :: ErrMsg + + Real(ReKi) :: k(3) + Real(ReKi) :: kkt(3,3) + Real(ReKi) :: Ak(3,3) + Integer(IntKi) :: ErrStat2 + Character(ErrMsgLen) :: ErrMsg2 + + Character(*), parameter :: RoutineName = 'YawMember' + + ErrStat = ErrID_None + ErrMsg = '' + + call hiFrameTransform(h2i,PtfmRefY,member%k,k,ErrStat2,ErrMsg2) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + member%k = k + + call hiFrameTransform(h2i,PtfmRefY,member%kkt,kkt,ErrStat2,ErrMsg2) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + member%kkt = kkt + + call hiFrameTransform(h2i,PtfmRefY,member%Ak,Ak,ErrStat2,ErrMsg2) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + member%Ak = Ak + + END SUBROUTINE YawMember + + SUBROUTINE YawJoint(JointNo,PtfmRefY,AM_End,An_End,DP_Const_End,I_MG_End,ErrStat,ErrMsg) + Integer(IntKi), intent(in ) :: JointNo + Real(ReKi), intent(in ) :: PtfmRefY + Real(ReKi), intent( out) :: AM_End(3,3) + Real(ReKi), intent( out) :: An_End(3) + Real(ReKi), intent( out) :: DP_Const_End(3) + Real(ReKi), intent( out) :: I_MG_End(3,3) + Integer(IntKi), intent( out) :: ErrStat + Character(*), intent( out) :: ErrMsg + + Integer(IntKi) :: ErrStat2 + Character(ErrMsgLen) :: ErrMsg2 + + Character(*), parameter :: RoutineName = 'YawJoint' + + ErrStat = ErrID_None + ErrMsg = '' + + call hiFrameTransform(h2i,PtfmRefY,p%AM_End(:,:,jointNo),AM_End,ErrStat2,ErrMsg2) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + call hiFrameTransform(h2i,PtfmRefY,p%An_End(:,jointNo),An_End,ErrStat2,ErrMsg2) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + call hiFrameTransform(h2i,PtfmRefY,p%DP_Const_End(:,jointNo),DP_Const_End,ErrStat2,ErrMsg2) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + call hiFrameTransform(h2i,PtfmRefY,p%I_MG_End(:,:,jointNo),I_MG_End,ErrStat2,ErrMsg2) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + + END SUBROUTINE YawJoint + + END SUBROUTINE Morison_CalcOutput !---------------------------------------------------------------------------------------------------------------------------------- subroutine LumpDistrHydroLoads( f_hydro, k_hat, dl, h_c, lumpedLoad ) @@ -4187,7 +4264,7 @@ SUBROUTINE Morison_UpdateDiscState( Time, u, p, x, xd, z, OtherState, m, errStat CHARACTER(*), INTENT( OUT) :: errMsg !< Error message if errStat /= ErrID_None INTEGER(IntKi) :: J INTEGER(IntKi) :: nodeInWater - REAL(ReKi) :: pos(3), vrel(3), FV(3), vmag, vmagf + REAL(ReKi) :: pos(3), vrel(3), FV(3), vmag, vmagf, An_End(3) REAL(SiKi) :: FVTmp(3) INTEGER(IntKi) :: errStat2 CHARACTER(ErrMsgLen) :: errMsg2 @@ -4222,8 +4299,12 @@ SUBROUTINE Morison_UpdateDiscState( Time, u, p, x, xd, z, OtherState, m, errStat FV = REAL(FVTmp, ReKi) vrel = ( FV - u%Mesh%TranslationVel(:,J) ) * nodeInWater + ! Transform An_End based on reference yaw offset + call hiFrameTransform(h2i,u%PtfmRefY,p%An_End(:,j),An_End,ErrStat2,ErrMsg2) + call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + ! Compute the dot product of the relative velocity vector with the directional Area of the Joint - vmag = vrel(1)*p%An_End(1,J) + vrel(2)*p%An_End(2,J) + vrel(3)*p%An_End(3,J) + vmag = vrel(1)*An_End(1) + vrel(2)*An_End(2) + vrel(3)*An_End(3) ! High-pass filtering vmagf = p%VRelNFiltConst(J) * (vmag + xd%V_rel_n_FiltStat(J)) ! Update relative normal velocity filter state for joint J diff --git a/modules/hydrodyn/src/Morison.txt b/modules/hydrodyn/src/Morison.txt index 43ff0ab18c..5276e6dfa3 100644 --- a/modules/hydrodyn/src/Morison.txt +++ b/modules/hydrodyn/src/Morison.txt @@ -269,7 +269,8 @@ typedef ^ ^ CHARACTER(C typedef ^ ^ INTEGER NumOuts - - - "" - typedef ^ ^ INTEGER UnSum - - - "" - typedef ^ ^ SeaSt_WaveFieldType *WaveField - - - "Pointer to SeaState wave field" - -typedef ^ ^ logical VisMeshes - .false. - "Output visualization meshes" - +typedef ^ ^ logical VisMeshes - .false. - "Output visualization meshes" - +typedef ^ ^ INTEGER PtfmYMod - - - "Large yaw model" - # # # Define outputs from the initialization routine here: @@ -353,13 +354,15 @@ typedef ^ ^ Morison_JOu typedef ^ ^ OutParmType OutParam {:} - - "" - typedef ^ ^ INTEGER NumOuts - - - "" - typedef ^ ^ SeaSt_WaveFieldType *WaveField - - - "SeaState wave field" - -typedef ^ ^ logical VisMeshes - .false. - "Output visualization meshes" - +typedef ^ ^ logical VisMeshes - .false. - "Output visualization meshes" - +typedef ^ ^ INTEGER PtfmYMod - - - "Large yaw model" - # # # ..... Inputs .................................................................................................................... # Define inputs that are contained on the mesh here: # typedef ^ InputType MeshType Mesh - - - "Kinematics of each node input mesh" - +typedef ^ ^ ReKi PtfmRefY - - - "Reference platform yaw offset" (rad) # # # ..... Outputs ................................................................................................................... diff --git a/modules/hydrodyn/src/Morison_Types.f90 b/modules/hydrodyn/src/Morison_Types.f90 index c68757261d..0fe65d6c83 100644 --- a/modules/hydrodyn/src/Morison_Types.f90 +++ b/modules/hydrodyn/src/Morison_Types.f90 @@ -333,6 +333,7 @@ MODULE Morison_Types INTEGER(IntKi) :: UnSum = 0_IntKi !< [-] TYPE(SeaSt_WaveFieldType) , POINTER :: WaveField => NULL() !< Pointer to SeaState wave field [-] LOGICAL :: VisMeshes = .false. !< Output visualization meshes [-] + INTEGER(IntKi) :: PtfmYMod = 0_IntKi !< Large yaw model [-] END TYPE Morison_InitInputType ! ======================= ! ========= Morison_InitOutputType ======= @@ -416,11 +417,13 @@ MODULE Morison_Types INTEGER(IntKi) :: NumOuts = 0_IntKi !< [-] TYPE(SeaSt_WaveFieldType) , POINTER :: WaveField => NULL() !< SeaState wave field [-] LOGICAL :: VisMeshes = .false. !< Output visualization meshes [-] + INTEGER(IntKi) :: PtfmYMod = 0_IntKi !< Large yaw model [-] END TYPE Morison_ParameterType ! ======================= ! ========= Morison_InputType ======= TYPE, PUBLIC :: Morison_InputType TYPE(MeshType) :: Mesh !< Kinematics of each node input mesh [-] + REAL(ReKi) :: PtfmRefY = 0.0_ReKi !< Reference platform yaw offset [(rad)] END TYPE Morison_InputType ! ======================= ! ========= Morison_OutputType ======= @@ -2568,6 +2571,7 @@ subroutine Morison_CopyInitInput(SrcInitInputData, DstInitInputData, CtrlCode, E DstInitInputData%UnSum = SrcInitInputData%UnSum DstInitInputData%WaveField => SrcInitInputData%WaveField DstInitInputData%VisMeshes = SrcInitInputData%VisMeshes + DstInitInputData%PtfmYMod = SrcInitInputData%PtfmYMod end subroutine subroutine Morison_DestroyInitInput(InitInputData, ErrStat, ErrMsg) @@ -2835,6 +2839,7 @@ subroutine Morison_PackInitInput(RF, Indata) end if end if call RegPack(RF, InData%VisMeshes) + call RegPack(RF, InData%PtfmYMod) if (RegCheckErr(RF, RoutineName)) return end subroutine @@ -3045,6 +3050,7 @@ subroutine Morison_UnPackInitInput(RF, OutData) OutData%WaveField => null() end if call RegUnpack(RF, OutData%VisMeshes); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%PtfmYMod); if (RegCheckErr(RF, RoutineName)) return end subroutine subroutine Morison_CopyInitOutput(SrcInitOutputData, DstInitOutputData, CtrlCode, ErrStat, ErrMsg) @@ -3951,6 +3957,7 @@ subroutine Morison_CopyParam(SrcParamData, DstParamData, CtrlCode, ErrStat, ErrM DstParamData%NumOuts = SrcParamData%NumOuts DstParamData%WaveField => SrcParamData%WaveField DstParamData%VisMeshes = SrcParamData%VisMeshes + DstParamData%PtfmYMod = SrcParamData%PtfmYMod end subroutine subroutine Morison_DestroyParam(ParamData, ErrStat, ErrMsg) @@ -4105,6 +4112,7 @@ subroutine Morison_PackParam(RF, Indata) end if end if call RegPack(RF, InData%VisMeshes) + call RegPack(RF, InData%PtfmYMod) if (RegCheckErr(RF, RoutineName)) return end subroutine @@ -4210,6 +4218,7 @@ subroutine Morison_UnPackParam(RF, OutData) OutData%WaveField => null() end if call RegUnpack(RF, OutData%VisMeshes); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%PtfmYMod); if (RegCheckErr(RF, RoutineName)) return end subroutine subroutine Morison_CopyInput(SrcInputData, DstInputData, CtrlCode, ErrStat, ErrMsg) @@ -4226,6 +4235,7 @@ subroutine Morison_CopyInput(SrcInputData, DstInputData, CtrlCode, ErrStat, ErrM call MeshCopy(SrcInputData%Mesh, DstInputData%Mesh, CtrlCode, ErrStat2, ErrMsg2 ) call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) if (ErrStat >= AbortErrLev) return + DstInputData%PtfmRefY = SrcInputData%PtfmRefY end subroutine subroutine Morison_DestroyInput(InputData, ErrStat, ErrMsg) @@ -4247,6 +4257,7 @@ subroutine Morison_PackInput(RF, Indata) character(*), parameter :: RoutineName = 'Morison_PackInput' if (RF%ErrStat >= AbortErrLev) return call MeshPack(RF, InData%Mesh) + call RegPack(RF, InData%PtfmRefY) if (RegCheckErr(RF, RoutineName)) return end subroutine @@ -4256,6 +4267,7 @@ subroutine Morison_UnPackInput(RF, OutData) character(*), parameter :: RoutineName = 'Morison_UnPackInput' if (RF%ErrStat /= ErrID_None) return call MeshUnpack(RF, OutData%Mesh) ! Mesh + call RegUnpack(RF, OutData%PtfmRefY); if (RegCheckErr(RF, RoutineName)) return end subroutine subroutine Morison_CopyOutput(SrcOutputData, DstOutputData, CtrlCode, ErrStat, ErrMsg) @@ -4429,6 +4441,7 @@ SUBROUTINE Morison_Input_ExtrapInterp1(u1, u2, tin, u_out, tin_out, ErrStat, Err CALL MeshExtrapInterp1(u1%Mesh, u2%Mesh, tin, u_out%Mesh, tin_out, ErrStat2, ErrMsg2) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + u_out%PtfmRefY = a1*u1%PtfmRefY + a2*u2%PtfmRefY END SUBROUTINE SUBROUTINE Morison_Input_ExtrapInterp2(u1, u2, u3, tin, u_out, tin_out, ErrStat, ErrMsg ) @@ -4486,6 +4499,7 @@ SUBROUTINE Morison_Input_ExtrapInterp2(u1, u2, u3, tin, u_out, tin_out, ErrStat, a3 = (t_out - t(1))*(t_out - t(2))/((t(3) - t(1))*(t(3) - t(2))) CALL MeshExtrapInterp2(u1%Mesh, u2%Mesh, u3%Mesh, tin, u_out%Mesh, tin_out, ErrStat2, ErrMsg2) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + u_out%PtfmRefY = a1*u1%PtfmRefY + a2*u2%PtfmRefY + a3*u3%PtfmRefY END SUBROUTINE subroutine Morison_Output_ExtrapInterp(y, t, y_out, t_out, ErrStat, ErrMsg) diff --git a/modules/hydrodyn/src/WAMIT.f90 b/modules/hydrodyn/src/WAMIT.f90 index e5049a9a8c..48def0882d 100644 --- a/modules/hydrodyn/src/WAMIT.f90 +++ b/modules/hydrodyn/src/WAMIT.f90 @@ -29,6 +29,7 @@ MODULE WAMIT USE SS_Radiation USE SS_Excitation USE NWTC_FFTPACK + USE YawOffset IMPLICIT NONE @@ -52,7 +53,11 @@ MODULE WAMIT PUBLIC :: WAMIT_CalcContStateDeriv ! Tight coupling routine for computing derivatives of continuous states PUBLIC :: WAMIT_UpdateDiscState ! Tight coupling routine for updating discrete states - + INTERFACE GetAngleInRange + MODULE PROCEDURE GetAngleInRangeR4 + MODULE PROCEDURE GetAngleInRangeR8 + END INTERFACE GetAngleInRange + CONTAINS !---------------------------------------------------------------------------------------------------------------------------------- @@ -133,8 +138,8 @@ SUBROUTINE WAMIT_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, ErrS ! Local Variables REAL(DbKi) :: Interval_Sub ! Local timestep for the SS_Rad and SS_Exc modules, based on RdtnDT COMPLEX(SiKi), ALLOCATABLE :: HdroExctn (:,:,:) ! Frequency- and direction-dependent complex hydrodynamic wave excitation force per unit wave amplitude vector (kg/s^2, kg-m/s^2) - COMPLEX(SiKi), ALLOCATABLE :: WaveExctnC(:,:) ! Discrete Fourier transform of the instantaneous value of the total excitation force on the support platfrom from incident waves (N, N-m) - COMPLEX(SiKi), ALLOCATABLE :: WaveExctnCGrid(:,:,:) ! Discrete Fourier transform of the instantaneous value of the total excitation force on the grid points from incident waves (N, N-m) + COMPLEX(SiKi), ALLOCATABLE :: WaveExctnC(:,:,:) ! Discrete Fourier transform of the instantaneous value of the total excitation force on the support platfrom from incident waves (N, N-m) + COMPLEX(SiKi), ALLOCATABLE :: WaveExctnCGrid(:,:,:,:) ! Discrete Fourier transform of the instantaneous value of the total excitation force on the grid points from incident waves (N, N-m) REAL(ReKi) :: DffrctDim (6) ! Matrix used to redimensionalize WAMIT hydrodynamic wave excitation force output (kg/s^2, kg-m/s^2 ) REAL(SiKi), ALLOCATABLE :: HdroAddMs (:,:,:) ! The frequency-dependent hydrodynamic added mass matrix from the radiation problem (kg , kg-m , kg-m^2 ) REAL(SiKi), ALLOCATABLE :: HdroDmpng (:,:,:) ! The frequency-dependent hydrodynamic damping matrix from the radiation problem (kg/s, kg-m/s, kg-m^2/s) @@ -142,6 +147,7 @@ SUBROUTINE WAMIT_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, ErrS REAL(SiKi), ALLOCATABLE :: HdroWvDir (:) ! Incident wave propagation heading direction components inherent in the complex wave excitation force per unit wave amplitude vector (degrees) REAL(ReKi) :: HighFreq ! The highest frequency component in the WAMIT file, not counting infinity. REAL(SiKi) :: Omega ! Wave frequency (rad/s) + REAL(ReKi) :: PRPHdg ! Platform reference heading (rad) REAL(ReKi) :: PrvDir ! The value of TmpDir from the previous line (degrees) REAL(ReKi) :: PrvPer ! The value of TmpPer from the previous line (sec ) REAL(ReKi) :: SttcDim (6,6) ! Matrix used to redimensionalize WAMIT hydrostatic restoring output (kg/s^2, kg-m/s^2, kg-m^2/s^2) @@ -158,7 +164,7 @@ SUBROUTINE WAMIT_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, ErrS REAL(ReKi), ALLOCATABLE :: WAMITPer (:) ! Period components as ordered in the WAMIT output files (sec ) REAL(ReKi), ALLOCATABLE :: WAMITWvDir(:) ! Wave direction components as ordered in the WAMIT output files (degrees) - INTEGER :: I,iGrid,iX,iY ! Generic index + INTEGER :: I,iGrid,iX,iY,iHdg ! Generic index INTEGER :: InsertInd ! The lowest sorted index whose associated frequency component is higher than the current frequency component -- this is to sort the frequency components from lowest to highest INTEGER :: J ! Generic index INTEGER :: K ! Generic index @@ -189,7 +195,15 @@ SUBROUTINE WAMIT_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, ErrS real(ReKi) :: WaveNmbr ! Frequency-dependent wave number COMPLEX(SiKi) :: Fxy ! Phase correction term for Wave excitation forces real(ReKi) :: tmpAngle ! Frequency and heading and platform offset dependent phase shift angle for Euler's Equation e^(-j*tmpAngle) + real(SiKi) :: tmpVec3(3) REAL(ReKi) :: RotateZdegOffset ! PtfmRefZtRot converted to degrees + REAL(SiKi) :: MinAllowedWvDir ! Minimum allowed wave heading in the global frame + REAL(SiKi) :: MaxAllowedWvDir ! Maximum allowed wave heading in the global frame + REAL(SiKi) :: unusedReal + REAL(SiKi) :: tmpDir2 + REAL(SiKi) :: AvgInpWvDirSpcg ! Average spacing of input wave directions used to check for potential gaps (deg) + LOGICAL :: dirInRange + REAL(SiKi), PARAMETER :: WvDirTol = 0.001 ! Tolerance for wave heading in degrees ! Error handling CHARACTER(ErrMsgLen) :: ErrMsg2 ! Temporary error message for calls INTEGER(IntKi) :: ErrStat2 ! Temporary error status for calls @@ -218,10 +232,32 @@ SUBROUTINE WAMIT_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, ErrS p%ExctnMod = InitInp%ExctnMod p%ExctnDisp = InitInp%ExctnDisp p%ExctnCutOff = InitInp%ExctnCutOff + p%NExctnHdg = InitInp%NExctnHdg p%NBodyMod = InitInp%NBodyMod p%NBody = InitInp%NBody ! In the context of this WAMIT object NBody is 1 if NBodyMod > 1 [there are NBody different WAMIT objects in this case] p%WaveField => InitInp%WaveField - + p%PtfmYMod = InitInp%PtfmYMod + + ! Set up wave excitation grid - Can no longer use the WaveField parameters due to different headings + ! Copy WaveField grid parameters + call SeaSt_WaveField_CopyParam(p%WaveField%GridParams, p%ExctnGridParams, 0, ErrStat2, ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + if ( p%ExctnDisp == 0 ) then + p%ExctnGridParams%n(2:3) = 1_IntKi + p%ExctnGridParams%delta(2:3) = 0.0_SiKi + p%ExctnGridParams%pZero(2:3) = 0.0_SiKi + end if + ! Set the fourth index based on PRP heading + if ( InitInp%PtfmYMod .EQ. 0) then ! Constant reference yaw offset + p%NExctnHdg = 0_IntKi + p%ExctnGridParams%delta(4) = 0.0 + p%ExctnGridParams%pZero(4) = InitInp%PtfmRefY + else if ( InitInp%PtfmYMod .EQ. 1 ) then ! Drifting reference yaw offset + p%ExctnGridParams%delta(4) = TwoPi/Real(MAX(p%NExctnHdg,1_IntKi),ReKi) + p%ExctnGridParams%pZero(4) = -Pi + end if + p%ExctnGridParams%n(4) = p%NExctnHdg+1 + p%ExctnGridParams%Z_depth = -1.0 ! Set to Z_depth to a negative value to indicate uniform "z" grid for platform heading + ! This module's implementation requires that if NBodyMod = 2 or 3, then there is one instance of a WAMIT module for each body, therefore, HydroDyn may have NBody > 1, but this WAMIT module will have NBody = 1 if ( (p%NBodyMod > 1) .and. (p%NBody > 1) ) then CALL SetErrStat( ErrID_Fatal, "DEVELOPER ERROR: If NBodyMod = 2 or 3, then NBody for the a WAMIT object must be equal to 1", ErrStat, ErrMsg, RoutineName) @@ -894,26 +930,25 @@ SUBROUTINE WAMIT_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, ErrS if ( ( p%ExctnMod == 0 ) ) then - ! no need to allocate the p%WaveExctn array because it won't be used + ! no need to allocate the p%WaveExctnGrid array because it won't be used else ! Initialize the variables associated with the incident wave: SELECT CASE ( p%WaveField%WaveMod ) ! Which incident wave kinematics model are we using? - CASE ( WaveMod_None ) ! No waves, NOTE: for this case we are forcing ExctnDisp = 0, so only p%WaveExctn needs to be allocated, not p%WaveExctnGrid + CASE ( WaveMod_None ) ! No waves, NOTE: for this case we are forcing ExctnDisp = 0 if ( p%ExctnMod == 1 ) then ! Initialize everything to zero: - - ALLOCATE ( p%WaveExctn (0:p%WaveField%NStepWave,6*p%NBody) , STAT=ErrStat2 ) + ALLOCATE ( p%WaveExctnGrid (0:p%WaveField%NStepWave, p%ExctnGridParams%n(2),p%ExctnGridParams%n(3),p%ExctnGridParams%n(4),6*p%NBody) , STAT=ErrStat2 ) IF ( ErrStat2 /= 0 ) THEN - CALL SetErrStat( ErrID_Fatal, 'Error allocating memory for the WaveExctn array.', ErrStat, ErrMsg, RoutineName) + CALL SetErrStat( ErrID_Fatal, 'Error allocating memory for the WaveExctnGrid array.', ErrStat, ErrMsg, RoutineName) CALL Cleanup() RETURN END IF - p%WaveExctn = 0.0 + p%WaveExctnGrid = 0.0 else if ( p%ExctnMod == 2 ) then Interval_Sub = InitInp%Conv_Rdtn%RdtnDT @@ -954,25 +989,54 @@ SUBROUTINE WAMIT_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, ErrS ! NOTE: we may end up inadvertantly aborting if the wave direction crosses ! the -Pi / Pi boundary (-180/180 degrees). + ! First do some check on the input wave heading angles + IF ( (HdroWvDir(NInpWvDir)-HdroWvDir(1)) > (360.0+WvDirTol) ) THEN + CALL SetErrStat( ErrID_Fatal,' The difference between any pair of wave directions in '//TRIM(InitInp%WAMITFile)//'.3 should be less than or equal to 360 deg.',ErrStat,ErrMsg,RoutineName) + END IF + ! The input wave headings should cover a contiguous region of directions. Check for gaps and warn user. + IF (NInpWvDir>1) THEN + AvgInpWvDirSpcg = (HdroWvDir(NInpWvDir)-HdroWvDir(1))/REAL(NInpWvDir-1,SiKi) + DO I = 2,NInpWvDir + IF ( (HdroWvDir(I)-HdroWvDir(I-1)) > (3.0*AvgInpWvDirSpcg) ) THEN + CALL SetErrStat( ErrID_Warn,'The wave headings in '//TRIM(InitInp%WAMITFile)//'.3 is likely not contiguous with a gap between '//TRIM(Num2LStr(HdroWvDir(I-1)))//' and '//TRIM(Num2LStr(HdroWvDir(I)))//' degs.', & + ErrStat, ErrMsg, RoutineName) + END IF + END DO + END IF + ! Need to account for PtfmRefZtRot if NBodyMod=2 (NBody=1 in the case) IF ( p%NBodyMod == 2 ) THEN RotateZdegOffset = InitInp%PtfmRefztRot(1)*R2D ELSE RotateZdegOffset = 0.0 END IF - IF ( ( (p%WaveField%WaveDirMin-RotateZdegOffset) HdroWvDir(NInpWvDir) ) ) THEN - ErrMsg2 = 'All Wave directions must be within the wave heading angle range available in "' & - //TRIM(InitInp%WAMITFile)//'.3" (inclusive).' - CALL SetErrStat( ErrID_Fatal, ErrMsg2, ErrStat, ErrMsg, RoutineName) - CALL Cleanup() - RETURN + IF ( InitInp%PtfmYMod == 0 ) THEN + ! Range of allowed wave headings in the global frame + MinAllowedWvDir = HdroWvDir(1) +RotateZdegOffset+InitInp%PtfmRefY*R2D + MaxAllowedWvDir = HdroWvDir(NInpWvDir)+RotateZdegOffset+InitInp%PtfmRefY*R2D + ! For robustness, check every single incident wave direction + DO I = 0,InitInp%WaveField%NStepWave2 + IF (.NOT. GetAngleInRange(InitInp%WaveField%WaveDirArr(I),MinAllowedWvDir,MaxAllowedWvDir,unusedReal)) THEN + CALL SetErrStat( ErrID_Fatal,TRIM(InitInp%WAMITFile)//'.3 does not cover the wave heading of '//TRIM(Num2LStr(InitInp%WaveField%WaveDirArr(I)))//' deg (in the global frame).', & + ErrStat, ErrMsg, RoutineName) + CALL Cleanup() + RETURN + END IF + END DO + ELSE IF ( InitInp%PtfmYMod == 1 ) THEN + IF ( (.not. EqualRealNos( HdroWvDir(1),REAL(-180,SiKi))) .OR. (.not. EqualRealNos( HdroWvDir(NInpWvDir),REAL(180,SiKi))) ) THEN + ErrMsg2 = 'With PtfmYMod=1 in ElastoDyn or HydroDyn driver, we need the lowest and highest wave headings to be exactly -180 deg and 180 deg, respectively, in "' & + //TRIM(InitInp%WAMITFile)//'.3" (inclusive).' + CALL SetErrStat( ErrID_Fatal, ErrMsg2, ErrStat, ErrMsg, RoutineName) + CALL Cleanup() + RETURN + END IF END IF - ! Calculate the WaveExctn data from WAMIT data if ExctnMod = 1 ! ALLOCATE the arrays: - ALLOCATE ( WaveExctnC(0:p%WaveField%NStepWave2 ,6*p%NBody) , STAT=ErrStat2 ) + ALLOCATE ( WaveExctnC(0:p%WaveField%NStepWave2,p%NExctnHdg+1,6*p%NBody) , STAT=ErrStat2 ) IF ( ErrStat2 /= 0 ) THEN CALL SetErrStat( ErrID_Fatal, 'Error allocating memory for the WaveExctnC array.', ErrStat, ErrMsg, RoutineName) CALL Cleanup() @@ -980,27 +1044,20 @@ SUBROUTINE WAMIT_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, ErrS END IF if (p%ExctnDisp > 0 ) then - ALLOCATE ( WaveExctnCGrid(0:p%WaveField%NStepWave2 ,p%WaveField%GridParams%n(2)*p%WaveField%GridParams%n(3),6*p%NBody) , STAT=ErrStat2 ) + ALLOCATE ( WaveExctnCGrid (0:p%WaveField%NStepWave2,p%ExctnGridParams%n(2)*p%ExctnGridParams%n(3),p%ExctnGridParams%n(4),6*p%NBody) , STAT=ErrStat2 ) IF ( ErrStat2 /= 0 ) THEN - CALL SetErrStat( ErrID_Fatal, 'Error allocating memory for the WaveExctnC array.', ErrStat, ErrMsg, RoutineName) - CALL Cleanup() - RETURN - END IF - ALLOCATE ( p%WaveExctnGrid (0:p%WaveField%NStepWave,p%WaveField%GridParams%n(2),p%WaveField%GridParams%n(3), 6*p%NBody) , STAT=ErrStat2 ) - IF ( ErrStat2 /= 0 ) THEN - CALL SetErrStat( ErrID_Fatal, 'Error allocating memory for the WaveExctn array.', ErrStat, ErrMsg, RoutineName) - CALL Cleanup() - RETURN - END IF - else - ALLOCATE ( p%WaveExctn (0:p%WaveField%NStepWave,6*p%NBody) , STAT=ErrStat2 ) - IF ( ErrStat2 /= 0 ) THEN - CALL SetErrStat( ErrID_Fatal, 'Error allocating memory for the WaveExctn array.', ErrStat, ErrMsg, RoutineName) + CALL SetErrStat( ErrID_Fatal, 'Error allocating memory for the WaveExctnCGrid array.', ErrStat, ErrMsg, RoutineName) CALL Cleanup() RETURN END IF end if - + + ALLOCATE ( p%WaveExctnGrid (0:p%WaveField%NStepWave, p%ExctnGridParams%n(2),p%ExctnGridParams%n(3),p%ExctnGridParams%n(4),6*p%NBody) , STAT=ErrStat2 ) + IF ( ErrStat2 /= 0 ) THEN + CALL SetErrStat( ErrID_Fatal, 'Error allocating memory for the WaveExctnGrid array.', ErrStat, ErrMsg, RoutineName) + CALL Cleanup() + RETURN + END IF !==================================== ! Transform the wave excitation coefs @@ -1057,56 +1114,64 @@ SUBROUTINE WAMIT_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, ErrS end do end if + if (p%ExctnDisp == 0 ) then ! Compute the positive-frequency components (including zero) of the discrete ! Fourier transform of the wave excitation force: DO I = 0,p%WaveField%NStepWave2 ! Loop through the positive frequency components (including zero) of the discrete Fourier transform - ! Compute the frequency of this component: - + ! Compute the frequency of this component: Omega = I*p%WaveField%WaveDOmega - + + DO iHdg = 1,p%NExctnHdg+1 + ! Compute the PRP heading angle + IF (p%PtfmYMod .EQ. 0) THEN + PRPHdg = InitInp%PtfmRefY + ELSE IF (p%PtfmYMod .EQ. 1) THEN + PRPHdg = -PI + (iHdg-1) * TwoPi/REAL(p%NExctnHdg,ReKi) + END IF ! Compute the discrete Fourier transform of the instantaneous value of the ! total excitation force on the support platfrom from incident waves: - - DO J = 1,6*p%NBody ! Loop through all wave excitation forces and moments - TmpCoord(1) = Omega - TmpCoord(2) = p%WaveField%WaveDirArr(I) - CALL WAMIT_Interp2D_Cplx( TmpCoord, HdroExctn(:,:,J), HdroFreq, HdroWvDir, LastInd2, WaveExctnC(I,J), ErrStat2, ErrMsg2 ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) - IF ( ErrStat >= AbortErrLev ) THEN - CALL Cleanup() - RETURN - END IF - WaveExctnC(I,J) = WaveExctnC(I,J) * CMPLX(p%WaveField%WaveElevC0(1,I), p%WaveField%WaveElevC0(2,I)) - - END DO ! J - All wave excitation forces and moments - - + DO J = 1,6*p%NBody ! Loop through all wave excitation forces and moments + TmpCoord(1) = Omega + TmpCoord(2) = p%WaveField%WaveDirArr(I) - PRPHdg*R2D + dirInRange = GetAngleInRange(TmpCoord(2),HdroWvDir(1),HdroWvDir(NInpWvDir),tmpDir2); TmpCoord(2) = tmpDir2 + IF (.NOT. dirInRange) THEN ! Somewhat redundant check. Can be removed in the future. + CALL SetErrStat(ErrID_Fatal,' Wave heading out of range.', ErrStat, ErrMsg, RoutineName) + END IF + CALL WAMIT_Interp2D_Cplx( TmpCoord, HdroExctn(:,:,J), HdroFreq, HdroWvDir, LastInd2, WaveExctnC(I,iHdg,J), ErrStat2, ErrMsg2 ) + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF ( ErrStat >= AbortErrLev ) THEN + CALL Cleanup() + RETURN + END IF + WaveExctnC(I,iHdg,J) = WaveExctnC(I,iHdg,J) * CMPLX(p%WaveField%WaveElevC0(1,I), p%WaveField%WaveElevC0(2,I)) + END DO ! J - All wave excitation forces and moments + END DO ! iHdg - All PRP heading END DO ! I - The positive frequency components (including zero) of the discrete Fourier transform - ! Compute the inverse discrete Fourier transform to find the time-domain + ! Compute the inverse discrete Fourier transform to find the time-domain ! representation of the wave excitation force: - CALL InitFFT ( p%WaveField%NStepWave, FFT_Data, .TRUE., ErrStat2 ) CALL SetErrStat( ErrStat2, 'Error in call to InitFFT.', ErrStat, ErrMsg, RoutineName) IF ( ErrStat >= AbortErrLev) THEN CALL Cleanup() RETURN END IF - - DO J = 1,6*p%NBody ! Loop through all wave excitation forces and moments - CALL ApplyFFT_cx ( p%WaveExctn(0:p%WaveField%NStepWave-1,J), WaveExctnC(:,J), FFT_Data, ErrStat2 ) - CALL SetErrStat( ErrStat2, ' An error occured while applying an FFT to WaveExctnC.', ErrStat, ErrMsg, RoutineName) - IF ( ErrStat >= AbortErrLev) THEN - CALL Cleanup() - RETURN - END IF + DO iHdg = 1,p%NExctnHdg+1 + DO J = 1,6*p%NBody ! Loop through all wave excitation forces and moments + CALL ApplyFFT_cx ( p%WaveExctnGrid(0:p%WaveField%NStepWave-1,1_IntKi,1_IntKi,iHdg,J), WaveExctnC(:,iHdg,J), FFT_Data, ErrStat2 ) + CALL SetErrStat( ErrStat2, ' An error occured while applying an FFT to WaveExctnC.', ErrStat, ErrMsg, RoutineName) + IF ( ErrStat >= AbortErrLev) THEN + CALL Cleanup() + RETURN + END IF ! Append first datpoint as the last as aid for repeated wave data - p%WaveExctn(p%WaveField%NStepWave,J) = p%WaveExctn(0,J) - END DO ! J - All wave excitation forces and moments + p%WaveExctnGrid(p%WaveField%NStepWave,1_IntKi,1_IntKi,iHdg,J) = p%WaveExctnGrid(0,1_IntKi,1_IntKi,iHdg,J) + END DO ! J - All wave excitation forces and moments + END DO ! iHdg - All PRP headings CALL ExitFFT(FFT_Data, ErrStat2) CALL SetErrStat( ErrStat2, 'Error in call to ExitFFT.', ErrStat, ErrMsg, RoutineName) @@ -1115,31 +1180,56 @@ SUBROUTINE WAMIT_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, ErrS RETURN END IF - else + DO iHdg = 1,p%NExctnHdg+1 + ! Compute the PRP heading angle + IF (p%PtfmYMod .EQ. 0) THEN + PRPHdg = InitInp%PtfmRefY + ELSE IF (p%PtfmYMod .EQ. 1) THEN + PRPHdg = -PI + (iHdg-1) * TwoPi/REAL(p%NExctnHdg,ReKi) + END IF + DO J = 0,p%WaveField%NStepWave + call hiFrameTransform(h2i,PRPHdg,p%WaveExctnGrid(J,1_IntKi,1_IntKi,iHdg,1:3),tmpVec3,ErrStat2,ErrMsg2); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + p%WaveExctnGrid(J,1_IntKi,1_IntKi,iHdg,1:3) = tmpVec3 + call hiFrameTransform(h2i,PRPHdg,p%WaveExctnGrid(J,1_IntKi,1_IntKi,iHdg,4:6),tmpVec3,ErrStat2,ErrMsg2); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + p%WaveExctnGrid(J,1_IntKi,1_IntKi,iHdg,4:6) = tmpVec3 + END DO + END DO + else ! p%ExctnDisp > 0 DO I = 0,p%WaveField%NStepWave2 ! Loop through the positive frequency components (including zero) of the discrete Fourier transform - ! Compute the frequency of this component: - + ! Compute the frequency of this component: Omega = I*p%WaveField%WaveDOmega - - ! Compute the discrete Fourier transform of the instantaneous value of the - ! total excitation force on the support platfrom from incident waves: - DO J = 1,6*p%NBody ! Loop through all wave excitation forces and moments - TmpCoord(1) = Omega - TmpCoord(2) = p%WaveField%WaveDirArr(I) - CALL WAMIT_Interp2D_Cplx( TmpCoord, HdroExctn(:,:,J), HdroFreq, HdroWvDir, LastInd2, WaveExctnC(I,J), ErrStat2, ErrMsg2 ) - CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) - IF ( ErrStat >= AbortErrLev ) THEN - CALL Cleanup() - RETURN + DO iHdg = 1,p%ExctnGridParams%n(4) + ! Compute the current PRP heading + IF (p%PtfmYMod .EQ. 0) THEN + PRPHdg = InitInp%PtfmRefY + ELSE IF (p%PtfmYMod .EQ. 1) THEN + PRPHdg = -PI + (iHdg-1) * TwoPi/REAL(p%NExctnHdg,ReKi) END IF - do iGrid = 1, p%WaveField%GridParams%n(2)*p%WaveField%GridParams%n(3) - WaveExctnCGrid(I,iGrid,J) = WaveExctnC(I,J) * CMPLX(p%WaveField%WaveElevC(1,I,iGrid), p%WaveField%WaveElevC(2,I,iGrid)) - end do - END DO ! J - All wave excitation forces and moments + + ! Compute the discrete Fourier transform of the instantaneous value of the + ! total excitation force on the support platfrom from incident waves: + DO J = 1,6*p%NBody ! Loop through all wave excitation forces and moments + TmpCoord(1) = Omega + TmpCoord(2) = p%WaveField%WaveDirArr(I) - PRPHdg*R2D + dirInRange = GetAngleInRange(TmpCoord(2),HdroWvDir(1),HdroWvDir(NInpWvDir),tmpDir2); TmpCoord(2) = tmpDir2 + IF (.NOT. dirInRange) THEN ! Somewhat redundant check. Can be removed in the future. + CALL SetErrStat(ErrID_Fatal,' Wave heading out of range.', ErrStat, ErrMsg, RoutineName) + END IF + CALL WAMIT_Interp2D_Cplx( TmpCoord, HdroExctn(:,:,J), HdroFreq, HdroWvDir, LastInd2, WaveExctnC(I,iHdg,J), ErrStat2, ErrMsg2 ) + CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF ( ErrStat >= AbortErrLev ) THEN + CALL Cleanup() + RETURN + END IF + do iGrid = 1, p%ExctnGridParams%n(2)*p%ExctnGridParams%n(3) + WaveExctnCGrid(I,iGrid,iHdg,J) = WaveExctnC(I,iHdg,J) * CMPLX(p%WaveField%WaveElevC(1,I,iGrid), p%WaveField%WaveElevC(2,I,iGrid)) + end do + END DO ! J - All wave excitation forces and moments + END DO ! iHdg - All PRP headings END DO ! I - The positive frequency components (including zero) of the discrete Fourier transform - + ! Compute the inverse discrete Fourier transform to find the time-domain ! representation of the wave excitation force: @@ -1150,21 +1240,22 @@ SUBROUTINE WAMIT_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, ErrS RETURN END IF - DO J = 1,6*p%NBody ! Loop through all wave excitation forces and moments - do iGrid = 1, p%WaveField%GridParams%n(2)*p%WaveField%GridParams%n(3) - iX = mod(iGrid-1, p%WaveField%GridParams%n(2)) + 1 ! 1st n index is time - iY = (iGrid-1) / p%WaveField%GridParams%n(2) + 1 - CALL ApplyFFT_cx ( p%WaveExctnGrid(0:p%WaveField%NStepWave-1,iX,iY,J), WaveExctnCGrid(:,iGrid,J), FFT_Data, ErrStat2 ) + DO iHdg = 1,p%ExctnGridParams%n(4) + DO iGrid = 1, p%ExctnGridParams%n(2)*p%ExctnGridParams%n(3) + iX = mod(iGrid-1, p%ExctnGridParams%n(2)) + 1 ! 1st n index is time + iY = (iGrid-1) / p%ExctnGridParams%n(2) + 1 + DO J = 1,6*p%NBody ! Loop through all wave excitation forces and moments + CALL ApplyFFT_cx ( p%WaveExctnGrid(0:p%WaveField%NStepWave-1,iX,iY,iHdg,J), WaveExctnCGrid(:,iGrid,iHdg,J), FFT_Data, ErrStat2 ) CALL SetErrStat( ErrStat2, ' An error occured while applying an FFT to WaveExctnC.', ErrStat, ErrMsg, RoutineName) IF ( ErrStat >= AbortErrLev) THEN CALL Cleanup() RETURN END IF ! Append first datpoint as the last as aid for repeated wave data - p%WaveExctnGrid(p%WaveField%NStepWave,iX,iY,J) = p%WaveExctnGrid(0,iX,iY,J) - end do - - END DO ! J - All wave excitation forces and moments + p%WaveExctnGrid(p%WaveField%NStepWave,iX,iY,iHdg,J) = p%WaveExctnGrid(0,iX,iY,iHdg,J) + END DO ! J - All wave excitation forces and moments + END DO ! iGrid - All wave grid points + END DO ! iHdg - All wave heading CALL ExitFFT(FFT_Data, ErrStat2) CALL SetErrStat( ErrStat2, 'Error in call to ExitFFT.', ErrStat, ErrMsg, RoutineName) @@ -1173,6 +1264,25 @@ SUBROUTINE WAMIT_Init( InitInp, u, p, x, xd, z, OtherState, y, m, Interval, ErrS RETURN END IF + DO iHdg = 1,p%ExctnGridParams%n(4) + ! Compute the PRP heading angle + IF (p%PtfmYMod .EQ. 0) THEN + PRPHdg = InitInp%PtfmRefY + ELSE IF (p%PtfmYMod .EQ. 1) THEN + PRPHdg = -PI + (iHdg-1) * TwoPi/REAL(p%NExctnHdg,ReKi) + END IF + DO iGrid = 1, p%ExctnGridParams%n(2)*p%ExctnGridParams%n(3) + iX = mod(iGrid-1, p%ExctnGridParams%n(2)) + 1 ! 1st n index is time + iY = (iGrid-1) / p%ExctnGridParams%n(2) + 1 + DO J = 0,p%WaveField%NStepWave + call hiFrameTransform(h2i,PRPHdg,p%WaveExctnGrid(J,iX,iY,iHdg,1:3),tmpVec3,ErrStat2,ErrMsg2); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + p%WaveExctnGrid(J,iX,iY,iHdg,1:3) = tmpVec3 + call hiFrameTransform(h2i,PRPHdg,p%WaveExctnGrid(J,iX,iY,iHdg,4:6),tmpVec3,ErrStat2,ErrMsg2); call SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName) + p%WaveExctnGrid(J,iX,iY,iHdg,4:6) = tmpVec3 + END DO + END DO + END DO + end if @@ -1581,7 +1691,8 @@ SUBROUTINE WAMIT_UpdateStates( t, n, Inputs, InputTimes, p, x, xd, z, OtherState ! INTEGER(IntKi) :: ErrStat2 ! Error status of the operation (secondary error) ! CHARACTER(ErrMsgLen) :: ErrMsg2 ! Error message if ErrStat2 /= ErrID_None REAL(ReKi) :: bodyPosition(2) - + REAL(ReKi) :: tmpVec6(6) + ! Create dummy variables required by framework but which are not used by the module TYPE(Conv_Rdtn_InputType), ALLOCATABLE :: Conv_Rdtn_u(:) ! Inputs @@ -1617,8 +1728,10 @@ SUBROUTINE WAMIT_UpdateStates( t, n, Inputs, InputTimes, p, x, xd, z, OtherState END IF do iBody=1,p%NBody indxStart = (iBody-1)*6+1 - indxEnd = indxStart+5 - Conv_Rdtn_u(I)%Velocity(indxStart:indxEnd) = (/Inputs(I)%Mesh%TranslationVel(:,iBody), Inputs(I)%Mesh%RotationVel(:,iBody)/) + indxEnd = indxStart+5 + call hiFrameTransform( i2h, Inputs(I)%PtfmRefY, Inputs(I)%Mesh%TranslationVel(:,iBody), tmpVec6(1:3), ErrStat, ErrMsg) + call hiFrameTransform( i2h, Inputs(I)%PtfmRefY, Inputs(I)%Mesh%RotationVel(:,iBody), tmpVec6(4:6), ErrStat, ErrMsg) + Conv_Rdtn_u(I)%Velocity(indxStart:indxEnd) = tmpVec6 end do END DO @@ -1644,10 +1757,10 @@ SUBROUTINE WAMIT_UpdateStates( t, n, Inputs, InputTimes, p, x, xd, z, OtherState RETURN END IF do iBody=1,p%NBody - indxStart = (iBody-1)*6+1 - SS_Rdtn_u(I)%dq(indxStart:indxStart+2) = Inputs(I)%Mesh%TranslationVel(:,iBody) - SS_Rdtn_u(I)%dq(indxStart+3:indxStart+5) = Inputs(I)%Mesh%RotationVel(:,iBody) - !SS_Rdtn_u(I)%dq = reshape((/Inputs(I)%Mesh%TranslationVel(:,1), Inputs(I)%Mesh%RotationVel(:,1)/), (/6,1/)) !reshape(u%Velocity, (/6,1/)) ! dq is a 6x1 matrix + indxStart = (iBody-1)*6+1 + call hiFrameTransform( i2h, Inputs(I)%PtfmRefY, Inputs(I)%Mesh%TranslationVel(:,iBody), tmpVec6(1:3), ErrStat, ErrMsg) + call hiFrameTransform( i2h, Inputs(I)%PtfmRefY, Inputs(I)%Mesh%RotationVel(:,iBody), tmpVec6(4:6), ErrStat, ErrMsg) + SS_Rdtn_u(I)%dq(indxStart:indxStart+5) = tmpVec6 end do END DO @@ -1755,12 +1868,20 @@ SUBROUTINE WAMIT_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, ErrStat, Er INTEGER(IntKi) :: I,iStart ! Generic index REAL(ReKi) :: q(6*p%NBody), qdot(6*p%NBody), qdotdot(6*p%NBody) ! kinematics for all WAMIT bodies REAL(ReKi) :: rotdisp(3) ! small angle rotational displacements - integer(IntKi) :: iBody ! Counter for WAMIT bodies. If NBodyMod > 1 then NBody = 1, and hence iBody = 1 - integer(IntKi) :: indxStart, indxEnd ! Starting and ending indices for the iBody_th sub vector in an NBody long vector - real(ReKi) :: bodyPosition(2) ! x-y displaced location of a WAMIT body (relative to - ! Error handling - CHARACTER(1024) :: ErrMsg2 ! Temporary error message for calls - INTEGER(IntKi) :: ErrStat2 ! Temporary error status for calls + INTEGER(IntKi) :: iBody ! Counter for WAMIT bodies. If NBodyMod > 1 then NBody = 1, and hence iBody = 1 + INTEGER(IntKi) :: indxStart, indxEnd ! Starting and ending indices for the iBody_th sub vector in an NBody long vector + REAL(ReKi) :: bodyPosition(3) ! x-y displaced location of a WAMIT body (relative to + REAL(ReKi) :: tmpVec3(3),tmpVec6(6) + REAL(ReKi), PARAMETER :: LrgAngle = 0.261799387799149 ! Threshold for platform roll and pitch rotation (15 deg). This is consistent with the ElastoDyn check. + + ! Error handling + CHARACTER(1024) :: ErrMsg2 ! Temporary error message for calls + INTEGER(IntKi) :: ErrStat2 ! Temporary error status for calls + CHARACTER(*), PARAMETER :: RoutineName = 'WAMIT_Init' + + LOGICAL, SAVE :: FrstWarn_LrgR = .TRUE. + LOGICAL, SAVE :: FrstWarn_LrgP = .TRUE. + LOGICAL, SAVE :: FrstWarn_LrgY = .TRUE. ! Initialize ErrStat @@ -1781,16 +1902,22 @@ SUBROUTINE WAMIT_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, ErrStat, Er if ( p%ExctnDisp == 0 ) then ! Abort if the wave excitation loads have not been computed yet: - IF ( .NOT. ALLOCATED ( p%WaveExctn ) ) THEN + IF ( .NOT. ALLOCATED ( p%WaveExctnGrid ) ) THEN ErrMsg = ' Routine WAMIT_Init() must be called before routine WAMIT_CalcOutput().' ErrStat = ErrID_Fatal RETURN END IF - - DO I = 1,6*p%NBody ! Loop through all wave excitation forces and moments - m%F_Waves1(I) = InterpWrappedStpReal ( REAL(Time, SiKi), p%WaveField%WaveTime, p%WaveExctn(:,I), & - m%LastIndWave, p%WaveField%NStepWave + 1 ) - END DO ! I - All wave excitation forces and moments + + DO iBody = 1,p%NBody + bodyPosition(1) = 0.0 + bodyPosition(2) = 0.0 + bodyPosition(3) = WrapToPi(u%PtfmRefY) + iStart = (iBody-1)*6+1 + ! WaveExctnGrid dimensions are: 1st: wavetime, 2nd: X, 3rd: Y, 4th: PRP yaw offset, 5th: Force component for each WAMIT Body + m%F_Waves1(iStart:iStart+5) = WAMIT_ForceWaves_Interp( Time, bodyPosition, p%WaveExctnGrid(:,:,:,:,iStart:iStart+5), p%ExctnGridParams, m%WaveField_m, ErrStat2, ErrMsg2 ) + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + END DO + else ! p%ExctnDisp > 0 IF ( .NOT. allocated ( p%WaveExctnGrid ) ) THEN ErrMsg = ' Routine WAMIT_Init() must be called before routine WAMIT_CalcOutput().' @@ -1809,17 +1936,19 @@ SUBROUTINE WAMIT_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, ErrStat, Er bodyPosition(1) = p%ExctnFiltConst * xd%BdyPosFilt(1,iBody,1) + (1.0_ReKi - p%ExctnFiltConst) * u%Mesh%TranslationDisp(1,iBody) bodyPosition(2) = p%ExctnFiltConst * xd%BdyPosFilt(2,iBody,1) + (1.0_ReKi - p%ExctnFiltConst) * u%Mesh%TranslationDisp(2,iBody) END IF + bodyPosition(3) = WrapToPi(u%PtfmRefY) iStart = (iBody-1)*6+1 - ! WaveExctnGrid dimensions are: 1st: wavetime, 2nd: X, 3rd: Y, 4th: Force component for each WAMIT Body - m%F_Waves1(iStart:iStart+5) = WAMIT_ForceWaves_Interp( Time, bodyPosition, p%WaveExctnGrid(:,:,:,iStart:iStart+5), p%WaveField%GridParams, m%WaveField_m, ErrStat2, ErrMsg2 ) - call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'SeaState_CalcOutput' ) + ! WaveExctnGrid dimensions are: 1st: wavetime, 2nd: X, 3rd: Y, 4th: PRP yaw offset, 5th: Force component for each WAMIT Body + m%F_Waves1(iStart:iStart+5) = WAMIT_ForceWaves_Interp( Time, bodyPosition, p%WaveExctnGrid(:,:,:,:,iStart:iStart+5), p%ExctnGridParams, m%WaveField_m, ErrStat2, ErrMsg2 ) + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) END DO end if - + else if ( p%ExctnMod == 2 ) then call SS_Exc_CalcOutput( Time, m%SS_Exctn_u, p%SS_Exctn, x%SS_Exctn, xd%SS_Exctn, & - z%SS_Exctn, OtherState%SS_Exctn, m%SS_Exctn_y, m%SS_Exctn, ErrStat, ErrMsg ) + z%SS_Exctn, OtherState%SS_Exctn, m%SS_Exctn_y, m%SS_Exctn, ErrStat2, ErrMsg2 ) + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) m%F_Waves1 (:) = m%SS_Exctn_y%y end if @@ -1827,24 +1956,69 @@ SUBROUTINE WAMIT_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, ErrStat, Er do iBody = 1, p%NBody - ! Determine the rotational angles from the direction-cosine matrix - rotdisp = GetSmllRotAngs ( u%Mesh%Orientation(:,:,iBody), ErrStat, ErrMsg ) + ! Determine the rotational angles from the direction-cosine matrix + ! rotdisp = GetRotAngs ( u%PtfmRefY, u%Mesh%Orientation(:,:,iBody), ErrStat, ErrMsg ) + ! rotdisp(3) = rotdisp(3) - u%PtfmRefY ! Remove the large yaw offset + rotdisp = EulerExtractZYX ( u%Mesh%Orientation(:,:,iBody) ) + IF ( (ABS(rotdisp(1)) > LrgAngle) .AND. FrstWarn_LrgR ) THEN + ErrStat2 = ErrID_Severe + ErrMsg2 = 'Roll angle of a potential-flow body violated the small angle assumption. The solution might be inaccurate. Simulation continuing, but future warnings will be suppressed.' + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + FrstWarn_LrgR = .FALSE. + END IF + IF ( (ABS(rotdisp(2)) > LrgAngle) .AND. FrstWarn_LrgP ) THEN + ErrStat2 = ErrID_Severe + ErrMsg2 = 'Pitch angle of a potential-flow body violated the small angle assumption. The solution might be inaccurate. Simulation continuing, but future warnings will be suppressed.' + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + FrstWarn_LrgP = .FALSE. + END IF + IF ( (ABS( WrapToPi(rotDisp(3)-u%PtfmRefY) ) > LrgAngle) .AND. FrstWarn_LrgY ) THEN + ErrStat2 = ErrID_Severe + ErrMsg2 = 'Yaw angle of a potential-flow body relative to the reference yaw position (PtfmRefY) violated the small angle assumption. The solution might be inaccurate. Consider using PtfmYMod=1 and adjust PtfmYCutoff in ElastoDyn. Simulation continuing, but future warnings will be suppressed.' + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) + FrstWarn_LrgY = .FALSE. + END IF + indxStart = (iBody-1)*6+1 indxEnd = indxStart+5 - q (indxStart:indxEnd) = reshape((/real(u%Mesh%TranslationDisp(:,iBody),ReKi),rotdisp(:)/),(/6/)) - qdot (indxStart:indxEnd) = reshape((/u%Mesh%TranslationVel(:,iBody),u%Mesh%RotationVel(:,iBody)/),(/6/)) - qdotdot(indxStart:indxEnd) = reshape((/u%Mesh%TranslationAcc(:,iBody),u%Mesh%RotationAcc(:,iBody)/),(/6/)) + + ! Displacement with Tait-Bryan angles following the Z-Y-X convention + q(indxStart:indxEnd) = reshape((/real(u%Mesh%TranslationDisp(:,iBody),ReKi),rotdisp(:)/),(/6/)) + + ! Get velocity and acceleration in the heading frame + call hiFrameTransform( i2h, u%PtfmRefY, u%Mesh%TranslationVel(:,iBody), tmpVec6(1:3), ErrStat2, ErrMsg2) + call hiFrameTransform( i2h, u%PtfmRefY, u%Mesh%RotationVel(:,iBody), tmpVec6(4:6), ErrStat2, ErrMsg2) + qdot (indxStart:indxEnd) = tmpVec6 + call hiFrameTransform( i2h, u%PtfmRefY, u%Mesh%TranslationAcc(:,iBody), tmpVec6(1:3), ErrStat2, ErrMsg2) + call hiFrameTransform( i2h, u%PtfmRefY, u%Mesh%RotationAcc(:,iBody), tmpVec6(4:6), ErrStat2, ErrMsg2) + qdotdot(indxStart:indxEnd) = tmpVec6 end do - ! Compute the load contribution from hydrostatics: - - m%F_HS = -matmul(p%HdroSttc,q) - + ! Compute the load contribution from hydrostatics: + ! Hydrostatic load in the yaw-offset frame + ! m%F_HS = -matmul(p%HdroSttc,q) + m%F_HS = 0. do iBody = 1, p%NBody indxStart = (iBody-1)*6+1 indxEnd = indxStart+5 - m%F_HS(indxStart:indxEnd) = m%F_HS(indxStart:indxEnd) + p%F_HS_Moment_Offset(:,iBody) ! except for the hydrostatic buoyancy force from Archimede's Principle when the support platform is in its undisplaced position + m%F_HS((indxStart+2):(indxEnd-1)) = -matmul(p%HdroSttc((indxStart+2):(indxEnd-1),(indxStart+2):(indxEnd-1)),& + q((indxStart+2):(indxEnd-1))) + m%F_HS(indxStart:indxEnd) = m%F_HS(indxStart:indxEnd) + p%F_HS_Moment_Offset(:,iBody) + end do + + ! Transform hydrostatic loads back to the inertial frame + do iBody = 1, p%NBody + indxStart = (iBody-1)*6+1 + indxEnd = indxStart+2 + ! call hiFrameTransform( h2i, u%PtfmRefY, m%F_HS(indxStart:indxEnd), tmpVec3, ErrStat2, ErrMsg2 ) + call hiFrameTransform( h2i, q(iBody*6), m%F_HS(indxStart:indxEnd), tmpVec3, ErrStat2, ErrMsg2 ) + m%F_HS(indxStart:indxEnd) = tmpVec3 + indxStart = indxEnd+1 + indxEnd = indxStart+2 + ! call hiFrameTransform( h2i, u%PtfmRefY, m%F_HS(indxStart:indxEnd), tmpVec3, ErrStat2, ErrMsg2 ) + call hiFrameTransform( h2i, q(iBody*6), m%F_HS(indxStart:indxEnd), tmpVec3, ErrStat2, ErrMsg2 ) + m%F_HS(indxStart:indxEnd) = tmpVec3 end do @@ -1854,25 +2028,36 @@ SUBROUTINE WAMIT_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, ErrStat, Er IF ( p%RdtnMod == 1 ) THEN ! .TRUE. when we will be modeling wave radiation damping. m%Conv_Rdtn_u%Velocity = qdot CALL Conv_Rdtn_CalcOutput( Time, m%Conv_Rdtn_u, p%Conv_Rdtn, x%Conv_Rdtn, xd%Conv_Rdtn, & - z%Conv_Rdtn, OtherState%Conv_Rdtn, m%Conv_Rdtn_y, m%Conv_Rdtn, ErrStat, ErrMsg ) + z%Conv_Rdtn, OtherState%Conv_Rdtn, m%Conv_Rdtn_y, m%Conv_Rdtn, ErrStat2, ErrMsg2 ) + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) m%F_Rdtn (:) = m%Conv_Rdtn_y%F_Rdtn ELSE IF ( p%RdtnMod == 2 ) THEN m%SS_Rdtn_u%dq = qdot CALL SS_Rad_CalcOutput( Time, m%SS_Rdtn_u, p%SS_Rdtn, x%SS_Rdtn, xd%SS_Rdtn, & - z%SS_Rdtn, OtherState%SS_Rdtn, m%SS_Rdtn_y, m%SS_Rdtn, ErrStat, ErrMsg ) + z%SS_Rdtn, OtherState%SS_Rdtn, m%SS_Rdtn_y, m%SS_Rdtn, ErrStat2, ErrMsg2 ) + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName ) m%F_Rdtn (:) = m%SS_Rdtn_y%y ELSE ! We must not be modeling wave radiation damping. - ! Set the total load contribution from radiation damping to zero: m%F_Rdtn (:) = 0.0 - END IF - + do iBody = 1, p%NBody + indxStart = (iBody-1)*6+1 + indxEnd = indxStart+2 + call hiFrameTransform( h2i, u%PtfmRefY, m%F_Rdtn(indxStart:indxEnd), tmpVec3, ErrStat2, ErrMsg2 ) + m%F_Rdtn(indxStart:indxEnd) = tmpVec3 + + indxStart = indxEnd+1 + indxEnd = indxStart+2 + call hiFrameTransform( h2i, u%PtfmRefY, m%F_Rdtn(indxStart:indxEnd), tmpVec3, ErrStat2, ErrMsg2 ) + m%F_Rdtn(indxStart:indxEnd) = tmpVec3 + end do + ! Compute Added Mass Forces ! Set the platform added mass matrix, PtfmAM, to be the infinite-frequency @@ -1881,9 +2066,18 @@ SUBROUTINE WAMIT_CalcOutput( Time, u, p, x, xd, z, OtherState, y, m, ErrStat, Er !added mass: - m%F_PtfmAM = -matmul(p%HdroAdMsI, qdotdot) - - + m%F_PtfmAM = -matmul(p%HdroAdMsI, qdotdot) ! In h-frame + do iBody = 1, p%NBody + indxStart = (iBody-1)*6+1 + indxEnd = indxStart+2 + call hiFrameTransform( h2i, u%PtfmRefY, m%F_PtfmAM(indxStart:indxEnd), tmpVec3, ErrStat2, ErrMsg2 ) + m%F_PtfmAM(indxStart:indxEnd) = tmpVec3 + + indxStart = indxEnd+1 + indxEnd = indxStart+2 + call hiFrameTransform( h2i, u%PtfmRefY, m%F_PtfmAM(indxStart:indxEnd), tmpVec3, ErrStat2, ErrMsg2 ) + m%F_PtfmAM(indxStart:indxEnd) = tmpVec3 + end do ! Compute outputs here: do iBody = 1, p%NBody @@ -1922,7 +2116,10 @@ SUBROUTINE WAMIT_CalcContStateDeriv( Time, u, p, x, xd, z, OtherState, m, dxdt, integer(IntKi) :: iBody ! WAMIT body index integer(IntKi) :: indxStart ! Starting and ending indices for the iBody_th sub vector in an NBody long vector - real(SiKi) :: waveElev0(p%NBody) + real(SiKi) :: waveElev0(p%NBody) + real(ReKi) :: tmpVec6(6) + + ! Initialize ErrStat ErrStat = ErrID_None @@ -1933,8 +2130,9 @@ SUBROUTINE WAMIT_CalcContStateDeriv( Time, u, p, x, xd, z, OtherState, m, dxdt, if (p%RdtnMod == 2) then do iBody = 1, p%NBody indxStart = (iBody-1)*6+1 - m%SS_Rdtn_u%dq(indxStart:indxStart+2) = u%Mesh%TranslationVel(:,iBody) - m%SS_Rdtn_u%dq(indxStart+3:indxStart+5) = u%Mesh%RotationVel(:,iBody) + call hiFrameTransform( i2h, u%PtfmRefY, u%Mesh%TranslationVel(:,iBody), tmpVec6(1:3), ErrStat, ErrMsg) + call hiFrameTransform( i2h, u%PtfmRefY, u%Mesh%RotationVel(:,iBody), tmpVec6(4:6), ErrStat, ErrMsg) + m%SS_Rdtn_u%dq(indxStart:indxStart+5) = tmpVec6 end do CALL SS_Rad_CalcContStateDeriv( Time, m%SS_Rdtn_u, p%SS_Rdtn, x%SS_Rdtn, xd%SS_Rdtn, z%SS_Rdtn, OtherState%SS_Rdtn, m%SS_Rdtn, dxdt%SS_Rdtn, ErrStat, ErrMsg ) @@ -1969,6 +2167,8 @@ SUBROUTINE WAMIT_UpdateDiscState( Time, n, u, p, x, xd, z, OtherState, m, ErrSta integer(IntKi) :: iBody ! WAMIT body index integer(IntKi) :: indxStart, indxEnd ! Starting and ending indices for the iBody_th sub vector in an NBody long vector + REAL(ReKi) :: tmpVec6(6) + ! Initialize ErrStat ErrStat = ErrID_None @@ -1979,8 +2179,10 @@ SUBROUTINE WAMIT_UpdateDiscState( Time, n, u, p, x, xd, z, OtherState, m, ErrSta IF ( p%RdtnMod == 1 ) THEN ! .TRUE. when we will be modeling wave radiation damping. do iBody=1,p%NBody indxStart = (iBody-1)*6+1 - indxEnd = indxStart+5 - m%Conv_Rdtn_u%Velocity(indxStart:indxEnd) = (/u%Mesh%TranslationVel(:,iBody), u%Mesh%RotationVel(:,iBody)/) + indxEnd = indxStart+5 + call hiFrameTransform( i2h, u%PtfmRefY, u%Mesh%TranslationVel(:,iBody), tmpVec6(1:3), ErrStat, ErrMsg) + call hiFrameTransform( i2h, u%PtfmRefY, u%Mesh%RotationVel(:,iBody), tmpVec6(4:6), ErrStat, ErrMsg) + m%Conv_Rdtn_u%Velocity(indxStart:indxEnd) = tmpVec6 end do CALL Conv_Rdtn_UpdateDiscState( Time, n, m%Conv_Rdtn_u, p%Conv_Rdtn, x%Conv_Rdtn, xd%Conv_Rdtn, z%Conv_Rdtn, & OtherState%Conv_Rdtn, m%Conv_Rdtn, ErrStat, ErrMsg ) @@ -2019,5 +2221,59 @@ SUBROUTINE WAMIT_CalcConstrStateResidual( Time, u, p, x, xd, z, OtherState, m, z END SUBROUTINE WAMIT_CalcConstrStateResidual !---------------------------------------------------------------------------------------------------------------------------------- +!> Other supporting subroutines +!.................................................................................................................................. +FUNCTION GetAngleInRangeR8(inAngle,minAngle,maxAngle,outAngle) + REAL(R8Ki), INTENT(IN ) :: inAngle + REAL(R8Ki), INTENT(IN ) :: minAngle + REAL(R8Ki), INTENT(IN ) :: maxAngle + REAL(R8Ki), INTENT( OUT) :: outAngle + LOGICAL :: GetAngleInRangeR8 + REAL(R8Ki), PARAMETER :: Tol = 0.001 ! deg + + GetAngleInRangeR8 = .FALSE. + if ( ( inAngle > (minAngle-Tol) ) .AND. ( inAngle < (maxAngle+Tol) ) ) then + GetAngleInRangeR8 = .TRUE. + outAngle = inAngle + else if (inAngle < minAngle ) then + outAngle = inAngle + ceiling((minAngle-inAngle)/360.0)*360.0 + if ( outAngle < (maxAngle+Tol) ) then + GetAngleInRangeR8 = .TRUE. + end if + else ! inAngle > maxAngle + outAngle = inAngle - ceiling((inAngle-maxAngle)/360.0)*360.0 + if ( outAngle > (minAngle-Tol) ) then + GetAngleInRangeR8 = .TRUE. + end if + end if + +END FUNCTION GetAngleInRangeR8 + +FUNCTION GetAngleInRangeR4(inAngle,minAngle,maxAngle,outAngle) + REAL(SiKi), INTENT(IN ) :: inAngle + REAL(SiKi), INTENT(IN ) :: minAngle + REAL(SiKi), INTENT(IN ) :: maxAngle + REAL(SiKi), INTENT( OUT) :: outAngle + LOGICAL :: GetAngleInRangeR4 + REAL(SiKi), PARAMETER :: Tol = 0.001 ! deg + + GetAngleInRangeR4 = .FALSE. + if ( ( inAngle > (minAngle-Tol) ) .AND. ( inAngle < (maxAngle+Tol) ) ) then + GetAngleInRangeR4 = .TRUE. + outAngle = inAngle + else if (inAngle < minAngle ) then + outAngle = inAngle + ceiling((minAngle-inAngle)/360.0)*360.0 + if ( outAngle < (maxAngle+Tol) ) then + GetAngleInRangeR4 = .TRUE. + end if + else ! inAngle > maxAngle + outAngle = inAngle - ceiling((inAngle-maxAngle)/360.0)*360.0 + if ( outAngle > (minAngle-Tol) ) then + GetAngleInRangeR4 = .TRUE. + end if + end if + +END FUNCTION GetAngleInRangeR4 +!---------------------------------------------------------------------------------------------------------------------------------- END MODULE WAMIT !********************************************************************************************************************************** diff --git a/modules/hydrodyn/src/WAMIT.txt b/modules/hydrodyn/src/WAMIT.txt index 608afe87b4..df76292930 100644 --- a/modules/hydrodyn/src/WAMIT.txt +++ b/modules/hydrodyn/src/WAMIT.txt @@ -34,11 +34,13 @@ typedef ^ ^ INTEGER typedef ^ ^ INTEGER ExctnMod - - - "" - typedef ^ ^ INTEGER ExctnDisp - - - "0: use undisplaced position, 1: use displaced position, 2: use low-pass filtered displaced position) [only used when PotMod=1 and ExctnMod>0]" - typedef ^ ^ ReKi ExctnCutOff - - - "Cutoff (corner) frequency of the low-pass time-filtered displaced position (Hz) [>0.0] " Hz +typedef ^ ^ IntKi NExctnHdg - - - "Number of PRP headings/yaw offset evenly distributed over the region [-180, 180) deg to be used when precomputing the wave excitation [only used when PtfmYMod=1 in the HydroDyn driver or in ElastoDyn]" typedef ^ ^ DbKi RdtnTMax - - - "" - typedef ^ ^ CHARACTER(1024) WAMITFile - - - "" - typedef ^ ^ Conv_Rdtn_InitInputType Conv_Rdtn - - - "" - typedef ^ ^ SeaSt_WaveFieldType *WaveField - - - "Pointer to wave field" - +typedef ^ ^ INTEGER PtfmYMod - - - "Large yaw model" - +typedef ^ ^ ReKi PtfmRefY - - - "Initial reference yaw offset" (rad) # # # Define outputs from the initialization routine here: @@ -104,23 +106,27 @@ typedef ^ ^ ReKi typedef ^ ^ SiKi HdroAdMsI {:}{:} - - "" (sec) typedef ^ ^ SiKi HdroSttc {:}{:} - - "" - typedef ^ ^ INTEGER RdtnMod - - - "" - -typedef ^ ^ INTEGER ExctnMod - - - "" - -typedef ^ ^ INTEGER ExctnDisp - - - "0: use undisplaced position, 1: use displaced position, 2: use low-pass filtered displaced position) [only used when PotMod=1 and ExctnMod>0]" - -typedef ^ ^ ReKi ExctnCutOff - - - "Cutoff (corner) frequency of the low-pass time-filtered displaced position (Hz) [>0.0] " Hz -typedef ^ ^ ReKi ExctnFiltConst - - - "Low-pass time filter constant computed from ExctnCutOff" -typedef ^ ^ SiKi WaveExctn {:}{:} - - "" - -typedef ^ ^ SiKi WaveExctnGrid {:}{:}{:}{:} - - "WaveExctnGrid dimensions are: 1st: wavetime, 2nd: X, 3rd: Y, 4th: Force component for eac WAMIT Body" - +typedef ^ ^ INTEGER ExctnMod - - - "" - +typedef ^ ^ INTEGER ExctnDisp - - - "0: use undisplaced position, 1: use displaced position, 2: use low-pass filtered displaced position) [only used when PotMod=1 and ExctnMod>0]" - +typedef ^ ^ ReKi ExctnCutOff - - - "Cutoff (corner) frequency of the low-pass time-filtered displaced position (Hz) [>0.0] " Hz +typedef ^ ^ IntKi NExctnHdg - - - "Number of PRP headings/yaw offset evenly distributed over the region [-180, 180) deg to be used when precomputing the wave excitation [only used when PtfmYMod=1 in the HydroDyn driver or in ElastoDyn]" +typedef ^ ^ ReKi ExctnFiltConst - - - "Low-pass time filter constant computed from ExctnCutOff" +typedef ^ ^ SiKi WaveExctn {:}{:}{:} - - "" - +typedef ^ ^ SiKi WaveExctnGrid {:}{:}{:}{:}{:} - - "WaveExctnGrid dimensions are: 1st: wavetime, 2nd: X, 3rd: Y, 4th: PRP Yaw, 5th: Force component for eac WAMIT Body" - typedef ^ ^ Conv_Rdtn_ParameterType Conv_Rdtn - - - "" - typedef ^ ^ SS_Rad_ParameterType SS_Rdtn - - - "" - typedef ^ ^ SS_Exc_ParameterType SS_Exctn - - - "" - typedef ^ ^ DbKi DT - - - "" - -typedef ^ ^ SeaSt_WaveFieldType *WaveField - - - "Pointer to wave field" +typedef ^ ^ SeaSt_WaveFieldType *WaveField - - - "Pointer to wave field" +typedef ^ ^ INTEGER PtfmYMod - - - "Large yaw model" - +typedef ^ ^ SeaSt_WaveField_ParameterType ExctnGridParams - - - "Parameters of WaveExctnGrid" - # # # ..... Inputs .................................................................................................................... # Define inputs that are contained on the mesh here: # typedef ^ InputType MeshType Mesh - - - "Displacements at the WAMIT reference point in the inertial frame" - +typedef ^ ^ ReKi PtfmRefY - - - "Reference yaw offset" (rad) # # # ..... Outputs ................................................................................................................... diff --git a/modules/hydrodyn/src/WAMIT2.f90 b/modules/hydrodyn/src/WAMIT2.f90 index b1c1f07ce8..8155bfa691 100644 --- a/modules/hydrodyn/src/WAMIT2.f90 +++ b/modules/hydrodyn/src/WAMIT2.f90 @@ -56,6 +56,7 @@ MODULE WAMIT2 USE WAMIT_Interp USE NWTC_Library USE NWTC_FFTPACK + USE YawOffset IMPLICIT NONE @@ -180,11 +181,21 @@ MODULE WAMIT2 TYPE(W2_InitData4D_Type) :: Data4D !< The 4D type from above END TYPE W2_SumData_Type + INTERFACE GetWAMIT2WvHdgRange + MODULE PROCEDURE GetWAMIT2WvHdgRangeDiffData + MODULE PROCEDURE GetWAMIT2WvHdgRangeSumData + END INTERFACE GetWAMIT2WvHdgRange + INTERFACE CheckWamit2WvHdg MODULE PROCEDURE CheckWAMIT2WvHdgDiffData MODULE PROCEDURE CheckWAMIT2WvHdgSumData END INTERFACE CheckWamit2WvHdg + INTERFACE GetAngleInRange + MODULE PROCEDURE GetAngleInRangeR4 + MODULE PROCEDURE GetAngleInRangeR8 + END INTERFACE GetAngleInRange + CONTAINS !---------------------------------------------------------------------------------------------------------------------------------- !> @brief @@ -204,6 +215,7 @@ SUBROUTINE WAMIT2_Init( InitInp, p, y, m, ErrStat, ErrMsg ) ! Local Variables INTEGER(IntKi) :: IBody !< Counter for current body + INTEGER(IntKi) :: iHdg !< Counter for platform heading INTEGER(IntKi) :: ThisDim !< Counter to currrent dimension INTEGER(IntKi) :: Idx !< Generic counter REAL(R8Ki) :: theta(3) !< rotation about z for the current body (0 about x,y) @@ -218,10 +230,10 @@ SUBROUTINE WAMIT2_Init( InitInp, p, y, m, ErrStat, ErrMsg ) TYPE(W2_SumData_Type) :: SumQTFData !< Data storage for the full sum QTF method ! Force arrays - REAL(SiKi), ALLOCATABLE :: MnDriftForce(:) !< MnDrift force array. Constant for all time. First index is force component - REAL(SiKi), ALLOCATABLE :: NewmanAppForce(:,:) !< NewmanApp force array. Index 1: Time, Index 2: force component - REAL(SiKi), ALLOCATABLE :: DiffQTFForce(:,:) !< DiffQTF force array. Index 1: Time, Index 2: force component - REAL(SiKi), ALLOCATABLE :: SumQTFForce(:,:) !< SumQTF force array. Index 1: Time, Index 2: force component + REAL(SiKi), ALLOCATABLE :: MnDriftForce(:,:) !< MnDrift force array. Constant for all time. Index 1: platform heading, Index 2: force component + REAL(SiKi), ALLOCATABLE :: NewmanAppForce(:,:,:) !< NewmanApp force array. Index 1: Time, Index 2: platform heading, Index 3: force component + REAL(SiKi), ALLOCATABLE :: DiffQTFForce(:,:) !< DiffQTF force array. Index 1: Time, Index 2: force component + REAL(SiKi), ALLOCATABLE :: SumQTFForce(:,:) !< SumQTF force array. Index 1: Time, Index 2: force component ! Temporary error trapping variables INTEGER(IntKi) :: ErrStatTmp !< Temporary variable for holding the error status returned from a CALL statement @@ -570,40 +582,41 @@ SUBROUTINE WAMIT2_Init( InitInp, p, y, m, ErrStat, ErrMsg ) !> Copy output forces over to parameters as needed. !---------------------------------------------------------------------- - ! Initialize the second order force to zero. - p%WaveExctn2 = 0.0_SiKi + ! Initialize the second order force to zero. (Currently the second and third indices, x and y, are not used, but the dimensions are maintained for consistency with first-order wave excitation and future use.) + p%WaveExctn2Grid = 0.0_SiKi ! Difference method data. Only one difference method can be calculated at a time. IF ( p%MnDriftF ) THEN - - DO IBody=1,p%NBody ! Loop through load components. Set ones that are calculated. - DO ThisDim=1,6 - Idx = (IBody-1)*6+ThisDim - IF ( p%MnDriftDims(ThisDim) ) THEN - p%WaveExctn2(:,Idx) = MnDriftForce(Idx) - ENDIF + DO iHdg = 1,p%NExctnHdg+1 + DO IBody=1,p%NBody ! Loop through load components. Set ones that are calculated. + DO ThisDim=1,6 + Idx = (IBody-1)*6+ThisDim + IF ( p%MnDriftDims(ThisDim) ) THEN + p%WaveExctn2Grid(:,1,1,iHdg,Idx) = MnDriftForce(iHdg,Idx) + ENDIF + ENDDO ENDDO ENDDO ELSE IF ( p%NewmanAppF ) THEN - - DO IBody=1,p%NBody ! Loop through load components. Set ones that are calculated. - DO ThisDim=1,6 - Idx = (IBody-1)*6+ThisDim - IF ( p%NewmanAppDims(ThisDim) ) THEN - p%WaveExctn2(:,Idx) = NewmanAppForce(:,Idx) - ENDIF + DO iHdg = 1,p%NExctnHdg+1 + DO IBody=1,p%NBody ! Loop through load components. Set ones that are calculated. + DO ThisDim=1,6 + Idx = (IBody-1)*6+ThisDim + IF ( p%NewmanAppDims(ThisDim) ) THEN + p%WaveExctn2Grid(:,1,1,iHdg,Idx) = NewmanAppForce(:,iHdg,Idx) + ENDIF + ENDDO ENDDO ENDDO - ELSE IF ( p%DiffQTFF ) THEN - DO IBody=1,p%NBody ! Loop through load components. Set ones that are calculated. + DO IBody=1,p%NBody ! Loop through load components. Set ones that are calculated. Multiple headings not supported. DO ThisDim=1,6 Idx = (IBody-1)*6+ThisDim IF ( p%DiffQTFDims(ThisDim) ) THEN - p%WaveExctn2(:,Idx) = DiffQTFForce(:,Idx) + p%WaveExctn2Grid(:,1,1,1,Idx) = DiffQTFForce(:,Idx) ENDIF ENDDO ENDDO @@ -614,11 +627,11 @@ SUBROUTINE WAMIT2_Init( InitInp, p, y, m, ErrStat, ErrMsg ) ! Summation method IF ( p%SumQTFF ) THEN - DO IBody=1,p%NBody ! Loop through load components. Set ones that are calculated. + DO IBody=1,p%NBody ! Loop through load components. Set ones that are calculated. Multiple headings not supported. DO ThisDim=1,6 Idx = (IBody-1)*6+ThisDim IF ( p%SumQTFDims(ThisDim) ) THEN - p%WaveExctn2(:,Idx) = p%WaveExctn2(:,Idx) + SumQTFForce(:,Idx) + p%WaveExctn2Grid(:,1,1,1,Idx) = p%WaveExctn2Grid(:,1,1,1,Idx) + SumQTFForce(:,Idx) ENDIF ENDDO ENDDO @@ -628,6 +641,7 @@ SUBROUTINE WAMIT2_Init( InitInp, p, y, m, ErrStat, ErrMsg ) ! Deallocate the force arrays since we are done with them. Note that the MnDrift force array is ! not deallocated since it is not time dependent. + IF (ALLOCATED(MnDriftForce)) DEALLOCATE(MnDriftForce) IF (ALLOCATED(NewmanAppForce)) DEALLOCATE(NewmanAppForce) IF (ALLOCATED(DiffQTFForce)) DEALLOCATE(DiffQTFForce) IF (ALLOCATED(SumQTFForce)) DEALLOCATE(SumQTFForce) @@ -766,7 +780,7 @@ SUBROUTINE MnDrift_InitCalc( InitInp, p, MnDriftData, MnDriftForce, ErrMsg, ErrS TYPE(WAMIT2_InitInputType), INTENT(IN ) :: InitInp !< Input data for initialization routine TYPE(WAMIT2_ParameterType), INTENT(IN ) :: p !< Parameters TYPE(W2_DiffData_Type), INTENT(INOUT) :: MnDriftData !< Data storage for the MnDrift method. Set to INOUT in case we need to convert 4D to 3D - REAL(SiKi), ALLOCATABLE, INTENT( OUT) :: MnDriftForce(:) !< Force data. Index 1 is the force component. Constant for all time. + REAL(SiKi), ALLOCATABLE, INTENT( OUT) :: MnDriftForce(:,:) !< Force data. Index 1 is platform heading and Index 2 is the force component. Constant for all time. CHARACTER(*), INTENT( OUT) :: ErrMsg INTEGER(IntKi), INTENT( OUT) :: ErrStat @@ -781,6 +795,8 @@ SUBROUTINE MnDrift_InitCalc( InitInp, p, MnDriftData, MnDriftForce, ErrMsg, ErrS INTEGER(IntKi) :: Idx !< Index to the full set of 6*NBody INTEGER(IntKi) :: J !< Generic counter ! INTEGER(IntKi) :: K !< Generic counter + INTEGER(IntKi) :: iHdg !< Heading counter + REAL(ReKi) :: PRPHdg !< PRP heading angle CHARACTER(*), PARAMETER :: RoutineName = 'MnDrift_InitCalc' @@ -798,14 +814,17 @@ SUBROUTINE MnDrift_InitCalc( InitInp, p, MnDriftData, MnDriftForce, ErrMsg, ErrS REAL(SiKi) :: Coord4(4) !< The (omega1,omega2,beta1,beta2) coordinate we want in the 4D dataset COMPLEX(SiKi),ALLOCATABLE :: TmpData3D(:,:,:) !< Temporary 3D array we put the 3D data into (minus the load component indice) COMPLEX(SiKi),ALLOCATABLE :: TmpData4D(:,:,:,:) !< Temporary 4D array we put the 4D data into (minus the load component indice) - + REAL(SiKi) :: W2WvDir1Range(2) !< Range of the first wave heading in the WAMIT second-order files + REAL(SiKi) :: W2WvDir2Range(2) !< Range of the second wave heading in the WAMIT second-order files + REAL(SiKi) :: tmpDir + LOGICAL :: dirInRange ! Initialize a few things ErrMsg = '' ErrStat = ErrID_None ! Initialize resulting forces - ALLOCATE( MnDriftForce(6*p%NBody), STAT=ErrStatTmp ) + ALLOCATE( MnDriftForce(p%NExctnHdg+1,6*p%NBody), STAT=ErrStatTmp ) IF (ErrStatTmp /= 0) THEN CALL SetErrStat(ErrID_Fatal,' Cannot allocate array for the resulting mean drift force '// & 'of the 2nd order force.',ErrStat, ErrMsg, RoutineName) @@ -823,10 +842,14 @@ SUBROUTINE MnDrift_InitCalc( InitInp, p, MnDriftData, MnDriftForce, ErrMsg, ErrS RETURN ENDIF - CALL CheckWAMIT2WvHdg(InitInp,MnDriftData,ErrStatTmp,ErrMsgTmp) + CALL GetWAMIT2WvHdgRange(MnDriftData,W2WvDir1Range,W2WvDir2Range,ErrStatTmp,ErrMsgTmp) CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,RoutineName) IF ( ErrStat >= AbortErrLev ) RETURN + CALL CheckWAMIT2WvHdg(InitInp,MnDriftData,ErrStatTmp,ErrMsgTmp) + CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,RoutineName) + IF ( ErrStat >= AbortErrLev ) RETURN + !> 4. Check the data to see if we need to convert to 3D arrays before continuing (4D is sparse in any dimension we want and !! frequency diagonal is complete). Only check if we don't have 3D data. @@ -920,28 +943,20 @@ SUBROUTINE MnDrift_InitCalc( InitInp, p, MnDriftData, MnDriftForce, ErrMsg, ErrS ENDIF - ! Now loop through all the dimensions and perform the calculation + ! Now loop through all the dimensions and compute the mean-drift load for each body in the body-local coordinate system DO IBody=1,p%NBody - ! Heading correction, only applies to NBodyMod == 2 + ! Wave-heading correction, only applies to NBodyMod == 2 if (p%NBodyMod==2) then RotateZdegOffset = InitInp%PtfmRefztRot(IBody)*R2D else RotateZdegOffset = 0.0_SiKi endif - ! NOTE: RotateZMatrixT is the rotation from local to global. - RotateZMatrixT(1,:) = (/ cos(InitInp%PtfmRefztRot(IBody)), -sin(InitInp%PtfmRefztRot(IBody)) /) - RotateZMatrixT(2,:) = (/ sin(InitInp%PtfmRefztRot(IBody)), cos(InitInp%PtfmRefztRot(IBody)) /) - - DO ThisDim=1,6 Idx = (IBody-1)*6 + ThisDim - ! Set the MnDrift force to 0.0 (Even ones we don't calculate) - MnDriftForce(Idx) = 0.0_SiKi - IF (MnDriftData%DataIs3D) THEN TmpFlag = MnDriftData%Data3D%LoadComponents(Idx) ELSE @@ -962,98 +977,128 @@ SUBROUTINE MnDrift_InitCalc( InitInp, p, MnDriftData, MnDriftForce, ErrMsg, ErrS TmpData4D = MnDriftData%Data4D%DataSet(:,:,:,:,Idx) END IF - - DO J=1,InitInp%WaveField%NStepWave2 + DO iHdg = 1,p%NExctnHdg+1 + ! Compute the PRP heading angle + IF (p%PtfmYMod .EQ. 0) THEN + PRPHdg = InitInp%PtfmRefY + ELSE IF (p%PtfmYMod .EQ. 1) THEN + PRPHdg = -PI + (iHdg-1) * TwoPi/REAL(p%NExctnHdg,ReKi) + END IF + DO J=1,InitInp%WaveField%NStepWave2 ! NOTE: since the Mean Drift only returns a static time independent average value for the drift force, we do not ! need to account for any offset in the location of the WAMIT body (this term vanishes). ! First get the wave amplitude -- must be reconstructed from the WaveElevC0 array. First index is the real (1) or ! imaginary (2) part. Divide by NStepWave2 to remove the built in normalization in WaveElevC0. - aWaveElevC = CMPLX( InitInp%WaveField%WaveElevC0(1,J), InitInp%WaveField%WaveElevC0(2,J), SiKi) / InitInp%WaveField%NStepWave2 + aWaveElevC = CMPLX( InitInp%WaveField%WaveElevC0(1,J), InitInp%WaveField%WaveElevC0(2,J), SiKi) / InitInp%WaveField%NStepWave2 ! Calculate the frequency - Omega1 = J * InitInp%WaveField%WaveDOmega + Omega1 = J * InitInp%WaveField%WaveDOmega ! Only get a QTF value if within the range of frequencies we have wave amplitudes for (first order cutoffs). This ! is done only for efficiency. - IF ( (Omega1 >= InitInp%WaveField%WvLowCOff) .AND. (Omega1 <= InitInp%WaveField%WvHiCOff) ) THEN + IF ( (Omega1 >= InitInp%WaveField%WvLowCOff) .AND. (Omega1 <= InitInp%WaveField%WvHiCOff) ) THEN ! Now get the QTF value that corresponds to this frequency and wavedirection pair. - IF ( MnDriftData%DataIs3D ) THEN + IF ( MnDriftData%DataIs3D ) THEN ! Set the (omega1,beta1,beta2) point we are looking for. (angles in degrees here) - Coord3 = (/ REAL(Omega1,SiKi), InitInp%WaveField%WaveDirArr(J), InitInp%WaveField%WaveDirArr(J) /) + Coord3 = (/ REAL(Omega1,SiKi), InitInp%WaveField%WaveDirArr(J), InitInp%WaveField%WaveDirArr(J) /) ! Apply local Z rotation to heading angle (degrees) to put wave direction into the local (rotated) body frame - Coord3(2) = Coord3(2) - RotateZdegOffset - Coord3(3) = Coord3(3) - RotateZdegOffset + Coord3(2) = Coord3(2) - RotateZdegOffset - PRPHdg*R2D + Coord3(3) = Coord3(3) - RotateZdegOffset - PRPHdg*R2D + + ! Make sure the wave headings are in the correct range + dirInRange = GetAngleInRange(Coord3(2),W2WvDir1Range(1),W2WvDir1Range(2),tmpDir); Coord3(2) = tmpDir + dirInRange = GetAngleInRange(Coord3(3),W2WvDir2Range(1),W2WvDir2Range(2),tmpDir); Coord3(3) = tmpDir + IF (.NOT. dirInRange) THEN ! Somewhat redundant check. Can be removed in the future. + CALL SetErrStat(ErrID_Fatal,' Wave heading out of range.', ErrStat, ErrMsg, RoutineName) + END IF ! get the interpolated value for F(omega1,beta1,beta2) - CALL WAMIT_Interp3D_Cplx( Coord3, TmpData3D, MnDriftData%Data3D%WvFreq1, & + CALL WAMIT_Interp3D_Cplx( Coord3, TmpData3D, MnDriftData%Data3D%WvFreq1, & MnDriftData%Data3D%WvDir1, MnDriftData%Data3D%WvDir2, LastIndex3, QTF_Value, ErrStatTmp, ErrMsgTmp ) - CALL SetErrStat(ErrStatTmp, ErrMsgTmp, ErrStat, ErrMsg, RoutineName) + CALL SetErrStat(ErrStatTmp, ErrMsgTmp, ErrStat, ErrMsg, RoutineName) - ELSE + ELSE ! Set the (omega1,omega2,beta1,beta2) point we are looking for. (angles in degrees here) - Coord4 = (/ REAL(Omega1,SiKi), REAL(Omega1,SiKi), InitInp%WaveField%WaveDirArr(J), InitInp%WaveField%WaveDirArr(J) /) + Coord4 = (/ REAL(Omega1,SiKi), REAL(Omega1,SiKi), InitInp%WaveField%WaveDirArr(J), InitInp%WaveField%WaveDirArr(J) /) ! Apply local Z rotation to heading angle (degrees) to put wave direction into the local (rotated) body frame - Coord4(3) = Coord4(3) - RotateZdegOffset - Coord4(4) = Coord4(4) - RotateZdegOffset + Coord4(3) = Coord4(3) - RotateZdegOffset - PRPHdg*R2D + Coord4(4) = Coord4(4) - RotateZdegOffset - PRPHdg*R2D + + ! Make sure the wave headings are in the correct range + dirInRange = GetAngleInRange(Coord4(3),W2WvDir1Range(1),W2WvDir1Range(2),tmpDir); Coord4(3) = tmpDir + dirInRange = GetAngleInRange(Coord4(4),W2WvDir2Range(1),W2WvDir2Range(2),tmpDir); Coord4(4) = tmpDir + IF (.NOT. dirInRange) THEN ! Somewhat redundant check. Can be removed in the future. + CALL SetErrStat(ErrID_Fatal,' Wave heading out of range.', ErrStat, ErrMsg, RoutineName) + END IF ! get the interpolated value for F(omega1,omega2,beta1,beta2) - CALL WAMIT_Interp4D_Cplx( Coord4, TmpData4D, MnDriftData%Data4D%WvFreq1, MnDriftData%Data4D%WvFreq2, & + CALL WAMIT_Interp4D_Cplx( Coord4, TmpData4D, MnDriftData%Data4D%WvFreq1, MnDriftData%Data4D%WvFreq2, & MnDriftData%Data4D%WvDir1, MnDriftData%Data4D%WvDir2, LastIndex4, QTF_Value, ErrStatTmp, ErrMsgTmp ) - CALL SetErrStat(ErrStatTmp, ErrMsgTmp, ErrStat, ErrMsg, RoutineName) + CALL SetErrStat(ErrStatTmp, ErrMsgTmp, ErrStat, ErrMsg, RoutineName) - ENDIF !QTF value find + ENDIF !QTF value find - ELSE ! outside the frequency range + ELSE ! outside the frequency range - QTF_Value = CMPLX(0.0,0.0,SiKi) + QTF_Value = CMPLX(0.0,0.0,SiKi) - ENDIF ! frequency check + ENDIF ! frequency check ! Check and make sure nothing bombed in the interpolation that we need to be aware of - IF ( ErrStat >= AbortErrLev ) THEN - call cleanup() - RETURN - ENDIF + IF ( ErrStat >= AbortErrLev ) THEN + call cleanup() + RETURN + ENDIF ! Now we have the value of the QTF. These values should only be real for the omega1=omega2 case of the mean drift. ! However if the value came from the 4D interpolation routine, it might have some residual complex part to it. So ! we throw the complex part out. - QTF_Value = CMPLX(REAL(QTF_Value,SiKi),0.0,SiKi) + QTF_Value = CMPLX(REAL(QTF_Value,SiKi),0.0,SiKi) ! NOTE: any offset in platform location vanishes when the only the REAL part is kept (the offset resides in the ! phase shift, which is in the imaginary part) ! Now put it all together... note the frequency stepsize is multiplied after the summation - MnDriftForce(Idx) = MnDriftForce(Idx) + REAL(QTF_Value * aWaveElevC * CONJG(aWaveElevC)) !bjj: put QTF_Value first so that if it's zero, the rest gets set to zero (to hopefully avoid overflow issues) - - ENDDO ! NStepWave2 + MnDriftForce(iHdg,Idx) = MnDriftForce(iHdg,Idx) + REAL(QTF_Value * aWaveElevC * CONJG(aWaveElevC)) !bjj: put QTF_Value first so that if it's zero, the rest gets set to zero (to hopefully avoid overflow issues) + ENDDO ! NStepWave2 + ENDDO ! NExctnHdg ENDIF ! Load component to calculate - ENDDO ! ThisDim -- Load Component on body + ! Rotate the loads back to the i-frame + DO iHdg = 1,p%NExctnHdg+1 + ! Compute the PRP heading angle + IF (p%PtfmYMod .EQ. 0) THEN + PRPHdg = InitInp%PtfmRefY + ELSE IF (p%PtfmYMod .EQ. 1) THEN + PRPHdg = -PI + (iHdg-1) * TwoPi/REAL(p%NExctnHdg,ReKi) + END IF + ! Correct for body rotation (applies to all NBodyMod because WAMIT always output loads in the body frame) and heading change + ! NOTE: RotateZMatrixT is the rotation from local to global. + RotateZMatrixT(1,:) = (/ cos(InitInp%PtfmRefztRot(IBody)+PRPHdg), -sin(InitInp%PtfmRefztRot(IBody)+PRPHdg) /) + RotateZMatrixT(2,:) = (/ sin(InitInp%PtfmRefztRot(IBody)+PRPHdg), cos(InitInp%PtfmRefztRot(IBody)+PRPHdg) /) ! Now rotate the force components with platform orientation - Idx = (IBody-1)*6 - MnDriftForce( (Idx+1):(Idx+2) ) = MATMUL( RotateZMatrixT, MnDriftForce( (Idx+1):(Idx+2) ) ) ! Fx and Fy, rotation about z - MnDriftForce( (Idx+4):(Idx+5) ) = MATMUL( RotateZMatrixT, MnDriftForce( (Idx+4):(Idx+5) ) ) ! Mx and My, rotation about z + Idx = (IBody-1)*6 + MnDriftForce(iHdg,(Idx+1):(Idx+2)) = MATMUL( RotateZMatrixT, MnDriftForce(iHdg,(Idx+1):(Idx+2)) ) ! Fx and Fy, rotation about z + MnDriftForce(iHdg,(Idx+4):(Idx+5)) = MATMUL( RotateZMatrixT, MnDriftForce(iHdg,(Idx+4):(Idx+5)) ) ! Mx and My, rotation about z + ENDDO ! NExctnHdg ENDDO ! IBody - - ! Cleanup call cleanup() @@ -1065,12 +1110,6 @@ end subroutine cleanup END SUBROUTINE MnDrift_InitCalc - - - - - - !------------------------------------------------------------------------------------------------------------------------------- !> This subroutine calculates the force time series using the NewmanApp calculation. !! The data is stored in either 3D or 4D arrays depending on the file type used. @@ -1124,7 +1163,7 @@ SUBROUTINE NewmanApp_InitCalc( InitInp, p, NewmanAppData, NewmanAppForce, ErrMsg TYPE(WAMIT2_InitInputType), INTENT(IN ) :: InitInp !< Input data for initialization routine TYPE(WAMIT2_ParameterType), INTENT(IN ) :: p !< Parameters TYPE(W2_DiffData_Type), INTENT(INOUT) :: NewmanAppData !< Data storage for the NewmanApp method. Set to INOUT in case we need to convert 4D to 3D - REAL(SiKi), ALLOCATABLE, INTENT( OUT) :: NewmanAppForce(:,:) !< Force data. Index 1 is the timestep, index 2 is the load component. + REAL(SiKi), ALLOCATABLE, INTENT( OUT) :: NewmanAppForce(:,:,:) !< Force data. Index 1 is the timestep, index 2 is the platform heading, and index 3 is the load component. CHARACTER(*), INTENT( OUT) :: ErrMsg INTEGER(IntKi), INTENT( OUT) :: ErrStat @@ -1139,14 +1178,16 @@ SUBROUTINE NewmanApp_InitCalc( InitInp, p, NewmanAppData, NewmanAppForce, ErrMsg INTEGER(IntKi) :: Idx !< Index to the full set of 6*NBody INTEGER(IntKi) :: J !< Generic counter ! INTEGER(IntKi) :: K !< Generic counter + INTEGER(IntKi) :: iHdg !< Heading counter + REAL(ReKi) :: PRPHdg !< PRP heading angle TYPE(FFT_DataType) :: FFT_Data !< Temporary array for the FFT module we're using CHARACTER(*), PARAMETER :: RoutineName = 'NewmanApp_InitCalc' ! Wave information and QTF temporary COMPLEX(SiKi) :: QTF_Value !< Temporary complex number for QTF - COMPLEX(SiKi), ALLOCATABLE :: NewmanTerm1C(:,:) !< First term in the newman calculation, complex frequency space. All dimensions, this body. - COMPLEX(SiKi), ALLOCATABLE :: NewmanTerm2C(:,:) !< Second term in the newman calculation, complex frequency space. All dimensions, this body. + COMPLEX(SiKi), ALLOCATABLE :: NewmanTerm1C(:,:,:) !< First term in the newman calculation, complex frequency space. All dimensions, all headings, this body. + COMPLEX(SiKi), ALLOCATABLE :: NewmanTerm2C(:,:,:) !< Second term in the newman calculation, complex frequency space. All dimensions, all headings, this body. COMPLEX(SiKi), ALLOCATABLE :: NewmanTerm1t(:) !< First term in the newman calculation, time domain. Current load dimension. COMPLEX(SiKi), ALLOCATABLE :: NewmanTerm2t(:) !< Second term in the newman calculation, time domain. Current load dimension. COMPLEX(SiKi) :: aWaveElevC !< Wave elevation of current frequency component. NStepWave2 factor removed. @@ -1163,7 +1204,10 @@ SUBROUTINE NewmanApp_InitCalc( InitInp, p, NewmanAppData, NewmanAppForce, ErrMsg REAL(SiKi) :: WaveNmbr1 !< Wavenumber for this frequency COMPLEX(SiKi), ALLOCATABLE :: TmpData3D(:,:,:) !< Temporary 3D array we put the 3D data into (minus the load component indice) COMPLEX(SiKi), ALLOCATABLE :: TmpData4D(:,:,:,:) !< Temporary 4D array we put the 4D data into (minus the load component indice) - + REAL(SiKi) :: W2WvDir1Range(2) !< Range of the first wave heading in the WAMIT second-order files + REAL(SiKi) :: W2WvDir2Range(2) !< Range of the second wave heading in the WAMIT second-order files + REAL(SiKi) :: tmpDir + LOGICAL :: dirInRange ! Initialize a few things ErrMsg = '' @@ -1171,6 +1215,10 @@ SUBROUTINE NewmanApp_InitCalc( InitInp, p, NewmanAppData, NewmanAppForce, ErrMsg ErrStat = ErrID_None ErrStatTmp = ErrID_None + CALL GetWAMIT2WvHdgRange(NewmanAppData,W2WvDir1Range,W2WvDir2Range,ErrStatTmp,ErrMsgTmp) + CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,RoutineName) + IF ( ErrStat >= AbortErrLev ) RETURN + CALL CheckWAMIT2WvHdg(InitInp,NewmanAppData,ErrStatTmp,ErrMsgTmp) CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,RoutineName) IF ( ErrStat >= AbortErrLev ) RETURN @@ -1260,13 +1308,13 @@ SUBROUTINE NewmanApp_InitCalc( InitInp, p, NewmanAppData, NewmanAppForce, ErrMsg ALLOCATE( NewmanTerm2t( 0:InitInp%WaveField%NStepWave ), STAT=ErrStatTmp ) IF (ErrStatTmp /= 0) CALL SetErrStat(ErrID_Fatal,' Cannot allocate array for calculating the second term of the Newmans '// & 'approximation in the time domain.',ErrStat, ErrMsg, RoutineName) - ALLOCATE( NewmanTerm1C( 0:InitInp%WaveField%NStepWave2, 6 ), STAT=ErrStatTmp ) + ALLOCATE( NewmanTerm1C( 0:InitInp%WaveField%NStepWave2, p%NExctnHdg+1, 6 ), STAT=ErrStatTmp ) IF (ErrStatTmp /= 0) CALL SetErrStat(ErrID_Fatal,' Cannot allocate array for calculating the first term of the Newmans '// & 'approximation in the frequency domain.',ErrStat, ErrMsg, RoutineName) - ALLOCATE( NewmanTerm2C( 0:InitInp%WaveField%NStepWave2, 6 ), STAT=ErrStatTmp ) + ALLOCATE( NewmanTerm2C( 0:InitInp%WaveField%NStepWave2, p%NExctnHdg+1, 6 ), STAT=ErrStatTmp ) IF (ErrStatTmp /= 0) CALL SetErrStat(ErrID_Fatal,' Cannot allocate array for calculating the second term of the Newmans '// & 'approximation in the frequency domain.',ErrStat, ErrMsg, RoutineName) - ALLOCATE( NewmanAppForce( 0:InitInp%WaveField%NStepWave, 6*p%NBody), STAT=ErrStatTmp ) + ALLOCATE( NewmanAppForce( 0:InitInp%WaveField%NStepWave, p%NExctnHdg+1, 6*p%NBody), STAT=ErrStatTmp ) IF (ErrStatTmp /= 0) CALL SetErrStat(ErrID_Fatal,' Cannot allocate array for the resulting Newmans '// & 'approximation of the 2nd order force.',ErrStat, ErrMsg, RoutineName) @@ -1308,8 +1356,8 @@ SUBROUTINE NewmanApp_InitCalc( InitInp, p, NewmanAppData, NewmanAppForce, ErrMsg DO IBody=1,p%NBody ! set all frequency terms to zero to start - NewmanTerm1C(:,:) = CMPLX(0.0, 0.0, SiKi) - NewmanTerm2C(:,:) = CMPLX(0.0, 0.0, SiKi) + NewmanTerm1C(:,:,:) = CMPLX(0.0, 0.0, SiKi) + NewmanTerm2C(:,:,:) = CMPLX(0.0, 0.0, SiKi) ! Heading correction, only applies to NBodyMod == 2 @@ -1347,95 +1395,117 @@ SUBROUTINE NewmanApp_InitCalc( InitInp, p, NewmanAppData, NewmanAppForce, ErrMsg TmpData4D = NewmanAppData%Data4D%DataSet(:,:,:,:,Idx) END IF + DO iHdg = 1,p%NExctnHdg+1 + ! Compute the PRP heading angle + IF (p%PtfmYMod .EQ. 0) THEN + PRPHdg = InitInp%PtfmRefY + ELSE IF (p%PtfmYMod .EQ. 1) THEN + PRPHdg = -PI + (iHdg-1) * TwoPi/REAL(p%NExctnHdg,ReKi) + END IF - DO J=1,InitInp%WaveField%NStepWave2 + DO J=1,InitInp%WaveField%NStepWave2 ! First get the wave amplitude -- must be reconstructed from the WaveElevC array. First index is the real (1) or ! imaginary (2) part. Divide by NStepWave2 so that the wave amplitude is of the same form as the paper. - aWaveElevC = CMPLX( InitInp%WaveField%WaveElevC0(1,J), InitInp%WaveField%WaveElevC0(2,J), SiKi) / InitInp%WaveField%NStepWave2 + aWaveElevC = CMPLX( InitInp%WaveField%WaveElevC0(1,J), InitInp%WaveField%WaveElevC0(2,J), SiKi) / InitInp%WaveField%NStepWave2 ! Calculate the frequency - Omega1 = J * InitInp%WaveField%WaveDOmega + Omega1 = J * InitInp%WaveField%WaveDOmega ! Only get a QTF value if within the range of frequencies between the cutoffs for the difference frequency - IF ( (Omega1 >= InitInp%WaveField%WvLowCOff) .AND. (Omega1 <= InitInp%WaveField%WvHiCOff) ) THEN + IF ( (Omega1 >= InitInp%WaveField%WvLowCOff) .AND. (Omega1 <= InitInp%WaveField%WvHiCOff) ) THEN ! Now get the QTF value that corresponds to this frequency and wavedirection pair. - IF ( NewmanAppData%DataIs3D ) THEN + IF ( NewmanAppData%DataIs3D ) THEN ! Set the (omega1,beta1,beta2) point we are looking for. - Coord3 = (/ REAL(Omega1,SiKi), InitInp%WaveField%WaveDirArr(J), InitInp%WaveField%WaveDirArr(J) /) + Coord3 = (/ REAL(Omega1,SiKi), InitInp%WaveField%WaveDirArr(J), InitInp%WaveField%WaveDirArr(J) /) ! Apply local Z rotation to heading angle (degrees) to put wave direction into the local (rotated) body frame - Coord3(2) = Coord3(2) - RotateZdegOffset - Coord3(3) = Coord3(3) - RotateZdegOffset + Coord3(2) = Coord3(2) - RotateZdegOffset - PRPHdg*R2D + Coord3(3) = Coord3(3) - RotateZdegOffset - PRPHdg*R2D + + ! Make sure the wave headings are in the correct range + dirInRange = GetAngleInRange(Coord3(2),W2WvDir1Range(1),W2WvDir1Range(2),tmpDir); Coord3(2) = tmpDir + dirInRange = GetAngleInRange(Coord3(3),W2WvDir2Range(1),W2WvDir2Range(2),tmpDir); Coord3(3) = tmpDir + IF (.NOT. dirInRange) THEN ! Somewhat redundant check. Can be removed in the future. + CALL SetErrStat(ErrID_Fatal,' Wave heading out of range.', ErrStat, ErrMsg, RoutineName) + END IF ! get the interpolated value for F(omega1,beta1,beta2) - CALL WAMIT_Interp3D_Cplx( Coord3, TmpData3D, NewmanAppData%Data3D%WvFreq1, & + CALL WAMIT_Interp3D_Cplx( Coord3, TmpData3D, NewmanAppData%Data3D%WvFreq1, & NewmanAppData%Data3D%WvDir1, NewmanAppData%Data3D%WvDir2, LastIndex3, QTF_Value, ErrStatTmp, ErrMsgTmp ) - ELSE + ELSE ! Set the (omega1,omega2,beta1,beta2) point we are looking for. - Coord4 = (/ REAL(Omega1,SiKi), REAL(Omega1,SiKi), InitInp%WaveField%WaveDirArr(J), InitInp%WaveField%WaveDirArr(J) /) + Coord4 = (/ REAL(Omega1,SiKi), REAL(Omega1,SiKi), InitInp%WaveField%WaveDirArr(J), InitInp%WaveField%WaveDirArr(J) /) ! Apply local Z rotation to heading angle (degrees) to put wave direction into the local (rotated) body frame - Coord4(3) = Coord4(3) - RotateZdegOffset - Coord4(4) = Coord4(4) - RotateZdegOffset + Coord4(3) = Coord4(3) - RotateZdegOffset - PRPHdg*R2D + Coord4(4) = Coord4(4) - RotateZdegOffset - PRPHdg*R2D + + ! Make sure the wave headings are in the correct range + dirInRange = GetAngleInRange(Coord4(3),W2WvDir1Range(1),W2WvDir1Range(2),tmpDir); Coord4(3) = tmpDir + dirInRange = GetAngleInRange(Coord4(4),W2WvDir2Range(1),W2WvDir2Range(2),tmpDir); Coord4(4) = tmpDir + IF (.NOT. dirInRange) THEN ! Somewhat redundant check. Can be removed in the future. + CALL SetErrStat(ErrID_Fatal,' Wave heading out of range.', ErrStat, ErrMsg, RoutineName) + END IF ! get the interpolated value for F(omega1,omega2,beta1,beta2) - CALL WAMIT_Interp4D_Cplx( Coord4, TmpData4D, NewmanAppData%Data4D%WvFreq1, NewmanAppData%Data4D%WvFreq2, & + CALL WAMIT_Interp4D_Cplx( Coord4, TmpData4D, NewmanAppData%Data4D%WvFreq1, NewmanAppData%Data4D%WvFreq2, & NewmanAppData%Data4D%WvDir1, NewmanAppData%Data4D%WvDir2, LastIndex4, QTF_Value, ErrStatTmp, ErrMsgTmp ) - ENDIF !QTF value find + ENDIF !QTF value find ! Now we have the value of the QTF. These values should only be real for the omega1=omega2 case of the approximation. ! However if the value came from the 4D interpolation routine, it might have some residual complex part to it. So ! we throw the complex part out. NOTE: the phase shift due to location will be added before the FFT. - QTF_Value = CMPLX(REAL(QTF_Value,SiKi),0.0,SiKi) + QTF_Value = CMPLX(REAL(QTF_Value,SiKi),0.0,SiKi) - ELSE ! outside the frequency range + ELSE ! outside the frequency range - QTF_Value = CMPLX(0.0,0.0,SiKi) + QTF_Value = CMPLX(0.0,0.0,SiKi) - ENDIF ! frequency check + ENDIF ! frequency check ! Check and make sure nothing bombed in the interpolation that we need to be aware of - CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,RoutineName) - IF ( ErrStat >= AbortErrLev ) THEN - IF (ALLOCATED(TmpData3D)) DEALLOCATE(TmpData3D,STAT=ErrStatTmp) - IF (ALLOCATED(TmpData4D)) DEALLOCATE(TmpData4D,STAT=ErrStatTmp) - IF (ALLOCATED(NewmanTerm1t)) DEALLOCATE(NewmanTerm1t,STAT=ErrStatTmp) - IF (ALLOCATED(NewmanTerm2t)) DEALLOCATE(NewmanTerm2t,STAT=ErrStatTmp) - IF (ALLOCATED(NewmanTerm1C)) DEALLOCATE(NewmanTerm1C,STAT=ErrStatTmp) - IF (ALLOCATED(NewmanTerm2C)) DEALLOCATE(NewmanTerm2C,STAT=ErrStatTmp) - IF (ALLOCATED(NewmanAppForce)) DEALLOCATE(NewmanAppForce,STAT=ErrStatTmp) - RETURN - ENDIF + CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,RoutineName) + IF ( ErrStat >= AbortErrLev ) THEN + IF (ALLOCATED(TmpData3D)) DEALLOCATE(TmpData3D,STAT=ErrStatTmp) + IF (ALLOCATED(TmpData4D)) DEALLOCATE(TmpData4D,STAT=ErrStatTmp) + IF (ALLOCATED(NewmanTerm1t)) DEALLOCATE(NewmanTerm1t,STAT=ErrStatTmp) + IF (ALLOCATED(NewmanTerm2t)) DEALLOCATE(NewmanTerm2t,STAT=ErrStatTmp) + IF (ALLOCATED(NewmanTerm1C)) DEALLOCATE(NewmanTerm1C,STAT=ErrStatTmp) + IF (ALLOCATED(NewmanTerm2C)) DEALLOCATE(NewmanTerm2C,STAT=ErrStatTmp) + IF (ALLOCATED(NewmanAppForce)) DEALLOCATE(NewmanAppForce,STAT=ErrStatTmp) + RETURN + ENDIF ! Now calculate the Newman terms - IF (REAL(QTF_Value) > 0.0_SiKi) THEN + IF (REAL(QTF_Value) > 0.0_SiKi) THEN - NewmanTerm1C(J,ThisDim) = aWaveElevC * (QTF_Value)**0.5_SiKi - NewmanTerm2C(J,ThisDim) = CMPLX(0.0_SiKi, 0.0_SiKi, SiKi) + NewmanTerm1C(J,iHdg,ThisDim) = aWaveElevC * (QTF_Value)**0.5_SiKi + NewmanTerm2C(J,iHdg,ThisDim) = CMPLX(0.0_SiKi, 0.0_SiKi, SiKi) - ELSE IF (REAL(QTF_Value) < 0.0_SiKi) THEN + ELSE IF (REAL(QTF_Value) < 0.0_SiKi) THEN - NewmanTerm1C(J,ThisDim) = CMPLX(0.0_SiKi, 0.0_SiKi, SiKi) - NewmanTerm2C(J,ThisDim) = aWaveElevC * (-QTF_Value)**0.5_SiKi + NewmanTerm1C(J,iHdg,ThisDim) = CMPLX(0.0_SiKi, 0.0_SiKi, SiKi) + NewmanTerm2C(J,iHdg,ThisDim) = aWaveElevC * (-QTF_Value)**0.5_SiKi - ELSE ! at 0 + ELSE ! at 0 - NewmanTerm1C(J,ThisDim) = CMPLX(0.0_SiKi, 0.0_SiKi, SiKi) - NewmanTerm2C(J,ThisDim) = CMPLX(0.0_SiKi, 0.0_SiKi, SiKi) + NewmanTerm1C(J,iHdg,ThisDim) = CMPLX(0.0_SiKi, 0.0_SiKi, SiKi) + NewmanTerm2C(J,iHdg,ThisDim) = CMPLX(0.0_SiKi, 0.0_SiKi, SiKi) - ENDIF + ENDIF - ENDDO ! J=1,InitInp%WaveField%NStepWave2 + ENDDO ! J=1,InitInp%WaveField%NStepWave2 + ENDDO ! iHdg = 1,p%NExctnHdg ENDIF ! Load component to calculate @@ -1445,98 +1515,106 @@ SUBROUTINE NewmanApp_InitCalc( InitInp, p, NewmanAppData, NewmanAppForce, ErrMsg !---------------------------------------------------- ! Rotate back to global frame and phase shift and set the terms for the summation !---------------------------------------------------- - + DO iHdg = 1,p%NExctnHdg+1 + ! Compute the PRP heading angle + IF (p%PtfmYMod .EQ. 0) THEN + PRPHdg = InitInp%PtfmRefY + ELSE IF (p%PtfmYMod .EQ. 1) THEN + PRPHdg = -PI + (iHdg-1) * TwoPi/REAL(p%NExctnHdg,ReKi) + END IF ! Set rotation ! NOTE: RotateZMatrixT is the rotation from local to global. - RotateZMatrixT(1,:) = (/ cos(InitInp%PtfmRefztRot(IBody)), -sin(InitInp%PtfmRefztRot(IBody)) /) - RotateZMatrixT(2,:) = (/ sin(InitInp%PtfmRefztRot(IBody)), cos(InitInp%PtfmRefztRot(IBody)) /) + RotateZMatrixT(1,:) = (/ cos(InitInp%PtfmRefztRot(IBody)+PRPHdg), -sin(InitInp%PtfmRefztRot(IBody)+PRPHdg) /) + RotateZMatrixT(2,:) = (/ sin(InitInp%PtfmRefztRot(IBody)+PRPHdg), cos(InitInp%PtfmRefztRot(IBody)+PRPHdg) /) ! Loop through all the frequencies - DO J=1,InitInp%WaveField%NStepWave2 + DO J=1,InitInp%WaveField%NStepWave2 ! Frequency - Omega1 = J * InitInp%WaveField%WaveDOmega + Omega1 = J * InitInp%WaveField%WaveDOmega - !> Phase shift due to offset in location, only for NBodyMod==2 - if (p%NBodyMod == 2) then + !> Phase shift due to offset in location, only for NBodyMod==2 + if (p%NBodyMod == 2) then - !> The phase shift due to an (x,y) offset is of the form - !! \f$ exp[-\imath k(\omega) ( X cos(\beta(w)) + Y sin(\beta(w)) )] \f$ - ! NOTE: the phase shift applies to the aWaveElevC of the incoming wave. Including it here instead - ! of above is mathematically equivalent, but only because each frequency has only one wave - ! direction associated with it through the equal energy approach used in multidirectional waves. + !> The phase shift due to an (x,y) offset is of the form + !! \f$ exp[-\imath k(\omega) ( X cos(\beta(w)) + Y sin(\beta(w)) )] \f$ + ! NOTE: the phase shift applies to the aWaveElevC of the incoming wave. Including it here instead + ! of above is mathematically equivalent, but only because each frequency has only one wave + ! direction associated with it through the equal energy approach used in multidirectional waves. - WaveNmbr1 = WaveNumber ( REAL(Omega1,SiKi), InitInp%Gravity, InitInp%WaveField%EffWtrDpth ) ! SiKi returned - TmpReal1 = WaveNmbr1 * ( InitInp%PtfmRefxt(1)*cos(InitInp%WaveField%WaveDirArr(J)*D2R) + InitInp%PtfmRefyt(1)*sin(InitInp%WaveField%WaveDirArr(J)*D2R) ) - PhaseShiftXY = CMPLX( cos(TmpReal1), -sin(TmpReal1) ) + WaveNmbr1 = WaveNumber ( REAL(Omega1,SiKi), InitInp%Gravity, InitInp%WaveField%EffWtrDpth ) ! SiKi returned + TmpReal1 = WaveNmbr1 * ( InitInp%PtfmRefxt(1)*cos(InitInp%WaveField%WaveDirArr(J)*D2R) + InitInp%PtfmRefyt(1)*sin(InitInp%WaveField%WaveDirArr(J)*D2R) ) + PhaseShiftXY = CMPLX( cos(TmpReal1), -sin(TmpReal1) ) - ! Apply the phase shift - DO ThisDim=1,6 - NewmanTerm1C(J,ThisDim) = NewmanTerm1C(J,ThisDim)*PhaseShiftXY ! Newman term 1 - NewmanTerm2C(J,ThisDim) = NewmanTerm2C(J,ThisDim)*PhaseShiftXY ! Newman term 2 - ENDDO - endif + ! Apply the phase shift + DO ThisDim=1,6 + NewmanTerm1C(J,iHdg,ThisDim) = NewmanTerm1C(J,iHdg,ThisDim)*PhaseShiftXY ! Newman term 1 + NewmanTerm2C(J,iHdg,ThisDim) = NewmanTerm2C(J,iHdg,ThisDim)*PhaseShiftXY ! Newman term 2 + ENDDO + endif ! Apply the rotation to get back to global frame -- Term 1 - NewmanTerm1C(J,1:2) = MATMUL(RotateZMatrixT, NewmanTerm1C(J,1:2)) - NewmanTerm1C(J,4:5) = MATMUL(RotateZMatrixT, NewmanTerm1C(J,4:5)) + NewmanTerm1C(J,iHdg,1:2) = MATMUL(RotateZMatrixT, NewmanTerm1C(J,iHdg,1:2)) + NewmanTerm1C(J,iHdg,4:5) = MATMUL(RotateZMatrixT, NewmanTerm1C(J,iHdg,4:5)) ! Apply the rotation to get back to global frame -- Term 2 - NewmanTerm2C(J,1:2) = MATMUL(RotateZMatrixT, NewmanTerm2C(J,1:2)) - NewmanTerm2C(J,4:5) = MATMUL(RotateZMatrixT, NewmanTerm2C(J,4:5)) + NewmanTerm2C(J,iHdg,1:2) = MATMUL(RotateZMatrixT, NewmanTerm2C(J,iHdg,1:2)) + NewmanTerm2C(J,iHdg,4:5) = MATMUL(RotateZMatrixT, NewmanTerm2C(J,iHdg,4:5)) - ENDDO ! J=1,InitInp%WaveField%NStepWave2 + ENDDO ! J=1,InitInp%WaveField%NStepWave2 + ENDDO ! iHdg = 1,p%NExctnHdg+1 !---------------------------------------------------- ! Apply the FFT to get time domain results !---------------------------------------------------- + DO iHdg = 1,p%NExctnHdg+1 - DO ThisDim=1,6 ! Loop through all dimensions + DO ThisDim=1,6 ! Loop through all dimensions - Idx= (IBody-1)*6+ThisDim + Idx= (IBody-1)*6+ThisDim ! Now we apply the FFT to the first piece. - CALL ApplyCFFT( NewmanTerm1t(:), NewmanTerm1C(:,ThisDim), FFT_Data, ErrStatTmp ) - CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,RoutineName) - IF ( ErrStat >= AbortErrLev ) THEN - IF (ALLOCATED(TmpData3D)) DEALLOCATE(TmpData3D,STAT=ErrStatTmp) - IF (ALLOCATED(TmpData4D)) DEALLOCATE(TmpData4D,STAT=ErrStatTmp) - IF (ALLOCATED(NewmanTerm1t)) DEALLOCATE(NewmanTerm1t,STAT=ErrStatTmp) - IF (ALLOCATED(NewmanTerm2t)) DEALLOCATE(NewmanTerm2t,STAT=ErrStatTmp) - IF (ALLOCATED(NewmanTerm1C)) DEALLOCATE(NewmanTerm1C,STAT=ErrStatTmp) - IF (ALLOCATED(NewmanTerm2C)) DEALLOCATE(NewmanTerm2C,STAT=ErrStatTmp) - IF (ALLOCATED(NewmanAppForce)) DEALLOCATE(NewmanAppForce,STAT=ErrStatTmp) - RETURN - END IF + CALL ApplyCFFT( NewmanTerm1t(:), NewmanTerm1C(:,iHdg,ThisDim), FFT_Data, ErrStatTmp ) + CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,RoutineName) + IF ( ErrStat >= AbortErrLev ) THEN + IF (ALLOCATED(TmpData3D)) DEALLOCATE(TmpData3D,STAT=ErrStatTmp) + IF (ALLOCATED(TmpData4D)) DEALLOCATE(TmpData4D,STAT=ErrStatTmp) + IF (ALLOCATED(NewmanTerm1t)) DEALLOCATE(NewmanTerm1t,STAT=ErrStatTmp) + IF (ALLOCATED(NewmanTerm2t)) DEALLOCATE(NewmanTerm2t,STAT=ErrStatTmp) + IF (ALLOCATED(NewmanTerm1C)) DEALLOCATE(NewmanTerm1C,STAT=ErrStatTmp) + IF (ALLOCATED(NewmanTerm2C)) DEALLOCATE(NewmanTerm2C,STAT=ErrStatTmp) + IF (ALLOCATED(NewmanAppForce)) DEALLOCATE(NewmanAppForce,STAT=ErrStatTmp) + RETURN + END IF ! Now we apply the FFT to the second piece. - CALL ApplyCFFT( NewmanTerm2t(:), NewmanTerm2C(:,ThisDim), FFT_Data, ErrStatTmp ) - CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,RoutineName) - IF ( ErrStat >= AbortErrLev ) THEN - IF (ALLOCATED(TmpData3D)) DEALLOCATE(TmpData3D,STAT=ErrStatTmp) - IF (ALLOCATED(TmpData4D)) DEALLOCATE(TmpData4D,STAT=ErrStatTmp) - IF (ALLOCATED(NewmanTerm1t)) DEALLOCATE(NewmanTerm1t,STAT=ErrStatTmp) - IF (ALLOCATED(NewmanTerm2t)) DEALLOCATE(NewmanTerm2t,STAT=ErrStatTmp) - IF (ALLOCATED(NewmanTerm1C)) DEALLOCATE(NewmanTerm1C,STAT=ErrStatTmp) - IF (ALLOCATED(NewmanTerm2C)) DEALLOCATE(NewmanTerm2C,STAT=ErrStatTmp) - IF (ALLOCATED(NewmanAppForce)) DEALLOCATE(NewmanAppForce,STAT=ErrStatTmp) - RETURN - ENDIF + CALL ApplyCFFT( NewmanTerm2t(:), NewmanTerm2C(:,iHdg,ThisDim), FFT_Data, ErrStatTmp ) + CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,RoutineName) + IF ( ErrStat >= AbortErrLev ) THEN + IF (ALLOCATED(TmpData3D)) DEALLOCATE(TmpData3D,STAT=ErrStatTmp) + IF (ALLOCATED(TmpData4D)) DEALLOCATE(TmpData4D,STAT=ErrStatTmp) + IF (ALLOCATED(NewmanTerm1t)) DEALLOCATE(NewmanTerm1t,STAT=ErrStatTmp) + IF (ALLOCATED(NewmanTerm2t)) DEALLOCATE(NewmanTerm2t,STAT=ErrStatTmp) + IF (ALLOCATED(NewmanTerm1C)) DEALLOCATE(NewmanTerm1C,STAT=ErrStatTmp) + IF (ALLOCATED(NewmanTerm2C)) DEALLOCATE(NewmanTerm2C,STAT=ErrStatTmp) + IF (ALLOCATED(NewmanAppForce)) DEALLOCATE(NewmanAppForce,STAT=ErrStatTmp) + RETURN + ENDIF ! Now square the real part of the resulting time domain pieces and add them together to get the final force time series. - DO J=0,InitInp%WaveField%NStepWave-1 - NewmanAppForce(J,Idx) = (abs(NewmanTerm1t(J)))**2 - (abs(NewmanTerm2t(J)))**2 - ENDDO + DO J=0,InitInp%WaveField%NStepWave-1 + NewmanAppForce(J,iHdg,Idx) = (abs(NewmanTerm1t(J)))**2 - (abs(NewmanTerm2t(J)))**2 + ENDDO ! Copy the last first term to the last so that it is cyclic - NewmanAppForce(InitInp%WaveField%NStepWave,Idx) = NewmanAppForce(0,Idx) - - ENDDO ! ThisDim -- index to current dimension + NewmanAppForce(InitInp%WaveField%NStepWave,iHdg,Idx) = NewmanAppForce(0,iHdg,Idx) + ENDDO ! ThisDim -- index to current dimension + ENDDO ! iHdg -- current PRP heading ENDDO ! IBody -- current body @@ -1642,7 +1720,7 @@ SUBROUTINE DiffQTF_InitCalc( InitInp, p, DiffQTFData, DiffQTFForce, ErrMsg, ErrS REAL(SiKi), ALLOCATABLE :: TmpDiffQTFForce(:) !< The resulting diffQTF force for this load component REAL(ReKi) :: Omega1 !< First wave frequency REAL(ReKi) :: Omega2 !< Second wave frequency - REAL(SiKi), ALLOCATABLE :: MnDriftForce(:) !< Mean drift force (first term). MnDrift_InitCalc routine will return this. + REAL(SiKi), ALLOCATABLE :: MnDriftForce(:,:) !< Mean drift force (first term). MnDrift_InitCalc routine will return this. REAL(SiKi) :: RotateZdegOffset !< Offset to wave heading (NBodyMod==2 only) REAL(SiKi) :: RotateZMatrixT(2,2) !< The transpose of rotation in matrix form for rotation about z (from global to local) COMPLEX(SiKi) :: PhaseShiftXY !< The phase shift offset to apply to the body @@ -1653,7 +1731,10 @@ SUBROUTINE DiffQTF_InitCalc( InitInp, p, DiffQTFData, DiffQTFForce, ErrMsg, ErrS INTEGER(IntKi) :: LastIndex4(4) !< Last used index for searching in the interpolation algorithms. First wave freq REAL(SiKi) :: Coord4(4) !< The (omega1,omega2,beta1,beta2) coordinate we want in the 4D dataset. First wave freq. COMPLEX(SiKi), ALLOCATABLE :: TmpData4D(:,:,:,:) !< Temporary 4D array we put the 4D data into (minus the load component indice) - + REAL(SiKi) :: W2WvDir1Range(2) !< Range of the first wave heading in the WAMIT second-order files + REAL(SiKi) :: W2WvDir2Range(2) !< Range of the second wave heading in the WAMIT second-order files + REAL(SiKi) :: tmpDir + LOGICAL :: dirInRange ! Initialize a few things ErrMsg = '' @@ -1661,6 +1742,10 @@ SUBROUTINE DiffQTF_InitCalc( InitInp, p, DiffQTFData, DiffQTFForce, ErrMsg, ErrS ErrStat = ErrID_None ErrStatTmp = ErrID_None + CALL GetWAMIT2WvHdgRange(DiffQTFData,W2WvDir1Range,W2WvDir2Range,ErrStatTmp,ErrMsgTmp) + CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,RoutineName) + IF ( ErrStat >= AbortErrLev ) RETURN + CALL CheckWAMIT2WvHdg(InitInp,DiffQTFData,ErrStatTmp,ErrMsgTmp) CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,RoutineName) IF ( ErrStat >= AbortErrLev ) RETURN @@ -1812,8 +1897,15 @@ SUBROUTINE DiffQTF_InitCalc( InitInp, p, DiffQTFData, DiffQTFForce, ErrMsg, ErrS Coord4 = (/ REAL(Omega1,SiKi), REAL(Omega2,SiKi), InitInp%WaveField%WaveDirArr(J+K), InitInp%WaveField%WaveDirArr(K) /) ! Apply local Z rotation to heading angle (degrees) to put wave direction into the local (rotated) body frame - Coord4(3) = Coord4(3) - RotateZdegOffset - Coord4(4) = Coord4(4) - RotateZdegOffset + Coord4(3) = Coord4(3) - RotateZdegOffset - InitInp%PtfmRefY*R2D + Coord4(4) = Coord4(4) - RotateZdegOffset - InitInp%PtfmRefY*R2D + + ! Make sure the wave headings are in the correct range + dirInRange = GetAngleInRange(Coord4(3),W2WvDir1Range(1),W2WvDir1Range(2),tmpDir); Coord4(3) = tmpDir + dirInRange = GetAngleInRange(Coord4(4),W2WvDir2Range(1),W2WvDir2Range(2),tmpDir); Coord4(4) = tmpDir + IF (.NOT. dirInRange) THEN ! Somewhat redundant check. Can be removed in the future. + CALL SetErrStat(ErrID_Fatal,' Wave heading out of range.', ErrStat, ErrMsg, RoutineName) + END IF ! get the interpolated value for F(omega1,omega2,beta1,beta2) --> QTF_Value CALL WAMIT_Interp4D_Cplx( Coord4, TmpData4D, DiffQTFData%Data4D%WvFreq1, DiffQTFData%Data4D%WvFreq2, & @@ -1875,8 +1967,8 @@ SUBROUTINE DiffQTF_InitCalc( InitInp, p, DiffQTFData, DiffQTFForce, ErrMsg, ErrS ! Set rotation ! NOTE: RotateZMatrixT is the rotation from local to global. - RotateZMatrixT(1,:) = (/ cos(InitInp%PtfmRefztRot(IBody)), -sin(InitInp%PtfmRefztRot(IBody)) /) - RotateZMatrixT(2,:) = (/ sin(InitInp%PtfmRefztRot(IBody)), cos(InitInp%PtfmRefztRot(IBody)) /) + RotateZMatrixT(1,:) = (/ cos(InitInp%PtfmRefztRot(IBody)+InitInp%PtfmRefY), -sin(InitInp%PtfmRefztRot(IBody)+InitInp%PtfmRefY) /) + RotateZMatrixT(2,:) = (/ sin(InitInp%PtfmRefztRot(IBody)+InitInp%PtfmRefY), cos(InitInp%PtfmRefztRot(IBody)+InitInp%PtfmRefY) /) ! Loop through all the frequencies DO J=1,InitInp%WaveField%NStepWave2 @@ -1910,7 +2002,7 @@ SUBROUTINE DiffQTF_InitCalc( InitInp, p, DiffQTFData, DiffQTFForce, ErrMsg, ErrS ! NOTE: phase shift and orientations on the MnDriftForce term have already been applied ! NOTE: the "-1" since TmpDiffQTFForce(InitInp%WaveField%NStepWave) is not set and DiffQTFForce(InitInp%WaveField%NStepWave,Idx) gets overwritten DO K=0,InitInp%WaveField%NStepWave-1 - DiffQTFForce(K,Idx) = 2.0_SiKi * TmpDiffQTFForce(K) + MnDriftForce(Idx) + DiffQTFForce(K,Idx) = 2.0_SiKi * TmpDiffQTFForce(K) + MnDriftForce(1,Idx) ENDDO ! Copy the last first term to the first so that it is cyclic @@ -1936,6 +2028,7 @@ SUBROUTINE DiffQTF_InitCalc( InitInp, p, DiffQTFData, DiffQTFForce, ErrMsg, ErrS subroutine cleanup() ! Cleanup + IF (ALLOCATED(MnDriftForce)) DEALLOCATE(MnDriftForce,STAT=ErrStatTmp) IF (ALLOCATED(TmpData4D)) DEALLOCATE(TmpData4D,STAT=ErrStatTmp) IF (ALLOCATED(TmpDiffQTFForce)) DEALLOCATE(TmpDiffQTFForce,STAT=ErrStatTmp) IF (ALLOCATED(TmpComplexArr)) DEALLOCATE(TmpComplexArr,STAT=ErrStatTmp) @@ -2059,7 +2152,10 @@ SUBROUTINE SumQTF_InitCalc( InitInp, p, SumQTFData, SumQTFForce, ErrMsg, ErrStat INTEGER(IntKi) :: LastIndex4(4) !< Last used index for searching in the interpolation algorithms. First wave freq REAL(SiKi) :: Coord4(4) !< The (omega1,omega2,beta1,beta2) coordinate we want in the 4D dataset. First wave freq. COMPLEX(SiKi), ALLOCATABLE :: TmpData4D(:,:,:,:) !< Temporary 4D array we put the 4D data into (minus the load component indice) - + REAL(SiKi) :: W2WvDir1Range(2) !< Range of the first wave heading in the WAMIT second-order files + REAL(SiKi) :: W2WvDir2Range(2) !< Range of the second wave heading in the WAMIT second-order files + REAL(SiKi) :: tmpDir + LOGICAL :: dirInRange ! Initialize a few things ErrMsg = '' @@ -2067,6 +2163,10 @@ SUBROUTINE SumQTF_InitCalc( InitInp, p, SumQTFData, SumQTFForce, ErrMsg, ErrStat ErrStat = ErrID_None ErrStatTmp = ErrID_None + CALL GetWAMIT2WvHdgRange(SumQTFData,W2WvDir1Range,W2WvDir2Range,ErrStatTmp,ErrMsgTmp) + CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,RoutineName) + IF ( ErrStat >= AbortErrLev ) RETURN + CALL CheckWAMIT2WvHdg(InitInp,SumQTFData,ErrStatTmp,ErrMsgTmp) CALL SetErrStat(ErrStatTmp,ErrMsgTmp,ErrStat,ErrMsg,RoutineName) IF ( ErrStat >= AbortErrLev ) RETURN @@ -2205,8 +2305,15 @@ SUBROUTINE SumQTF_InitCalc( InitInp, p, SumQTFData, SumQTFForce, ErrMsg, ErrStat Coord4 = (/ REAL(Omega1,SiKi), REAL(Omega1,SiKi), InitInp%WaveField%WaveDirArr(J), InitInp%WaveField%WaveDirArr(J) /) ! Apply local Z rotation to heading angle (degrees) to put wave direction into the local (rotated) body frame - Coord4(3) = Coord4(3) - RotateZdegOffset - Coord4(4) = Coord4(4) - RotateZdegOffset + Coord4(3) = Coord4(3) - RotateZdegOffset - InitInp%PtfmRefY*R2D + Coord4(4) = Coord4(4) - RotateZdegOffset - InitInp%PtfmRefY*R2D + + ! Make sure the wave headings are in the correct range + dirInRange = GetAngleInRange(Coord4(3),W2WvDir1Range(1),W2WvDir1Range(2),tmpDir); Coord4(3) = tmpDir + dirInRange = GetAngleInRange(Coord4(4),W2WvDir2Range(1),W2WvDir2Range(2),tmpDir); Coord4(4) = tmpDir + IF (.NOT. dirInRange) THEN ! Somewhat redundant check. Can be removed in the future. + CALL SetErrStat(ErrID_Fatal,' Wave heading out of range.', ErrStat, ErrMsg, RoutineName) + END IF ! get the interpolated value for F(omega1,omega2,beta1,beta2) --> QTF_Value CALL WAMIT_Interp4D_Cplx( Coord4, TmpData4D, SumQTFData%Data4D%WvFreq1, SumQTFData%Data4D%WvFreq2, & @@ -2319,8 +2426,15 @@ SUBROUTINE SumQTF_InitCalc( InitInp, p, SumQTFData, SumQTFForce, ErrMsg, ErrStat Coord4 = (/ REAL(Omega1,SiKi), REAL(Omega2,SiKi), InitInp%WaveField%WaveDirArr(K), InitInp%WaveField%WaveDirArr(J-K) /) ! Apply local Z rotation to heading angle (degrees) to put wave direction into the local (rotated) body frame - Coord4(3) = Coord4(3) - RotateZdegOffset - Coord4(4) = Coord4(4) - RotateZdegOffset + Coord4(3) = Coord4(3) - RotateZdegOffset - InitInp%PtfmRefY*R2D + Coord4(4) = Coord4(4) - RotateZdegOffset - InitInp%PtfmRefY*R2D + + ! Make sure the wave headings are in the correct range + dirInRange = GetAngleInRange(Coord4(3),W2WvDir1Range(1),W2WvDir1Range(2),tmpDir); Coord4(3) = tmpDir + dirInRange = GetAngleInRange(Coord4(4),W2WvDir2Range(1),W2WvDir2Range(2),tmpDir); Coord4(4) = tmpDir + IF (.NOT. dirInRange) THEN ! Somewhat redundant check. Can be removed in the future. + CALL SetErrStat(ErrID_Fatal,' Wave heading out of range.', ErrStat, ErrMsg, RoutineName) + END IF ! get the interpolated value for F(omega1,omega2,beta1,beta2) --> QTF_Value CALL WAMIT_Interp4D_Cplx( Coord4, TmpData4D, SumQTFData%Data4D%WvFreq1, SumQTFData%Data4D%WvFreq2, & @@ -2375,8 +2489,8 @@ SUBROUTINE SumQTF_InitCalc( InitInp, p, SumQTFData, SumQTFForce, ErrMsg, ErrStat ! Set rotation ! NOTE: RotateZMatrixT is the rotation from local to global. - RotateZMatrixT(1,:) = (/ cos(InitInp%PtfmRefztRot(IBody)), -sin(InitInp%PtfmRefztRot(IBody)) /) - RotateZMatrixT(2,:) = (/ sin(InitInp%PtfmRefztRot(IBody)), cos(InitInp%PtfmRefztRot(IBody)) /) + RotateZMatrixT(1,:) = (/ cos(InitInp%PtfmRefztRot(IBody)+InitInp%PtfmRefY), -sin(InitInp%PtfmRefztRot(IBody)+InitInp%PtfmRefY) /) + RotateZMatrixT(2,:) = (/ sin(InitInp%PtfmRefztRot(IBody)+InitInp%PtfmRefY), cos(InitInp%PtfmRefztRot(IBody)+InitInp%PtfmRefY) /) ! Loop through all the frequencies DO J=1,InitInp%WaveField%NStepWave2 @@ -2484,7 +2598,7 @@ SUBROUTINE CheckInitInput( InitInp, p, MnDriftData, NewmanAppData, DiffQTFData, ! Temporary Error Variables INTEGER(IntKi) :: ErrStatTmp !< Temporary variable for the local error status -! CHARACTER(2048) :: ErrMsgTmp !< Temporary error message variable + CHARACTER(ErrMsgLen) :: ErrMsgTmp !< Temporary error message variable CHARACTER(*), PARAMETER :: RoutineName = 'CheckInitInput' !> ## Subroutine contents @@ -2527,6 +2641,28 @@ SUBROUTINE CheckInitInput( InitInp, p, MnDriftData, NewmanAppData, DiffQTFData, !! for each method listing which dimensions to use. !! !-------------------------------------------------------------------------------- + ! Platform large yaw offset model + p%PtfmYMod = InitInp%PtfmYMod + + ! Set up 2nd-order wave excitation grid + ! Copy WaveField grid parameters + call SeaSt_WaveField_CopyParam(InitInp%WaveField%GridParams, p%Exctn2GridParams, 0, ErrStatTmp, ErrMsgTmp); CALL SetErrStat( ErrStatTmp, ErrMsgTmp, ErrStat, ErrMsg, RoutineName) + ! x and y grids are currently not used for second-order wave excitation + p%Exctn2GridParams%n(2:3) = 1_IntKi + p%Exctn2GridParams%delta(2:3) = 0.0_SiKi + p%Exctn2GridParams%pZero(2:3) = 0.0_SiKi + ! Set the fourth index based on PRP heading + if ( InitInp%PtfmYMod .EQ. 0) then ! Constant reference yaw offset + p%NExctnHdg = 0_IntKi + p%Exctn2GridParams%delta(4) = 0.0 + p%Exctn2GridParams%pZero(4) = InitInp%PtfmRefY + else if ( InitInp%PtfmYMod .EQ. 1 ) then ! Drifting reference yaw offset + p%NExctnHdg = InitInp%NExctnHdg + p%Exctn2GridParams%delta(4) = TwoPi/Real(MAX(p%NExctnHdg,1_IntKi),ReKi) + p%Exctn2GridParams%pZero(4) = -Pi + end if + p%Exctn2GridParams%n(4) = p%NExctnHdg+1 + p%Exctn2GridParams%Z_depth = -1.0 ! Set to Z_depth to a negative value to indicate uniform "z" grid for platform heading !> 1. Check that we only specified one of MnDrift, NewmanApp, or DiffQTF @@ -2858,8 +2994,8 @@ SUBROUTINE CheckInitInput( InitInp, p, MnDriftData, NewmanAppData, DiffQTFData, !-------------------------------------------------------------------------------- ! Allocate array for the WaveExtcn2. - ALLOCATE( p%WaveExctn2(0:InitInp%WaveField%NStepWave,6*p%NBody), STAT=ErrStatTmp) - IF (ErrStatTmp /= 0) CALL SetErrStat(ErrID_Fatal,'Cannot allocate array p%WaveExctn2 to store '// & + ALLOCATE( p%WaveExctn2Grid(0:InitInp%WaveField%NStepWave,1,1,p%NExctnHdg+1,6*p%NBody), STAT=ErrStatTmp) + IF (ErrStatTmp /= 0) CALL SetErrStat(ErrID_Fatal,'Cannot allocate array p%WaveExctn2Grid to store '// & 'the 2nd order force data.', ErrStat,ErrMsg,'CheckInitInp') IF (ErrStat >= AbortErrLev ) RETURN @@ -4799,10 +4935,11 @@ END SUBROUTINE ReadRealNumber !---------------------------------------------------------------------------------------------------------------------------------- !> Routine for computing outputs, used in both loose and tight coupling. -SUBROUTINE WAMIT2_CalcOutput( Time, WaveField, p, y, m, ErrStat, ErrMsg ) +SUBROUTINE WAMIT2_CalcOutput( Time, PtfmRefY, WaveField, p, y, m, ErrStat, ErrMsg ) !.................................................................................................................................. REAL(DbKi), INTENT(IN ) :: Time !< Current simulation time in seconds + REAL(ReKi), INTENT(IN ) :: PtfmRefY !< Platform reference yaw offset at Time TYPE(SeaSt_WaveFieldType), INTENT(IN ) :: WaveField !< Wave data TYPE(WAMIT2_ParameterType), INTENT(IN ) :: p !< Parameters TYPE(WAMIT2_OutputType), INTENT(INOUT) :: y !< Outputs computed at Time (Input only so that mesh @@ -4815,8 +4952,13 @@ SUBROUTINE WAMIT2_CalcOutput( Time, WaveField, p, y, m, ErrStat, ErrMsg ) ! Local Variables: INTEGER(IntKi) :: I ! Generic index - INTEGER(IntKi) :: IBody ! Index to body number - INTEGER(IntKi) :: indxStart ! Starting index + INTEGER(IntKi) :: iBody ! Index to body number + INTEGER(IntKi) :: iStart ! Starting index + REAL(ReKi) :: bodyPosition(3) + + INTEGER(IntKi) :: ErrStatTmp !< Temporary variable for the local error status + CHARACTER(ErrMsgLen) :: ErrMsgTmp !< Temporary error message variable + CHARACTER(*), PARAMETER :: RoutineName = 'WAMIT2_CalcOutput' ! Initialize ErrStat @@ -4828,20 +4970,21 @@ SUBROUTINE WAMIT2_CalcOutput( Time, WaveField, p, y, m, ErrStat, ErrMsg ) ! Compute the 2nd order load contribution from incident waves: do iBody = 1, p%NBody - indxStart = (iBody-1)*6 - - DO I = 1,6 ! Loop through all wave excitation forces and moments - m%F_Waves2(indxStart+I) = InterpWrappedStpReal ( REAL(Time, SiKi), WaveField%WaveTime, p%WaveExctn2(:,indxStart+I), & - m%LastIndWave(IBody), WaveField%NStepWave + 1 ) - END DO ! I - All wave excitation forces and moments - + bodyPosition(1) = 0.0 + bodyPosition(2) = 0.0 + bodyPosition(3) = WrapToPi(PtfmRefY) + iStart = (iBody-1)*6+1 + ! WaveExctn2Grid dimensions are: 1st: wavetime, 2nd: X, 3rd: Y, 4th: PRP yaw offset, 5th: Force component for each WAMIT Body + m%F_Waves2(iStart:iStart+5) = WAMIT_ForceWaves_Interp( Time, bodyPosition, p%WaveExctn2Grid(:,:,:,:,iStart:iStart+5), p%Exctn2GridParams, m%WaveField_m, ErrStatTmp, ErrMsgTmp ) + call SetErrStat( ErrStatTmp, ErrMsgTmp, ErrStat, ErrMsg, RoutineName ) ! Copy results to the output point mesh + iStart = iStart - 1 DO I=1,3 - y%Mesh%Force(I,IBody) = m%F_Waves2(indxStart+I) + y%Mesh%Force(I,iBody) = m%F_Waves2(iStart+I) END DO DO I=1,3 - y%Mesh%Moment(I,IBody) = m%F_Waves2(indxStart+I+3) + y%Mesh%Moment(I,iBody) = m%F_Waves2(iStart+I+3) END DO enddo @@ -5031,6 +5174,146 @@ SUBROUTINE Destroy_InitData4D(Data4D) END SUBROUTINE Destroy_InitData4D +SUBROUTINE GetWAMIT2WvHdgRangeDiffData(W2Data,W2WvDir1Range,W2WvDir2Range,ErrStat,ErrMsg) + TYPE(W2_DiffData_Type), INTENT(IN ) :: W2Data + REAL(SiKi), INTENT( OUT) :: W2WvDir1Range(2) + REAL(SiKi), INTENT( OUT) :: W2WvDir2Range(2) + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + INTEGER(IntKi) :: NumWvDir1, NumWvDir2, I + REAL(SiKi) :: AvgInpWvDirSpcg + REAL(SiKi), PARAMETER :: Tol = 0.001 ! deg + CHARACTER(*), PARAMETER :: RoutineName = 'GetWAMIT2WvHdgRangeDiffData' + ErrStat = ErrID_None + ErrMsg = "" + + ! WvDir1 and WvDir2 should be the same, but treating them as potentially different for now + IF ( W2Data%DataIs3D) THEN + W2WvDir1Range(1) = MINVAL(W2Data%Data3D%WvDir1) + W2WvDir1Range(2) = MAXVAL(W2Data%Data3D%WvDir1) + W2WvDir2Range(1) = MINVAL(W2Data%Data3D%WvDir2) + W2WvDir2Range(2) = MAXVAL(W2Data%Data3D%WvDir2) + NumWvDir1 = W2Data%Data3D%NumWvDir1 + NumWvDir2 = W2Data%Data3D%NumWvDir2 + ELSE IF ( W2Data%DataIs4D) THEN + W2WvDir1Range(1) = MINVAL(W2Data%Data4D%WvDir1) + W2WvDir1Range(2) = MAXVAL(W2Data%Data4D%WvDir1) + W2WvDir2Range(1) = MINVAL(W2Data%Data4D%WvDir2) + W2WvDir2Range(2) = MAXVAL(W2Data%Data4D%WvDir2) + NumWvDir1 = W2Data%Data4D%NumWvDir1 + NumWvDir2 = W2Data%Data4D%NumWvDir2 + ELSE + ! No data. This is a catastrophic issue. We should not have called this routine without data that is usable for the MnDrift calculation + CALL SetErrStat( ErrID_Fatal, ' Second-order wave-load calculation called without data.',ErrStat,ErrMsg,RoutineName) + END IF + + IF ( (W2WvDir1Range(2)-W2WvDir1Range(1))>(360.0+Tol) ) THEN + CALL SetErrStat( ErrID_Fatal,' The difference between any pair of wave directions in '//TRIM(W2Data%Filename)//' should be less than or equal to 360 deg.',ErrStat,ErrMsg,RoutineName) + END IF + IF ( (W2WvDir2Range(2)-W2WvDir2Range(1))>(360.0+Tol) ) THEN + CALL SetErrStat( ErrID_Fatal,' The difference between any pair of wave directions in '//TRIM(W2Data%Filename)//' should be less than or equal to 360 deg.',ErrStat,ErrMsg,RoutineName) + END IF + + ! The input wave headings should cover a contiguous region of directions. Check for gaps and warn user. + IF ( W2Data%DataIs3D) THEN + IF (NumWvDir1>1) THEN + AvgInpWvDirSpcg = (W2WvDir1Range(2)-W2WvDir1Range(1))/REAL(NumWvDir1-1,SiKi) + DO I = 2,NumWvDir1 + IF ( (W2Data%Data3D%WvDir1(I)-W2Data%Data3D%WvDir1(I-1)) > (3.0*AvgInpWvDirSpcg) ) THEN + CALL SetErrStat( ErrID_Warn,'The wave headings in '//TRIM(W2Data%Filename)//' is likely not contiguous with a gap between '//TRIM(Num2LStr(W2Data%Data3D%WvDir1(I-1)))//' and '//TRIM(Num2LStr(W2Data%Data3D%WvDir1(I)))//' degs.', & + ErrStat, ErrMsg, RoutineName) + END IF + END DO + END IF + IF (NumWvDir2>1) THEN + AvgInpWvDirSpcg = (W2WvDir2Range(2)-W2WvDir2Range(1))/REAL(NumWvDir2-1,SiKi) + DO I = 2,NumWvDir2 + IF ( (W2Data%Data3D%WvDir2(I)-W2Data%Data3D%WvDir2(I-1)) > (3.0*AvgInpWvDirSpcg) ) THEN + CALL SetErrStat( ErrID_Warn,'The wave headings in '//TRIM(W2Data%Filename)//' is likely not contiguous with a gap between '//TRIM(Num2LStr(W2Data%Data3D%WvDir2(I-1)))//' and '//TRIM(Num2LStr(W2Data%Data3D%WvDir2(I)))//' degs.', & + ErrStat, ErrMsg, RoutineName) + END IF + END DO + END IF + ELSE IF ( W2Data%DataIs4D) THEN + IF (NumWvDir1>1) THEN + AvgInpWvDirSpcg = (W2WvDir1Range(2)-W2WvDir1Range(1))/REAL(NumWvDir1-1,SiKi) + DO I = 2,NumWvDir1 + IF ( (W2Data%Data4D%WvDir1(I)-W2Data%Data4D%WvDir1(I-1)) > (3.0*AvgInpWvDirSpcg) ) THEN + CALL SetErrStat( ErrID_Warn,'The wave headings in '//TRIM(W2Data%Filename)//' is likely not contiguous with a gap between '//TRIM(Num2LStr(W2Data%Data4D%WvDir1(I-1)))//' and '//TRIM(Num2LStr(W2Data%Data4D%WvDir1(I)))//' degs.', & + ErrStat, ErrMsg, RoutineName) + END IF + END DO + END IF + IF (NumWvDir2>1) THEN + AvgInpWvDirSpcg = (W2WvDir2Range(2)-W2WvDir2Range(1))/REAL(NumWvDir2-1,SiKi) + DO I = 2,NumWvDir2 + IF ( (W2Data%Data4D%WvDir2(I)-W2Data%Data4D%WvDir2(I-1)) > (3.0*AvgInpWvDirSpcg) ) THEN + CALL SetErrStat( ErrID_Warn,'The wave headings in '//TRIM(W2Data%Filename)//' is likely not contiguous with a gap between '//TRIM(Num2LStr(W2Data%Data4D%WvDir2(I-1)))//' and '//TRIM(Num2LStr(W2Data%Data4D%WvDir2(I)))//' degs.', & + ErrStat, ErrMsg, RoutineName) + END IF + END DO + END IF + END IF + +END SUBROUTINE GetWAMIT2WvHdgRangeDiffData + +SUBROUTINE GetWAMIT2WvHdgRangeSumData(W2Data,W2WvDir1Range,W2WvDir2Range,ErrStat,ErrMsg) + TYPE(W2_SumData_Type), INTENT(IN ) :: W2Data + REAL(SiKi), INTENT( OUT) :: W2WvDir1Range(2) + REAL(SiKi), INTENT( OUT) :: W2WvDir2Range(2) + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + INTEGER(IntKi) :: NumWvDir1, NumWvDir2, I + REAL(SiKi) :: AvgInpWvDirSpcg + REAL(SiKi), PARAMETER :: Tol = 0.001 ! deg + CHARACTER(*), PARAMETER :: RoutineName = 'GetWAMIT2WvHdgRangeSumData' + ErrStat = ErrID_None + ErrMsg = "" + + ! WvDir1 and WvDir2 should be the same, but treating them as potentially different for now + IF ( W2Data%DataIs4D) THEN + W2WvDir1Range(1) = MINVAL(W2Data%Data4D%WvDir1) + W2WvDir1Range(2) = MAXVAL(W2Data%Data4D%WvDir1) + W2WvDir2Range(1) = MINVAL(W2Data%Data4D%WvDir2) + W2WvDir2Range(2) = MAXVAL(W2Data%Data4D%WvDir2) + NumWvDir1 = W2Data%Data4D%NumWvDir1 + NumWvDir2 = W2Data%Data4D%NumWvDir2 + ELSE + ! No data. This is a catastrophic issue. We should not have called this routine without data that is usable for the MnDrift calculation + CALL SetErrStat( ErrID_Fatal, ' Second-order wave-load calculation called without data.',ErrStat,ErrMsg,RoutineName) + END IF + + IF ( (W2WvDir1Range(2)-W2WvDir1Range(1))>(360.0+Tol) ) THEN + CALL SetErrStat( ErrID_Fatal,' The difference between any pair of wave directions in '//TRIM(W2Data%Filename)//' should be less than or equal to 360 deg.',ErrStat,ErrMsg,RoutineName) + END IF + IF ( (W2WvDir2Range(2)-W2WvDir2Range(1))>(360.0+Tol) ) THEN + CALL SetErrStat( ErrID_Fatal,' The difference between any pair of wave directions in '//TRIM(W2Data%Filename)//' should be less than or equal to 360 deg.',ErrStat,ErrMsg,RoutineName) + END IF + + ! The input wave headings should cover a contiguous region of directions. Check for gaps and warn user. + IF ( W2Data%DataIs4D) THEN + IF (NumWvDir1>1) THEN + AvgInpWvDirSpcg = (W2WvDir1Range(2)-W2WvDir1Range(1))/REAL(NumWvDir1-1,SiKi) + DO I = 2,NumWvDir1 + IF ( (W2Data%Data4D%WvDir1(I)-W2Data%Data4D%WvDir1(I-1)) > (3.0*AvgInpWvDirSpcg) ) THEN + CALL SetErrStat( ErrID_Warn,'The wave headings in '//TRIM(W2Data%Filename)//' is likely not contiguous with a gap between '//TRIM(Num2LStr(W2Data%Data4D%WvDir1(I-1)))//' and '//TRIM(Num2LStr(W2Data%Data4D%WvDir1(I)))//' degs.', & + ErrStat, ErrMsg, RoutineName) + END IF + END DO + END IF + IF (NumWvDir2>1) THEN + AvgInpWvDirSpcg = (W2WvDir2Range(2)-W2WvDir2Range(1))/REAL(NumWvDir2-1,SiKi) + DO I = 2,NumWvDir2 + IF ( (W2Data%Data4D%WvDir2(I)-W2Data%Data4D%WvDir2(I-1)) > (3.0*AvgInpWvDirSpcg) ) THEN + CALL SetErrStat( ErrID_Warn,'The wave headings in '//TRIM(W2Data%Filename)//' is likely not contiguous with a gap between '//TRIM(Num2LStr(W2Data%Data4D%WvDir2(I-1)))//' and '//TRIM(Num2LStr(W2Data%Data4D%WvDir2(I)))//' degs.', & + ErrStat, ErrMsg, RoutineName) + END IF + END DO + END IF + END IF + +END SUBROUTINE GetWAMIT2WvHdgRangeSumData + SUBROUTINE CheckWAMIT2WvHdgDiffData(InitInp,W2Data,ErrStat,ErrMsg) TYPE(WAMIT2_InitInputType), INTENT(IN ) :: InitInp !< Input data for initialization routine TYPE(W2_DiffData_Type), INTENT(IN ) :: W2Data @@ -5044,19 +5327,40 @@ SUBROUTINE CheckWAMIT2WvHdgDiffData(InitInp,W2Data,ErrStat,ErrMsg) ErrStat = ErrID_None ErrMsg = "" - IF ( W2Data%DataIs3D ) THEN - CALL CheckWvHdg(InitInp,W2Data%Data3D%NumWvDir1,W2Data%data3d%WvDir1,'first',ErrStatTmp,ErrMsgTmp) - CALL SetErrStat(ErrStatTmp,' WAMIT output file '//TRIM(W2Data%Filename)//ErrMsgTmp,ErrStat,ErrMsg,RoutineName) - CALL CheckWvHdg(InitInp,W2Data%Data3D%NumWvDir2,W2Data%data3d%WvDir2,'second',ErrStatTmp,ErrMsgTmp) - CALL SetErrStat(ErrStatTmp,' WAMIT output file '//TRIM(W2Data%Filename)//ErrMsgTmp,ErrStat,ErrMsg,RoutineName) - ELSEIF ( W2Data%DataIs4D ) THEN - CALL CheckWvHdg(InitInp,W2Data%Data4D%NumWvDir1,W2Data%data4D%WvDir1,'first',ErrStatTmp,ErrMsgTmp) - CALL SetErrStat(ErrStatTmp,' WAMIT output file '//TRIM(W2Data%Filename)//ErrMsgTmp,ErrStat,ErrMsg,RoutineName) - CALL CheckWvHdg(InitInp,W2Data%Data4D%NumWvDir2,W2Data%data4D%WvDir2,'second',ErrStatTmp,ErrMsgTmp) - CALL SetErrStat(ErrStatTmp,' WAMIT output file '//TRIM(W2Data%Filename)//ErrMsgTmp,ErrStat,ErrMsg,RoutineName) - ELSE - ! No data. This is a catastrophic issue. We should not have called this routine without data that is usable for the MnDrift calculation - CALL SetErrStat( ErrID_Fatal, ' Second-order wave-load calculation called without data.',ErrStat,ErrMsg,RoutineName) + IF ( InitInp%PtfmYMod == 1 ) THEN ! Need to cover -180 deg to 180 deg + IF ( W2Data%DataIs3D ) THEN + IF ( (.not. EqualRealNos( minval(W2Data%data3d%WvDir1),REAL(-180,SiKi))) .OR. (.not. EqualRealNos( maxval(W2Data%data3d%WvDir1),REAL(180,SiKi))) ) THEN + CALL SetErrStat( ErrID_Fatal,' Both wave directions in the WAMIT output file '//TRIM(W2Data%Filename)//' must go from exactly -180 deg to +180 deg.',ErrStat,ErrMsg,RoutineName) + END IF + IF ( (.not. EqualRealNos( minval(W2Data%data3d%WvDir2),REAL(-180,SiKi))) .OR. (.not. EqualRealNos( maxval(W2Data%data3d%WvDir2),REAL(180,SiKi))) ) THEN + CALL SetErrStat( ErrID_Fatal,' Both wave directions in the WAMIT output file '//TRIM(W2Data%Filename)//' must go from exactly -180 deg to +180 deg.',ErrStat,ErrMsg,RoutineName) + END IF + ELSE IF ( W2Data%DataIs4D ) THEN + IF ( (.not. EqualRealNos( minval(W2Data%data4d%WvDir1),REAL(-180,SiKi))) .OR. (.not. EqualRealNos( maxval(W2Data%data4d%WvDir1),REAL(180,SiKi))) ) THEN + CALL SetErrStat( ErrID_Fatal,' Both wave directions in the WAMIT output file '//TRIM(W2Data%Filename)//' must go from exactly -180 deg to +180 deg.',ErrStat,ErrMsg,RoutineName) + END IF + IF ( (.not. EqualRealNos( minval(W2Data%data4d%WvDir2),REAL(-180,SiKi))) .OR. (.not. EqualRealNos( maxval(W2Data%data4d%WvDir2),REAL(180,SiKi))) ) THEN + CALL SetErrStat( ErrID_Fatal,' Both wave directions in the WAMIT output file '//TRIM(W2Data%Filename)//' must go from exactly -180 deg to +180 deg.',ErrStat,ErrMsg,RoutineName) + END IF + ELSE + ! No data. This is a catastrophic issue. We should not have called this routine without data that is usable for the MnDrift calculation + CALL SetErrStat( ErrID_Fatal, ' Second-order wave-load calculation called without data.',ErrStat,ErrMsg,RoutineName) + END IF + ELSE IF ( InitInp%PtfmYMod == 0) THEN ! Only need to cover the range of incident wave headings + IF ( W2Data%DataIs3D ) THEN + CALL CheckWvHdg(InitInp,W2Data%Data3D%NumWvDir1,W2Data%data3d%WvDir1,'first',ErrStatTmp,ErrMsgTmp) + CALL SetErrStat(ErrStatTmp,' WAMIT output file '//TRIM(W2Data%Filename)//ErrMsgTmp,ErrStat,ErrMsg,RoutineName) + CALL CheckWvHdg(InitInp,W2Data%Data3D%NumWvDir2,W2Data%data3d%WvDir2,'second',ErrStatTmp,ErrMsgTmp) + CALL SetErrStat(ErrStatTmp,' WAMIT output file '//TRIM(W2Data%Filename)//ErrMsgTmp,ErrStat,ErrMsg,RoutineName) + ELSE IF ( W2Data%DataIs4D ) THEN + CALL CheckWvHdg(InitInp,W2Data%Data4D%NumWvDir1,W2Data%data4D%WvDir1,'first',ErrStatTmp,ErrMsgTmp) + CALL SetErrStat(ErrStatTmp,' WAMIT output file '//TRIM(W2Data%Filename)//ErrMsgTmp,ErrStat,ErrMsg,RoutineName) + CALL CheckWvHdg(InitInp,W2Data%Data4D%NumWvDir2,W2Data%data4D%WvDir2,'second',ErrStatTmp,ErrMsgTmp) + CALL SetErrStat(ErrStatTmp,' WAMIT output file '//TRIM(W2Data%Filename)//ErrMsgTmp,ErrStat,ErrMsg,RoutineName) + ELSE + ! No data. This is a catastrophic issue. We should not have called this routine without data that is usable for the MnDrift calculation + CALL SetErrStat( ErrID_Fatal, ' Second-order wave-load calculation called without data.',ErrStat,ErrMsg,RoutineName) + END IF ENDIF RETURN @@ -5076,30 +5380,43 @@ SUBROUTINE CheckWAMIT2WvHdgSumData(InitInp,W2Data,ErrStat,ErrMsg) ErrStat = ErrID_None ErrMsg = "" - IF ( W2Data%DataIs4D ) THEN - CALL CheckWvHdg(InitInp,W2Data%Data4D%NumWvDir1,W2Data%data4D%WvDir1,'first',ErrStatTmp,ErrMsgTmp) - CALL SetErrStat(ErrStatTmp,' WAMIT output file '//TRIM(W2Data%Filename)//ErrMsgTmp,ErrStat,ErrMsg,RoutineName) - CALL CheckWvHdg(InitInp,W2Data%Data4D%NumWvDir2,W2Data%data4D%WvDir2,'second',ErrStatTmp,ErrMsgTmp) - CALL SetErrStat(ErrStatTmp,' WAMIT output file '//TRIM(W2Data%Filename)//ErrMsgTmp,ErrStat,ErrMsg,RoutineName) - ELSE - ! No data. This is a catastrophic issue. We should not have called this routine without data that is usable for the MnDrift calculation - CALL SetErrStat( ErrID_Fatal, ' Second-order wave-load calculation called without data.',ErrStat,ErrMsg,RoutineName) - ENDIF + IF ( InitInp%PtfmYMod == 1 ) THEN ! Need to cover -180 deg to 180 deg + IF ( W2Data%DataIs4D ) THEN + IF ( (.not. EqualRealNos( minval(W2Data%data4d%WvDir1),REAL(-180,SiKi))) .OR. (.not. EqualRealNos( maxval(W2Data%data4d%WvDir1),REAL(180,SiKi))) ) THEN + CALL SetErrStat( ErrID_Fatal,' Both wave directions in the WAMIT output file '//TRIM(W2Data%Filename)//' must go from exactly -180 deg to +180 deg.',ErrStat,ErrMsg,RoutineName) + END IF + IF ( (.not. EqualRealNos( minval(W2Data%data4d%WvDir2),REAL(-180,SiKi))) .OR. (.not. EqualRealNos( maxval(W2Data%data4d%WvDir2),REAL(180,SiKi))) ) THEN + CALL SetErrStat( ErrID_Fatal,' Both wave directions in the WAMIT output file '//TRIM(W2Data%Filename)//' must go from exactly -180 deg to +180 deg.',ErrStat,ErrMsg,RoutineName) + END IF + ELSE + ! No data. This is a catastrophic issue. We should not have called this routine without data that is usable for the MnDrift calculation + CALL SetErrStat( ErrID_Fatal, ' Second-order wave-load calculation called without data.',ErrStat,ErrMsg,RoutineName) + END IF + ELSE IF ( InitInp%PtfmYMod == 0) THEN ! Only need to cover the range of incident wave headings + IF ( W2Data%DataIs4D ) THEN + CALL CheckWvHdg(InitInp,W2Data%Data4D%NumWvDir1,W2Data%data4D%WvDir1,'first',ErrStatTmp,ErrMsgTmp) + CALL SetErrStat(ErrStatTmp,' WAMIT output file '//TRIM(W2Data%Filename)//ErrMsgTmp,ErrStat,ErrMsg,RoutineName) + CALL CheckWvHdg(InitInp,W2Data%Data4D%NumWvDir2,W2Data%data4D%WvDir2,'second',ErrStatTmp,ErrMsgTmp) + CALL SetErrStat(ErrStatTmp,' WAMIT output file '//TRIM(W2Data%Filename)//ErrMsgTmp,ErrStat,ErrMsg,RoutineName) + ELSE + ! No data. This is a catastrophic issue. We should not have called this routine without data that is usable for the MnDrift calculation + CALL SetErrStat( ErrID_Fatal, ' Second-order wave-load calculation called without data.',ErrStat,ErrMsg,RoutineName) + END IF + END IF RETURN END SUBROUTINE CheckWAMIT2WvHdgSumData SUBROUTINE CheckWvHdg(InitInp,NumWAMITWvDir,WAMITWvDir,WvDirName,ErrStat,ErrMsg) - TYPE(WAMIT2_InitInputType), INTENT(IN ) :: InitInp !< Input data for initialization routine + TYPE(WAMIT2_InitInputType), INTENT(IN ) :: InitInp INTEGER(IntKi), INTENT(IN ) :: NumWAMITWvDir REAL(SiKi), INTENT(IN ) :: WAMITWvDir(:) CHARACTER(*), INTENT(IN ) :: WvDirName INTEGER(IntKi), INTENT( OUT) :: ErrStat CHARACTER(*), INTENT( OUT) :: ErrMsg - - REAL(ReKi), PARAMETER :: WvDirTol = 0.001 ! deg - REAL(ReKi) :: RotateZdegOffset + REAL(SiKi) :: RotateZdegOffset,MinAllowedWvDir,MaxAllowedWvDir,unusedReal + INTEGER(IntKi) :: I ErrStat = ErrID_None ErrMsg = "" @@ -5110,38 +5427,79 @@ SUBROUTINE CheckWvHdg(InitInp,NumWAMITWvDir,WAMITWvDir,WvDirName,ErrStat,ErrMsg) 'It cannot be used with multidirectional waves. Set WaveDirMod to 0 to use this file.', & ErrStat,ErrMsg,'') ELSE - ! See Known Issues #1 at the top of this file. There may be problems if the data spans the +/- Pi boundary. For - ! now (since time is limited) we will issue a warning if any of the wave directions for multidirectional waves - ! or data from the WAMIT file for the wavedirections is close to the +/-pi boundary (>150 degrees, <-150 degrees), - ! we will issue a warning. - IF ( (InitInp%WaveField%WaveDirMin > 150.0_SiKi) .OR. (InitInp%WaveField%WaveDirMax < -150.0_SiKi) .OR. & - (minval(WAMITWvDir) > 150.0_SiKi) .OR. (maxval(WAMITWvDir) < -150.0_SiKi)) THEN - CALL SetErrStat( ErrID_Warn,' There may be issues with how the wave direction data is handled when the wave '// & - 'direction of interest is near the +/- 180 direction. This is a known issue with '// & - 'the WAMIT2 module that has not yet been addressed.',ErrStat,ErrMsg,'') - ENDIF - - if (InitInp%NBodyMod==2) then ! Need to account for PtfmRefztRot (the current WAMIT2 module can only contain one body in this case) + ! Need to account for PtfmRefztRot (the current WAMIT2 module can only contain one body in this case) + IF (InitInp%NBodyMod==2) THEN RotateZdegOffset = InitInp%PtfmRefztRot(1)*R2D - else + ELSE RotateZdegOffset = 0.0 - end if - - ! Now check the limits for the wave direction - ! --> FIXME: sometime fix this to handle the above case. See Known Issue #1 at top of file. - IF ( (InitInp%WaveField%WaveDirMin-RotateZdegOffset) < (MINVAL(WAMITWvDir)-WvDirTol) ) THEN - CALL SetErrStat( ErrID_Fatal,' does not contain the minimum wave direction required of '//TRIM(Num2LStr(InitInp%WaveField%WaveDirMin))//' for the '//WvDirName//' wave direction.', & - ErrStat, ErrMsg, '') - ENDIF - IF ( (InitInp%WaveField%WaveDirMax-RotateZdegOffset) > (MAXVAL(WAMITWvDir)+WvDirTol) ) THEN - CALL SetErrStat( ErrID_Fatal,' does not contain the maximum wave direction required of '//TRIM(Num2LStr(InitInp%WaveField%WaveDirMax))//' for the '//WvDirName//' wave direction.', & + END IF + ! Allowed range of incident wave angles in the global frame + MinAllowedWvDir = MINVAL(WAMITWvDir)+RotateZdegOffset+InitInp%PtfmRefY*R2D + MaxAllowedWvDir = MAXVAL(WAMITWvDir)+RotateZdegOffset+InitInp%PtfmRefY*R2D + ! For robustness, check every single incident wave direction + DO I = 0,InitInp%WaveField%NStepWave2 + IF (.NOT. GetAngleInRange(InitInp%WaveField%WaveDirArr(I),MinAllowedWvDir,MaxAllowedWvDir,unusedReal)) THEN + CALL SetErrStat( ErrID_Fatal,' does not contain the range of wave directions covering '//TRIM(Num2LStr(InitInp%WaveField%WaveDirArr(I)))//' deg for the '//WvDirName//' wave direction.', & ErrStat, ErrMsg, '') - ENDIF - + RETURN + END IF + END DO ENDIF END SUBROUTINE CheckWvHdg +FUNCTION GetAngleInRangeR8(inAngle,minAngle,maxAngle,outAngle) + REAL(R8Ki), INTENT(IN ) :: inAngle + REAL(R8Ki), INTENT(IN ) :: minAngle + REAL(R8Ki), INTENT(IN ) :: maxAngle + REAL(R8Ki), INTENT( OUT) :: outAngle + LOGICAL :: GetAngleInRangeR8 + REAL(R8Ki), PARAMETER :: Tol = 0.001 ! deg + + GetAngleInRangeR8 = .FALSE. + if ( ( inAngle > (minAngle-Tol) ) .AND. ( inAngle < (maxAngle+Tol) ) ) then + GetAngleInRangeR8 = .TRUE. + outAngle = inAngle + else if (inAngle < minAngle ) then + outAngle = inAngle + ceiling((minAngle-inAngle)/360.0)*360.0 + if ( outAngle < (maxAngle+Tol) ) then + GetAngleInRangeR8 = .TRUE. + end if + else ! inAngle > maxAngle + outAngle = inAngle - ceiling((inAngle-maxAngle)/360.0)*360.0 + if ( outAngle > (minAngle-Tol) ) then + GetAngleInRangeR8 = .TRUE. + end if + end if + +END FUNCTION GetAngleInRangeR8 + +FUNCTION GetAngleInRangeR4(inAngle,minAngle,maxAngle,outAngle) + REAL(SiKi), INTENT(IN ) :: inAngle + REAL(SiKi), INTENT(IN ) :: minAngle + REAL(SiKi), INTENT(IN ) :: maxAngle + REAL(SiKi), INTENT( OUT) :: outAngle + LOGICAL :: GetAngleInRangeR4 + REAL(SiKi), PARAMETER :: Tol = 0.001 ! deg + + GetAngleInRangeR4 = .FALSE. + if ( ( inAngle > (minAngle-Tol) ) .AND. ( inAngle < (maxAngle+Tol) ) ) then + GetAngleInRangeR4 = .TRUE. + outAngle = inAngle + else if (inAngle < minAngle ) then + outAngle = inAngle + ceiling((minAngle-inAngle)/360.0)*360.0 + if ( outAngle < (maxAngle+Tol) ) then + GetAngleInRangeR4 = .TRUE. + end if + else ! inAngle > maxAngle + outAngle = inAngle - ceiling((inAngle-maxAngle)/360.0)*360.0 + if ( outAngle > (minAngle-Tol) ) then + GetAngleInRangeR4 = .TRUE. + end if + end if + +END FUNCTION GetAngleInRangeR4 + !---------------------------------------------------------------------------------------------------------------------------------- END MODULE WAMIT2 diff --git a/modules/hydrodyn/src/WAMIT2.txt b/modules/hydrodyn/src/WAMIT2.txt index 4461144d7e..c2b948f610 100644 --- a/modules/hydrodyn/src/WAMIT2.txt +++ b/modules/hydrodyn/src/WAMIT2.txt @@ -30,7 +30,10 @@ typedef ^ ^ R8Ki PtfmRefztRo typedef ^ ^ ReKi WAMITULEN - - - "WAMIT unit length scale" - typedef ^ ^ ReKi Gravity - - - "Supplied by Driver: Gravitational acceleration" (m/s^2) -typedef ^ ^ SeaSt_WaveFieldType *WaveField - - - "Pointer to wave field" +typedef ^ ^ SeaSt_WaveFieldType *WaveField - - - "Pointer to wave field" +typedef ^ ^ INTEGER PtfmYMod - - - "Large yaw model" - +typedef ^ ^ ReKi PtfmRefY - - - "Initial reference yaw offset" (rad) +typedef ^ ^ IntKi NExctnHdg - - - "Number of PRP headings/yaw offset evenly distributed over the region [-180, 180) deg to be used when precomputing the wave excitation [only used when PtfmYMod=1 in the HydroDyn driver or in ElastoDyn]" #[note: only one of MnDriff / NewmanApp / DiffQTF can be non-zero typedef ^ ^ INTEGER MnDrift - - - "Calculate the mean drift force {0: no mean drift; [7,8,9,10,11, or 12]: WAMIT file to use}" - @@ -42,16 +45,12 @@ typedef ^ ^ LOGICAL NewmanAppF typedef ^ ^ LOGICAL DiffQTFF - - - "Flag indicating the full difference QTF should be calculated" - typedef ^ ^ LOGICAL SumQTFF - - - "Flag indicating the full sum QTF should be calculated" - - - - # ..... Misc/Optimization variables................................................................................................. # Define any data that are used only for efficiency purposes (these variables are not associated with time): # e.g. indices for searching in an array, large arrays that are local variables in any routine called multiple times, etc. typedef ^ MiscVarType INTEGER LastIndWave : - - "Index for last interpolation step of 2nd order forces" - -typedef ^ ^ ReKi F_Waves2 {:} - - "2nd order force from this timestep" - - - +typedef ^ ^ ReKi F_Waves2 {:} - - "2nd order force from this timestep" - +typedef ^ ^ SeaSt_WaveField_MiscVarType WaveField_m - - - "misc var information from the SeaState Interpolation module" - # ..... Parameters ................................................................................................................ # Define parameters here: @@ -60,8 +59,9 @@ typedef ^ ^ ReKi F_Waves2 typedef ^ ParameterType INTEGER NBody - - - "[>=1; only used when PotMod=1. If NBodyMod=1, the WAMIT data contains a vector of size 6*NBody x 1 and matrices of size 6*NBody x 6*NBody; if NBodyMod>1, there are NBody sets of WAMIT data each with a vector of size 6 x 1 and matrices of size 6 x 6]" - typedef ^ ^ INTEGER NBodyMod - - - "Body coupling model {1: include coupling terms between each body and NBody in HydroDyn equals NBODY in WAMIT, 2: neglect coupling terms between each body and NBODY=1 with XBODY=0 in WAMIT, 3: Neglect coupling terms between each body and NBODY=1 with XBODY=/0 in WAMIT} (switch) [only used when PotMod=1]" - -#The 2nd order force time series -typedef ^ ^ SiKi WaveExctn2 {:}{:} - - "Time series of the resulting 2nd order force (first index is timestep, second index is load component)" (N) +#The 2nd order force time series grid +typedef ^ ^ SiKi WaveExctn2Grid {:}{:}{:}{:}{:} - - "Grid of time series of the resulting 2nd order force (Index 1: Time, Index 2: x, Index 3: y, Index 4: platform heading, and Index 5: load component)" (N) +typedef ^ ^ SeaSt_WaveField_ParameterType Exctn2GridParams - - - "Parameters of WaveExctn2Grid" - #Flags set for dimensions to use with each method (MnDrift, NewmanApp, etc). These are stored by method because .8 files that can be used in MnDrift or NewmanApp don't have some of the dimensions. typedef ^ ^ LOGICAL MnDriftDims {6} - - "Flags for which dimensions to calculate in MnDrift calculations" - @@ -73,8 +73,8 @@ typedef ^ ^ LOGICAL MnDriftF typedef ^ ^ LOGICAL NewmanAppF - - - "Flag indicating Newman approximation should be calculated" - typedef ^ ^ LOGICAL DiffQTFF - - - "Flag indicating the full difference QTF should be calculated" - typedef ^ ^ LOGICAL SumQTFF - - - "Flag indicating the full sum QTF should be calculated" - - - +typedef ^ ^ INTEGER PtfmYMod - - - "Large yaw model" - +typedef ^ ^ INTEGER NExctnHdg - - - "Number of PRP headings/yaw offset evenly distributed over the region [-180, 180) deg to be used when precomputing the wave excitation [only used when PtfmYMod=1 in the HydroDyn driver or in ElastoDyn]" # ..... Outputs ................................................................................................................... # Define outputs that are contained on the mesh here: diff --git a/modules/hydrodyn/src/WAMIT2_Types.f90 b/modules/hydrodyn/src/WAMIT2_Types.f90 index c2b6e5baeb..11ccaf43c7 100644 --- a/modules/hydrodyn/src/WAMIT2_Types.f90 +++ b/modules/hydrodyn/src/WAMIT2_Types.f90 @@ -48,6 +48,9 @@ MODULE WAMIT2_Types REAL(ReKi) :: WAMITULEN = 0.0_ReKi !< WAMIT unit length scale [-] REAL(ReKi) :: Gravity = 0.0_ReKi !< Supplied by Driver: Gravitational acceleration [(m/s^2)] TYPE(SeaSt_WaveFieldType) , POINTER :: WaveField => NULL() !< Pointer to wave field [-] + INTEGER(IntKi) :: PtfmYMod = 0_IntKi !< Large yaw model [-] + REAL(ReKi) :: PtfmRefY = 0.0_ReKi !< Initial reference yaw offset [(rad)] + INTEGER(IntKi) :: NExctnHdg = 0_IntKi !< Number of PRP headings/yaw offset evenly distributed over the region [-180, 180) deg to be used when precomputing the wave excitation [only used when PtfmYMod=1 in the HydroDyn driver or in ElastoDyn] [-] INTEGER(IntKi) :: MnDrift = 0_IntKi !< Calculate the mean drift force {0: no mean drift; [7,8,9,10,11, or 12]: WAMIT file to use} [-] INTEGER(IntKi) :: NewmanApp = 0_IntKi !< Slow drift forces computed with Newman approximation from WAMIT file:{0: No slow drift; [7,8,9,10,11, or 12]: WAMIT file to use} [-] INTEGER(IntKi) :: DiffQTF = 0_IntKi !< Full Difference-Frequency forces computed with full QTF's from WAMIT file: {0: No diff-QTF; [10,11, or 12]: WAMIT file to use} [-] @@ -62,13 +65,15 @@ MODULE WAMIT2_Types TYPE, PUBLIC :: WAMIT2_MiscVarType INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: LastIndWave !< Index for last interpolation step of 2nd order forces [-] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: F_Waves2 !< 2nd order force from this timestep [-] + TYPE(SeaSt_WaveField_MiscVarType) :: WaveField_m !< misc var information from the SeaState Interpolation module [-] END TYPE WAMIT2_MiscVarType ! ======================= ! ========= WAMIT2_ParameterType ======= TYPE, PUBLIC :: WAMIT2_ParameterType INTEGER(IntKi) :: NBody = 0_IntKi !< [>=1; only used when PotMod=1. If NBodyMod=1, the WAMIT data contains a vector of size 6*NBody x 1 and matrices of size 6*NBody x 6*NBody; if NBodyMod>1, there are NBody sets of WAMIT data each with a vector of size 6 x 1 and matrices of size 6 x 6] [-] INTEGER(IntKi) :: NBodyMod = 0_IntKi !< Body coupling model {1: include coupling terms between each body and NBody in HydroDyn equals NBODY in WAMIT, 2: neglect coupling terms between each body and NBODY=1 with XBODY=0 in WAMIT, 3: Neglect coupling terms between each body and NBODY=1 with XBODY=/0 in WAMIT} (switch) [only used when PotMod=1] [-] - REAL(SiKi) , DIMENSION(:,:), ALLOCATABLE :: WaveExctn2 !< Time series of the resulting 2nd order force (first index is timestep, second index is load component) [(N)] + REAL(SiKi) , DIMENSION(:,:,:,:,:), ALLOCATABLE :: WaveExctn2Grid !< Grid of time series of the resulting 2nd order force (Index 1: Time, Index 2: x, Index 3: y, Index 4: platform heading, and Index 5: load component) [(N)] + TYPE(SeaSt_WaveField_ParameterType) :: Exctn2GridParams !< Parameters of WaveExctn2Grid [-] LOGICAL , DIMENSION(1:6) :: MnDriftDims = .false. !< Flags for which dimensions to calculate in MnDrift calculations [-] LOGICAL , DIMENSION(1:6) :: NewmanAppDims = .false. !< Flags for which dimensions to calculate in NewmanApp calculations [-] LOGICAL , DIMENSION(1:6) :: DiffQTFDims = .false. !< Flags for which dimensions to calculate in DiffQTF calculations [-] @@ -77,6 +82,8 @@ MODULE WAMIT2_Types LOGICAL :: NewmanAppF = .false. !< Flag indicating Newman approximation should be calculated [-] LOGICAL :: DiffQTFF = .false. !< Flag indicating the full difference QTF should be calculated [-] LOGICAL :: SumQTFF = .false. !< Flag indicating the full sum QTF should be calculated [-] + INTEGER(IntKi) :: PtfmYMod = 0_IntKi !< Large yaw model [-] + INTEGER(IntKi) :: NExctnHdg = 0_IntKi !< Number of PRP headings/yaw offset evenly distributed over the region [-180, 180) deg to be used when precomputing the wave excitation [only used when PtfmYMod=1 in the HydroDyn driver or in ElastoDyn] [-] END TYPE WAMIT2_ParameterType ! ======================= ! ========= WAMIT2_OutputType ======= @@ -153,6 +160,9 @@ subroutine WAMIT2_CopyInitInput(SrcInitInputData, DstInitInputData, CtrlCode, Er DstInitInputData%WAMITULEN = SrcInitInputData%WAMITULEN DstInitInputData%Gravity = SrcInitInputData%Gravity DstInitInputData%WaveField => SrcInitInputData%WaveField + DstInitInputData%PtfmYMod = SrcInitInputData%PtfmYMod + DstInitInputData%PtfmRefY = SrcInitInputData%PtfmRefY + DstInitInputData%NExctnHdg = SrcInitInputData%NExctnHdg DstInitInputData%MnDrift = SrcInitInputData%MnDrift DstInitInputData%NewmanApp = SrcInitInputData%NewmanApp DstInitInputData%DiffQTF = SrcInitInputData%DiffQTF @@ -210,6 +220,9 @@ subroutine WAMIT2_PackInitInput(RF, Indata) call SeaSt_WaveField_PackSeaSt_WaveFieldType(RF, InData%WaveField) end if end if + call RegPack(RF, InData%PtfmYMod) + call RegPack(RF, InData%PtfmRefY) + call RegPack(RF, InData%NExctnHdg) call RegPack(RF, InData%MnDrift) call RegPack(RF, InData%NewmanApp) call RegPack(RF, InData%DiffQTF) @@ -259,6 +272,9 @@ subroutine WAMIT2_UnPackInitInput(RF, OutData) else OutData%WaveField => null() end if + call RegUnpack(RF, OutData%PtfmYMod); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%PtfmRefY); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%NExctnHdg); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%MnDrift); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%NewmanApp); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%DiffQTF); if (RegCheckErr(RF, RoutineName)) return @@ -277,6 +293,7 @@ subroutine WAMIT2_CopyMisc(SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg) character(*), intent( out) :: ErrMsg integer(B8Ki) :: LB(1), UB(1) integer(IntKi) :: ErrStat2 + character(ErrMsgLen) :: ErrMsg2 character(*), parameter :: RoutineName = 'WAMIT2_CopyMisc' ErrStat = ErrID_None ErrMsg = '' @@ -304,12 +321,17 @@ subroutine WAMIT2_CopyMisc(SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg) end if DstMiscData%F_Waves2 = SrcMiscData%F_Waves2 end if + call SeaSt_WaveField_CopyMisc(SrcMiscData%WaveField_m, DstMiscData%WaveField_m, CtrlCode, ErrStat2, ErrMsg2) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + if (ErrStat >= AbortErrLev) return end subroutine subroutine WAMIT2_DestroyMisc(MiscData, ErrStat, ErrMsg) type(WAMIT2_MiscVarType), intent(inout) :: MiscData integer(IntKi), intent( out) :: ErrStat character(*), intent( out) :: ErrMsg + integer(IntKi) :: ErrStat2 + character(ErrMsgLen) :: ErrMsg2 character(*), parameter :: RoutineName = 'WAMIT2_DestroyMisc' ErrStat = ErrID_None ErrMsg = '' @@ -319,6 +341,8 @@ subroutine WAMIT2_DestroyMisc(MiscData, ErrStat, ErrMsg) if (allocated(MiscData%F_Waves2)) then deallocate(MiscData%F_Waves2) end if + call SeaSt_WaveField_DestroyMisc(MiscData%WaveField_m, ErrStat2, ErrMsg2) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) end subroutine subroutine WAMIT2_PackMisc(RF, Indata) @@ -328,6 +352,7 @@ subroutine WAMIT2_PackMisc(RF, Indata) if (RF%ErrStat >= AbortErrLev) return call RegPackAlloc(RF, InData%LastIndWave) call RegPackAlloc(RF, InData%F_Waves2) + call SeaSt_WaveField_PackMisc(RF, InData%WaveField_m) if (RegCheckErr(RF, RoutineName)) return end subroutine @@ -341,6 +366,7 @@ subroutine WAMIT2_UnPackMisc(RF, OutData) if (RF%ErrStat /= ErrID_None) return call RegUnpackAlloc(RF, OutData%LastIndWave); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%F_Waves2); if (RegCheckErr(RF, RoutineName)) return + call SeaSt_WaveField_UnpackMisc(RF, OutData%WaveField_m) ! WaveField_m end subroutine subroutine WAMIT2_CopyParam(SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg) @@ -349,25 +375,29 @@ subroutine WAMIT2_CopyParam(SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMs integer(IntKi), intent(in ) :: CtrlCode integer(IntKi), intent( out) :: ErrStat character(*), intent( out) :: ErrMsg - integer(B8Ki) :: LB(2), UB(2) + integer(B8Ki) :: LB(5), UB(5) integer(IntKi) :: ErrStat2 + character(ErrMsgLen) :: ErrMsg2 character(*), parameter :: RoutineName = 'WAMIT2_CopyParam' ErrStat = ErrID_None ErrMsg = '' DstParamData%NBody = SrcParamData%NBody DstParamData%NBodyMod = SrcParamData%NBodyMod - if (allocated(SrcParamData%WaveExctn2)) then - LB(1:2) = lbound(SrcParamData%WaveExctn2, kind=B8Ki) - UB(1:2) = ubound(SrcParamData%WaveExctn2, kind=B8Ki) - if (.not. allocated(DstParamData%WaveExctn2)) then - allocate(DstParamData%WaveExctn2(LB(1):UB(1),LB(2):UB(2)), stat=ErrStat2) + if (allocated(SrcParamData%WaveExctn2Grid)) then + LB(1:5) = lbound(SrcParamData%WaveExctn2Grid, kind=B8Ki) + UB(1:5) = ubound(SrcParamData%WaveExctn2Grid, kind=B8Ki) + if (.not. allocated(DstParamData%WaveExctn2Grid)) then + allocate(DstParamData%WaveExctn2Grid(LB(1):UB(1),LB(2):UB(2),LB(3):UB(3),LB(4):UB(4),LB(5):UB(5)), stat=ErrStat2) if (ErrStat2 /= 0) then - call SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%WaveExctn2.', ErrStat, ErrMsg, RoutineName) + call SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%WaveExctn2Grid.', ErrStat, ErrMsg, RoutineName) return end if end if - DstParamData%WaveExctn2 = SrcParamData%WaveExctn2 + DstParamData%WaveExctn2Grid = SrcParamData%WaveExctn2Grid end if + call SeaSt_WaveField_CopyParam(SrcParamData%Exctn2GridParams, DstParamData%Exctn2GridParams, CtrlCode, ErrStat2, ErrMsg2) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + if (ErrStat >= AbortErrLev) return DstParamData%MnDriftDims = SrcParamData%MnDriftDims DstParamData%NewmanAppDims = SrcParamData%NewmanAppDims DstParamData%DiffQTFDims = SrcParamData%DiffQTFDims @@ -376,18 +406,24 @@ subroutine WAMIT2_CopyParam(SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMs DstParamData%NewmanAppF = SrcParamData%NewmanAppF DstParamData%DiffQTFF = SrcParamData%DiffQTFF DstParamData%SumQTFF = SrcParamData%SumQTFF + DstParamData%PtfmYMod = SrcParamData%PtfmYMod + DstParamData%NExctnHdg = SrcParamData%NExctnHdg end subroutine subroutine WAMIT2_DestroyParam(ParamData, ErrStat, ErrMsg) type(WAMIT2_ParameterType), intent(inout) :: ParamData integer(IntKi), intent( out) :: ErrStat character(*), intent( out) :: ErrMsg + integer(IntKi) :: ErrStat2 + character(ErrMsgLen) :: ErrMsg2 character(*), parameter :: RoutineName = 'WAMIT2_DestroyParam' ErrStat = ErrID_None ErrMsg = '' - if (allocated(ParamData%WaveExctn2)) then - deallocate(ParamData%WaveExctn2) + if (allocated(ParamData%WaveExctn2Grid)) then + deallocate(ParamData%WaveExctn2Grid) end if + call SeaSt_WaveField_DestroyParam(ParamData%Exctn2GridParams, ErrStat2, ErrMsg2) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) end subroutine subroutine WAMIT2_PackParam(RF, Indata) @@ -397,7 +433,8 @@ subroutine WAMIT2_PackParam(RF, Indata) if (RF%ErrStat >= AbortErrLev) return call RegPack(RF, InData%NBody) call RegPack(RF, InData%NBodyMod) - call RegPackAlloc(RF, InData%WaveExctn2) + call RegPackAlloc(RF, InData%WaveExctn2Grid) + call SeaSt_WaveField_PackParam(RF, InData%Exctn2GridParams) call RegPack(RF, InData%MnDriftDims) call RegPack(RF, InData%NewmanAppDims) call RegPack(RF, InData%DiffQTFDims) @@ -406,6 +443,8 @@ subroutine WAMIT2_PackParam(RF, Indata) call RegPack(RF, InData%NewmanAppF) call RegPack(RF, InData%DiffQTFF) call RegPack(RF, InData%SumQTFF) + call RegPack(RF, InData%PtfmYMod) + call RegPack(RF, InData%NExctnHdg) if (RegCheckErr(RF, RoutineName)) return end subroutine @@ -413,13 +452,14 @@ subroutine WAMIT2_UnPackParam(RF, OutData) type(RegFile), intent(inout) :: RF type(WAMIT2_ParameterType), intent(inout) :: OutData character(*), parameter :: RoutineName = 'WAMIT2_UnPackParam' - integer(B8Ki) :: LB(2), UB(2) + integer(B8Ki) :: LB(5), UB(5) integer(IntKi) :: stat logical :: IsAllocAssoc if (RF%ErrStat /= ErrID_None) return call RegUnpack(RF, OutData%NBody); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%NBodyMod); if (RegCheckErr(RF, RoutineName)) return - call RegUnpackAlloc(RF, OutData%WaveExctn2); if (RegCheckErr(RF, RoutineName)) return + call RegUnpackAlloc(RF, OutData%WaveExctn2Grid); if (RegCheckErr(RF, RoutineName)) return + call SeaSt_WaveField_UnpackParam(RF, OutData%Exctn2GridParams) ! Exctn2GridParams call RegUnpack(RF, OutData%MnDriftDims); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%NewmanAppDims); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%DiffQTFDims); if (RegCheckErr(RF, RoutineName)) return @@ -428,6 +468,8 @@ subroutine WAMIT2_UnPackParam(RF, OutData) call RegUnpack(RF, OutData%NewmanAppF); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%DiffQTFF); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%SumQTFF); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%PtfmYMod); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%NExctnHdg); if (RegCheckErr(RF, RoutineName)) return end subroutine subroutine WAMIT2_CopyOutput(SrcOutputData, DstOutputData, CtrlCode, ErrStat, ErrMsg) diff --git a/modules/hydrodyn/src/WAMIT_Interp.f90 b/modules/hydrodyn/src/WAMIT_Interp.f90 index 585867a33a..90a95e3432 100644 --- a/modules/hydrodyn/src/WAMIT_Interp.f90 +++ b/modules/hydrodyn/src/WAMIT_Interp.f90 @@ -58,7 +58,7 @@ MODULE WAMIT_Interp !! 1. It is complex valued. The values represent the second order wave force as calculated by WAMIT. !! 2. The dimenions of DataSet2D are Frequency1 (positive valued) and Wave Direction (degrees). !! 3. The wave direction requested might be between end points of wave direction dimension (ie. at 179 degrees when -!! WvDir1(1)=175, WvDir(Dims(3))=-175) +!! WvDir1(1)=175, WvDir(Dims(3))=-175) <- This is no longer the case. See comment below. !! 4. The arrays WvFreq1 and WvDir1 will give the values for each dimension that correspond to each index of DataSet2D. !! 5. The data is not necessarily equally spaced in any direction: ie. WvFreq1 may not have uniform spacing between points. !! 6. If a point is requested, it can be assumed that it lies within DataSet2D (this is checked before calling this subroutine) @@ -83,6 +83,7 @@ SUBROUTINE WAMIT_Interp2D_Cplx( InCoord, DataSet2D, WvFreq1, WvDir1, LastIndex, ! Local variables REAL(SiKi) :: Coords(2) !< coordinates with wave directions converted to range [-180, 180) + INTEGER(IntKi) :: i !< generic counter INTEGER(IntKi) :: n(2) !< number of points in WvFreq1 and WvDir1, and WvDir2 INTEGER(IntKi) :: Indx_Lo(2) !< index associated with lower bound of dimension 1,2 where val(Indx_lo(i)) <= InCoord(i) <= val(Indx_hi(i)) @@ -106,10 +107,14 @@ SUBROUTINE WAMIT_Interp2D_Cplx( InCoord, DataSet2D, WvFreq1, WvDir1, LastIndex, ! find the indices into the arrays representing coordinates of each dimension: Coords = InCoord - - ! make sure these requested degrees fall in the range -180 <= Coords(2) < 180 - Coords(2) = MODULO( Coords(2), 360.0_SiKi ) - IF ( Coords(2) >= 180.0_SiKi ) Coords(2) = Coords(2) - 360.0_SiKi + + ! The periodic "looping" behavior of the wave direction interpolation has been commented out due to the potential for large error without warning. + ! With the WAMIT2 module updated to handle ranges of input wave directions crossing +/-180 deg, it is now the responsibility of the calling code + ! to make sure the wave heading interpolation point is strictly in range. + + ! ! make sure these requested degrees fall in the range -180 <= Coords(2) < 180 + ! Coords(2) = MODULO( Coords(2), 360.0_SiKi ) + ! IF ( Coords(2) >= 180.0_SiKi ) Coords(2) = Coords(2) - 360.0_SiKi CALL LocateStp( Coords(1), WvFreq1, LastIndex(1), n(1) ) CALL LocateStp( Coords(2), WvDir1, LastIndex(2), n(2) ) @@ -118,23 +123,24 @@ SUBROUTINE WAMIT_Interp2D_Cplx( InCoord, DataSet2D, WvFreq1, WvDir1, LastIndex, ! WvFreq1 (indx 1) - IF (Indx_Lo(1) == 0) THEN - Indx_Lo(1) = 1 - ELSEIF (Indx_Lo(1) == n(1) ) THEN - Indx_Lo(1) = max( n(1) - 1, 1 ) ! make sure it's a valid index - END IF - Indx_Hi(1) = min( Indx_Lo(1) + 1 , n(1) ) ! make sure it's a valid index - - - ! WvDir1 (indx 2) [use modular arithmetic] - IF (Indx_Lo(2) == 0) THEN - Indx_Hi(2) = 1 - Indx_Lo(2) = n(2) - ELSEIF (Indx_Lo(2) == n(2) ) THEN - Indx_Hi(2) = 1 - ELSE - Indx_Hi(2) = min( Indx_Lo(2) + 1, n(2) ) ! make sure it's a valid index - END IF + DO i = 1,2 + IF (Indx_Lo(i) == 0) THEN + Indx_Lo(i) = 1 + ELSEIF (Indx_Lo(i) == n(i) ) THEN + Indx_Lo(i) = max( n(i) - 1, 1 ) ! make sure it's a valid index + END IF + Indx_Hi(i) = min( Indx_Lo(i) + 1 , n(i) ) ! make sure it's a valid index + END DO + + ! ! WvDir1 (indx 2) [use modular arithmetic] + ! IF (Indx_Lo(2) == 0) THEN + ! Indx_Hi(2) = 1 + ! Indx_Lo(2) = n(2) + ! ELSEIF (Indx_Lo(2) == n(2) ) THEN + ! Indx_Hi(2) = 1 + ! ELSE + ! Indx_Hi(2) = min( Indx_Lo(2) + 1, n(2) ) ! make sure it's a valid index + ! END IF ! calculate the positions of all dimensions: @@ -145,14 +151,14 @@ SUBROUTINE WAMIT_Interp2D_Cplx( InCoord, DataSet2D, WvFreq1, WvDir1, LastIndex, pos_Hi(2) = WvDir1(Indx_Hi(2)) - ! angles have to be adjusted so that pos_Lo(2) <= Coords(2) <= pos_Hi(2) - IF ( Indx_Hi(2) == 1 .AND. n(2) > 1 ) THEN ! we're looping around the array [periodic] - IF ( pos_Lo(2) < Coords(2) ) THEN - pos_Hi(2) = pos_Hi(2) + 360.0_SiKi - ELSEIF ( pos_Lo(2) /= Coords(2) ) THEN !bjj: I think it's okay if we don't use equalRealNos here - pos_Lo(2) = pos_Lo(2) - 360.0_SiKi - END IF - END IF + ! ! angles have to be adjusted so that pos_Lo(2) <= Coords(2) <= pos_Hi(2) + ! IF ( Indx_Hi(2) == 1 .AND. n(2) > 1 ) THEN ! we're looping around the array [periodic] + ! IF ( pos_Lo(2) < Coords(2) ) THEN + ! pos_Hi(2) = pos_Hi(2) + 360.0_SiKi + ! ELSEIF ( pos_Lo(2) /= Coords(2) ) THEN !bjj: I think it's okay if we don't use equalRealNos here + ! pos_Lo(2) = pos_Lo(2) - 360.0_SiKi + ! END IF + ! END IF CALL Interp2D_withIndx_Cplx( Coords, DataSet2D, Indx_Lo, Indx_Hi, pos_Lo, pos_Hi, OutForce ) @@ -169,7 +175,7 @@ END SUBROUTINE WAMIT_Interp2D_Cplx !! 1. It is complex valued. The values represent the second order wave force as calculated by WAMIT. !! 2. The dimenions of DataSet3D are Frequency1 (positive valued), Wave Direction1 (degrees), and Wave Direction2 (degrees). !! 3. The wave direction requested might be between end points of wave direction dimension (ie. at 179 degrees when -!! WvDir1(1)=175, WvDir(Dims(3))=-175) +!! WvDir1(1)=175, WvDir(Dims(3))=-175) <- This is no longer the case. See comment below. !! 4. The arrays WvFreq1, WvDir1, and WvDir2, will give the values for each dimension that correspond to each index of DataSet3D. !! 5. The data is not necessarily equally spaced in any direction: ie. WvFreq1 may not have uniform spacing between points. !! 6. If a point is requested, it can be assumed that it lies within DataSet3D (this is checked before calling this subroutine) @@ -221,11 +227,15 @@ SUBROUTINE WAMIT_Interp3D_Cplx( InCoord, DataSet3D, WvFreq1, WvDir1, WvDir2, Las ! find the indices into the arrays representing coordinates of each dimension: Coords = InCoord - - DO i=2,3 ! make sure these requested degrees fall in the range -180 <= Coord(2:3) < 180 - Coords(i) = MODULO( Coords(i), 360.0_SiKi ) - IF ( Coords(i) >= 180.0_SiKi ) Coords(i) = Coords(i) - 360.0_SiKi - END DO + + ! The periodic "looping" behavior of the wave direction interpolation has been commented out due to the potential for large error without warning. + ! With the WAMIT2 module updated to handle ranges of input wave directions crossing +/-180 deg, it is now the responsibility of the calling code + ! to make sure the wave heading interpolation point is strictly in range. + + ! DO i=2,3 ! make sure these requested degrees fall in the range -180 <= Coord(2:3) < 180 + ! Coords(i) = MODULO( Coords(i), 360.0_SiKi ) + ! IF ( Coords(i) >= 180.0_SiKi ) Coords(i) = Coords(i) - 360.0_SiKi + ! END DO CALL LocateStp( Coords(1), WvFreq1, LastIndex(1), n(1) ) CALL LocateStp( Coords(2), WvDir1, LastIndex(2), n(2) ) @@ -235,25 +245,27 @@ SUBROUTINE WAMIT_Interp3D_Cplx( InCoord, DataSet3D, WvFreq1, WvDir1, WvDir2, Las ! WvFreq1 (indx 1) - IF (Indx_Lo(1) == 0) THEN - Indx_Lo(1) = 1 - ELSEIF (Indx_Lo(1) == n(1) ) THEN - Indx_Lo(1) = max( n(1) - 1, 1 ) ! make sure it's a valid index - END IF - Indx_Hi(1) = min( Indx_Lo(1) + 1 , n(1) ) ! make sure it's a valid index - - ! WvDir1, WvDir2 (indx 2,3) [use modular arithmetic] - DO i=2,3 + DO i = 1,3 IF (Indx_Lo(i) == 0) THEN - Indx_Hi(i) = 1 - Indx_Lo(i) = n(i) + Indx_Lo(i) = 1 ELSEIF (Indx_Lo(i) == n(i) ) THEN - Indx_Hi(i) = 1 - ELSE - Indx_Hi(i) = min( Indx_Lo(i) + 1, n(i) ) ! make sure it's a valid index - END IF + Indx_Lo(i) = max( n(i) - 1, 1 ) ! make sure it's a valid index + END IF + Indx_Hi(i) = min( Indx_Lo(i) + 1 , n(i) ) ! make sure it's a valid index END DO + + ! ! WvDir1, WvDir2 (indx 2,3) [use modular arithmetic] + ! DO i=2,3 + ! IF (Indx_Lo(i) == 0) THEN + ! Indx_Hi(i) = 1 + ! Indx_Lo(i) = n(i) + ! ELSEIF (Indx_Lo(i) == n(i) ) THEN + ! Indx_Hi(i) = 1 + ! ELSE + ! Indx_Hi(i) = min( Indx_Lo(i) + 1, n(i) ) ! make sure it's a valid index + ! END IF + ! END DO ! calculate the positions of all dimensions: @@ -267,15 +279,15 @@ SUBROUTINE WAMIT_Interp3D_Cplx( InCoord, DataSet3D, WvFreq1, WvDir1, WvDir2, Las pos_Hi(3) = WvDir2(Indx_Hi(3)) ! angles have to be adjusted so that pos_Lo(i) <= Coords(i) <= pos_Hi(i) - DO i=2,3 - IF ( Indx_Hi(i) == 1 .AND. n(i) > 1 ) THEN ! we're looping around the array [periodic] - IF ( pos_Lo(i) < Coords(i) ) THEN - pos_Hi(i) = pos_Hi(i) + 360.0_SiKi - ELSEIF ( pos_Lo(i) /= Coords(i) ) THEN !bjj: I think it's okay if we don't use equalRealNos here - pos_Lo(i) = pos_Lo(i) - 360.0_SiKi - END IF - END IF - END DO + ! DO i=2,3 + ! IF ( Indx_Hi(i) == 1 .AND. n(i) > 1 ) THEN ! we're looping around the array [periodic] + ! IF ( pos_Lo(i) < Coords(i) ) THEN + ! pos_Hi(i) = pos_Hi(i) + 360.0_SiKi + ! ELSEIF ( pos_Lo(i) /= Coords(i) ) THEN !bjj: I think it's okay if we don't use equalRealNos here + ! pos_Lo(i) = pos_Lo(i) - 360.0_SiKi + ! END IF + ! END IF + ! END DO @@ -293,7 +305,7 @@ END SUBROUTINE WAMIT_Interp3D_Cplx !! 2. The dimenions of DataSet4D are Frequency1 (positive valued), Frequency2 (positive valued), Wave Direction 1 (degrees), !! and Wave Direction 2 (degrees). !! 3. The wave direction requested might be between end points of wave direction dimension (ie. at 179 degrees when -!! WvDir1(1)=175, WvDir(Dims(3))=-175) +!! WvDir1(1)=175, WvDir(Dims(3))=-175) <- This is no longer the case. See comment below. !! 4. The arrays WvFreq1, WvFreq2, WvDir1, and WvDir2 will give the values for each dimension that correspond to !! each index of DataSet4D. !! 5. The data is not necessarily equally spaced in any direction: ie. WvFreq1 may not have uniform spacing between points. @@ -353,10 +365,14 @@ SUBROUTINE WAMIT_Interp4D_Cplx( InCoord, DataSet4D, WvFreq1, WvFreq2, WvDir1, Wv Coords = InCoord - DO i=3,4 ! make sure these requested degrees fall in the range -180 <= Coord(3:4) < 180 - Coords(i) = MODULO( Coords(i), 360.0_SiKi ) - IF ( Coords(i) >= 180.0_SiKi ) Coords(i) = Coords(i) - 360.0_SiKi - END DO + ! The periodic "looping" behavior of the wave direction interpolation has been commented out due to the potential for large error without warning. + ! With the WAMIT2 module updated to handle ranges of input wave directions crossing +/-180 deg, it is now the responsibility of the calling code + ! to make sure the wave heading interpolation point is strictly in range. + + ! DO i=3,4 ! make sure these requested degrees fall in the range -180 <= Coord(3:4) < 180 + ! Coords(i) = MODULO( Coords(i), 360.0_SiKi ) + ! IF ( Coords(i) >= 180.0_SiKi ) Coords(i) = Coords(i) - 360.0_SiKi + ! END DO CALL LocateStp( Coords(1), WvFreq1, LastIndex(1), n(1) ) CALL LocateStp( Coords(2), WvFreq2, LastIndex(2), n(2) ) @@ -367,7 +383,7 @@ SUBROUTINE WAMIT_Interp4D_Cplx( InCoord, DataSet4D, WvFreq1, WvFreq2, WvDir1, Wv ! WvFreq1, WvFreq2 (indx 1, 2) - DO i=1,2 + DO i=1,4 IF (Indx_Lo(i) == 0) THEN Indx_Lo(i) = 1 ELSEIF (Indx_Lo(i) == n(i) ) THEN @@ -378,16 +394,16 @@ SUBROUTINE WAMIT_Interp4D_Cplx( InCoord, DataSet4D, WvFreq1, WvFreq2, WvDir1, Wv ! WvDir1, WvDir2 (indx 3,4) [use modular arithmetic] - DO i=3,4 - IF (Indx_Lo(i) == 0) THEN - Indx_Hi(i) = 1 - Indx_Lo(i) = n(i) - ELSEIF (Indx_Lo(i) == n(i) ) THEN - Indx_Hi(i) = 1 - ELSE - Indx_Hi(i) = min( Indx_Lo(i) + 1, n(i) ) ! make sure it's a valid index - END IF - END DO + ! DO i=3,4 + ! IF (Indx_Lo(i) == 0) THEN + ! Indx_Hi(i) = 1 + ! Indx_Lo(i) = n(i) + ! ELSEIF (Indx_Lo(i) == n(i) ) THEN + ! Indx_Hi(i) = 1 + ! ELSE + ! Indx_Hi(i) = min( Indx_Lo(i) + 1, n(i) ) ! make sure it's a valid index + ! END IF + ! END DO ! calculate the positions of all dimensions: @@ -405,15 +421,15 @@ SUBROUTINE WAMIT_Interp4D_Cplx( InCoord, DataSet4D, WvFreq1, WvFreq2, WvDir1, Wv pos_Hi(4) = WvDir2(Indx_Hi(4)) ! angles have to be adjusted so that pos_Lo(i) <= Coords(i) <= pos_Hi(i) - DO i=3,4 - IF ( Indx_Hi(i) == 1 .AND. n(i) > 1 ) THEN ! we're looping around the array [periodic] - IF ( pos_Lo(i) < Coords(i) ) THEN - pos_Hi(i) = pos_Hi(i) + 360.0_SiKi - ELSEIF ( pos_Lo(i) /= Coords(i) ) THEN !bjj: I think it's okay if we don't use equalRealNos here - pos_Lo(i) = pos_Lo(i) - 360.0_SiKi - END IF - END IF - END DO + ! DO i=3,4 + ! IF ( Indx_Hi(i) == 1 .AND. n(i) > 1 ) THEN ! we're looping around the array [periodic] + ! IF ( pos_Lo(i) < Coords(i) ) THEN + ! pos_Hi(i) = pos_Hi(i) + 360.0_SiKi + ! ELSEIF ( pos_Lo(i) /= Coords(i) ) THEN !bjj: I think it's okay if we don't use equalRealNos here + ! pos_Lo(i) = pos_Lo(i) - 360.0_SiKi + ! END IF + ! END IF + ! END DO diff --git a/modules/hydrodyn/src/WAMIT_Types.f90 b/modules/hydrodyn/src/WAMIT_Types.f90 index 1f1bbd75dd..4878ae1e85 100644 --- a/modules/hydrodyn/src/WAMIT_Types.f90 +++ b/modules/hydrodyn/src/WAMIT_Types.f90 @@ -54,10 +54,13 @@ MODULE WAMIT_Types INTEGER(IntKi) :: ExctnMod = 0_IntKi !< [-] INTEGER(IntKi) :: ExctnDisp = 0_IntKi !< 0: use undisplaced position, 1: use displaced position, 2: use low-pass filtered displaced position) [only used when PotMod=1 and ExctnMod>0] [-] REAL(ReKi) :: ExctnCutOff = 0.0_ReKi !< Cutoff (corner) frequency of the low-pass time-filtered displaced position (Hz) [>0.0] [Hz] + INTEGER(IntKi) :: NExctnHdg = 0_IntKi !< Number of PRP headings/yaw offset evenly distributed over the region [-180, 180) deg to be used when precomputing the wave excitation [only used when PtfmYMod=1 in the HydroDyn driver or in ElastoDyn] [-] REAL(DbKi) :: RdtnTMax = 0.0_R8Ki !< [-] CHARACTER(1024) :: WAMITFile !< [-] TYPE(Conv_Rdtn_InitInputType) :: Conv_Rdtn !< [-] TYPE(SeaSt_WaveFieldType) , POINTER :: WaveField => NULL() !< Pointer to wave field [-] + INTEGER(IntKi) :: PtfmYMod = 0_IntKi !< Large yaw model [-] + REAL(ReKi) :: PtfmRefY = 0.0_ReKi !< Initial reference yaw offset [(rad)] END TYPE WAMIT_InitInputType ! ======================= ! ========= WAMIT_ContinuousStateType ======= @@ -119,19 +122,23 @@ MODULE WAMIT_Types INTEGER(IntKi) :: ExctnMod = 0_IntKi !< [-] INTEGER(IntKi) :: ExctnDisp = 0_IntKi !< 0: use undisplaced position, 1: use displaced position, 2: use low-pass filtered displaced position) [only used when PotMod=1 and ExctnMod>0] [-] REAL(ReKi) :: ExctnCutOff = 0.0_ReKi !< Cutoff (corner) frequency of the low-pass time-filtered displaced position (Hz) [>0.0] [Hz] + INTEGER(IntKi) :: NExctnHdg = 0_IntKi !< Number of PRP headings/yaw offset evenly distributed over the region [-180, 180) deg to be used when precomputing the wave excitation [only used when PtfmYMod=1 in the HydroDyn driver or in ElastoDyn] [-] REAL(ReKi) :: ExctnFiltConst = 0.0_ReKi !< Low-pass time filter constant computed from ExctnCutOff [-] - REAL(SiKi) , DIMENSION(:,:), ALLOCATABLE :: WaveExctn !< [-] - REAL(SiKi) , DIMENSION(:,:,:,:), ALLOCATABLE :: WaveExctnGrid !< WaveExctnGrid dimensions are: 1st: wavetime, 2nd: X, 3rd: Y, 4th: Force component for eac WAMIT Body [-] + REAL(SiKi) , DIMENSION(:,:,:), ALLOCATABLE :: WaveExctn !< [-] + REAL(SiKi) , DIMENSION(:,:,:,:,:), ALLOCATABLE :: WaveExctnGrid !< WaveExctnGrid dimensions are: 1st: wavetime, 2nd: X, 3rd: Y, 4th: PRP Yaw, 5th: Force component for eac WAMIT Body [-] TYPE(Conv_Rdtn_ParameterType) :: Conv_Rdtn !< [-] TYPE(SS_Rad_ParameterType) :: SS_Rdtn !< [-] TYPE(SS_Exc_ParameterType) :: SS_Exctn !< [-] REAL(DbKi) :: DT = 0.0_R8Ki !< [-] TYPE(SeaSt_WaveFieldType) , POINTER :: WaveField => NULL() !< Pointer to wave field [-] + INTEGER(IntKi) :: PtfmYMod = 0_IntKi !< Large yaw model [-] + TYPE(SeaSt_WaveField_ParameterType) :: ExctnGridParams !< Parameters of WaveExctnGrid [-] END TYPE WAMIT_ParameterType ! ======================= ! ========= WAMIT_InputType ======= TYPE, PUBLIC :: WAMIT_InputType TYPE(MeshType) :: Mesh !< Displacements at the WAMIT reference point in the inertial frame [-] + REAL(ReKi) :: PtfmRefY = 0.0_ReKi !< Reference yaw offset [(rad)] END TYPE WAMIT_InputType ! ======================= ! ========= WAMIT_OutputType ======= @@ -246,12 +253,15 @@ subroutine WAMIT_CopyInitInput(SrcInitInputData, DstInitInputData, CtrlCode, Err DstInitInputData%ExctnMod = SrcInitInputData%ExctnMod DstInitInputData%ExctnDisp = SrcInitInputData%ExctnDisp DstInitInputData%ExctnCutOff = SrcInitInputData%ExctnCutOff + DstInitInputData%NExctnHdg = SrcInitInputData%NExctnHdg DstInitInputData%RdtnTMax = SrcInitInputData%RdtnTMax DstInitInputData%WAMITFile = SrcInitInputData%WAMITFile call Conv_Rdtn_CopyInitInput(SrcInitInputData%Conv_Rdtn, DstInitInputData%Conv_Rdtn, CtrlCode, ErrStat2, ErrMsg2) call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) if (ErrStat >= AbortErrLev) return DstInitInputData%WaveField => SrcInitInputData%WaveField + DstInitInputData%PtfmYMod = SrcInitInputData%PtfmYMod + DstInitInputData%PtfmRefY = SrcInitInputData%PtfmRefY end subroutine subroutine WAMIT_DestroyInitInput(InitInputData, ErrStat, ErrMsg) @@ -311,6 +321,7 @@ subroutine WAMIT_PackInitInput(RF, Indata) call RegPack(RF, InData%ExctnMod) call RegPack(RF, InData%ExctnDisp) call RegPack(RF, InData%ExctnCutOff) + call RegPack(RF, InData%NExctnHdg) call RegPack(RF, InData%RdtnTMax) call RegPack(RF, InData%WAMITFile) call Conv_Rdtn_PackInitInput(RF, InData%Conv_Rdtn) @@ -321,6 +332,8 @@ subroutine WAMIT_PackInitInput(RF, Indata) call SeaSt_WaveField_PackSeaSt_WaveFieldType(RF, InData%WaveField) end if end if + call RegPack(RF, InData%PtfmYMod) + call RegPack(RF, InData%PtfmRefY) if (RegCheckErr(RF, RoutineName)) return end subroutine @@ -350,6 +363,7 @@ subroutine WAMIT_UnPackInitInput(RF, OutData) call RegUnpack(RF, OutData%ExctnMod); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%ExctnDisp); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%ExctnCutOff); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%NExctnHdg); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%RdtnTMax); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%WAMITFile); if (RegCheckErr(RF, RoutineName)) return call Conv_Rdtn_UnpackInitInput(RF, OutData%Conv_Rdtn) ! Conv_Rdtn @@ -371,6 +385,8 @@ subroutine WAMIT_UnPackInitInput(RF, OutData) else OutData%WaveField => null() end if + call RegUnpack(RF, OutData%PtfmYMod); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%PtfmRefY); if (RegCheckErr(RF, RoutineName)) return end subroutine subroutine WAMIT_CopyContState(SrcContStateData, DstContStateData, CtrlCode, ErrStat, ErrMsg) @@ -824,7 +840,7 @@ subroutine WAMIT_CopyParam(SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg integer(IntKi), intent(in ) :: CtrlCode integer(IntKi), intent( out) :: ErrStat character(*), intent( out) :: ErrMsg - integer(B8Ki) :: LB(4), UB(4) + integer(B8Ki) :: LB(5), UB(5) integer(IntKi) :: ErrStat2 character(ErrMsgLen) :: ErrMsg2 character(*), parameter :: RoutineName = 'WAMIT_CopyParam' @@ -872,12 +888,13 @@ subroutine WAMIT_CopyParam(SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg DstParamData%ExctnMod = SrcParamData%ExctnMod DstParamData%ExctnDisp = SrcParamData%ExctnDisp DstParamData%ExctnCutOff = SrcParamData%ExctnCutOff + DstParamData%NExctnHdg = SrcParamData%NExctnHdg DstParamData%ExctnFiltConst = SrcParamData%ExctnFiltConst if (allocated(SrcParamData%WaveExctn)) then - LB(1:2) = lbound(SrcParamData%WaveExctn, kind=B8Ki) - UB(1:2) = ubound(SrcParamData%WaveExctn, kind=B8Ki) + LB(1:3) = lbound(SrcParamData%WaveExctn, kind=B8Ki) + UB(1:3) = ubound(SrcParamData%WaveExctn, kind=B8Ki) if (.not. allocated(DstParamData%WaveExctn)) then - allocate(DstParamData%WaveExctn(LB(1):UB(1),LB(2):UB(2)), stat=ErrStat2) + allocate(DstParamData%WaveExctn(LB(1):UB(1),LB(2):UB(2),LB(3):UB(3)), stat=ErrStat2) if (ErrStat2 /= 0) then call SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%WaveExctn.', ErrStat, ErrMsg, RoutineName) return @@ -886,10 +903,10 @@ subroutine WAMIT_CopyParam(SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg DstParamData%WaveExctn = SrcParamData%WaveExctn end if if (allocated(SrcParamData%WaveExctnGrid)) then - LB(1:4) = lbound(SrcParamData%WaveExctnGrid, kind=B8Ki) - UB(1:4) = ubound(SrcParamData%WaveExctnGrid, kind=B8Ki) + LB(1:5) = lbound(SrcParamData%WaveExctnGrid, kind=B8Ki) + UB(1:5) = ubound(SrcParamData%WaveExctnGrid, kind=B8Ki) if (.not. allocated(DstParamData%WaveExctnGrid)) then - allocate(DstParamData%WaveExctnGrid(LB(1):UB(1),LB(2):UB(2),LB(3):UB(3),LB(4):UB(4)), stat=ErrStat2) + allocate(DstParamData%WaveExctnGrid(LB(1):UB(1),LB(2):UB(2),LB(3):UB(3),LB(4):UB(4),LB(5):UB(5)), stat=ErrStat2) if (ErrStat2 /= 0) then call SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%WaveExctnGrid.', ErrStat, ErrMsg, RoutineName) return @@ -908,6 +925,10 @@ subroutine WAMIT_CopyParam(SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg if (ErrStat >= AbortErrLev) return DstParamData%DT = SrcParamData%DT DstParamData%WaveField => SrcParamData%WaveField + DstParamData%PtfmYMod = SrcParamData%PtfmYMod + call SeaSt_WaveField_CopyParam(SrcParamData%ExctnGridParams, DstParamData%ExctnGridParams, CtrlCode, ErrStat2, ErrMsg2) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + if (ErrStat >= AbortErrLev) return end subroutine subroutine WAMIT_DestroyParam(ParamData, ErrStat, ErrMsg) @@ -941,6 +962,8 @@ subroutine WAMIT_DestroyParam(ParamData, ErrStat, ErrMsg) call SS_Exc_DestroyParam(ParamData%SS_Exctn, ErrStat2, ErrMsg2) call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) nullify(ParamData%WaveField) + call SeaSt_WaveField_DestroyParam(ParamData%ExctnGridParams, ErrStat2, ErrMsg2) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) end subroutine subroutine WAMIT_PackParam(RF, Indata) @@ -958,6 +981,7 @@ subroutine WAMIT_PackParam(RF, Indata) call RegPack(RF, InData%ExctnMod) call RegPack(RF, InData%ExctnDisp) call RegPack(RF, InData%ExctnCutOff) + call RegPack(RF, InData%NExctnHdg) call RegPack(RF, InData%ExctnFiltConst) call RegPackAlloc(RF, InData%WaveExctn) call RegPackAlloc(RF, InData%WaveExctnGrid) @@ -972,6 +996,8 @@ subroutine WAMIT_PackParam(RF, Indata) call SeaSt_WaveField_PackSeaSt_WaveFieldType(RF, InData%WaveField) end if end if + call RegPack(RF, InData%PtfmYMod) + call SeaSt_WaveField_PackParam(RF, InData%ExctnGridParams) if (RegCheckErr(RF, RoutineName)) return end subroutine @@ -979,7 +1005,7 @@ subroutine WAMIT_UnPackParam(RF, OutData) type(RegFile), intent(inout) :: RF type(WAMIT_ParameterType), intent(inout) :: OutData character(*), parameter :: RoutineName = 'WAMIT_UnPackParam' - integer(B8Ki) :: LB(4), UB(4) + integer(B8Ki) :: LB(5), UB(5) integer(IntKi) :: stat logical :: IsAllocAssoc integer(B8Ki) :: PtrIdx @@ -994,6 +1020,7 @@ subroutine WAMIT_UnPackParam(RF, OutData) call RegUnpack(RF, OutData%ExctnMod); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%ExctnDisp); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%ExctnCutOff); if (RegCheckErr(RF, RoutineName)) return + call RegUnpack(RF, OutData%NExctnHdg); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%ExctnFiltConst); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%WaveExctn); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%WaveExctnGrid); if (RegCheckErr(RF, RoutineName)) return @@ -1019,6 +1046,8 @@ subroutine WAMIT_UnPackParam(RF, OutData) else OutData%WaveField => null() end if + call RegUnpack(RF, OutData%PtfmYMod); if (RegCheckErr(RF, RoutineName)) return + call SeaSt_WaveField_UnpackParam(RF, OutData%ExctnGridParams) ! ExctnGridParams end subroutine subroutine WAMIT_CopyInput(SrcInputData, DstInputData, CtrlCode, ErrStat, ErrMsg) @@ -1035,6 +1064,7 @@ subroutine WAMIT_CopyInput(SrcInputData, DstInputData, CtrlCode, ErrStat, ErrMsg call MeshCopy(SrcInputData%Mesh, DstInputData%Mesh, CtrlCode, ErrStat2, ErrMsg2 ) call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) if (ErrStat >= AbortErrLev) return + DstInputData%PtfmRefY = SrcInputData%PtfmRefY end subroutine subroutine WAMIT_DestroyInput(InputData, ErrStat, ErrMsg) @@ -1056,6 +1086,7 @@ subroutine WAMIT_PackInput(RF, Indata) character(*), parameter :: RoutineName = 'WAMIT_PackInput' if (RF%ErrStat >= AbortErrLev) return call MeshPack(RF, InData%Mesh) + call RegPack(RF, InData%PtfmRefY) if (RegCheckErr(RF, RoutineName)) return end subroutine @@ -1065,6 +1096,7 @@ subroutine WAMIT_UnPackInput(RF, OutData) character(*), parameter :: RoutineName = 'WAMIT_UnPackInput' if (RF%ErrStat /= ErrID_None) return call MeshUnpack(RF, OutData%Mesh) ! Mesh + call RegUnpack(RF, OutData%PtfmRefY); if (RegCheckErr(RF, RoutineName)) return end subroutine subroutine WAMIT_CopyOutput(SrcOutputData, DstOutputData, CtrlCode, ErrStat, ErrMsg) @@ -1210,6 +1242,7 @@ SUBROUTINE WAMIT_Input_ExtrapInterp1(u1, u2, tin, u_out, tin_out, ErrStat, ErrMs CALL MeshExtrapInterp1(u1%Mesh, u2%Mesh, tin, u_out%Mesh, tin_out, ErrStat2, ErrMsg2) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + u_out%PtfmRefY = a1*u1%PtfmRefY + a2*u2%PtfmRefY END SUBROUTINE SUBROUTINE WAMIT_Input_ExtrapInterp2(u1, u2, u3, tin, u_out, tin_out, ErrStat, ErrMsg ) @@ -1267,6 +1300,7 @@ SUBROUTINE WAMIT_Input_ExtrapInterp2(u1, u2, u3, tin, u_out, tin_out, ErrStat, E a3 = (t_out - t(1))*(t_out - t(2))/((t(3) - t(1))*(t(3) - t(2))) CALL MeshExtrapInterp2(u1%Mesh, u2%Mesh, u3%Mesh, tin, u_out%Mesh, tin_out, ErrStat2, ErrMsg2) CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + u_out%PtfmRefY = a1*u1%PtfmRefY + a2*u2%PtfmRefY + a3*u3%PtfmRefY END SUBROUTINE subroutine WAMIT_Output_ExtrapInterp(y, t, y_out, t_out, ErrStat, ErrMsg) diff --git a/modules/hydrodyn/src/YawOffset.f90 b/modules/hydrodyn/src/YawOffset.f90 new file mode 100644 index 0000000000..22a98a4717 --- /dev/null +++ b/modules/hydrodyn/src/YawOffset.f90 @@ -0,0 +1,269 @@ +MODULE YawOffset + +USE NWTC_LIBRARY + +IMPLICIT NONE + +INTEGER(IntKi), PARAMETER :: i2h = 1_IntKi +INTEGER(IntKi), PARAMETER :: h2i = 2_IntKi + +INTERFACE hiFrameTransform + MODULE PROCEDURE hiFrameTransformVec3R8 + MODULE PROCEDURE hiFrameTransformVec3R4 + MODULE PROCEDURE hiFrameTransformMat +END INTERFACE hiFrameTransform + +INTERFACE GetRotAngs + MODULE PROCEDURE GetRotAngsR + MODULE PROCEDURE GetRotAngsD +END INTERFACE GetRotAngs + +INTERFACE WrapToPi + MODULE PROCEDURE WrapToPiR + MODULE PROCEDURE WrapToPiD +END INTERFACE WrapToPi + +INTERFACE WrapTo180 + MODULE PROCEDURE WrapTo180R + MODULE PROCEDURE WrapTo180D +END INTERFACE WrapTo180 + +CONTAINS + +SUBROUTINE GetPtfmRefYOrient(PtfmRefY, Orient, ErrStat, ErrMsg) + + REAL(ReKi), INTENT(IN ) :: PtfmRefY + REAL(ReKi), INTENT( OUT) :: Orient(3,3) + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + + REAL(ReKi) :: cosRefY + REAL(ReKi) :: sinRefY + + ErrStat = ErrID_None + ErrMsg = '' + + call Eye(Orient, ErrStat, ErrMsg) + cosRefY = cos(PtfmRefY) + sinRefY = sin(PtfmRefY) + Orient(1,1) = cosRefY + Orient(1,2) = sinRefY + Orient(2,1) = -sinRefY + Orient(2,2) = cosRefY + +END SUBROUTINE GetPtfmRefYOrient + +SUBROUTINE RotTrans(RotationType,PtfmRefY,Rotation,Orientation,ErrTxt,ErrStat,ErrMsg) + ! Compute the orientation matrix with potentially large reference yaw offset + ! This subroutine essentially extends SmllRotTrans to accommodate a large yaw offset + CHARACTER(*), INTENT(IN ) :: RotationType + REAL(ReKi), INTENT(IN ) :: PtfmRefY + REAL(R8Ki), INTENT(IN ) :: Rotation(3) + REAL(R8Ki), INTENT( OUT) :: Orientation(3,3) + CHARACTER(*), INTENT(IN ) :: ErrTxt + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + + REAL(ReKi) :: PtfmRefYOrient(3,3) + REAL(R8Ki) :: SmllOMat(3,3) + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'RotTrans' + + ErrStat = ErrID_None + ErrMsg = '' + + ! Orientation matrix associated with large reference yaw offset + call GetPtfmRefYOrient(PtfmRefY, PtfmRefYOrient, ErrStat2, ErrMsg2) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + + ! Orientation matrix for the remaining small rotation from the large reference yaw offset + call SmllRotTrans( RotationType, Rotation(1), Rotation(2), Rotation(3)-REAL(PtfmRefY,R8Ki), SmllOMat, ErrTxt, ErrStat2, ErrMsg2 ) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + + ! Combine the contributions + Orientation = matmul(SmllOMat,PtfmRefYOrient) + +END SUBROUTINE RotTrans + + +FUNCTION GetRotAngsR(PtfmRefY, DCMat, ErrStat, ErrMsg) + ! Compute the intrinsic Tait-Bryan angles (yaw first, pitch second, roll last) based on large yaw offset + ! The subroutine essentially extends GetSmllRotAngs to accommodate a large yaw offset + REAL(ReKi), INTENT(IN ) :: PtfmRefY + REAL(SiKi), INTENT(IN ) :: DCMat(3,3) + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + + REAL(SiKi) :: GetRotAngsR( 3 ) + + REAL(ReKi) :: PtfmRefYOrient(3,3) + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'GetRotAngs' + + ErrStat = ErrID_None + ErrMsg = '' + + ! Orientation matrix associated with large reference yaw offset + call GetPtfmRefYOrient(PtfmRefY, PtfmRefYOrient, ErrStat2, ErrMsg2) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + + GetRotAngsR = GetSmllRotAngsR ( matmul(DCMat,transpose(REAL(PtfmRefYOrient,SiKi))), ErrStat2, ErrMsg2 ) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + GetRotAngsR(3) = GetRotAngsR(3) + PtfmRefY + +END FUNCTION GetRotAngsR + +FUNCTION GetRotAngsD(PtfmRefY, DCMat, ErrStat, ErrMsg) + ! Compute the intrinsic Tait-Bryan angles (yaw first, pitch second, roll last) based on large yaw offset + ! The subroutine essentially extends GetSmllRotAngs to accommodate a large yaw offset + REAL(ReKi), INTENT(IN ) :: PtfmRefY + REAL(DbKi), INTENT(IN ) :: DCMat(3,3) + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + + REAL(DbKi) :: GetRotAngsD( 3 ) + + REAL(ReKi) :: PtfmRefYOrient(3,3) + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + CHARACTER(*), PARAMETER :: RoutineName = 'GetRotAngs' + + ErrStat = ErrID_None + ErrMsg = '' + + ! Orientation matrix associated with large reference yaw offset + call GetPtfmRefYOrient(PtfmRefY, PtfmRefYOrient, ErrStat2, ErrMsg2) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + + GetRotAngsD = GetSmllRotAngsD ( matmul(DCMat,transpose(REAL(PtfmRefYOrient,DbKi))), ErrStat2, ErrMsg2 ) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + GetRotAngsD(3) = GetRotAngsD(3) + PtfmRefY + +END FUNCTION GetRotAngsD + + +SUBROUTINE hiFrameTransformVec3R8(Mode,PtfmRefY,VecIn,VecOut,ErrStat,ErrMsg) + INTEGER(IntKi), INTENT(IN ) :: Mode + REAL(ReKi), INTENT(IN ) :: PtfmRefY + REAL(R8Ki), INTENT(IN ) :: VecIn(3) + REAL(R8Ki), INTENT( OUT) :: VecOut(3) + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + + REAL(ReKi) :: PtfmRefYOrient(3,3) + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + + CHARACTER(*), PARAMETER :: RoutineName = 'hiFrameTransformVec3' + + ErrStat = ErrID_None + ErrMsg = '' + + call GetPtfmRefYOrient(PtfmRefY, PtfmRefYOrient, ErrStat2, ErrMsg2) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + + if (Mode .EQ. i2h) then ! i-frame to h-frame + VecOut = matmul(PtfmRefYOrient,VecIn) + else if (Mode .EQ. h2i) then ! h-frame to i-frame + VecOut = matmul(transpose(PtfmRefYOrient),VecIn) + else + call SetErrStat(ErrID_Fatal, "Mode must be 1 or 2", ErrStat, ErrMsg, RoutineName) + end if + +END SUBROUTINE hiFrameTransformVec3R8 + +SUBROUTINE hiFrameTransformVec3R4(Mode,PtfmRefY,VecIn,VecOut,ErrStat,ErrMsg) + INTEGER(IntKi), INTENT(IN ) :: Mode + REAL(ReKi), INTENT(IN ) :: PtfmRefY + REAL(SiKi), INTENT(IN ) :: VecIn(3) + REAL(SiKi), INTENT( OUT) :: VecOut(3) + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + + REAL(ReKi) :: PtfmRefYOrient(3,3) + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + + CHARACTER(*), PARAMETER :: RoutineName = 'hiFrameTransformVec3' + + ErrStat = ErrID_None + ErrMsg = '' + + call GetPtfmRefYOrient(PtfmRefY, PtfmRefYOrient, ErrStat2, ErrMsg2) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + + if (Mode .EQ. i2h) then ! i-frame to h-frame + VecOut = matmul(PtfmRefYOrient,VecIn) + else if (Mode .EQ. h2i) then ! h-frame to i-frame + VecOut = matmul(transpose(PtfmRefYOrient),VecIn) + else + call SetErrStat(ErrID_Fatal, "Mode must be 1 or 2", ErrStat, ErrMsg, RoutineName) + end if + +END SUBROUTINE hiFrameTransformVec3R4 + +SUBROUTINE hiFrameTransformMat(Mode,PtfmRefY,MatIn,MatOut,ErrStat,ErrMsg) + INTEGER(IntKi), INTENT(IN ) :: Mode + REAL(ReKi), INTENT(IN ) :: PtfmRefY + REAL(ReKi), INTENT(IN ) :: MatIn(3,3) + REAL(ReKi), INTENT( OUT) :: MatOut(3,3) + INTEGER(IntKi), INTENT( OUT) :: ErrStat + CHARACTER(*), INTENT( OUT) :: ErrMsg + + REAL(ReKi) :: PtfmRefYOrient(3,3) + INTEGER(IntKi) :: ErrStat2 + CHARACTER(ErrMsgLen) :: ErrMsg2 + + CHARACTER(*), PARAMETER :: RoutineName = 'hiFrameTransformMat' + + ErrStat = ErrID_None + ErrMsg = '' + + call GetPtfmRefYOrient(PtfmRefY, PtfmRefYOrient, ErrStat2, ErrMsg2) + call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + + if (Mode .EQ. i2h) then ! i-frame to h-frame + MatOut = matmul(matmul(PtfmRefYOrient,MatIn),transpose(PtfmRefYOrient)) + else if (Mode .EQ. h2i) then ! h-frame to i-frame + MatOut = matmul(matmul(transpose(PtfmRefYOrient),MatIn),PtfmRefYOrient) + else + call SetErrStat(ErrID_Fatal, "Mode must be 1 or 2", ErrStat, ErrMsg, RoutineName) + end if + +END SUBROUTINE hiFrameTransformMat + +FUNCTION WrapTo180R(angle) + + REAL(SiKi), INTENT(IN) :: angle + REAL(SiKi) :: WrapTo180R + WrapTo180R = modulo(angle + 180.0, 360.0) - 180.0 + +END FUNCTION WrapTo180R + +FUNCTION WrapTo180D(angle) + + REAL(R8Ki), INTENT(IN) :: angle + REAL(R8Ki) :: WrapTo180D + WrapTo180D = modulo(angle + 180.0, 360.0) - 180.0 + +END FUNCTION WrapTo180D + +FUNCTION WrapToPiR(angle) + + REAL(SiKi), INTENT(IN) :: angle + REAL(SiKi) :: WrapToPiR + WrapToPiR = modulo(angle + PI, TwoPi) - PI + +END FUNCTION WrapToPiR + +FUNCTION WrapToPiD(angle) + + REAL(R8Ki), INTENT(IN) :: angle + REAL(R8Ki) :: WrapToPiD + WrapToPiD = modulo(angle + PI, TwoPi) - PI + +END FUNCTION WrapToPiD + +END MODULE YawOffset \ No newline at end of file diff --git a/modules/moordyn/src/MoorDyn.f90 b/modules/moordyn/src/MoorDyn.f90 index f98219f040..ff21a2db4b 100644 --- a/modules/moordyn/src/MoorDyn.f90 +++ b/modules/moordyn/src/MoorDyn.f90 @@ -2092,9 +2092,10 @@ SUBROUTINE MD_Init(InitInp, u, p, x, xd, z, other, y, m, DTcoupling, InitOut, Er DO iTurb = 1,p%nTurbines ! 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 + ! 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 + OrMat = EulerConstructZYX((/InitInp%PtfmInit(4,iTurb),InitInp%PtfmInit(5,iTurb),InitInp%PtfmInit(6,iTurb)/)) ! count number of coupling nodes needed for the mesh of this turbine K = p%nCpldBodies(iTurb) + p%nCpldRods(iTurb) + p%nCpldPoints(iTurb) @@ -4674,7 +4675,7 @@ SUBROUTINE MD_Perturb_u( p, n, perturb_sign, u, du ) CASE ( 1) u%CoupledKinematics(1)%TranslationDisp( fieldIndx,node) = u%CoupledKinematics(1)%TranslationDisp( fieldIndx,node) + du * perturb_sign CASE ( 2) - CALL PerturbOrientationMatrix( u%CoupledKinematics(1)%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. ) + CALL PerturbOrientationMatrix( u%CoupledKinematics(1)%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.false. ) CASE ( 3) u%CoupledKinematics(1)%TranslationVel( fieldIndx,node) = u%CoupledKinematics(1)%TranslationVel( fieldIndx,node) + du * perturb_sign CASE ( 4) diff --git a/modules/moordyn/src/MoorDyn_C_Binding.f90 b/modules/moordyn/src/MoorDyn_C_Binding.f90 index 28c8e0bcac..c4220322da 100644 --- a/modules/moordyn/src/MoorDyn_C_Binding.f90 +++ b/modules/moordyn/src/MoorDyn_C_Binding.f90 @@ -397,7 +397,7 @@ SUBROUTINE MD_C_UpdateStates(Time_C, TimeNext_C, POSITIONS_C, VELOCITIES_C, ACCE END DO ! Transfer motions to input meshes - CALL Set_MotionMesh( ErrStat2, ErrMsg2 ); IF (Failed()) RETURN + CALL Set_MotionMesh() CALL MD_SetInputMotion( u(INPUT_PRED), ErrStat2, ErrMsg2 ); IF (Failed()) RETURN ! Set copy the current state over to the predicted state for sending to UpdateStates @@ -484,7 +484,7 @@ SUBROUTINE MD_C_CalcOutput(Time_C, POSITIONS_C, VELOCITIES_C, ACCELERATIONS_C, F END DO ! Transfer motions to input meshes - CALL Set_MotionMesh(ErrStat2, ErrMsg2 ); if (Failed()) return; + CALL Set_MotionMesh() ! transfer input motion mesh to u(1) meshes CALL MD_SetInputMotion( u(1), ErrStat2, ErrMsg2 ); if (Failed()) return; @@ -624,7 +624,8 @@ SUBROUTINE SetMotionLoadsInterfaceMeshes(ErrStat,ErrMsg) ! initial position and orientation of node InitPos = tmpPositions(1:3,1) theta = REAL(tmpPositions(4:6,1),DbKi) ! convert ReKi to DbKi to avoid roundoff - CALL SmllRotTrans( 'InputRotation', theta(1), theta(2), theta(3), Orient, 'Orient', ErrStat, ErrMsg ) + ! CALL SmllRotTrans( 'InputRotation', theta(1), theta(2), theta(3), Orient, 'Orient', ErrStat, ErrMsg ) + Orient = EulerConstructZYX((/theta(1), theta(2), theta(3)/)) CALL MeshPositionNode( MD_MotionMesh , & 1 , & InitPos , & ! position @@ -678,14 +679,13 @@ END SUBROUTINE SetMotionLoadsInterfaceMeshes !--------------------------------------------------------------------------------------------------------------- !> This routine is operating on module level data, hence few inputs -SUBROUTINE Set_MotionMesh(ErrStat, ErrMsg) +SUBROUTINE Set_MotionMesh() REAL(R8Ki) :: theta(3) REAL(R8Ki) :: Orient(3,3) - INTEGER(IntKi), INTENT( OUT) :: ErrStat - CHARACTER(ErrMsgLen), INTENT( OUT) :: ErrMsg ! Set mesh corresponding to input motions theta = REAL(tmpPositions(4:6,1),DbKi) ! convert ReKi to DbKi to avoid roundoff - CALL SmllRotTrans( 'InputRotation', theta(1), theta(2), theta(3), Orient, 'Orient', ErrStat, ErrMsg ) + ! CALL SmllRotTrans( 'InputRotation', theta(1), theta(2), theta(3), Orient, 'Orient', ErrStat, ErrMsg ) + Orient = EulerConstructZYX((/theta(1), theta(2), theta(3)/)) MD_MotionMesh%TranslationDisp(1:3,1) = tmpPositions(1:3,1) - MD_MotionMesh%Position(1:3,1) ! relative displacement only MD_MotionMesh%Orientation(1:3,1:3,1) = Orient MD_MotionMesh%TranslationVel( 1:3,1) = tmpVelocities(1:3,1) diff --git a/modules/nwtc-library/src/NWTC_Num.f90 b/modules/nwtc-library/src/NWTC_Num.f90 index e7057e8129..3d7189919b 100644 --- a/modules/nwtc-library/src/NWTC_Num.f90 +++ b/modules/nwtc-library/src/NWTC_Num.f90 @@ -103,6 +103,7 @@ MODULE NWTC_Num END INTERFACE INTERFACE EulerConstructZYX + MODULE PROCEDURE EulerConstructZYXR4 MODULE PROCEDURE EulerConstructZYXR8 END INTERFACE @@ -112,6 +113,11 @@ MODULE NWTC_Num MODULE PROCEDURE EulerExtractR8 END INTERFACE + INTERFACE EulerExtractZYX + MODULE PROCEDURE EulerExtractZYXR4 + MODULE PROCEDURE EulerExtractZYXR8 + END INTERFACE + !> \copydoc nwtc_num::fzero_r4() INTERFACE fZeros MODULE PROCEDURE fzero_r4 @@ -2050,6 +2056,245 @@ FUNCTION EulerConstructZYXR8(theta) result(M) M(3,3) = cx*cy END FUNCTION EulerConstructZYXR8 + +!======================================================================= +!> + FUNCTION EulerConstructZYXR4(theta) result(M) + + ! this function creates a rotation matrix, M, from a 3-2-1 rotation + ! sequence of the 3 Euler angles, theta_z, theta_y, and theta_x, in radians. + ! M represents a change of basis (from global to local coordinates; + ! not a physical rotation of the body). + ! + REAL(SiKi) :: M(3,3) ! rotation matrix M + REAL(SiKi), INTENT(IN) :: theta(3) ! the 3 rotation angles: theta_x, theta_y, theta_z + + REAL(SiKi) :: cx ! cos(theta_x) + REAL(SiKi) :: sx ! sin(theta_x) + REAL(SiKi) :: cy ! cos(theta_y) + REAL(SiKi) :: sy ! sin(theta_y) + REAL(SiKi) :: cz ! cos(theta_z) + REAL(SiKi) :: sz ! sin(theta_z) + + + cx = cos( theta(1) ) + sx = sin( theta(1) ) + + cy = cos( theta(2) ) + sy = sin( theta(2) ) + + cz = cos( theta(3) ) + sz = sin( theta(3) ) + + M(1,1) = cy*cz + M(2,1) = sx*sy*cz - sz*cx + M(3,1) = sx*sz + sy*cx*cz + + M(1,2) = sz*cy + M(2,2) = sx*sy*sz + cx*cz + M(3,2) = -sx*cz + sy*sz*cx + + M(1,3) = -sy + M(2,3) = sx*cy + M(3,3) = cx*cy + + END FUNCTION EulerConstructZYXR4 + + +!======================================================================= +!> + FUNCTION EulerExtractZYXR8(M) result(theta) + + ! if M is a rotation matrix from a 3-2-1 rotation sequence, this function returns + ! the 3 Euler angles, theta_x, theta_y, and theta_z (in radians), that formed + ! the matrix. M represents a change of basis (from global to local coordinates; + ! not a physical rotation of the body). M is the inverse of EulerConstruct(). + ! + ! M = R(theta_x) * R(theta_y) * R(theta_z) + ! = [ 1 0 0 | [ cy 0 -sy | [ cz sz 0 | + ! | 0 cx sx | * | 0 1 0 | * |-sz cz 0 | + ! | 0 -sx cx ] | sy 0 cy ] | 0 0 1 ] + ! = [cy*cz sz*cy -sy| + ! |sx*sy*cz-sz*cx sx*sy*sz+cx*cz sx*cy| + ! |sx*sz+sy*cx*cz -sx*cz+sy*sz*cx cx*cy] + ! where cz = cos(theta_z), sz = sin(theta_z), cy = cos(theta_y), etc. + ! + ! returned angles are in the range [-pi, pi] + + REAL(R8Ki), INTENT(IN) :: M(3,3) ! rotation matrix M + REAL(R8Ki) :: theta(3) ! the 3 rotation angles: theta_x, theta_y, theta_z + + REAL(R8Ki) :: cx ! cos(theta_x) + REAL(R8Ki) :: sx ! sin(theta_x) + REAL(R8Ki) :: cy ! cos(theta_y) +! REAL(R8Ki) :: sy ! sin(theta_y) + REAL(R8Ki) :: cz ! cos(theta_z) + REAL(R8Ki) :: sz ! sin(theta_z) + + ! use trig identity sz**2 + cz**2 = 1 to get abs(cy): + cy = sqrt( m(1,1)**2 + m(1,2)**2 ) +! cy = sqrt( m(3,3)**2 + m(2,3)**2 ) + + if ( EqualRealNos(cy,0.0_R8Ki) ) then + !if ( cy < 16*epsilon(0.0_ReKi) ) then + + theta(2) = atan2( -m(1,3), cy ) ! theta_y + + ! cy = 0 -> sy = +/-1 + ! M = [0 0 +/-1| + ! |+/-sx*cz-sz*cx +/-sx*sz+cx*cz 0| + ! |sx*sz+/-cx*cz -sx*cz+/-sz*cx 0] + + ! gimbal lock allows us to choose theta_z = 0 + theta(3) = 0.0_R8Ki ! theta_z + + ! which reduces the matrix to + ! M = [0 0 +/-1| + ! |+/-sx cx 0| + ! |+/-cx -sx 0] + + theta(1) = atan2( -m(3,2), m(2,2) ) ! theta_x + + else + ! atan2( cy*sz, cy*cz ) + theta(3) = atan2( m(1,2), m(1,1) ) ! theta_z + cz = cos( theta(3) ) + sz = sin( theta(3) ) + + ! get the appropriate sign for cy: + if ( EqualRealNos(cz, 0.0_R8Ki) ) then + cy = sign( cy, m(1,2)/sz ) + !cy = m(1,2)/sz + else + cy = sign( cy, m(1,1)/cz ) + !cy = m(1,1)/cz + end if + theta(2) = atan2( -m(1,3), cy ) ! theta_y + + !theta(1) = atan2( m(2,3), m(3,3) ) ! theta_x + + ! for numerical reasons, we're going to get theta_x using + ! M' = M * (R(theta_y)*R(theta_z))^T = R(theta_x) + ! = [ cz -sz 0 | [ cy 0 sy | [ 1 0 0 | + ! M * | sz cz 0 | * | 0 1 0 | = | 0 cx sx | + ! | 0 0 1 ] |-sy 0 cy ] | 0 -sx cx ] + ! = [ cy*cz -sz sy*cz | [ 1 0 0 | + ! M * | cy*sz cz sy*sz | = | 0 cx sx | + ! | -sy 0 cy ] | 0 -sx cx ] + ! taking M'(2,2) and M'(2,3) , we get cx and sx: + ! -sz*m(2,1) + cz*m(2,2) = cx + ! -sz*m(3,1) + cz*m(3,2) = -sx + + cz = cos( theta(3) ) + sz = sin( theta(3) ) + + cx = -sz*m(2,1) + cz*m(2,2) + sx = sz*m(3,1) - cz*m(3,2) + + theta(1) = atan2( sx, cx ) + + end if + + + END FUNCTION EulerExtractZYXR8 + +!======================================================================= +!> + FUNCTION EulerExtractZYXR4(M) result(theta) + + ! if M is a rotation matrix from a 3-2-1 rotation sequence, this function returns + ! the 3 Euler angles, theta_x, theta_y, and theta_z (in radians), that formed + ! the matrix. M represents a change of basis (from global to local coordinates; + ! not a physical rotation of the body). M is the inverse of EulerConstruct(). + ! + ! M = R(theta_x) * R(theta_y) * R(theta_z) + ! = [ 1 0 0 | [ cy 0 -sy | [ cz sz 0 | + ! | 0 cx sx | * | 0 1 0 | * |-sz cz 0 | + ! | 0 -sx cx ] | sy 0 cy ] | 0 0 1 ] + ! = [cy*cz sz*cy -sy| + ! |sx*sy*cz-sz*cx sx*sy*sz+cx*cz sx*cy| + ! |sx*sz+sy*cx*cz -sx*cz+sy*sz*cx cx*cy] + ! where cz = cos(theta_z), sz = sin(theta_z), cy = cos(theta_y), etc. + ! + ! returned angles are in the range [-pi, pi] + + REAL(SiKi), INTENT(IN) :: M(3,3) ! rotation matrix M + REAL(SiKi) :: theta(3) ! the 3 rotation angles: theta_x, theta_y, theta_z + + REAL(SiKi) :: cx ! cos(theta_x) + REAL(SiKi) :: sx ! sin(theta_x) + REAL(SiKi) :: cy ! cos(theta_y) +! REAL(SiKi) :: sy ! sin(theta_y) + REAL(SiKi) :: cz ! cos(theta_z) + REAL(SiKi) :: sz ! sin(theta_z) + + ! use trig identity sz**2 + cz**2 = 1 to get abs(cy): + cy = sqrt( m(1,1)**2 + m(1,2)**2 ) +! cy = sqrt( m(3,3)**2 + m(2,3)**2 ) + + if ( EqualRealNos(cy,0.0_SiKi) ) then + !if ( cy < 16*epsilon(0.0_ReKi) ) then + + theta(2) = atan2( -m(1,3), cy ) ! theta_y + + ! cy = 0 -> sy = +/-1 + ! M = [0 0 +/-1| + ! |+/-sx*cz-sz*cx +/-sx*sz+cx*cz 0| + ! |sx*sz+/-cx*cz -sx*cz+/-sz*cx 0] + + ! gimbal lock allows us to choose theta_z = 0 + theta(3) = 0.0_SiKi ! theta_z + + ! which reduces the matrix to + ! M = [0 0 +/-1| + ! |+/-sx cx 0| + ! |+/-cx -sx 0] + + theta(1) = atan2( -m(3,2), m(2,2) ) ! theta_x + + else + ! atan2( cy*sz, cy*cz ) + theta(3) = atan2( m(1,2), m(1,1) ) ! theta_z + cz = cos( theta(3) ) + sz = sin( theta(3) ) + + ! get the appropriate sign for cy: + if ( EqualRealNos(cz, 0.0_SiKi) ) then + cy = sign( cy, m(1,2)/sz ) + !cy = m(1,2)/sz + else + cy = sign( cy, m(1,1)/cz ) + !cy = m(1,1)/cz + end if + theta(2) = atan2( -m(1,3), cy ) ! theta_y + + !theta(1) = atan2( m(2,3), m(3,3) ) ! theta_x + + ! for numerical reasons, we're going to get theta_x using + ! M' = M * (R(theta_y)*R(theta_z))^T = R(theta_x) + ! = [ cz -sz 0 | [ cy 0 sy | [ 1 0 0 | + ! M * | sz cz 0 | * | 0 1 0 | = | 0 cx sx | + ! | 0 0 1 ] |-sy 0 cy ] | 0 -sx cx ] + ! = [ cy*cz -sz sy*cz | [ 1 0 0 | + ! M * | cy*sz cz sy*sz | = | 0 cx sx | + ! | -sy 0 cy ] | 0 -sx cx ] + ! taking M'(2,2) and M'(2,3) , we get cx and sx: + ! -sz*m(2,1) + cz*m(2,2) = cx + ! -sz*m(3,1) + cz*m(3,2) = -sx + + cz = cos( theta(3) ) + sz = sin( theta(3) ) + + cx = -sz*m(2,1) + cz*m(2,2) + sx = sz*m(3,1) - cz*m(3,2) + + theta(1) = atan2( sx, cx ) + + end if + + + END FUNCTION EulerExtractZYXR4 + !======================================================================= !> This routine sets the matrices in the first two dimensions of A equal !! to the identity matrix (all zeros, with ones on the diagonal). diff --git a/modules/seastate/src/SeaSt_WaveField.f90 b/modules/seastate/src/SeaSt_WaveField.f90 index a117d3db79..0c1fb951e2 100644 --- a/modules/seastate/src/SeaSt_WaveField.f90 +++ b/modules/seastate/src/SeaSt_WaveField.f90 @@ -454,6 +454,12 @@ subroutine SetCartesianXYIndex(p, pZero, delta, nMax, Indx_Lo, Indx_Hi, isopc, F Indx_Lo = 0 Indx_Hi = 0 + if ( nMax .EQ. 1_IntKi ) then ! Only one grid point + Indx_Lo = 1_IntKi + Indx_Hi = 1_IntKi + isopc = 0_SiKi + return + end if Tmp = (p-pZero) / delta Indx_Lo = INT( Tmp ) + 1 ! convert REAL to INTEGER, then add one since our grid indices start at 1, not 0 @@ -628,8 +634,13 @@ subroutine WaveField_Interp_Setup4D( Time, Position, p, m, ErrStat, ErrMsg ) ! Find the bounding indices for Z position i=4 ! z component - call SetCartesianZIndex(Position(i-1), p%Z_Depth, p%delta(i), p%n(i), m%Indx_Lo(i), m%Indx_Hi(i), isopc(i), m%FirstWarn_Clamp, ErrStat2, ErrMsg2) - if (Failed()) return; + if (p%Z_Depth>0) then + call SetCartesianZIndex(Position(i-1), p%Z_Depth, p%delta(i), p%n(i), m%Indx_Lo(i), m%Indx_Hi(i), isopc(i), m%FirstWarn_Clamp, ErrStat2, ErrMsg2) + if (Failed()) return; + else ! Regular z-grid + call SetCartesianXYIndex(Position(i-1), p%pZero(i), p%delta(i), p%n(i), m%Indx_Lo(i), m%Indx_Hi(i), isopc(i), m%FirstWarn_Clamp, ErrStat2, ErrMsg2) + if (Failed()) return; + end if ! compute weighting factors m%N4D( 1) = ( 1.0_SiKi - isopc(1) ) * ( 1.0_SiKi - isopc(2) ) * ( 1.0_SiKi - isopc(3) ) * ( 1.0_SiKi - isopc(4) ) diff --git a/modules/servodyn/src/ServoDyn.f90 b/modules/servodyn/src/ServoDyn.f90 index b6b79bca21..6866cd3241 100644 --- a/modules/servodyn/src/ServoDyn.f90 +++ b/modules/servodyn/src/ServoDyn.f90 @@ -3324,7 +3324,7 @@ subroutine SrvD_Perturb_u( p, n, perturb_sign, u, du ) case (13) ! TranslationDisp = 1; u%TStCMotionMesh(instance)%TranslationDisp(fieldIndx,1) = u%TStCMotionMesh(instance)%TranslationDisp(fieldIndx,1) + du * perturb_sign case (14) ! Orientation = 2; - CALL PerturbOrientationMatrix( u%TStCMotionMesh(instance)%Orientation(:,:,1), du * perturb_sign, fieldIndx, UseSmlAngle=.true. ) + CALL PerturbOrientationMatrix( u%TStCMotionMesh(instance)%Orientation(:,:,1), du * perturb_sign, fieldIndx, UseSmlAngle=.false. ) case (15) ! TranslationVel = 3; u%TStCMotionMesh(instance)%TranslationVel( fieldIndx,1) = u%TStCMotionMesh(instance)%TranslationVel( fieldIndx,1) + du * perturb_sign case (16) ! RotationVel = 4; @@ -3338,7 +3338,7 @@ subroutine SrvD_Perturb_u( p, n, perturb_sign, u, du ) case (19) ! TranslationDisp = 1; u%SStCMotionMesh(instance)%TranslationDisp(fieldIndx,1) = u%SStCMotionMesh(instance)%TranslationDisp(fieldIndx,1) + du * perturb_sign case (20) ! Orientation = 2; - CALL PerturbOrientationMatrix( u%SStCMotionMesh(instance)%Orientation(:,:,1), du * perturb_sign, fieldIndx, UseSmlAngle=.true. ) + CALL PerturbOrientationMatrix( u%SStCMotionMesh(instance)%Orientation(:,:,1), du * perturb_sign, fieldIndx, UseSmlAngle=.false. ) case (21) ! TranslationVel = 3; u%SStCMotionMesh(instance)%TranslationVel( fieldIndx,1) = u%SStCMotionMesh(instance)%TranslationVel( fieldIndx,1) + du * perturb_sign case (22) ! RotationVel = 4; diff --git a/modules/subdyn/src/SD_FEM.f90 b/modules/subdyn/src/SD_FEM.f90 index 2e7390b032..f2284d8ba8 100644 --- a/modules/subdyn/src/SD_FEM.f90 +++ b/modules/subdyn/src/SD_FEM.f90 @@ -1226,10 +1226,12 @@ SUBROUTINE AssembleKM(Init, p, ErrStat, ErrMsg) CALL AllocAry( Init%K, p%nDOF, p%nDOF , 'Init%K', ErrStat2, ErrMsg2); if(Failed()) return; ! system stiffness matrix CALL AllocAry( Init%M, p%nDOF, p%nDOF , 'Init%M', ErrStat2, ErrMsg2); if(Failed()) return; ! system mass matrix - CALL AllocAry( p%FG, p%nDOF, 'p%FG' , ErrStat2, ErrMsg2); if(Failed()) return; ! system gravity force vector + CALL AllocAry( p%FG, p%nDOF, 'p%FG' , ErrStat2, ErrMsg2); if(Failed()) return; ! system gravity force vector with line pretension + CALL AllocAry( p%FC, p%nDOF, 'p%FC' , ErrStat2, ErrMsg2); if(Failed()) return; ! line pretension only Init%K = 0.0_FEKi Init%M = 0.0_FEKi p%FG = 0.0_FEKi + p%FC = 0.0_FEKi ! loop over all elements, compute element matrices and assemble into global matrices DO i = 1, Init%NElem @@ -1240,7 +1242,8 @@ SUBROUTINE AssembleKM(Init, p, ErrStat, ErrMsg) ! --- Assembly in global unconstrained system IDOF = p%ElemsDOF(1:12, i) - p%FG ( IDOF ) = p%FG( IDOF ) + FGe(1:12)+ FCe(1:12) ! Note: gravity and pretension cable forces + p%FC ( IDOF ) = p%FC( IDOF ) + FCe(1:12) ! Note: Pretension cable forces only + p%FG ( IDOF ) = p%FG( IDOF ) + FGe(1:12) ! Note: Gravity forces only Init%K(IDOF, IDOF) = Init%K( IDOF, IDOF) + Ke(1:12,1:12) Init%M(IDOF, IDOF) = Init%M( IDOF, IDOF) + Me(1:12,1:12) ENDDO diff --git a/modules/subdyn/src/SubDyn.f90 b/modules/subdyn/src/SubDyn.f90 index 7534b8b174..8f377cd786 100644 --- a/modules/subdyn/src/SubDyn.f90 +++ b/modules/subdyn/src/SubDyn.f90 @@ -192,6 +192,12 @@ SUBROUTINE SD_Init( InitInput, u, p, x, xd, z, OtherState, y, m, Interval, InitO real(FEKi), dimension(:) , allocatable :: Omega real(FEKi), dimension(:) , allocatable :: Omega_Gy ! Frequencies of Guyan modes logical, allocatable :: bDOF(:) ! Mask for DOF to keep (True), or reduce (False) + + REAL(ReKi) :: rOG(3) ! Vector from origin to G + REAL(ReKi) :: M_O(6,6) ! Rigid-body inertia matrix + REAL(FEKi),allocatable :: MBB(:,:) ! Leader DOFs mass matrix + REAL(ReKi), dimension(:,:), allocatable :: TI2 ! For Equivalent mass matrix + INTEGER(IntKi) :: ErrStat2 ! Error status of the operation CHARACTER(ErrMsgLen) :: ErrMsg2 ! Error message if ErrStat /= ErrID_None @@ -237,7 +243,8 @@ SUBROUTINE SD_Init( InitInput, u, p, x, xd, z, OtherState, y, m, Interval, InitO !bjj added this ugly check (mostly for checking SubDyn driver). not sure if anyone would want to play with different values of gravity so I don't return an error. IF (Init%g < 0.0_ReKi ) CALL ProgWarn( ' SubDyn calculations use gravity assuming it is input as a positive number; the input value is negative.' ) - + p%g = Init%g + ! Establish the GLUECODE requested/suggested time step. This may be overridden by SubDyn based on the SDdeltaT parameter of the SubDyn input file. Init%DT = Interval IF ( LEN_TRIM(Init%RootName) == 0 ) THEN @@ -347,6 +354,29 @@ SUBROUTINE SD_Init( InitInput, u, p, x, xd, z, OtherState, y, m, Interval, InitO ! Construct the input mesh (u%LMesh, force on nodes) and output mesh (y%Y2Mesh, displacements) CALL CreateInputOutputMeshes( p%nNodes, Init%Nodes, u%LMesh, y%Y2Mesh, y%Y3Mesh, ErrStat2, ErrMsg2 ); if(Failed()) return + ! If floating, compute the vector from the reference point to the rigid-body CoG in Guyan frame + IF (p%Floating) THEN + ! Compute the vector from reference point P to rigid-body CoG for floating structures + ! Set TI2, transformation matrix from R DOFs to SubDyn Origin + CALL AllocAry( TI2, p%nDOFR__ , 6, 'TI2', ErrStat2, ErrMsg2 ); if(Failed()) return + CALL RigidTrnsf(Init, p, (/0._ReKi, 0._ReKi, 0._ReKi/), p%IDR__, p%nDOFR__, TI2, ErrStat2, ErrMsg2); if(Failed()) return + ! Compute Rigid body mass matrix (without Soil, and using both Interface and Reactions nodes as leader DOF) + if (p%nDOFR__/=p%nDOF__Rb) then + call SD_Guyan_RigidBodyMass(Init, p, MBB, ErrStat2, ErrMsg2); if(Failed()) return + M_O=matmul(TRANSPOSE(TI2),matmul(MBB,TI2)) !Equivalent mass matrix of the rigid body + else + M_O=matmul(TRANSPOSE(TI2),matmul(CBparams%MBB,TI2)) !Equivalent mass matrix of the rigid body + endif + deallocate(TI2) + ! Clean up for values that ought to be 0 + M_O(1,2:4)= 0.0_ReKi; + M_O(2,1 )= 0.0_ReKi; M_O(2,3 )= 0.0_ReKi; M_O(2,5 )= 0.0_ReKi; + M_O(3,1:2)= 0.0_ReKi; M_O(3,6 )= 0.0_ReKi + M_O(4,1 )= 0.0_ReKi; M_O(5,2 )= 0.0_ReKi; M_O(6,3 )= 0.0_ReKi; + CALL rigidBodyMassMatrixCOG(M_O, rOG); + p%rPG = rOG-InitInput%TP_RefPoint + END IF + ! --- Eigen values of full system (for summary file output only) IF ( Init%SSSum .or. p%OutFEMModes>idOutputFormatNone) THEN ! M and K are reduced matrices, but Boundary conditions are not applied, so @@ -481,6 +511,7 @@ SUBROUTINE SD_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) REAL(ReKi) :: udotdot_TP(6) INTEGER(IntKi), pointer :: DOFList(:) REAL(ReKi) :: DCM(3,3) + REAL(ReKi) :: MBB(6,6), CBB(6,6) ! Guyan mode inertia and damping matrices transformed to earth-fixed frame of reference REAL(ReKi) :: F_I(6*p%nNodes_I) ! !Forces from all interface nodes listed in one big array ( those translated to TP ref point HydroTP(6) are implicitly calculated in the equations) TYPE(SD_ContinuousStateType) :: dxdt ! Continuous state derivatives at t- for output file qmdotdot purposes only ! Variables for Guyan rigid body motion @@ -493,7 +524,7 @@ SUBROUTINE SD_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) real(ReKi), dimension(3) :: aP ! Rigid-body acceleration of node real(R8Ki), dimension(3,3) :: Rg2b ! Rotation matrix global 2 body coordinates real(R8Ki), dimension(3,3) :: Rb2g ! Rotation matrix body 2 global coordinates - real(R8Ki), dimension(6,6) :: RRb2g ! Rotation matrix global 2 body coordinates, acts on a 6-vector + real(R8Ki), dimension(6,6) :: RRb2g ! Rotation matrix body 2 global coordinates, acts on a 6-vector INTEGER(IntKi) :: ErrStat2 ! Error status of the operation (occurs after initial error) CHARACTER(ErrMsgLen) :: ErrMsg2 ! Error message if ErrStat2 /= ErrID_None ! Initialize ErrStat @@ -501,17 +532,23 @@ SUBROUTINE SD_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) ErrMsg = "" ! --- Convert inputs to FEM DOFs and convenient 6-vector storage - ! Compute the small rotation angles given the input direction cosine matrix - rotations = GetSmllRotAngs(u%TPMesh%Orientation(:,:,1), ErrStat2, Errmsg2); if(Failed()) return + ! Compute the roll, pitch, and yaw angles given the input direction cosine matrix + IF ( p%Floating ) THEN + ! Only needed for outputs when floating + rotations = EulerExtractZYX(u%TPMesh%Orientation(:,:,1)) + ELSE + ! Need to be small angles due to the Guyan stiffness terms + rotations = GetSmllRotAngs(u%TPMesh%Orientation(:,:,1), ErrStat2, ErrMsg2); if(Failed()) return + END IF m%u_TP = (/REAL(u%TPMesh%TranslationDisp(:,1),ReKi), rotations/) m%udot_TP = (/u%TPMesh%TranslationVel( :,1), u%TPMesh%RotationVel(:,1)/) m%udotdot_TP = (/u%TPMesh%TranslationAcc( :,1), u%TPMesh%RotationAcc(:,1)/) Rg2b(1:3,1:3) = u%TPMesh%Orientation(:,:,1) ! global 2 body coordinates Rb2g(1:3,1:3) = transpose(u%TPMesh%Orientation(:,:,1)) - RRb2g(:,:) = 0.0_ReKi + RRb2g(:,:) = 0.0_R8Ki RRb2g(1:3,1:3) = Rb2g RRb2g(4:6,4:6) = Rb2g - + ! -------------------------------------------------------------------------------- ! --- Output Meshes 2&3 ! -------------------------------------------------------------------------------- @@ -576,7 +613,7 @@ SUBROUTINE SD_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) ! Storing elastic motion (full motion for fixed bottom, CB motion+SIM for floating) m%U_full_elast = m%U_full - + ! --- Place displacement/velocity/acceleration into Y2 output mesh if (p%Floating) then ! For floating, we compute the Guyan motion directly (rigid body motion with TP as origin) @@ -596,9 +633,11 @@ SUBROUTINE SD_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) ! Full displacements CB-rotated + Guyan (KEEP ME) >>> Rotate All if (p%GuyanLoadCorrection) then m%U_full_NS (DOFList(1:3)) = matmul(Rb2g, m%U_full_NS (DOFList(1:3))) + duP(1:3) - m%U_full_NS (DOFList(4:6)) = matmul(Rb2g, m%U_full_NS (DOFList(4:6))) + rotations(1:3) + CALL SmllRotTrans('Nodal rotation',m%U_full_NS(DOFList(4)),m%U_full_NS(DOFList(5)),m%U_full_NS(DOFList(6)),DCM,'',ErrStat2,ErrMsg2); if(Failed()) return + m%U_full_NS (DOFList(4:6)) = EulerExtractZYX( matmul(DCM,Rg2b) ) m%U_full (DOFList(1:3)) = matmul(Rb2g, m%U_full (DOFList(1:3))) + duP(1:3) - m%U_full (DOFList(4:6)) = matmul(Rb2g, m%U_full (DOFList(4:6))) + rotations(1:3) + CALL SmllRotTrans('Nodal rotation',m%U_full(DOFList(4)),m%U_full(DOFList(5)),m%U_full(DOFList(6)),DCM,'',ErrStat2,ErrMsg2); if(Failed()) return + m%U_full (DOFList(4:6)) = EulerExtractZYX( matmul(DCM,Rg2b) ) m%U_full_dot (DOFList(1:3)) = matmul(Rb2g, m%U_full_dot (DOFList(1:3))) + vP(1:3) m%U_full_dot (DOFList(4:6)) = matmul(Rb2g, m%U_full_dot (DOFList(4:6))) + Om(1:3) m%U_full_dotdot(DOFList(1:3)) = matmul(Rb2g, m%U_full_dotdot(DOFList(1:3))) + aP(1:3) @@ -615,15 +654,10 @@ SUBROUTINE SD_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) endif ! --- Rigid body displacements for hydrodyn - ! Construct the direction cosine matrix given the output angles - call SmllRotTrans( 'UR_bar input angles Guyan', rotations(1), rotations(2), rotations(3), DCM, '', ErrStat2, ErrMsg2) ! NOTE: using only Guyan rotations - call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'SD_CalcOutput') - y%Y2mesh%Orientation (:,:,iSDNode) = DCM + y%Y2mesh%Orientation (:,:,iSDNode) = u%TPMesh%Orientation(:,:,1) y%Y2mesh%TranslationDisp (:,iSDNode) = duP(1:3) ! Y2: NOTE: only the Guyan displacements for floating ! --- Full elastic displacements for others (moordyn) - call SmllRotTrans( 'Nodal rotation', m%U_full_NS(DOFList(4)), m%U_full_NS(DOFList(5)), m%U_full_NS(DOFList(6)), DCM, '', ErrStat2, ErrMsg2) - call SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'SD_CalcOutput') - y%Y3mesh%Orientation (:,:,iSDNode) = DCM + y%Y3mesh%Orientation (:,:,iSDNode) = EulerConstructZYX(m%U_full_NS(DOFList(4:6))) y%Y3mesh%TranslationDisp (:,iSDNode) = m%U_full_NS (DOFList(1:3)) ! Y3: Guyan+CB (but no SIM) displacements ! --- Elastic velocities and accelerations y%Y2mesh%TranslationVel (:,iSDNode) = m%U_full_dot (DOFList(1:3)) @@ -637,8 +671,7 @@ SUBROUTINE SD_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) DOFList => p%NodesDOF(iSDNode)%List ! Alias to shorten notations ! TODO TODO which orientation to give for joints with more than 6 dofs? ! Construct the direction cosine matrix given the output angles - CALL SmllRotTrans( 'UR_bar input angles', m%U_full_NS(DOFList(4)), m%U_full_NS(DOFList(5)), m%U_full_NS(DOFList(6)), DCM, '', ErrStat2, ErrMsg2) - CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'SD_CalcOutput') + CALL SmllRotTrans( 'UR_bar input angles', m%U_full_NS(DOFList(4)), m%U_full_NS(DOFList(5)), m%U_full_NS(DOFList(6)), DCM, '', ErrStat2, ErrMsg2); if(Failed()) return y%Y2mesh%Orientation (:,:,iSDNode) = DCM y%Y2mesh%TranslationDisp (:,iSDNode) = m%U_full_NS (DOFList(1:3)) !Y2: Guyan+CB (but no SIM) displacements y%Y2mesh%TranslationVel (:,iSDNode) = m%U_full_dot (DOFList(1:3)) @@ -670,41 +703,76 @@ SUBROUTINE SD_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) else Y1_CB = 0.0_ReKi endif + ! print *, 'Y1_CB: ', Y1_CB + + ! Contribution from U_TP, Udot_TP, Uddot_TP, Reaction/coupling force at TP + if (p%GuyanLoadCorrection.and.p%Floating) then + ! Transform the body-frame Guyan mode (rigid-body) inertia and damping matrix to global frame + MBB = matmul(RRb2g, matmul(p%MBB,transpose(RRb2g))) + CBB = matmul(RRb2g, matmul(p%CBB,transpose(RRb2g))) + ! Y1_Utp = - (matmul(p%KBB, m%u_TP) + matmul(p%CBB, m%udot_TP) + matmul(MBB,m%udotdot_TP) ) + Y1_Utp = - ( matmul(CBB,m%udot_TP) + matmul(MBB,m%udotdot_TP) ) + ! Add back the nonlinear terms of the Guyan mode equation of motion + Y1_Utp(1:3) = Y1_Utp(1:3) - MBB(1,1)*cross_product(m%udot_TP(4:6),cross_product(m%udot_TP(4:6),matmul(Rb2g,p%rPG))) + Y1_Utp(4:6) = Y1_Utp(4:6) - cross_product(m%udot_TP(4:6),matmul(MBB(4:6,4:6),m%udot_TP(4:6))) + else + Y1_Utp = - (matmul(p%KBB, m%u_TP) + matmul(p%CBB, m%udot_TP) + matmul(p%MBB,m%udotdot_TP) ) + end if - ! Contribution from U_TP, Udot_TP, Uddot_TP, Reaction/coupling force at TP - Y1_Utp = - (matmul(p%KBB, m%u_TP) + matmul(p%CBB, m%udot_TP) + matmul(p%MBB, m%udotdot_TP) ) if (p%nDOFM>0) then !>>> Rotate All ! NOTE: this introduces some hysteresis - !if (p%Floating) then - ! udotdot_TP(1:3) = matmul(Rg2b, u%TPMesh%TranslationAcc( :,1)) - ! udotdot_TP(4:6) = matmul(Rg2b, u%TPMesh%RotationAcc(:,1) ) - ! Y1_Utp = Y1_Utp + matmul(RRb2g, matmul(p%MBmmB, udotdot_TP)) - !else - Y1_Utp = Y1_Utp + matmul(p%MBmmB, m%udotdot_TP) - !endif + if (p%GuyanLoadCorrection.and.p%Floating) then + udotdot_TP(1:3) = matmul(Rg2b, u%TPMesh%TranslationAcc( :,1)) + udotdot_TP(4:6) = matmul(Rg2b, u%TPMesh%RotationAcc(:,1) ) + Y1_Utp = Y1_Utp + matmul(RRb2g, matmul(p%MBmmB, udotdot_TP)) + else + Y1_Utp = Y1_Utp + matmul(p%MBmmB, m%udotdot_TP) + endif endif - ! --- Special case for floating with extramoment, we use "rotated loads" m%F_L previously computed if (p%GuyanLoadCorrection.and.p%Floating) then - Y1_CB_L = - (matmul(p%D1_141, m%F_L)) ! = - (M_Bm . Phi_m^T) "F_L", where "F_L"=Rg2b F_L are rotated loads - Y1_CB_L = matmul(RRb2g, Y1_CB_L) ! = - Rb2g (M_Bm . Phi_m^T) Rg2b F_L + ! --- Special case for floating with extra moment, we use "rotated loads" m%F_L previously computed + ! Contributions from external forces - Note: T_I is in the rotated frame + call GetExtForceOnInterfaceDOF(p, m%Fext, F_I) + Y1_Guy_R = matmul( F_I, p%TI ) ! = - [-T_I.^T] F_R = [T_I.^T] F_R =~ F_R T_I (~: FORTRAN convention) + Y1_Guy_R = matmul(RRb2g, Y1_Guy_R) + Y1_Guy_L = - matmul(p%D1_142, m%F_L) ! = - (- T_I^T . Phi_Rb^T) F_L, rotated loads + Y1_Guy_L = matmul(RRb2g, Y1_Guy_L) + Y1_CB_L = - matmul(p%D1_141, m%F_L) ! = - (M_Bm . Phi_m^T) "F_L", where "F_L"=Rg2b F_L are rotated loads + Y1_CB_L = matmul(RRb2g, Y1_CB_L) ! = - Rb2g (M_Bm . Phi_m^T) Rg2b F_L + else ! .not.(p%GuyanLoadCorrection.and.p%Floating) + ! Compute "non-rotated" external force on internal (F_L) and interface nodes (F_I) + call GetExtForceOnInternalDOF(u, p, x, m, m%F_L, ErrStat2, ErrMsg2, GuyanLoadCorrection=(p%GuyanLoadCorrection), RotateLoads=.False.); if(Failed()) return + call GetExtForceOnInterfaceDOF(p, m%Fext, F_I) + ! Contributions from external forces + Y1_Guy_R = matmul( F_I, p%TI ) ! = - [-T_I.^T] F_R = [T_I.^T] F_R =~ F_R T_I (~: FORTRAN convention) + Y1_Guy_L = - matmul(p%D1_142, m%F_L) ! = - (- T_I^T . Phi_Rb^T) F_L, non-rotated loads + Y1_CB_L = - matmul(p%D1_141, m%F_L) ! = - (M_Bm . Phi_m^T) F_L, non-rotated loads endif - ! Compute "non-rotated" external force on internal (F_L) and interface nodes (F_I) - call GetExtForceOnInternalDOF(u, p, x, m, m%F_L, ErrStat2, ErrMsg2, GuyanLoadCorrection=(p%GuyanLoadCorrection), RotateLoads=.False.); if(Failed()) return - call GetExtForceOnInterfaceDOF(p, m%Fext, F_I) - - ! Contributions from external forces - Y1_Guy_R = matmul( F_I, p%TI ) ! = - [-T_I.^T] F_R = [T_I.^T] F_R =~ F_R T_I (~: FORTRAN convention) - Y1_Guy_L = - matmul(p%D1_142, m%F_L) ! = - (- T_I^T . Phi_Rb^T) F_L, non-rotated loads - - if (.not.(p%GuyanLoadCorrection.and.p%Floating)) then - Y1_CB_L = - (matmul(p%D1_141, m%F_L)) ! = - (M_Bm . Phi_m^T) F_L, non-rotated loads - endif + ! Old implementation below + ! ! --- Special case for floating with extramoment, we use "rotated loads" m%F_L previously computed + ! if (p%GuyanLoadCorrection.and.p%Floating) then + ! Y1_CB_L = - (matmul(p%D1_141, m%F_L)) ! = - (M_Bm . Phi_m^T) "F_L", where "F_L"=Rg2b F_L are rotated loads + ! Y1_CB_L = matmul(RRb2g, Y1_CB_L) ! = - Rb2g (M_Bm . Phi_m^T) Rg2b F_L + ! endif + ! + ! ! Compute "non-rotated" external force on internal (F_L) and interface nodes (F_I) + ! call GetExtForceOnInternalDOF(u, p, x, m, m%F_L, ErrStat2, ErrMsg2, GuyanLoadCorrection=(p%GuyanLoadCorrection), RotateLoads=.False.); if(Failed()) return + ! call GetExtForceOnInterfaceDOF(p, m%Fext, F_I) + ! + ! ! Contributions from external forces + ! Y1_Guy_R = matmul( F_I, p%TI ) ! = - [-T_I.^T] F_R = [T_I.^T] F_R =~ F_R T_I (~: FORTRAN convention) + ! Y1_Guy_L = - matmul(p%D1_142, m%F_L) ! = - (- T_I^T . Phi_Rb^T) F_L, non-rotated loads + ! + ! if (.not.(p%GuyanLoadCorrection.and.p%Floating)) then + ! Y1_CB_L = - (matmul(p%D1_141, m%F_L)) ! = - (M_Bm . Phi_m^T) F_L, non-rotated loads + ! endif ! Total contribution Y1 = Y1_CB + Y1_Utp + Y1_CB_L+ Y1_Guy_L + Y1_Guy_R + ! KEEP ME !if ( p%nDOFM > 0) then ! Y1 = -( matmul(p%C1_11, x%qm) + matmul(p%C1_12,x%qmdot) & @@ -749,8 +817,8 @@ SUBROUTINE SD_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg ) CALL SD_DestroyContState( dxdt, ErrStat2, ErrMsg2); if(Failed()) return END IF ! 6-vectors (making sure they are up to date for outputs - m%udot_TP = (/u%TPMesh%TranslationVel( :,1),u%TPMesh%RotationVel(:,1)/) - m%udotdot_TP = (/u%TPMesh%TranslationAcc(:,1), u%TPMesh%RotationAcc(:,1)/) + m%udot_TP = (/u%TPMesh%TranslationVel(:,1),u%TPMesh%RotationVel(:,1)/) + m%udotdot_TP = (/u%TPMesh%TranslationAcc(:,1),u%TPMesh%RotationAcc(:,1)/) ! Write the previous output data into the output file IF ( ( p%OutSwtch == 1 .OR. p%OutSwtch == 3 ) .AND. ( t > m%LastOutTime ) ) THEN @@ -930,17 +998,21 @@ SUBROUTINE SD_Input(SDInputFile, Init, p, ErrStat,ErrMsg) IF (Check(.not.(any(idSIM_Valid==p%SttcSolve)), 'Invalid value entered for SttcSolve')) return ! GuyanLoadCorrection - For legacy, allowing this line to be a comment -CALL ReadVar (UnIn, SDInputFile, Dummy_Str, 'GuyanLoadCorrection', 'Add extra lever arm contribution to interface loads', ErrStat2, ErrMsg2, UnEc); if(Failed()) return -if (is_logical(Dummy_Str, Dummy_Bool)) then ! the parameter was present - p%GuyanLoadCorrection=Dummy_Bool - ! We still need to read the comment on the next line - CALL ReadCom ( UnIn, SDInputFile, ' FEA and CRAIG-BAMPTON PARAMETERS ', ErrStat2, ErrMsg2, UnEc ); if(Failed()) return -else ! we have a actually read a comment line, we do nothing. - call LegacyWarning('ExtraMom line missing from input file. Assuming no extra moment.') - p%GuyanLoadCorrection=.False. ! For Legacy, GuyanLoadCorrection is False -endif +! CALL ReadVar (UnIn, SDInputFile, Dummy_Str, 'GuyanLoadCorrection', 'Add extra lever arm contribution to interface loads', ErrStat2, ErrMsg2, UnEc); if(Failed()) return +! if (is_logical(Dummy_Str, Dummy_Bool)) then ! the parameter was present +! p%GuyanLoadCorrection=Dummy_Bool +! ! We still need to read the comment on the next line +! CALL ReadCom ( UnIn, SDInputFile, ' FEA and CRAIG-BAMPTON PARAMETERS ', ErrStat2, ErrMsg2, UnEc ); if(Failed()) return +! else ! we have a actually read a comment line, we do nothing. +! call LegacyWarning('ExtraMom line missing from input file. Assuming no extra moment.') +! p%GuyanLoadCorrection=.False. ! For Legacy, GuyanLoadCorrection is False +! endif + +! GuyanLoadCorrection will always be set to true. The corresponding user input is commented out above. +p%GuyanLoadCorrection=.True. !-------------------- FEA and CRAIG-BAMPTON PARAMETERS--------------------------- +CALL ReadCom ( UnIn, SDInputFile, ' FEA and CRAIG-BAMPTON PARAMETERS ', ErrStat2, ErrMsg2, UnEc ); if(Failed()) return CALL ReadIVar ( UnIn, SDInputFile, Init%FEMMod, 'FEMMod', 'FEM analysis mode' ,ErrStat2, ErrMsg2, UnEc ); if(Failed()) return ! 0= Euler-Bernoulli(E-B); 1=Tapered E-B; 2= Timoshenko; 3= tapered Timoshenko CALL ReadIVar ( UnIn, SDInputFile, Init%NDiv , 'NDiv' , 'Number of divisions per member',ErrStat2, ErrMsg2, UnEc ); if(Failed()) return CALL ReadLVar ( UnIn, SDInputFile, Init%CBMod , 'CBMod' , 'C-B mod flag' ,ErrStat2, ErrMsg2, UnEc ); if(Failed()) return @@ -2810,6 +2882,7 @@ SUBROUTINE AllocMiscVars(p, Misc, ErrStat, ErrMsg) CALL AllocAry( Misc%Fext, p%nDOF , 'm%Fext ', ErrStat2, ErrMsg2 );CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'AllocMiscVars') CALL AllocAry( Misc%Fext_red, p%nDOF_red , 'm%Fext_red', ErrStat2, ErrMsg2 );CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'AllocMiscVars') + CALL AllocAry( Misc%FG, p%nDOF , 'm%FG ', ErrStat2, ErrMsg2 );CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'AllocMiscVars') END SUBROUTINE AllocMiscVars @@ -3114,13 +3187,11 @@ SUBROUTINE LeverArm(u, p, x, m, DU_full, bGuyan, bElastic) real(ReKi), dimension(3) :: rIP0 ! Vector from TP to Node (undeflected) real(ReKi), dimension(3) :: duP ! Displacement of node due to rigid rotation real(R8Ki), dimension(3,3) :: Rb2g ! Rotation matrix body 2 global coordinates + real(ReKi), dimension(3,3) :: DCM INTEGER(IntKi) :: ErrStat2 ! Error status of the operation (occurs after initial error) CHARACTER(ErrMsgLen) :: ErrMsg2 ! Error message if ErrStat2 /= ErrID_None ! --- Convert inputs to FEM DOFs and convenient 6-vector storage - ! Compute the small rotation angles given the input direction cosine matrix - rotations = GetSmllRotAngs(u%TPMesh%Orientation(:,:,1), ErrStat2, Errmsg2); - m%u_TP = (/REAL(u%TPMesh%TranslationDisp(:,1),ReKi), rotations/) - + ! --- CB modes contribution to motion (L-DOF only), NO STATIC IMPROVEMENT if (bElastic .and. p%nDOFM > 0) then m%UL = matmul( p%PhiM, x%qm ) @@ -3129,6 +3200,9 @@ SUBROUTINE LeverArm(u, p, x, m, DU_full, bGuyan, bElastic) end if ! --- Adding Guyan contribution to R and L DOFs if (bGuyan .and. .not.p%Floating) then + ! Compute the small rotation angles given the input direction cosine matrix + rotations = GetSmllRotAngs(u%TPMesh%Orientation(:,:,1), ErrStat2, Errmsg2); + m%u_TP = (/REAL(u%TPMesh%TranslationDisp(:,1),ReKi), rotations/) m%UR_bar = matmul( p%TI , m%u_TP ) m%UL = m%UL + matmul( p%PhiRb_TI, m%u_TP ) else @@ -3150,8 +3224,9 @@ SUBROUTINE LeverArm(u, p, x, m, DU_full, bGuyan, bElastic) duP(1:3) = rIP - rIP0 ! NOTE: without m%u_TP(1:3) ! Full diplacements Guyan + rotated CB (if asked) >>> Rotate All if (p%GuyanLoadCorrection) then - DU_full(DOFList(1:3)) = matmul(Rb2g, DU_full(DOFList(1:3))) + duP(1:3) - DU_full(DOFList(4:6)) = matmul(Rb2g, DU_full(DOFList(4:6))) + rotations(1:3) + DU_full(DOFList(1:3)) = matmul(Rb2g, DU_full(DOFList(1:3))) + duP(1:3) + CALL SmllRotTrans('Nodal rotation',DU_full(DOFList(4)),DU_full(DOFList(5)),DU_full(DOFList(6)),DCM,'',ErrStat2,ErrMsg2); + DU_full(DOFList(4:6)) = EulerExtractZYX( matmul(DCM,transpose(Rb2g)) ) else DU_full(DOFList(1:3)) = DU_full(DOFList(1:3)) + duP(1:3) DU_full(DOFList(4:6)) = DU_full(DOFList(4:6)) + rotations(1:3) @@ -3184,6 +3259,7 @@ SUBROUTINE GetExtForceOnInternalDOF(u, p, x, m, F_L, ErrStat, ErrMsg, GuyanLoadC real(ReKi) :: rotations(3) real(ReKi) :: du(3), Moment(3), Force(3) real(ReKi) :: u_TP(6) + real(FEKi) :: FGe(12) ! element gravity force vector ! Variables for Guyan Rigid motion real(ReKi), dimension(3) :: rIP ! Vector from TP to rotated Node real(ReKi), dimension(3) :: rIP0 ! Vector from TP to Node (undeflected) @@ -3191,6 +3267,9 @@ SUBROUTINE GetExtForceOnInternalDOF(u, p, x, m, F_L, ErrStat, ErrMsg, GuyanLoadC real(R8Ki), dimension(3,3) :: Rb2g ! Rotation matrix body 2 global real(R8Ki), dimension(3,3) :: Rg2b ! Rotation matrix global 2 body coordinates + ErrStat = ErrID_None + ErrMsg = "" + if (GuyanLoadCorrection) then ! Compute node displacements "DU_full" for lever arm call LeverArm(u, p, x, m, m%DU_full, bGuyan=.True., bElastic=.False.) @@ -3210,11 +3289,11 @@ SUBROUTINE GetExtForceOnInternalDOF(u, p, x, m, F_L, ErrStat, ErrMsg, GuyanLoadC if (RotateLoads) then ! Forces in body coordinates Rg2b(1:3,1:3) = u%TPMesh%Orientation(:,:,1) ! global 2 body coordinates do iNode = 1,p%nNodes - m%Fext( p%NodesDOF(iNode)%List(1:3) ) = matmul(Rg2b, u%LMesh%Force(:,iNode) + p%FG(p%NodesDOF(iNode)%List(1:3))) + m%Fext( p%NodesDOF(iNode)%List(1:3) ) = matmul(Rg2b, u%LMesh%Force(:,iNode) + p%FG(p%NodesDOF(iNode)%List(1:3)) ) + p%FC(p%NodesDOF(iNode)%List(1:3)) enddo else ! Forces in global do iNode = 1,p%nNodes - m%Fext( p%NodesDOF(iNode)%List(1:3) ) = u%LMesh%Force(:,iNode) + p%FG(p%NodesDOF(iNode)%List(1:3)) + m%Fext( p%NodesDOF(iNode)%List(1:3) ) = u%LMesh%Force(:,iNode) + p%FG(p%NodesDOF(iNode)%List(1:3)) + p%FC(p%NodesDOF(iNode)%List(1:3)) enddo endif @@ -3248,15 +3327,33 @@ SUBROUTINE GetExtForceOnInternalDOF(u, p, x, m, F_L, ErrStat, ErrMsg, GuyanLoadC endif ! --- Build vector of external moment + ! For floating structure with potentially large Guyan (rigid-body) rotation, nodal self-weight needs to be recomputed based on the current rigid-body orientation + m%FG = 0.0_R8Ki + if ( RotateLoads ) then + Rb2g = transpose(Rg2b) ! Body (Guyan) to global + do i = 1, size(p%ElemProps) + ! --- Element Fg in the earth-fixed frame + CALL ElemG(p%ElemProps(i)%Area, p%ElemProps(i)%Length, p%ElemProps(i)%Rho, matmul(Rb2g,p%ElemProps(i)%DirCos), FGe, p%g) + ! --- Element Fg in the Guyan rigid-body frame + FGe( 1: 3) = matmul(Rg2b,FGe( 1: 3)) ! Node 1 force + FGe( 4: 6) = matmul(Rg2b,FGe( 4: 6)) ! Node 1 moment + FGe( 7: 9) = matmul(Rg2b,FGe( 7: 9)) ! Node 2 force + FGe(10:12) = matmul(Rg2b,FGe(10:12)) ! Node 2 moment + ! --- Assembly in global unconstrained system + IDOF = p%ElemsDOF(1:12,i) + m%FG( IDOF ) = m%FG( IDOF ) + FGe(1:12) + end do + end if + do iNode = 1,p%nNodes Force(1:3) = m%Fext(p%NodesDOF(iNode)%List(1:3) ) ! Controllable cable + External Forces on LMesh Moment(1:3) = m%Fext(p%NodesDOF(iNode)%List(4:6) ) ! Controllable cable - ! Moment ext + gravity + ! Moment ext + gravity (Cable pretension has no moment contribution) if (RotateLoads) then ! In body coordinates - Moment(1:3) = matmul(Rg2b, Moment(1:3)+ u%LMesh%Moment(1:3,iNode) + p%FG(p%NodesDOF(iNode)%List(4:6))) + Moment(1:3) = matmul(Rg2b, Moment(1:3)+ u%LMesh%Moment(1:3,iNode) ) + m%FG(p%NodesDOF(iNode)%List(4:6)) ! Use updated m%FG instead of p%FG else - Moment(1:3) = Moment(1:3)+ u%LMesh%Moment(1:3,iNode) + p%FG(p%NodesDOF(iNode)%List(4:6)) + Moment(1:3) = Moment(1:3)+ u%LMesh%Moment(1:3,iNode) + p%FG(p%NodesDOF(iNode)%List(4:6)) endif ! Extra moment dm = Delta u x (fe + fg) diff --git a/modules/subdyn/src/SubDyn_Output.f90 b/modules/subdyn/src/SubDyn_Output.f90 index 769caf5ec0..76d470711c 100644 --- a/modules/subdyn/src/SubDyn_Output.f90 +++ b/modules/subdyn/src/SubDyn_Output.f90 @@ -285,9 +285,25 @@ SUBROUTINE SDOut_MapOutputs(u,p,x, y, m, AllOuts, ErrStat, ErrMsg ) integer(IntKi) :: sgn !+1/-1 for node force calculations type(MeshAuxDataType), pointer :: pLst !< Info for a given member-output (Alias to shorten notation) integer(IntKi), pointer :: DOFList(:) !< List of DOF indices for a given Nodes (Alias to shorten notation) + real(R8Ki), dimension(3,3) :: Rg2b ! Rotation matrix global 2 body (Guyan) coordinates + real(R8Ki), dimension(6,6) :: RRg2b ! Rotation matrix global 2 body (Guyan) coordinates, acts on a 6-vector + INTEGER(IntKi) :: ErrStat2 ! Error status of the operation + CHARACTER(ErrMsgLen) :: ErrMsg2 ! Error message if ErrStat /= ErrID_None + ErrStat = ErrID_None ErrMsg = "" - + + if ( p%Floating ) then + ! For floating, m%U_full_dotdot is currently in the earth-fixed frame. + ! Need to transform back to the Guyan frame when computing MαNβFMxe, MαNβFMye, MαNβFMze, MαNβMMxe, MαNβMMye, MαNβMMze. + Rg2b = u%TPMesh%Orientation(:,:,1) ! global 2 body coordinates + else + call Eye(Rg2b, ErrStat2, ErrMsg2) + end if + RRg2b = 0.0_R8Ki + RRg2b(1:3,1:3) = Rg2b + RRg2b(4:6,4:6) = Rg2b + AllOuts = 0.0_ReKi ! initialize for those outputs that aren't valid (and thus aren't set in this routine) ! -------------------------------------------------------------------------------- @@ -322,14 +338,14 @@ SUBROUTINE SDOut_MapOutputs(u,p,x, y, m, AllOuts, ErrStat, ErrMsg ) ! Displacement- Translational -no need for averaging since it is a node translation - In global reference SS ! "MαNβTDxss, MαNβTDyss, MαNβTDzss" AllOuts(MNTDss (:,iiNode,iMemberOutput)) = m%U_full(DOFList(1:3)) - ! Displacement- Rotational - need direction cosine matrix to tranform rotations - In Local reference Element Ref Sys - ! "MαNβRDxss, MαNβRDye, MαNβRDze" - AllOuts(MNRDe (:,iiNode,iMemberOutput)) = matmul(DIRCOS,m%U_full(DOFList(4:6))) !local ref + ! Displacement- Rotational - need direction cosine matrix to tranform rotations - In Local reference Element Ref Sys <- Need to rethink this for large platform rotation + ! "MαNβRDxe, MαNβRDye, MαNβRDze" + AllOuts(MNRDe (:,iiNode,iMemberOutput)) = matmul(DIRCOS,m%U_full_elast(DOFList(4:6))) ! Element elastic rotation only in Guyan frame for floating. Full motion for fixed-bottom. ! Accelerations- I need to get the direction cosine matrix to tranform displacement and rotations ! "MαNβTAxe, MαNβTAye, MαNβTAze" ! "MαNβRAxe, MαNβRAye, MαNβRAze" - AllOuts(MNTRAe (1:3,iiNode,iMemberOutput)) = matmul(DIRCOS,m%U_full_dotdot(DOFList(1:3))) ! translational accel local ref - AllOuts(MNTRAe (4:6,iiNode,iMemberOutput)) = matmul(DIRCOS,m%U_full_dotdot(DOFList(4:6))) ! rotational accel local ref + AllOuts(MNTRAe (1:3,iiNode,iMemberOutput)) = matmul(DIRCOS,matmul(Rg2b,m%U_full_dotdot(DOFList(1:3)))) ! translational accel local ref + AllOuts(MNTRAe (4:6,iiNode,iMemberOutput)) = matmul(DIRCOS,matmul(Rg2b,m%U_full_dotdot(DOFList(4:6)))) ! rotational accel local ref ENDDO ! iiNode, Loop on requested nodes for that member ENDDO ! iMemberOutput, Loop on member outputs END IF @@ -435,10 +451,10 @@ subroutine ElementForce(pLst, iiNode, JJ, FM_elm, FK_elm, sgn, DIRCOS, bUseInput FirstOrSecond = pLst%ElmNds(iiNode,JJ) ! first or second node of the element to be considered sgn = NodeNumber_To_Sign(FirstOrSecond) ! Assign sign depending if it's the 1st or second node ElemNodes = p%Elems(iElem,2:3) ! first and second node ID associated with element iElem - X_e(1:6) = m%U_full_elast (p%NodesDOF(ElemNodes(1))%List(1:6)) - X_e(7:12) = m%U_full_elast (p%NodesDOF(ElemNodes(2))%List(1:6)) - Xdd_e(1:6) = m%U_full_dotdot(p%NodesDOF(ElemNodes(1))%List(1:6)) - Xdd_e(7:12) = m%U_full_dotdot(p%NodesDOF(ElemNodes(2))%List(1:6)) + X_e(1:6) = m%U_full_elast (p%NodesDOF(ElemNodes(1))%List(1:6)) ! For floating, m%U_full_elast is the CB+SIM elastic deformation only in the Guyan (rigid-body) frame + X_e(7:12) = m%U_full_elast (p%NodesDOF(ElemNodes(2))%List(1:6)) ! No additional transformation required + Xdd_e(1:6) = matmul(RRg2b,m%U_full_dotdot(p%NodesDOF(ElemNodes(1))%List(1:6))) ! Transform acceleration to be back in the Guyan frame + Xdd_e(7:12) = matmul(RRg2b,m%U_full_dotdot(p%NodesDOF(ElemNodes(2))%List(1:6))) if (.not. bUseInputDirCos) then DIRCOS=transpose(p%ElemProps(iElem)%DirCos)! global to local endif @@ -984,7 +1000,7 @@ SUBROUTINE SD_Perturb_u( p, n, perturb_sign, u, du ) CASE ( 1) !Module/Mesh/Field: u%TPMesh%TranslationDisp = 1; u%TPMesh%TranslationDisp( fieldIndx,node) = u%TPMesh%TranslationDisp( fieldIndx,node) + du * perturb_sign CASE ( 2) !Module/Mesh/Field: u%TPMesh%Orientation = 2; - CALL PerturbOrientationMatrix( u%TPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. ) + CALL PerturbOrientationMatrix( u%TPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.false. ) CASE ( 3) !Module/Mesh/Field: u%TPMesh%TranslationVel = 3; u%TPMesh%TranslationVel( fieldIndx,node) = u%TPMesh%TranslationVel( fieldIndx,node) + du * perturb_sign CASE ( 4) !Module/Mesh/Field: u%TPMesh%RotationVel = 4; @@ -1013,8 +1029,8 @@ SUBROUTINE SD_Compute_dY(p, y_p, y_m, delta, dY) INTEGER(IntKi) :: indx_first ! index indicating next value of dY to be filled indx_first = 1 call PackLoadMesh_dY( y_p%Y1Mesh, y_m%Y1Mesh, dY, indx_first) - call PackMotionMesh_dY(y_p%Y2Mesh, y_m%Y2Mesh, dY, indx_first, UseSmlAngle=.true.) ! all 6 motion fields - call PackMotionMesh_dY(y_p%Y3Mesh, y_m%Y3Mesh, dY, indx_first, UseSmlAngle=.true.) ! all 6 motion fields + call PackMotionMesh_dY(y_p%Y2Mesh, y_m%Y2Mesh, dY, indx_first, UseSmlAngle=.false.) ! all 6 motion fields + call PackMotionMesh_dY(y_p%Y3Mesh, y_m%Y3Mesh, dY, indx_first, UseSmlAngle=.false.) ! all 6 motion fields do i=1,p%NumOuts dY(i+indx_first-1) = y_p%WriteOutput(i) - y_m%WriteOutput(i) end do diff --git a/modules/subdyn/src/SubDyn_Registry.txt b/modules/subdyn/src/SubDyn_Registry.txt index ecc7a0f1bd..1fb738e12d 100644 --- a/modules/subdyn/src/SubDyn_Registry.txt +++ b/modules/subdyn/src/SubDyn_Registry.txt @@ -95,7 +95,7 @@ typedef ^ ^ LOGICAL CableCChanRqst {:} .FALSE. typedef ^ SD_InitType CHARACTER(1024) RootName - - - "SubDyn rootname" typedef ^ SD_InitType ReKi TP_RefPoint {3} - - "global position of transition piece reference point (could also be defined in SubDyn itself)" typedef ^ SD_InitType ReKi SubRotateZ - - - "Rotation angle in degrees about global Z" -typedef ^ SD_InitType ReKi g - - - "Gravity acceleration" +typedef ^ SD_InitType ReKi g - - - "Gravity acceleration" m/s^2 typedef ^ SD_InitType DbKi DT - - - "Time step from Glue Code" seconds typedef ^ SD_InitType INTEGER NJoints - - - "Number of joints of the sub structure" typedef ^ SD_InitType INTEGER NPropSetsX - - - "Number of extended property sets" @@ -189,6 +189,7 @@ typedef ^ MiscVarType DbKi LastOutTime - - - "The time of typedef ^ MiscVarType IntKi Decimat - - - "Current output decimation counter" "-" typedef ^ MiscVarType ReKi Fext {:} - - "External loads on unconstrained DOFs" "-" typedef ^ MiscVarType ReKi Fext_red {:} - - "External loads on constrained DOFs, Fext_red= T^t Fext" "-" +typedef ^ MiscVarType R8Ki FG {:} - - "Gravity force vector (without initial cable force T0) based on the instantaneous platform orientation, not reduced (floating only)" N # SIM typedef ^ MiscVarType ReKi UL_SIM {:} - - "UL for SIM = PhiL qL0- PhiM qm0, size nL" typedef ^ MiscVarType ReKi UL_0m {:} - - "Intermediate UL term for SIM = PhiM qm0, size nL" @@ -196,6 +197,7 @@ typedef ^ MiscVarType ReKi UL_0m {:} - - "Intermedia # ============================== Parameters ============================================================================================================================================ # --- Parameters - Algo +typedef ^ ParameterType ReKi g - - - "Gravity acceleration" m/s^2 typedef ^ ParameterType DbKi SDDeltaT - - - "Time step (for integration of continuous states)" seconds typedef ^ ParameterType IntKi IntMethod - - - "Integration Method (1/2/3)Length of y2 array" # --- Parameters - FEM @@ -204,8 +206,10 @@ typedef ^ ParameterType INTEGER nDOF_red - - - "Total degree typedef ^ ParameterType IntKi Nmembers - - - "Number of members of the sub structure" typedef ^ ParameterType IntKi Elems {:}{:} - - "Element nodes connections" typedef ^ ParameterType ElemPropType ElemProps {:} - - "List of element properties" +typedef ^ ParameterType R8Ki FC {:} - - "Initial cable force T0, not reduced" N typedef ^ ParameterType R8Ki FG {:} - - "Gravity force vector (with initial cable force T0), not reduced" N typedef ^ ParameterType ReKi DP0 {:}{:} - - "Vector from TP to a Node at t=0, used for Floating Rigid Body motion" m +typedef ^ ParameterType ReKi rPG {:} - - "Vector from TP to rigid-body CoG in the Guyan (rigid-body) frame, used for Floating Rigid Body Motion" m typedef ^ ParameterType IntKi NodeID2JointID {:} - - "Store Joint ID for each NodeID since SubDyn re-label nodes (and add more nodes)" "-" # --- Parameters - Constraints reduction typedef ^ ParameterType Logical reduced - - - "True if system has been reduced to account for constraints" "-" diff --git a/modules/subdyn/src/SubDyn_Types.f90 b/modules/subdyn/src/SubDyn_Types.f90 index a84a27d839..7b8a836d20 100644 --- a/modules/subdyn/src/SubDyn_Types.f90 +++ b/modules/subdyn/src/SubDyn_Types.f90 @@ -135,7 +135,7 @@ MODULE SubDyn_Types CHARACTER(1024) :: RootName !< SubDyn rootname [-] REAL(ReKi) , DIMENSION(1:3) :: TP_RefPoint = 0.0_ReKi !< global position of transition piece reference point (could also be defined in SubDyn itself) [-] REAL(ReKi) :: SubRotateZ = 0.0_ReKi !< Rotation angle in degrees about global Z [-] - REAL(ReKi) :: g = 0.0_ReKi !< Gravity acceleration [-] + REAL(ReKi) :: g = 0.0_ReKi !< Gravity acceleration [m/s^2] REAL(DbKi) :: DT = 0.0_R8Ki !< Time step from Glue Code [seconds] INTEGER(IntKi) :: NJoints = 0_IntKi !< Number of joints of the sub structure [-] INTEGER(IntKi) :: NPropSetsX = 0_IntKi !< Number of extended property sets [-] @@ -240,12 +240,14 @@ MODULE SubDyn_Types INTEGER(IntKi) :: Decimat = 0_IntKi !< Current output decimation counter [-] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: Fext !< External loads on unconstrained DOFs [-] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: Fext_red !< External loads on constrained DOFs, Fext_red= T^t Fext [-] + REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: FG !< Gravity force vector (without initial cable force T0) based on the instantaneous platform orientation, not reduced (floating only) [N] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: UL_SIM !< UL for SIM = PhiL qL0- PhiM qm0, size nL [-] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: UL_0m !< Intermediate UL term for SIM = PhiM qm0, size nL [-] END TYPE SD_MiscVarType ! ======================= ! ========= SD_ParameterType ======= TYPE, PUBLIC :: SD_ParameterType + REAL(ReKi) :: g = 0.0_ReKi !< Gravity acceleration [m/s^2] REAL(DbKi) :: SDDeltaT = 0.0_R8Ki !< Time step (for integration of continuous states) [seconds] INTEGER(IntKi) :: IntMethod = 0_IntKi !< Integration Method (1/2/3)Length of y2 array [-] INTEGER(IntKi) :: nDOF = 0_IntKi !< Total degree of freedom [-] @@ -253,8 +255,10 @@ MODULE SubDyn_Types INTEGER(IntKi) :: Nmembers = 0_IntKi !< Number of members of the sub structure [-] INTEGER(IntKi) , DIMENSION(:,:), ALLOCATABLE :: Elems !< Element nodes connections [-] TYPE(ElemPropType) , DIMENSION(:), ALLOCATABLE :: ElemProps !< List of element properties [-] + REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: FC !< Initial cable force T0, not reduced [N] REAL(R8Ki) , DIMENSION(:), ALLOCATABLE :: FG !< Gravity force vector (with initial cable force T0), not reduced [N] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: DP0 !< Vector from TP to a Node at t=0, used for Floating Rigid Body motion [m] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: rPG !< Vector from TP to rigid-body CoG in the Guyan (rigid-body) frame, used for Floating Rigid Body Motion [m] INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: NodeID2JointID !< Store Joint ID for each NodeID since SubDyn re-label nodes (and add more nodes) [-] LOGICAL :: reduced = .false. !< True if system has been reduced to account for constraints [-] REAL(R8Ki) , DIMENSION(:,:), ALLOCATABLE :: T_red !< Transformation matrix performing the constraint reduction x = T. xtilde [-] @@ -2326,6 +2330,18 @@ subroutine SD_CopyMisc(SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg) end if DstMiscData%Fext_red = SrcMiscData%Fext_red end if + if (allocated(SrcMiscData%FG)) then + LB(1:1) = lbound(SrcMiscData%FG, kind=B8Ki) + UB(1:1) = ubound(SrcMiscData%FG, kind=B8Ki) + if (.not. allocated(DstMiscData%FG)) then + allocate(DstMiscData%FG(LB(1):UB(1)), stat=ErrStat2) + if (ErrStat2 /= 0) then + call SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%FG.', ErrStat, ErrMsg, RoutineName) + return + end if + end if + DstMiscData%FG = SrcMiscData%FG + end if if (allocated(SrcMiscData%UL_SIM)) then LB(1:1) = lbound(SrcMiscData%UL_SIM, kind=B8Ki) UB(1:1) = ubound(SrcMiscData%UL_SIM, kind=B8Ki) @@ -2425,6 +2441,9 @@ subroutine SD_DestroyMisc(MiscData, ErrStat, ErrMsg) if (allocated(MiscData%Fext_red)) then deallocate(MiscData%Fext_red) end if + if (allocated(MiscData%FG)) then + deallocate(MiscData%FG) + end if if (allocated(MiscData%UL_SIM)) then deallocate(MiscData%UL_SIM) end if @@ -2465,6 +2484,7 @@ subroutine SD_PackMisc(RF, Indata) call RegPack(RF, InData%Decimat) call RegPackAlloc(RF, InData%Fext) call RegPackAlloc(RF, InData%Fext_red) + call RegPackAlloc(RF, InData%FG) call RegPackAlloc(RF, InData%UL_SIM) call RegPackAlloc(RF, InData%UL_0m) if (RegCheckErr(RF, RoutineName)) return @@ -2505,6 +2525,7 @@ subroutine SD_UnPackMisc(RF, OutData) call RegUnpack(RF, OutData%Decimat); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%Fext); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%Fext_red); if (RegCheckErr(RF, RoutineName)) return + call RegUnpackAlloc(RF, OutData%FG); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%UL_SIM); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%UL_0m); if (RegCheckErr(RF, RoutineName)) return end subroutine @@ -2522,6 +2543,7 @@ subroutine SD_CopyParam(SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg) character(*), parameter :: RoutineName = 'SD_CopyParam' ErrStat = ErrID_None ErrMsg = '' + DstParamData%g = SrcParamData%g DstParamData%SDDeltaT = SrcParamData%SDDeltaT DstParamData%IntMethod = SrcParamData%IntMethod DstParamData%nDOF = SrcParamData%nDOF @@ -2555,6 +2577,18 @@ subroutine SD_CopyParam(SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg) if (ErrStat >= AbortErrLev) return end do end if + if (allocated(SrcParamData%FC)) then + LB(1:1) = lbound(SrcParamData%FC, kind=B8Ki) + UB(1:1) = ubound(SrcParamData%FC, kind=B8Ki) + if (.not. allocated(DstParamData%FC)) then + allocate(DstParamData%FC(LB(1):UB(1)), stat=ErrStat2) + if (ErrStat2 /= 0) then + call SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%FC.', ErrStat, ErrMsg, RoutineName) + return + end if + end if + DstParamData%FC = SrcParamData%FC + end if if (allocated(SrcParamData%FG)) then LB(1:1) = lbound(SrcParamData%FG, kind=B8Ki) UB(1:1) = ubound(SrcParamData%FG, kind=B8Ki) @@ -2579,6 +2613,18 @@ subroutine SD_CopyParam(SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg) end if DstParamData%DP0 = SrcParamData%DP0 end if + if (allocated(SrcParamData%rPG)) then + LB(1:1) = lbound(SrcParamData%rPG, kind=B8Ki) + UB(1:1) = ubound(SrcParamData%rPG, kind=B8Ki) + if (.not. allocated(DstParamData%rPG)) then + allocate(DstParamData%rPG(LB(1):UB(1)), stat=ErrStat2) + if (ErrStat2 /= 0) then + call SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%rPG.', ErrStat, ErrMsg, RoutineName) + return + end if + end if + DstParamData%rPG = SrcParamData%rPG + end if if (allocated(SrcParamData%NodeID2JointID)) then LB(1:1) = lbound(SrcParamData%NodeID2JointID, kind=B8Ki) UB(1:1) = ubound(SrcParamData%NodeID2JointID, kind=B8Ki) @@ -3327,12 +3373,18 @@ subroutine SD_DestroyParam(ParamData, ErrStat, ErrMsg) end do deallocate(ParamData%ElemProps) end if + if (allocated(ParamData%FC)) then + deallocate(ParamData%FC) + end if if (allocated(ParamData%FG)) then deallocate(ParamData%FG) end if if (allocated(ParamData%DP0)) then deallocate(ParamData%DP0) end if + if (allocated(ParamData%rPG)) then + deallocate(ParamData%rPG) + end if if (allocated(ParamData%NodeID2JointID)) then deallocate(ParamData%NodeID2JointID) end if @@ -3543,6 +3595,7 @@ subroutine SD_PackParam(RF, Indata) integer(B8Ki) :: i1, i2 integer(B8Ki) :: LB(2), UB(2) if (RF%ErrStat >= AbortErrLev) return + call RegPack(RF, InData%g) call RegPack(RF, InData%SDDeltaT) call RegPack(RF, InData%IntMethod) call RegPack(RF, InData%nDOF) @@ -3558,8 +3611,10 @@ subroutine SD_PackParam(RF, Indata) call SD_PackElemPropType(RF, InData%ElemProps(i1)) end do end if + call RegPackAlloc(RF, InData%FC) call RegPackAlloc(RF, InData%FG) call RegPackAlloc(RF, InData%DP0) + call RegPackAlloc(RF, InData%rPG) call RegPackAlloc(RF, InData%NodeID2JointID) call RegPack(RF, InData%reduced) call RegPackAlloc(RF, InData%T_red) @@ -3714,6 +3769,7 @@ subroutine SD_UnPackParam(RF, OutData) integer(IntKi) :: stat logical :: IsAllocAssoc if (RF%ErrStat /= ErrID_None) return + call RegUnpack(RF, OutData%g); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%SDDeltaT); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%IntMethod); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%nDOF); if (RegCheckErr(RF, RoutineName)) return @@ -3733,8 +3789,10 @@ subroutine SD_UnPackParam(RF, OutData) call SD_UnpackElemPropType(RF, OutData%ElemProps(i1)) ! ElemProps end do end if + call RegUnpackAlloc(RF, OutData%FC); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%FG); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%DP0); if (RegCheckErr(RF, RoutineName)) return + call RegUnpackAlloc(RF, OutData%rPG); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%NodeID2JointID); if (RegCheckErr(RF, RoutineName)) return call RegUnpack(RF, OutData%reduced); if (RegCheckErr(RF, RoutineName)) return call RegUnpackAlloc(RF, OutData%T_red); if (RegCheckErr(RF, RoutineName)) return diff --git a/reg_tests/CTestList.cmake b/reg_tests/CTestList.cmake index 0ee94bda6b..465c97fe90 100644 --- a/reg_tests/CTestList.cmake +++ b/reg_tests/CTestList.cmake @@ -443,6 +443,9 @@ hd_regression("hd_MCF_WaveStMod2" "hydrodyn;offshore") hd_regression("hd_MCF_WaveStMod3" "hydrodyn;offshore") hd_regression("hd_ExctnMod1_ExctnDisp1" "hydrodyn;offshore") hd_regression("hd_ExctnMod1_ExctnDisp2" "hydrodyn;offshore") +hd_regression("hd_ExctnMod1_ExctnDisp2_PtfmYMod1" "hydrodyn;offshore") +hd_regression("hd_5MW_OC4Semi_WSt_WavesWN_PtfmYMod0_LargeYaw" "hydrodyn;offshore") +hd_regression("hd_5MW_OC4Semi_WSt_WavesWN_PtfmYMod1_LargeYaw" "hydrodyn;offshore") # Py-HydroDyn regression tests py_hd_regression("py_hd_5MW_OC4Semi_WSt_WavesWN" "hydrodyn;offshore;python") diff --git a/reg_tests/r-test b/reg_tests/r-test index b074a96dbe..ab79948833 160000 --- a/reg_tests/r-test +++ b/reg_tests/r-test @@ -1 +1 @@ -Subproject commit b074a96dbeff873af30c415942590cd50b7e026f +Subproject commit ab799488331a1bb1dd0cf7382425cbcbe6027b93 diff --git a/vs-build/FASTlib/FASTlib.vfproj b/vs-build/FASTlib/FASTlib.vfproj index a5594307e3..957b8842b9 100644 --- a/vs-build/FASTlib/FASTlib.vfproj +++ b/vs-build/FASTlib/FASTlib.vfproj @@ -1041,7 +1041,8 @@ - + + diff --git a/vs-build/HydroDyn/HydroDynDriver.vfproj b/vs-build/HydroDyn/HydroDynDriver.vfproj index 58e9f36ee7..84f9acfd31 100644 --- a/vs-build/HydroDyn/HydroDynDriver.vfproj +++ b/vs-build/HydroDyn/HydroDynDriver.vfproj @@ -266,7 +266,8 @@ - + + diff --git a/vs-build/HydroDyn_c_binding/HydroDyn_c_binding.vfproj b/vs-build/HydroDyn_c_binding/HydroDyn_c_binding.vfproj index 0bbd4017c2..efe02d92b2 100644 --- a/vs-build/HydroDyn_c_binding/HydroDyn_c_binding.vfproj +++ b/vs-build/HydroDyn_c_binding/HydroDyn_c_binding.vfproj @@ -223,7 +223,8 @@ - + +