[Fortran] 纯文本查看 复制代码
SUBROUTINE FUNCTIONS_GUIDING_CONSTRAINTS
C
C.....Use the module MUBODYNA_ARRAYS defined in the main routine
USE MUBODYNA_ARRAYS
C
C.....Use the module NUMERICAL_LIBRARIES
USE NUMERICAL_LIBRARIES
C
C.....Force explicit declaration of variables
IMPLICIT NONE
C
C... Specifications for local variables
INTEGER NINTV, NOUT, IEQUAL
REAL*8 BREAK(NGD),CSCOEF(4,NGD),XDATA(NGD),FDATA(NGD)
C
C.....Evaluate functions and jacobian
DO K=1,NGC
I=GUIDING_DATA(1,K)
J=GUIDING_DATA(2,K)
DO L=1, NGD
XDATA(L)=GUIDING_COORDINATES(L,1) !time parameters
FDATA(L)=GUIDING_COORDINATES(L,K+1)
END DO
C
C........Use Cubic Spline Interpolation available in NUMERICAL_LIBRARIES module
CALL DCSSCV (NGD,XDATA,FDATA,IEQUAL,BREAK,CSCOEF)
C
C........Get output unit number.
CALL UMACH (2, NOUT)
NINTV=NGD-1
C
C........Evaluate constraint equations for positions using Cubic Spline
C........Evaluation of Derivative
CONSTRAINT_POSITIONS(ROW+1)=
+ POSITIONS(J,I)-DCSDER(0,TIME,NINTV,BREAK,CSCOEF)
C
C........Evaluate right-hand-side of velocity equations using Cubic Spline
C........Evaluation of Derivative
RIGHT_HAND_SIDE_VELOCITIES(ROW+1)=
+ DCSDER(1,TIME,NINTV,BREAK,CSCOEF)
C
C........Evaluate right-hand-side of acceleration equations using Cubic Spline
C........Evaluation of Derivative
RIGHT_HAND_SIDE_ACCELERATIONS(ROW+1)=
+ DCSDER(2,TIME,NINTV,BREAK,CSCOEF)
C
C........Evaluate Jacobian matrix nonzero entires
ICI= 3*(I-1)
JACOBIAN_MATRIX(ROW+1,ICI+J)= 1.0D0
ROW=ROW+1
END DO
C
RETURN
C
END SUBROUTINE FUNCTIONS_GUIDING_CONSTRAINTS