-
Notifications
You must be signed in to change notification settings - Fork 465
/
BeamDyn.f90
7006 lines (5417 loc) · 361 KB
/
BeamDyn.f90
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
!**********************************************************************************************************************************
! LICENSING
! Copyright (C) 2015-2016 National Renewable Energy Laboratory
! Copyright (C) 2016-2018 Envision Energy USA, LTD
!
! Licensed under the Apache License, Version 2.0 (the "License");
! you may not use this file except in compliance with the License.
! You may obtain a copy of the License at
!
! http://www.apache.org/licenses/LICENSE-2.0
!
! Unless required by applicable law or agreed to in writing, software
! distributed under the License is distributed on an "AS IS" BASIS,
! WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
! See the License for the specific language governing permissions and
! limitations under the License.
!**********************************************************************************************************************************
MODULE BeamDyn
USE BeamDyn_BldNdOuts_IO
USE BeamDyn_IO
USE BeamDyn_Subs
!USE NWTC_LAPACK inherited from BeamDyn_Subs and BeamDyn_IO
IMPLICIT NONE
#ifndef UNIT_TEST
PRIVATE
! ..... Public Subroutines....................................................................
PUBLIC :: BD_Init ! Initialization routine
PUBLIC :: BD_End ! Ending routine (includes clean up)
PUBLIC :: BD_UpdateStates ! Loose coupling routine for solving for constraint states, integrating
PUBLIC :: BD_CalcOutput ! Routine for computing outputs
PUBLIC :: BD_CalcContStateDeriv ! Tight coupling routine for computing derivatives of continuous states
PUBLIC :: BD_UpdateDiscState ! Tight coupling routine for updating discrete states
#endif
PUBLIC :: BD_JacobianPInput ! Routine to compute the Jacobians of the output(Y), continuous - (X), discrete -
! (Xd), and constraint - state(Z) functions all with respect to the inputs(u)
PUBLIC :: BD_JacobianPContState ! Routine to compute the Jacobians of the output(Y), continuous - (X), discrete -
! (Xd), and constraint - state(Z) functions all with respect to the continuous
! states(x)
PUBLIC :: BD_JacobianPDiscState ! Routine to compute the Jacobians of the output(Y), continuous - (X), discrete -
! (Xd), and constraint - state(Z) functions all with respect to the discrete
! states(xd)
PUBLIC :: BD_JacobianPConstrState ! Routine to compute the Jacobians of the output(Y), continuous - (X), discrete -
! (Xd), and constraint - state(Z) functions all with respect to the constraint
! states(z)
PUBLIC :: BD_GetOP !< Routine to pack the operating point values (for linearization) into arrays
PUBLIC :: BD_UpdateGlobalRef !< update the BeamDyn reference. The reference for the calculations follows u%RootMotionMesh
! and therefore x%q must be updated from T -> T+DT to include the root motion from T->T+DT
! The original formulation kept all states in the inertial reference frame. This has been leading to convergence issues
! when there is a large rotational change from the reference frame (i.e. large turbine yaw, large blade pitch). During
! the development of the tight coupling algorithm for OpenFAST, we decided to try changing all the states in BeamDyn to
! follow the moving BladeRootMotion mesh. This requires changing the states after an UpdateStates call to be relative to
! the new BladeRootMotion mesh orientation and position.
! Upadate the reference frame after each State update (or use the old method)?
LOGICAL, PARAMETER :: ChangeRefFrame = .false.
CONTAINS
!-----------------------------------------------------------------------------------------------------------------------------------
!> This routine is called at the start of the simulation to perform initialization steps.
!! The parameters are set here and not changed during the simulation.
!! The initial states and initial guess for the input are defined.
SUBROUTINE BD_Init( InitInp, u, p, x, xd, z, OtherState, y, MiscVar, Interval, InitOut, ErrStat, ErrMsg )
TYPE(BD_InitInputType), INTENT(IN ) :: InitInp !< Input data for initialization routine
TYPE(BD_InputType), INTENT( OUT) :: u !< An initial guess for the input; input mesh must be defined
TYPE(BD_ParameterType), INTENT( OUT) :: p !< Parameters
TYPE(BD_ContinuousStateType), INTENT( OUT) :: x !< Initial continuous states
TYPE(BD_DiscreteStateType), INTENT( OUT) :: xd !< Initial discrete states
TYPE(BD_ConstraintStateType), INTENT( OUT) :: z !< Initial guess of the constraint states
TYPE(BD_OtherStateType), INTENT( OUT) :: OtherState !< Initial other states
TYPE(BD_OutputType), INTENT( OUT) :: y !< Initial system outputs (outputs are not calculated;
!! only the output mesh is initialized)
TYPE(BD_MiscVarType), INTENT( OUT) :: MiscVar !< Misc variables for optimization (not copied in glue code)
REAL(DbKi), INTENT(INOUT) :: Interval !< Coupling interval in seconds: the rate that
!! (1) Mod1_UpdateStates() is called in loose coupling &
!! (2) Mod1_UpdateDiscState() is called in tight coupling. ! Input is the suggested time from the glue code;
!! Output is the actual coupling interval that will be used
!! by the glue code.
TYPE(BD_InitOutputType), INTENT( OUT) :: InitOut !< Output for initialization routine
INTEGER(IntKi), INTENT( OUT) :: ErrStat !< Error status of the operation
CHARACTER(*), INTENT( OUT) :: ErrMsg !< Error message if ErrStat /= ErrID_None
! local variables
TYPE(BD_InputFile) :: InputFileData ! Data stored in the module's input file
REAL(BDKi) :: temp_CRV(3)
REAL(BDKi),ALLOCATABLE :: GLL_nodes(:)
REAL(BDKi) :: TmpDCM(3,3)
LOGICAL :: QuasiStaticInitialized !< True if quasi-static solution was found
INTEGER(IntKi) :: nelem
INTEGER(IntKi) :: ErrStat2 ! Temporary Error status
CHARACTER(ErrMsgLen) :: ErrMsg2 ! Temporary Error message
character(*), parameter :: RoutineName = 'BD_Init'
! Initialize ErrStat
ErrStat = ErrID_None
ErrMsg = ""
! Initialize the NWTC Subroutine Library
CALL NWTC_Init( )
! Display the module information
CALL DispNVD( BeamDyn_Ver )
CALL BD_ReadInput(InitInp%InputFile,InputFileData,InitInp%RootName,Interval,ErrStat2,ErrMsg2); if (Failed()) return
CALL BD_ValidateInputData( InitInp, InputFileData, ErrStat2, ErrMsg2 ); if (Failed()) return
! The reference frame is set by the root motion mesh initial position
call InitRefFrame( InitInp, OtherState, ErrStat2, ErrMsg2 ); if (Failed()) return
! this routine sets *some* of the parameters (basically the "easy" ones)
call SetParameters(InitInp, InputFileData, p, OtherState, ErrStat2, ErrMsg2); if (Failed()) return
! Temporary GLL point intrinsic coordinates array
CALL BD_GenerateGLL(p%nodes_per_elem,GLL_nodes,ErrStat2,ErrMsg2); if (Failed()) return
! In the following, trapezoidalpointweight should be generalized to multi-element; likewise for gausspointweight
IF(p%quadrature .EQ. GAUSS_QUADRATURE) THEN
CALL BD_GaussPointWeight(p%nqp,p%QPtN,p%QPtWeight,ErrStat2,ErrMsg2); if (Failed()) return !calculates p%QPtN and p%QPtWeight
ELSEIF(p%quadrature .EQ. TRAP_QUADRATURE) THEN
CALL BD_TrapezoidalPointWeight(p, InputFileData%InpBl%station_eta, InputFileData%InpBl%station_total) ! computes p%QPtN and p%QPtWeight
ENDIF
! compute physical distances to set positions of p%uuN0 (FE GLL_Nodes) (depends on p%SP_Coef):
call InitializeNodalLocations(InputFileData%member_total,InputFileData%kp_member,InputFileData%kp_coordinate,p,GLL_nodes,ErrStat2,ErrMsg2); if (Failed()) return
! compute p%Shp, p%ShpDer, and p%Jacobian:
CALL BD_InitShpDerJaco( GLL_Nodes, p )
! set mass and stiffness matrices: p%Stif0_QP and p%Mass0_QP
call InitializeMassStiffnessMatrices(InputFileData, p, ErrStat2,ErrMsg2); if (Failed()) return
! Set the initial displacements: p%uu0, p%rrN0, p%E10
CALL BD_QuadraturePointDataAt0(p)
call Initialize_FEweights(p,GLL_nodes,ErrStat2,ErrMsg2); if (Failed()) return ! set p%FEweight; needs p%uuN0 and p%uu0
! compute blade mass, CG, and IN for summary file:
CALL BD_ComputeBladeMassNew( p, ErrStat2, ErrMsg2 ); if (Failed()) return !computes p%blade_mass,p%blade_CG,p%blade_IN
if (p%UsePitchAct) then
! Calculate the pitch angle
TmpDCM(:,:) = MATMUL(u%RootMotion%Orientation(:,:,1),TRANSPOSE(u%HubMotion%Orientation(:,:,1)))
temp_CRV(:) = EulerExtract(TmpDCM)
xd%thetaP = -temp_CRV(3)
xd%thetaPD = 0.0_BDKi
end if
! Define and initialize system inputs (set up and initialize input meshes) here:
call Init_u(InitInp, p, OtherState, u, ErrStat2, ErrMsg2); if (Failed()) return
! allocate and initialize other states: Also sets the GlbRot for the displaced position (needed for x%q initialization)
call Init_OtherStates(u, p, OtherState, ErrStat2, ErrMsg2); if (Failed()) return
! allocate and initialize continuous states (need to do this after initializing inputs):
call Init_ContinuousStates(p, u, x, OtherState, ErrStat2, ErrMsg2); if (Failed()) return
! initialize outputs (need to do this after initializing inputs and parameters (p%nnu0))
call Init_y(p, OtherState, u, y, ErrStat2, ErrMsg2); if (Failed()) return
! allocate and initialize misc vars (do this after initializing input and output meshes):
call Init_MiscVars(p, u, y, MiscVar, ErrStat2, ErrMsg2); if (Failed()) return
! Now that we have the initial conditions, we can run a quasi-steady-state solve
! We want to do this before calculating the output mesh or setting QP data.
IF(p%analysis_type == BD_DYN_SSS_ANALYSIS) THEN
! Solve for the displacements with the acceleration and rotational velocity terms included
! This will set m%qp%aaa, OtherState%Acc, x%q, and x%dqdt
! (note that we won't ramp loads as there are no loads provided yet.)
! if this is not successful, it restores the values of x and sets OtherState%Acc=0
CALL BD_QuasiStatic(u,p,x,OtherState,MiscVar,ErrStat2,ErrMsg2, RampLoad=.false.); if (Failed()) return
QuasiStaticInitialized = ErrStat2 == ErrID_None ! We have now run the quasi-static initialization once, so don't run again.
ELSE
QuasiStaticInitialized = .FALSE.
ENDIF
!.................................
! initialization of output mesh values (used for initial guess to AeroDyn)
if (p%BldMotionNodeLoc==BD_MESH_QP) then
DO nelem=1,p%elem_total
CALL BD_DisplacementQP( nelem, p, x, MiscVar )
CALL BD_RotationalInterpQP( nelem, p, x, MiscVar )
end do
call BD_QPDataVelocity( p, x, MiscVar ) ! set MiscVar%qp%vvv
call BD_QPDataAcceleration( p, OtherState, MiscVar ) ! set MiscVar%qp%aaa
end if
CALL Set_BldMotion_NoAcc(p, x, OtherState, MiscVar, y)
IF(QuasiStaticInitialized) THEN
! Set the BldMotion mesh acceleration but only if quasistatic succeeded
call Set_BldMotion_InitAcc(p,u,OtherState,MiscVar,y)
ELSE
y%BldMotion%TranslationAcc = 0.0_BDKi
y%BldMotion%RotationAcc = 0.0_BDKi
ENDIF
!.................................
! set initialization outputs
call SetInitOut(p, InitOut, errStat2, errMsg2); if (Failed()) return
!...............................................
! Print the summary file if requested:
if (InputFileData%SumPrint) then
call BD_PrintSum( p, x, OtherState, MiscVar, InitInp, ErrStat2, ErrMsg2 ); if (Failed()) return
end if
!...............................................
z%DummyConstrState = 0.0_BDKi
!............................................................................................
! Initialize Jacobian:
!............................................................................................
if (InitInp%Linearize) then
call Init_Jacobian( p, u, y, MiscVar, InitOut, ErrStat2, ErrMsg2); if (Failed()) return
end if
call Cleanup()
return
CONTAINS
SUBROUTINE Cleanup()
if (allocated(GLL_nodes )) deallocate(GLL_nodes )
CALL BD_DestroyInputFile( InputFileData, ErrStat2, ErrMsg2)
END SUBROUTINE Cleanup
logical function Failed()
CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
Failed = ErrStat >= AbortErrLev
if (Failed) call Cleanup()
end function Failed
END SUBROUTINE BD_Init
!-----------------------------------------------------------------------------------------------------------------------------------
!> This subroutine computes the mass (p%Mass0_QP) and stiffness (p%Stif0_QP) matrices at the quadrature nodes.
subroutine InitializeMassStiffnessMatrices(InputFileData,p,ErrStat, ErrMsg)
type(BD_InputFile), intent(in ) :: InputFileData !< data from the input file
type(BD_ParameterType), intent(inout) :: p !< Parameters
integer(IntKi), intent( out) :: ErrStat !< Error status of the operation
character(*), intent( out) :: ErrMsg !< Error message if ErrStat /= ErrID_None
INTEGER(IntKi) :: i ! do-loop counter
INTEGER(IntKi) :: j ! do-loop counter
INTEGER(IntKi) :: idx_qp !< index of current quadrature point in loop
INTEGER(IntKi) :: nelem !< index to the element counter
INTEGER(IntKi) :: k ! do-loop counter
INTEGER(IntKi) :: temp_id
REAL(BDKi) :: temp66(6,6)
REAL(BDKi),ALLOCATABLE :: temp_ratio(:,:) ! array that contains the relative location of each quadrature point along the (curve of the) blade, in [0,1]
REAL(BDKi),PARAMETER :: EPS = 1.0D-10
INTEGER(IntKi) :: idx1
INTEGER(IntKi) :: idx2
integer(intKi) :: ErrStat2 ! temporary Error status
character(ErrMsgLen) :: ErrMsg2 ! temporary Error message
character(*), parameter :: RoutineName = 'InitializeMassStiffnessMatrices'
ErrStat = ErrID_None
ErrMsg = ""
! compute member length ratio w.r.t blade length
CALL BD_MemberEta( InputFileData%member_total, p%QPtWeight, p%Jacobian, p%member_eta, p%blade_length )
CALL AllocAry(p%Stif0_QP,6,6,p%nqp*p%elem_total,'Stif0_QP',ErrStat2,ErrMsg2)
CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%Mass0_QP,6,6,p%nqp*p%elem_total,'Mass0_QP',ErrStat2,ErrMsg2)
CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
if (ErrStat >= AbortErrLev) return
IF(p%quadrature .EQ. GAUSS_QUADRATURE) THEN
! Compute sectional propertities ( 6 by 6 stiffness and mass matrices)
! at Gauss points
!.... Compute the relative location of each quadrature node along the length of the curved blade ....
CALL AllocAry(temp_ratio,p%nqp,p%elem_total,'temp_ratio',ErrStat2,ErrMsg2)
CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
if (ErrStat >= AbortErrLev) then
call cleanup()
return
end if
temp_ratio(:,:) = 0.0_BDKi
DO idx_qp=1,p%nqp
temp_ratio(idx_qp,1) = ((p%QPtN(idx_qp) + 1.0_BDKi)/2.0_BDKi)*p%member_eta(1) ! get QPtN ratio in [0,1] and multiply by member (element)'s relative length along the beam [0,1]
ENDDO
DO i=2,p%elem_total
! add lengths of all previous members (elements)
DO j=1,i-1
temp_ratio(:,i) = temp_ratio(:,i) + p%member_eta(j) ! compute the relative distance along the blade at the start of the member (element)
ENDDO
! then add ratio of length of quadrature point along this member (element)
DO idx_qp=1,p%nqp
temp_ratio(idx_qp,i) = temp_ratio(idx_qp,i) + ((p%QPtN(idx_qp) + 1.0_BDKi)/2.0_BDKi)*p%member_eta(i)
ENDDO
ENDDO
!.... now compute mass and stiffness matrices based on this
DO i=1,p%elem_total
DO idx_qp=1,p%nqp
temp_id = (i-1)*p%nqp+idx_qp
DO k=1,InputFileData%InpBl%station_total
IF(temp_ratio(idx_qp,i) - InputFileData%InpBl%station_eta(k) <= EPS) THEN
IF(ABS(temp_ratio(idx_qp,i) - InputFileData%InpBl%station_eta(k)) <= EPS) THEN
p%Stif0_QP(1:6,1:6,temp_id) = InputFileData%InpBl%stiff0(1:6,1:6,k)
p%Mass0_QP(1:6,1:6,temp_id) = InputFileData%InpBl%mass0(1:6,1:6,k)
ELSE
temp66(1:6,1:6) = (InputFileData%InpBl%stiff0(1:6,1:6,k)-InputFileData%InpBl%stiff0(1:6,1:6,k-1)) / &
(InputFileData%InpBl%station_eta(k) - InputFileData%InpBl%station_eta(k-1))
p%Stif0_QP(1:6,1:6,temp_id) = temp66(1:6,1:6) * temp_ratio(idx_qp,i) + &
InputFileData%InpBl%stiff0(1:6,1:6,k-1) - &
temp66(1:6,1:6) * InputFileData%InpBl%station_eta(k-1)
temp66(1:6,1:6) = (InputFileData%InpBl%mass0(1:6,1:6,k)-InputFileData%InpBl%mass0(1:6,1:6,k-1)) / &
(InputFileData%InpBl%station_eta(k) - InputFileData%InpBl%station_eta(k-1))
p%Mass0_QP(1:6,1:6,temp_id) = temp66(1:6,1:6) * temp_ratio(idx_qp,i) + &
InputFileData%InpBl%mass0(1:6,1:6,k-1) - &
temp66(1:6,1:6) * InputFileData%InpBl%station_eta(k-1)
ENDIF
EXIT
ENDIF
ENDDO ! k=InputFileData%InpBl%station_total
ENDDO ! idx_qp=quadrature point
ENDDO ! i=element
DEALLOCATE(temp_ratio)
ELSEIF(p%quadrature .EQ. TRAP_QUADRATURE) THEN
!bjj: this assumes trap quadrature has only one member.
p%Stif0_QP(1:6,1:6,1) = InputFileData%InpBl%stiff0(1:6,1:6,1)
p%Mass0_QP(1:6,1:6,1) = InputFileData%InpBl%mass0( 1:6,1:6,1)
DO idx_qp = 2,p%nqp
idx2 = (idx_qp-2)/p%refine + 1 ! bjj: integer math here; p%nqp = (station_total-1)*p%refine + 1
idx1 = idx2 + 1
! this is interpolating the mass and stiffness matrices ONLY if p%refine is not 1
p%Stif0_QP(1:6,1:6,idx_qp) = InputFileData%InpBl%stiff0(1:6,1:6,idx2) + &
((InputFileData%InpBl%stiff0(1:6,1:6,idx1) - &
InputFileData%InpBl%stiff0(1:6,1:6,idx2))/p%refine) * &
(MOD(idx_qp-2,p%refine) + 1)
p%Mass0_QP(1:6,1:6,idx_qp) = InputFileData%InpBl%mass0(1:6,1:6,idx2) + &
((InputFileData%InpBl%mass0(1:6,1:6,idx1) - &
InputFileData%InpBl%mass0(1:6,1:6,idx2))/p%refine) * &
(MOD(idx_qp-2,p%refine) + 1)
ENDDO
ENDIF
!FIXME: if we must shift the K and M matrices, this will need to be recaulculated (or calculated elsewhere)
! Initialize some mass and inertia properies at the quadrature points
DO nelem=1,p%elem_total
DO idx_qp=1,p%nqp
p%qp%mmm(idx_qp,nelem) = p%Mass0_QP(1,1,(nelem-1)*p%nqp+idx_qp) ! mass
p%qp%mEta(1,idx_qp,nelem) = -p%Mass0_QP(3,5,(nelem-1)*p%nqp+idx_qp) ! -mass*X_cm term from equation 3.9 (after applying transform to BD coords, (3,5) in original)
p%qp%mEta(2,idx_qp,nelem) = p%Mass0_QP(3,4,(nelem-1)*p%nqp+idx_qp) ! maxx*Y_cm term from equation 3.9 (after applying transform to BD coords, (3,4) in original)
p%qp%mEta(3,idx_qp,nelem) = 0.0_BDKi
ENDDO
ENDDO
call Cleanup()
return
CONTAINS
SUBROUTINE Cleanup()
if (allocated(temp_ratio)) deallocate(temp_ratio)
END SUBROUTINE Cleanup
end subroutine InitializeMassStiffnessMatrices
!-----------------------------------------------------------------------------------------------------------------------------------
!> This subroutine computes the positions and rotations stored in p%uuN0 (output GLL nodes) and p%QuadPt (input quadrature nodes). p%QPtN must be already set.
subroutine InitializeNodalLocations(member_total,kp_member,kp_coordinate,p,GLL_nodes,ErrStat, ErrMsg)
INTEGER(IntKi),INTENT(IN ):: member_total
INTEGER(IntKi),INTENT(IN ):: kp_member(:) !< Number of key points of each member, InputFileData%kp_member from BD input file
REAL(BDKi), INTENT(IN ):: kp_coordinate(:,:) !< Keypoints coordinates, from BD input file InputFileData%kp_coordinate(member key points,1:4);
type(BD_ParameterType), intent(inout) :: p !< Parameters
REAL(BDKi), INTENT(IN ) :: GLL_nodes(:) !< GLL_nodes(p%nodes_per_elem): location of the (p%nodes_per_elem) p%GLL points
!type(BD_InitOutputType), intent(inout) :: InitOut !< initialization output type (for setting z_coordinate variable)
integer(IntKi), intent( out) :: ErrStat !< Error status of the operation
character(*), intent( out) :: ErrMsg !< Error message if ErrStat /= ErrID_None
REAL(BDKi),PARAMETER :: EPS = 1.0D-10
! ----------------------------------------
! local variables
INTEGER(IntKi) :: elem ! do-loop counter
INTEGER(IntKi) :: i ! do-loop counter
INTEGER(IntKi) :: j ! do-loop counter
INTEGER(IntKi) :: k ! do-loop counter
INTEGER(IntKi) :: kp ! do-loop counter
integer(IntKi) :: nkp ! number keypoints for an element
integer(IntKi) :: qfit ! polynomial order used for first fit
REAL(BDKi),allocatable :: least_sq_mat(:,:)
REAL(BDKi),allocatable :: least_sq_rhs(:,:) ! RHS for X,Y,Z,Twist
integer(IntKi),allocatable :: least_sq_indx(:)
REAL(BDKi),allocatable :: least_sq_gll(:)
REAL(BDKi) :: twist
REAL(BDKi) :: tangent(3)
REAL(BDKi),allocatable :: least_sq_shp(:,:)
REAL(BDKi),allocatable :: least_sq_shpder(:,:)
REAL(BDKi),allocatable :: kp_param(:)
INTEGER(IntKi) :: first_kp
INTEGER(IntKi) :: last_kp
REAL(BDKi) :: temp_POS(3)
REAL(BDKi) :: temp_CRV(3)
integer(intKi) :: ErrStat2 ! temporary Error status
character(ErrMsgLen) :: ErrMsg2 ! temporary Error message
character(*), parameter :: RoutineName = 'InitializeNodalLocations'
ErrStat = ErrID_None
ErrMsg = ""
!MIKE
!-------------------------------------------------
! p%uuN0 contains the initial (physical) positions and orientations of the (FE) GLL nodes
!-------------------------------------------------
p%uuN0(:,:,:) = 0.0_BDKi
first_kp = 1 !first key point on member (element)
DO elem=1,p%elem_total
last_kp = first_kp + kp_member(elem) - 1 !last key point of member (element)
nkp = kp_member(elem) ! number of keypoints in this element
if (p%nodes_per_elem .le. nkp) then
qfit = p%nodes_per_elem ! if LSFE points-per-element is less than number of keypoints fit to final poly
else
qfit = nkp ! if points-per-element more that number of keypoints, fit to LSFE with order (nkp-1)
endif
if (qfit .gt. 7) qfit = 7
call AllocAry(least_sq_gll, qfit, "least-squares GLL nodes",ErrStat2, ErrMsg2)
call BD_GenerateGLL(qfit,least_sq_gll,ErrStat2,ErrMsg2)
CALL AllocAry(least_sq_Shp,qfit,nkp,'least-squares fit shp',ErrStat2,ErrMsg2)
CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
CALL AllocAry(least_sq_ShpDer,qfit,nkp,'ShpDer',ErrStat2,ErrMsg2)
CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
CALL AllocAry(kp_param,nkp,'parameterization of keypoints',ErrStat2,ErrMsg2)
CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
! parameterize the keypoint data to [-1,1] based on z-coordinate
kp = first_kp
do j = 1, nkp
kp_param(j) = 2._BDki*(kp_coordinate(kp,3)-kp_coordinate(first_kp,3))/(kp_coordinate(last_kp,3)-kp_coordinate(first_kp,3)) - 1._BDKi
kp = kp + 1
enddo
! Create shape functions evaluated at the kp positions
call BD_diffmtc(qfit,least_sq_gll,kp_param,nkp,least_sq_Shp,least_sq_ShpDer)
CALL AllocAry(least_sq_mat,qfit,qfit,'matrix for least-squares fit',ErrStat2,ErrMsg2)
CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
CALL AllocAry(least_sq_indx,qfit,'indx solving least-squares fit',ErrStat2,ErrMsg2)
CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
CALL AllocAry(least_sq_rhs,qfit,4,'indx solving least-squares fit',ErrStat2,ErrMsg2)
CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
! build the least-squares-fit matrix and RHS vectors
least_sq_mat = 0._BDKi
do i = 1, qfit
do j = 1, qfit
do k = 1, nkp
least_sq_mat(i,j) = least_sq_mat(i,j) + least_sq_shp(i,k)*least_sq_shp(j,k)
enddo
enddo
enddo
least_sq_rhs = 0._BDKi
do j = 1, 4
do i = 1, qfit
kp = first_kp
do k = 1, nkp
least_sq_rhs(i,j) = least_sq_rhs(i,j) + least_sq_shp(i,k)*kp_coordinate(kp,j)
kp = kp+1
enddo
enddo
enddo
! modify linear system so that fitted function captures keypoint endpoints
do i = 1, qfit
least_sq_mat(1,i) = 0._BDKi
least_sq_mat(qfit,i) = 0._BDKi
enddo
least_sq_mat(1,1) = 1._BDKi
least_sq_mat(qfit,qfit) = 1._BDKi
do j = 1,4
least_sq_rhs(1,j) = kp_coordinate(first_kp,j)
least_sq_rhs(qfit,j) = kp_coordinate(last_kp,j)
enddo
! factor matrix system
CALL LAPACK_getrf( qfit, qfit, least_sq_mat, least_sq_indx, ErrStat2, ErrMsg2)
CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
! solve the linear system
CALL LAPACK_getrs( 'N', qfit, least_sq_mat, least_sq_indx, least_sq_rhs, ErrStat2, ErrMsg2)
CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
! we now have qfit LSFE coefficent that are a least squares fit to the keypoint data for XYZT
! next, we calculate the coefficent of the p%nodes_per_elem LSFE for this element
! need to re-evalate qfit-node shape functions at the p%nodes_per_elem GLL points
deallocate(least_sq_shp)
deallocate(least_sq_shpder)
CALL AllocAry(least_sq_Shp,qfit,p%nodes_per_elem,'least-squares fit shp',ErrStat2,ErrMsg2)
CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
CALL AllocAry(least_sq_ShpDer,qfit,p%nodes_per_elem,'ShpDer',ErrStat2,ErrMsg2)
CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
! BD_diffmtc( nodes_per_elem,GLL_nodes,QPtN,nqp,Shp,ShpDer )
call BD_diffmtc(qfit,least_sq_gll,gll_nodes,p%nodes_per_elem,least_sq_Shp,least_sq_ShpDer)
do i = 1, p%nodes_per_elem
! start with XYZ
twist = 0._BDKi
tangent = 0._BDKi
do k = 1, qfit
do j = 1, 3
p%uuN0(j,i,elem) = p%uuN0(j,i,elem) + least_sq_rhs(k,j)*least_sq_shp(k,i)
tangent(j) = tangent(j) + least_sq_rhs(k,j)*least_sq_shpder(k,i)
enddo
twist = twist + least_sq_rhs(k,4)*least_sq_shp(k,i)
enddo
tangent = tangent / TwoNorm(tangent)
CALL BD_ComputeIniNodalCrv(tangent, twist, temp_CRV, ErrStat, ErrMsg)
p%uuN0(4:6,i,elem) = temp_CRV
enddo
! set for next element:
first_kp = last_kp
deallocate(least_sq_gll)
deallocate(least_sq_Shp)
deallocate(least_sq_ShpDer)
deallocate(kp_param)
deallocate(least_sq_mat)
deallocate(least_sq_indx)
deallocate(least_sq_rhs)
ENDDO
return
end subroutine InitializeNodalLocations
!-----------------------------------------------------------------------------------------------------------------------------------
!> This routine calculates the contributions of the integral of shape functions outboard of an FE node. These weighting values are
!! used as part of the integration scheme for the output of the internal forces from the Fc and Fd terms. This is simply a numerical
!! integration of those shape functions.
!! Note from ADP: I don't like this method, but haven't found a better method yet. I think a better approach may be to use the
!! inverse H' matrix and inverse shape functions, but I have not tried deriving that yet.
subroutine Initialize_FEweights(p,GLL_nodes,ErrStat,ErrMsg)
type(BD_ParameterType), intent(inout) :: p !< Parameters
real(BDKi), intent(in ) :: GLL_nodes(:)
integer(IntKi), intent( out) :: ErrStat !< Error status of the operation
character(*), intent( out) :: ErrMsg !< Error message if ErrStat /= ErrID_None
! local variables
integer(IntKi) :: i ! do-loop counter
integer(IntKi) :: nelem ! do-loop counter over number of elements
integer(IntKi) :: IntPtIdx !< index of current quadrature point in loop
real(BDKi) :: SumShp
real(BDKi), allocatable :: Shp(:,:) !< High resolution of Shp functions
real(BDKi), allocatable :: ShpDer(:,:) !< High resolution of ShpDer functions
integer(IntKi) :: IntPoints !< number of points in the high res
REAL(BDKi), allocatable :: EtaVals(:) !< Integeration points along Z, scaled [-1 1]
REAL(BDKi), allocatable :: DistVals(:) !< Integeration points along Z, actual distance
REAL(BDKi) :: ElemLength
integer(intKi) :: ErrStat2 ! temporary Error status
character(ErrMsgLen) :: ErrMsg2 ! temporary Error message
character(*), parameter :: RoutineName = 'Initialize_FEweights'
ErrStat = ErrID_None
ErrMsg = ""
! Set number of points for the integrations. Number chosen based on convergence tests
IntPoints=100001
CALL AllocAry(EtaVals,IntPoints,'Distance along blade for high res Shp functions',ErrStat2,ErrMsg2)
CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
CALL AllocAry(DistVals,IntPoints,'Distance along blade for high res Shp functions',ErrStat2,ErrMsg2)
CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
CALL AllocAry(Shp,p%nodes_per_elem,IntPoints,'Shp',ErrStat2,ErrMsg2)
CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
CALL AllocAry(ShpDer,p%nodes_per_elem,IntPoints,'ShpDer',ErrStat2,ErrMsg2)
CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,RoutineName)
if (ErrStat >= AbortErrLev) then
call Cleanup
return
endif
p%FEweight= 0.0_BDKi
! Loop over the elements in case we change the number of FE points between elements in the future
do nelem=1,p%elem_total
! Find the length of this element (straight line approximation)
ElemLength = TwoNorm(p%uuN0(1:3,p%nodes_per_elem,nelem) - p%uuN0(1:3,1,nelem))
! Setup the corresponding EtaVals for all the integration points
do IntPtIdx=1,IntPoints
EtaVals(IntPtIdx) = REAL(IntPtIdx-1,BDKi)/REAL(IntPoints-1,BDKi)
enddo
! Calculate corresponding distances for the integration region
DistVals = EtaVals*ElemLength
! Scale the EtaVals to [-1 1] range for the diffmtc routine
EtaVals = 2.0_BDKi*EtaVals - 1.0_BDKi
! Get the high resolution Shp functions. We won't use the ShpDer results at all
call BD_diffmtc(p%nodes_per_elem,GLL_nodes,EtaVals,IntPoints,Shp,ShpDer)
! Integrate region outboard shape function contributions to this FE node!
do i=1,p%nodes_per_elem
SumShp=0.0_BDKi
do IntPtIdx=IntPoints,1,-1 ! Step inwards and integrate
if ( DistVals(IntPtIdx) > TwoNorm(p%uuN0(1:3,i,nelem)-p%uuN0(1:3,1,nelem))) THEN
p%FEweight(i,nelem) = p%FEweight(i,nelem) + Shp(i,IntPtIdx)
endif
SumShp=SumShp+Shp(i,IntPtIdx)
enddo
p%FEweight(i,nelem) = p%FEweight(i,nelem) / SumShp
enddo
enddo
call Cleanup
contains
subroutine Cleanup()
if (allocated(EtaVals)) deallocate(EtaVals)
if (allocated(DistVals)) deallocate(DistVals)
if (allocated(EtaVals)) deallocate(Shp)
if (allocated(EtaVals)) deallocate(ShpDer)
end subroutine Cleanup
end subroutine Initialize_FEweights
!-----------------------------------------------------------------------------------------------------------------------------------
SUBROUTINE BD_InitShpDerJaco( GLL_Nodes, p )
! Bauchau chapter 17.1 gives an intro to displacement fields, the shape functions and the jacobian
! see Bauchau equation 17.12
! also https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant
REAL(BDKi), INTENT(IN ) :: GLL_nodes(:) !< p%GLL point locations
TYPE(BD_ParameterType), INTENT(INOUT) :: p !< Parameters
REAL(BDKi) :: Gup0(3)
INTEGER(IntKi) :: i, j
INTEGER(IntKi) :: nelem, idx_qp
CHARACTER(*), PARAMETER :: RoutineName = 'BD_InitShpDerJaco'
CALL BD_diffmtc(p%nodes_per_elem,GLL_nodes,p%QPtN,p%nqp,p%Shp,p%ShpDer)
DO nelem = 1,p%elem_total
DO idx_qp = 1, p%nqp
Gup0(:) = 0.0_BDKi
DO i=1,p%nodes_per_elem
Gup0(:) = Gup0(:) + p%ShpDer(i,idx_qp)*p%uuN0(1:3,i,nelem)
ENDDO
p%Jacobian(idx_qp,nelem) = TwoNorm(Gup0)
ENDDO
ENDDO
! save some variables so we don't recalculate them so often:
DO nelem=1,p%elem_total
DO idx_qp=1,p%nqp
DO j=1,p%nodes_per_elem
DO i=1,p%nodes_per_elem
p%QPtw_Shp_Shp_Jac( idx_qp,i,j,nelem) = p%Shp( i,idx_qp)*p%Shp( j,idx_qp)*p%QPtWeight(idx_qp)*p%Jacobian(idx_qp,nelem)
p%QPtw_ShpDer_ShpDer_Jac(idx_qp,i,j,nelem) = p%ShpDer(i,idx_qp)*p%ShpDer(j,idx_qp)*p%QPtWeight(idx_qp)/p%Jacobian(idx_qp,nelem)
END DO
END DO
END DO
END DO
DO idx_qp=1,p%nqp
DO j=1,p%nodes_per_elem
DO i=1,p%nodes_per_elem
p%QPtw_Shp_ShpDer(idx_qp,i,j) = p%Shp(i,idx_qp)*p%ShpDer(j,idx_qp)*p%QPtWeight(idx_qp)
END DO
END DO
END DO
DO nelem=1,p%elem_total
DO i=1,p%nodes_per_elem
DO idx_qp=1,p%nqp
p%QPtw_Shp_Jac(idx_qp,i,nelem) = p%Shp( i,idx_qp)*p%QPtWeight(idx_qp)*p%Jacobian(idx_qp,nelem)
END DO
END DO
END DO
DO i=1,p%nodes_per_elem
DO idx_qp=1,p%nqp
p%QPtw_ShpDer(idx_qp,i) = p%ShpDer(i,idx_qp)*p%QPtWeight(idx_qp)
END DO
END DO
END SUBROUTINE BD_InitShpDerJaco
!-----------------------------------------------------------------------------------------------------------------------------------
!> This subroutine initializes data in the InitOut type, which is returned to the glue code.
subroutine SetInitOut(p, InitOut, ErrStat, ErrMsg)
type(BD_InitOutputType), intent(inout) :: InitOut !< output data (we've already set InitOut%z_coordinate)
type(BD_ParameterType), intent(in ) :: p !< Parameters
integer(IntKi), intent( out) :: ErrStat !< Error status of the operation
character(*), intent( out) :: ErrMsg !< Error message if ErrStat /= ErrID_None
! Local variables
integer(intKi) :: i ! loop counter
integer(intKi) :: ErrStat2 ! temporary Error status
character(ErrMsgLen) :: ErrMsg2 ! temporary Error message
character(*), parameter :: RoutineName = 'SetInitOut'
! Initialize variables for this routine
errStat = ErrID_None
errMsg = ""
! p%BldNd_BlOutNd contains the list of nodes we are outputting. At each node there are BldNd_NumOuts output channels.
call AllocAry( InitOut%WriteOutputHdr, p%numOuts + p%BldNd_TotNumOuts, 'WriteOutputHdr', errStat2, errMsg2 )
call SetErrStat( errStat2, errMsg2, errStat, errMsg, RoutineName )
call AllocAry( InitOut%WriteOutputUnt, p%numOuts + p%BldNd_TotNumOuts, 'WriteOutputUnt', errStat2, errMsg2 )
call SetErrStat( errStat2, errMsg2, errStat, errMsg, RoutineName )
if (ErrStat >= AbortErrLev) return
do i=1,p%NumOuts
InitOut%WriteOutputHdr(i) = p%OutParam(i)%Name
InitOut%WriteOutputUnt(i) = p%OutParam(i)%Units
end do
InitOut%Ver = BeamDyn_Ver
! Set the info in WriteOutputHdr and WriteOutputUnt for BldNd sections.
CALL BldNdOuts_InitOut( InitOut, p, ErrStat2, ErrMsg2 )
call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
end subroutine SetInitOut
!-----------------------------------------------------------------------------------------------------------------------------------
!> Set the global rotation information -- stored in OtherStates
subroutine InitRefFrame( InitInp, OtherState, ErrStat, ErrMsg )
type(BD_InitInputType), intent(in ) :: InitInp !< Input data for initialization routine
type(BD_OtherStateType), intent(inout) :: OtherState !< Global rotations are stored in otherstate
integer(IntKi), intent( out) :: ErrStat !< Error status of the operation
character(*), intent( out) :: ErrMsg !< Error message if ErrStat /= ErrID_None
integer(intKi) :: ErrStat2 ! temporary Error status
character(ErrMsgLen) :: ErrMsg2 ! temporary Error message
character(*), parameter :: RoutineName = 'InitRefFrame'
ErrStat = ErrID_None
ErrMsg = ""
! Global position vector
OtherState%GlbPos = InitInp%GlbPos
! Global rotation tensor. What comes from the driver may not be a properly formed
! DCM (may have roundoff), so recalculate it from the extracted WM parameters.
OtherState%GlbRot = TRANSPOSE(InitInp%GlbRot) ! matrix that now transfers from local to global (FAST's DCMs convert from global to local)
CALL BD_CrvExtractCrv(OtherState%GlbRot,OtherState%Glb_crv, ErrStat2, ErrMsg2)
CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName)
if (ErrStat >= AbortErrLev) return
CALL BD_CrvMatrixR(OtherState%Glb_crv,OtherState%GlbRot) ! ensure that the rotation matrix is a DCM in double precision (this should be the same as TRANSPOSE(InitInp%GlbRot))
end subroutine InitRefFrame
!-----------------------------------------------------------------------------------------------------------------------------------
!> Set the global rotation information -- stored in OtherStates
!! This only works for u in the global frame!!!!
subroutine SetRefFrame( u, GlbPos, GlbRot, Glb_Crv, ErrStat, ErrMsg )
type(BD_InputType), intent(in ) :: u !< Inputs
real(R8Ki), intent( out) :: GlbPos(3)
real(R8Ki), intent( out) :: GlbRot(3,3)
real(R8Ki), intent( out) :: Glb_crv(3)
integer(IntKi), intent( out) :: ErrStat !< Error status of the operation
character(*), intent( out) :: ErrMsg !< Error message if ErrStat /= ErrID_None
integer(intKi) :: ErrStat2 ! temporary Error status
character(ErrMsgLen) :: ErrMsg2 ! temporary Error message
character(*), parameter :: RoutineName = 'SetRefFrame'
ErrStat = ErrID_None
ErrMsg = ""
! Calculate new global position, rotation, and WM from root motion. Note that this is similar to the InitRefFrame routine
GlbPos = u%RootMotion%Position(:, 1) + &
u%RootMotion%TranslationDisp(:, 1)
GlbRot = transpose(u%RootMotion%Orientation(:, :, 1))
CALL BD_CrvExtractCrv(GlbRot, Glb_crv, ErrStat2, ErrMsg2)
CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName)
CALL BD_CrvMatrixR(Glb_crv, GlbRot)
end subroutine SetRefFrame
!-----------------------------------------------------------------------------------------------------------------------------------
!> This subroutine allocates and initializes most (not all) of the parameters used in BeamDyn.
subroutine SetParameters(InitInp, InputFileData, p, OtherState, ErrStat, ErrMsg)
type(BD_InitInputType), intent(in ) :: InitInp !< Input data for initialization routine
type(BD_InputFile), intent(inout) :: InputFileData !< data from the input file [we may need to shift the keypoint to match a MK matrix eta for trap multi-element]
type(BD_ParameterType), intent(inout) :: p !< Parameters ! intent(out) only because it changes p%NdIndx
type(BD_OtherStateType), intent(in ) :: OtherState !< Global rotations are stored in otherstate
integer(IntKi), intent( out) :: ErrStat !< Error status of the operation
character(*), intent( out) :: ErrMsg !< Error message if ErrStat /= ErrID_None
!local variables
INTEGER(IntKi) :: i, j ! generic counter index
INTEGER(IntKi) :: indx ! counter into index array (p%NdIndx)
INTEGER(IntKi) :: nUniqueQP ! number of unique quadrature points (not double-counting nodes at element boundaries)
REAL(BDKi) :: denom
integer(intKi) :: ErrStat2 ! temporary Error status
character(ErrMsgLen) :: ErrMsg2 ! temporary Error message
character(*), parameter :: RoutineName = 'SetParameters'
ErrStat = ErrID_None
ErrMsg = ""
p%CompAeroMaps = InitInp%CompAeroMaps
! Gravity vector -- inertial frame! This must be multiplied by OtherState%GlbRot to get into the BD rotating reference frame
p%gravity = InitInp%gravity
!....................
! data copied/derived from input file
!....................
! Set solve type
IF (.NOT. InitInp%DynamicSolve) THEN
p%analysis_type = BD_STATIC_ANALYSIS
ELSE
! QuasiStatic only valid with dynamic solves
IF ( InputFileData%QuasiStaticInit ) THEN
p%analysis_type = BD_DYN_SSS_ANALYSIS
ELSE
p%analysis_type = BD_DYNAMIC_ANALYSIS
ENDIF
ENDIF
p%RotStates = InputFileData%RotStates ! Rotate states in linearization?
if (ChangeRefFrame) p%RotStates = .true.
p%RelStates = InputFileData%RelStates ! Define states relative to root motion in linearization?
p%rhoinf = InputFileData%rhoinf ! Numerical damping coefficient: [0,1]. No numerical damping if rhoinf = 1; maximum numerical damping if rhoinf = 0.
p%dt = InputFileData%DTBeam ! Time step size
CALL BD_TiSchmComputeCoefficients(p) ! Compute generalized-alpha time integrator coefficients requires p%rhoinf,p%dt; sets p%coef
p%tngt_stf_fd = InputFileData%tngt_stf_fd ! flag used to compute tangent stiffness matrix using finite differencing
p%tngt_stf_comp = InputFileData%tngt_stf_comp ! flag used to compare finite differenced and analytical tangent stiffness matrix
p%tngt_stf_pert = InputFileData%tngt_stf_pert ! perturbation size used to compute finite differenced tangent stiffness matrix
p%tngt_stf_difftol = InputFileData%tngt_stf_difftol ! tolerance for informing user of significant differences in analytical and fd tangent stiffness
p%ld_retries = InputFileData%load_retries ! Maximum number of iterations in Newton-Raphson algorithm
p%niter = InputFileData%NRMax ! Maximum number of iterations in Newton-Raphson algorithm
p%tol = InputFileData%stop_tol ! Tolerance used in stopping criterion
p%elem_total = InputFileData%member_total ! Total number of elements
p%nodes_per_elem = InputFileData%order_elem + 1 ! Number of GLL nodes per element
p%n_fact = InputFileData%n_fact ! Factorization frequency
p%quadrature = InputFileData%quadrature ! Quadrature method: 1 Gauss 2 Trapezoidal
p%BldMotionNodeLoc = BD_MESH_FE
IF(p%quadrature .EQ. GAUSS_QUADRATURE) THEN
! Number of Gauss points
p%nqp = p%nodes_per_elem !- 1
p%qp_indx_offset = 1 ! we skip the first node on the input mesh (AD needs values at the end points, but BD doesn't use them)
ELSEIF(p%quadrature .EQ. TRAP_QUADRATURE) THEN ! at least one quadrature point associated with each blade station
p%refine = InputFileData%refine
p%nqp = (InputFileData%InpBl%station_total - 1)*p%refine + 1
p%qp_indx_offset = 0
p%BldMotionNodeLoc = BD_MESH_QP ! we want to output y%BldMotion at the blade input property stations, and this will be a short-cut
ENDIF
p%dof_node = 6 ! Degree-of-freedom (DoF) per node
p%node_total = p%elem_total*(p%nodes_per_elem-1) + 1 ! Total number of (finite element) nodes
p%dof_total = p%node_total*p%dof_node ! Total number of (finite element) dofs
p%dof_elem = p%dof_node * p%nodes_per_elem
p%rot_elem = (p%dof_node/2) * p%nodes_per_elem
!................................
! allocate some parameter arrays
!................................
CALL AllocAry(p%member_eta, p%elem_total,'member length ratio array', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%segment_eta,InputFileData%kp_total-1,'segment length ratio array',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%node_elem_idx,p%elem_total,2,'start and end node numbers of elements in p%node_total sized arrays',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%Shp, p%nodes_per_elem,p%nqp, 'p%Shp', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%ShpDer, p%nodes_per_elem,p%nqp, 'p%ShpDer', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%Jacobian,p%nqp, p%elem_total,'p%Jacobian',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%QPtw_Shp_Shp_Jac ,p%nqp,p%nodes_per_elem,p%nodes_per_elem,p%elem_total,'p%QPtw_Shp_Shp_Jac', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%QPtw_Shp_ShpDer ,p%nqp,p%nodes_per_elem,p%nodes_per_elem, 'p%QPtw_Shp_ShpDer', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%QPtw_ShpDer_ShpDer_Jac,p%nqp,p%nodes_per_elem,p%nodes_per_elem,p%elem_total,'p%QPtw_ShpDer_ShpDer_Jac',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%QPtw_Shp_Jac ,p%nqp ,p%nodes_per_elem,p%elem_total,'p%QPtw_Shp_Jac', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%QPtw_ShpDer ,p%nqp ,p%nodes_per_elem, 'p%QPtw_ShpDer', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%uuN0, p%dof_node,p%nodes_per_elem, p%elem_total,'uuN0 (initial position) array',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%rrN0, (p%dof_node/2),p%nodes_per_elem, p%elem_total,'p%rrN0',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%uu0, p%dof_node, p%nqp, p%elem_total,'p%uu0', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%E10, (p%dof_node/2),p%nqp, p%elem_total,'p%E10', ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%FEweight,p%nodes_per_elem,p%elem_total,'p%FEweight array',ErrStat2,ErrMsg2) ; CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
! Quadrature point and weight arrays in natural frame
CALL AllocAry(p%QPtN, p%nqp,'p%QPtN', ErrStat2,ErrMsg2) ; CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%QPtWeight,p%nqp,'p%QPtWeight array',ErrStat2,ErrMsg2) ; CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
! Quadrature mass and inertia terms
CALL AllocAry(p%qp%mmm, p%nqp,p%elem_total, 'p%qp%mmm mass at quadrature point',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
CALL AllocAry(p%qp%mEta, p%dof_node/2,p%nqp,p%elem_total, 'p%qp%mEta at quadrature point',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )