C---- SUB. NODAL_FORCE

      SUBROUTINE NODAL_FORCE

     &          (ID,TIME,UPAR,NPAR,POS,VEL,JFLAG,IFLAG,RESULT)

C---- TO EXPORT * SUBROUTINE

      !DEC$ ATTRIBUTES DLLEXPORT,C::NODAL_FORCE

C---- INCLUDE SYSTEM CALL

      INCLUDE 'SYSCAL.F'

C---- DEFINE VARIABLES

C     Parameter Information

C     ID     : Nodal Force sequential identification. (Input)

C     TIME   : Simulation time of RD/Solver. (Input)

C     UPAR   : Parameters defined by user. (Input)

C     NPAR   : Number of user parameters. (Input)

C     POS    : Nodal position w.r.t. Inertia REF. (Input)

C     VEL    : Nodal velocity w.r.t. Inertia REF. (Input)

C     JFLAG  : When RD/Solver evaluates a Jacobian, the flag is true. (Input)

C     IFLAG  : When RD/Solver initializes arraies, the flag is true. (Input)

C     RESULT : Returned nodal force vector. (Input, Size : 6)

      DOUBLE PRECISION TIME, UPAR(*),POS(12),VEL(6)

      INTEGER ID, NPAR

      LOGICAL JFLAG, IFLAG

      DOUBLE PRECISION RESULT[REFERENCE](6)

C---- USER STATEMENT

      INTEGER ActionMarker, BaseMarker

      INTEGER ifTXActive, ifTYActive, ifTZActive

      INTEGER ifRXActive, ifRYActive, ifRZActive

      INTEGER nfid, ifbody, nodeseq, nodeid

      INTEGER mkid(3), i, err

      LOGICAL errflg

      DOUBLE PRECISION TK, TC, RK, RC

      DOUBLE PRECISION djip(3), vjip(3)

      DOUBLE PRECISION Phijip(3), wjip(3), fp(6)

      DOUBLE PRECISION A(9), ea313(3)

      DOUBLE PRECISION PI

      CHARACTER msg*256

      PI=dacos(-1.0d0)

      IF (NPAR .LT. 6) THEN

        CALL ERRMES(1000,'Number of the input parameter must be 6'

     &              , ID, 'NodalForceUSUB')

        GOTO 10

      ENDIF

      ActionMarker = UPAR(1)

      BaseMarker = UPAR(2)

      TK = UPAR(3)

      TC = UPAR(4)

      RK = UPAR(5)

      RC = UPAR(6)

      IF (NPAR .LT. 12) THEN

        ifTXActive = 1

        ifTYActive = 1

        ifTZActive = 1

        ifRXActive = 1

        ifRYActive = 1

        ifRZActive = 0

      ELSE

        ifTXActive = UPAR(7)

        ifTYActive = UPAR(8)

        ifTZActive = UPAR(9)

        ifRXActive = UPAR(10)

        ifRYActive = UPAR(11)

        ifRZActive = UPAR(12)

      ENDIF

      nfid = 0;

           ifbody = 0;

           nodeseq = 0;

           nodeid = 0;

           err = 0;

           errflg = 0;

      CALL GET_NFORCE_ADDARG(nfid,ifbody,nodeseq,nodeid,err)

      IF(IFLAG) THEN

        WRITE(msg,*) 'Nodal Force Seq = ', nfid

        CALL PRINTMSG(msg//char(0), len(trim(msg)))

        WRITE(msg,*) 'FFlex Body Seq = ', ifbody

        CALL PRINTMSG(msg//char(0), len(trim(msg)))

        WRITE(msg,*) 'Current Node Seq = ', nodeseq

        CALL PRINTMSG(msg//char(0), len(trim(msg)))

        WRITE(msg,*) 'Current Node ID = ', nodeid

        CALL PRINTMSG(msg//char(0), len(trim(msg)))

      ENDIF

      mkid(1) = ActionMarker

      mkid(2) = BaseMarker

      mkid(3) = ActionMarker

      CALL SYSARY('TDISP', mkid, 3, djip, 3, errflg)

      CALL SYSARY('TVEL', mkid, 3, vjip, 3, errflg)

      CALL SYSARY('RVEL', mkid, 3, wjip, 3, errflg)

      mkid(1) =  BaseMarker

      mkid(2) =  ActionMarker

      CALL SYSFNC('ROLL', mkid, 2, Phijip(1), errflg)

      CALL SYSFNC('PITCH', mkid, 2, Phijip(2), errflg)  

      CALL SYSFNC('YAW', mkid, 2, Phijip(3), errflg)

      Phijip(1)=Phijip(1)*PI/180.0d0

      Phijip(2)=Phijip(2)*PI/180.0d0

      Phijip(3)=Phijip(3)*PI/180.0d0

      mkid(1) =  ActionMarker

      CALL SYSARY('RDISP', mkid, 1, ea313, 3, errflg)

      CALL ORIENTATIONMATRIX(313, ea313, A, err)

      DO i= 1, 3

        fp(i)   = -djip(i)*TK-vjip(i)*TC

        fp(i+3) = Phijip(i)*RK-wjip(i)*RC

      ENDDO

      IF(ifTXActive .EQ. 0) fp(1) = 0.0d0

      IF(ifTYActive .EQ. 0) fp(2) = 0.0d0

      IF(ifTZActive .EQ. 0) fp(3) = 0.0d0

      IF(ifRXActive .EQ. 0) fp(4) = 0.0d0

      IF(ifRYActive .EQ. 0) fp(5) = 0.0d0

      IF(ifRZActive .EQ. 0) fp(6) = 0.0d0  

      call ASP(result, A, fp)

      call ASP(result(4), A, fp(4))

10    RETURN

      END

      SUBROUTINE ASP(s, A, sp)

      DOUBLE PRECISION s(3), A(9), sp(3)

      s(1) = A(1)*sp(1)+A(4)*sp(2)+A(7)*sp(3);

      s(2) = A(2)*sp(1)+A(5)*sp(2)+A(8)*sp(3);

      s(3) = A(3)*sp(1)+A(6)*sp(2)+A(9)*sp(3);

      RETURN

      END