anchor.diff:  
      PRIVATE
      
  -   !............................
  -   ! NOTE: for debugging, add preprocessor definition SD_SUMMARY_DEBUG
  -   !       this will add additional matrices to the SubDyn summary file.
  -   !............................
      TYPE(ProgDesc), PARAMETER  :: SD_ProgDesc = ProgDesc( 'SubDyn', '', '' )
         
      ! ..... Public Subroutines ...................................................................................................
  
positive.code:             rIP0(1:3)   = p%DP0(1:3, iSDNode)
              rIP(1:3)    = matmul(Rb2g, rIP0)
              duP(1:3)    = rIP - rIP0 ! NOTE: without m%u_TP(1:3)
              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)
              ! Full diplacements Guyan + rotated CB (if asked) >>> Rotate All
              !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)
           enddo
        endif 
     endif ! U_full no provided
  
negative.code:    CALL SD_Input_ExtrapInterp( u, utimes, u_interp, t, ErrStat2, ErrMsg2 ); CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,'SD_AM2')
     CALL GetExtForceOnInternalDOF(u_interp, p, x, m, m%F_L, ErrStat2, ErrMsg2, ExtraMoment=(p%ExtraMoment.and..not.p%Floating), RotateLoads=(p%ExtraMoment.and.p%Floating))
     m%udotdot_TP = (/u_interp%TPMesh%TranslationAcc(:,1), u_interp%TPMesh%RotationAcc(:,1)/)
     if (p%Floating) then
        ! >>> Rotate All - udotdot_TP to body coordinates
        m%udotdot_TP(1:3) = matmul(u_interp%TPMesh%Orientation(:,:,1), m%udotdot_TP(1:3)) 
        m%udotdot_TP(4:6) = matmul(u_interp%TPMesh%Orientation(:,:,1), m%udotdot_TP(4:6)) 
     endif
                  
     ! extrapolate u to find u_interp = u(t + dt)=u_n+1
     CALL SD_Input_ExtrapInterp(u, utimes, u_interp, t+p%SDDeltaT, ErrStat2, ErrMsg2); CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,'SD_AM2')
     CALL GetExtForceOnInternalDOF(u_interp, p, x, m, F_L2, ErrStat2, ErrMsg2, ExtraMoment=(p%ExtraMoment.and..not.p%Floating), RotateLoads=(p%ExtraMoment.and.p%Floating))
     udotdot_TP2 = (/u_interp%TPMesh%TranslationAcc(:,1), u_interp%TPMesh%RotationAcc(:,1)/)
     if (p%Floating) then
        ! >>> Rotate All - udotdot_TP to body coordinates
        udotdot_TP2(1:3) = matmul(u_interp%TPMesh%Orientation(:,:,1), udotdot_TP2(1:3)) 
        udotdot_TP2(4:6) = matmul(u_interp%TPMesh%Orientation(:,:,1), udotdot_TP2(4:6)) 
     endif
     
     ! calculate (u_n + u_n+1)/2
     udotdot_TP2 = 0.5_ReKi * ( udotdot_TP2 + m%udotdot_TP )
  