anchor.diff:     !Start by getting u_n and u_n+1 
      ! interpolate u to find u_interp = u(t) = u_n     
      CALL SD_Input_ExtrapInterp( u, utimes, u_interp, t, ErrStat2, ErrMsg2 ); CALL SetErrStat(ErrStat2,ErrMsg2,ErrStat,ErrMsg,'SD_AM2')
  -   CALL GetExtForceOnInternalDOF(u_interp, p, m, m%F_L, ErrStat2, ErrMsg2, ExtraMoment=(p%ExtraMoment.and..not.p%Floating), RotateLoads=(p%ExtraMoment.and.p%Floating))
  +   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
         ! >>> New 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)) 
  +      !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, m, F_L2, ErrStat2, ErrMsg2, ExtraMoment=(p%ExtraMoment.and..not.p%Floating), RotateLoads=(p%ExtraMoment.and.p%Floating))
  +   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
         ! >>> New 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)) 
  +      !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
  
positive.code:       endif
  
        ! Compute external force on internal (F_L) and interface nodes (F_I)
        call GetExtForceOnInternalDOF(u, p, m, m%F_L, ErrStat2, ErrMsg2, ExtraMoment=(p%ExtraMoment), RotateLoads=.False.); if(Failed()) return
        call GetExtForceOnInterfaceDOF(p, m%Fext, F_I)
  
        ! Compute 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
           if (p%Floating) then
              Y1_Utp  = Y1_Utp + matmul(RRb2g, matmul(p%MBmmB, matmul(transpose(RRb2g), m%udotdot_TP)))  !>>> New
           else
              Y1_Utp  = Y1_Utp + matmul(p%MBmmB, m%udotdot_TP)  
           endif
        endif
        if ( p%nDOFM > 0) then
           Y1_CB = -( matmul(p%C1_11, x%qm)   + matmul(p%C1_12,x%qmdot))
           if (p%Floating) then
              Y1_CB = matmul(RRb2g, Y1_CB) !>>> New 
           endif
        else
           Y1_CB = 0.0_ReKi
        endif
        Y1_Guy_R =   matmul( F_I, p%TI )
        Y1_Guy_L = - matmul(p%D1_142, m%F_L) 
        if (.not.(p%ExtraMoment.and.p%Floating)) then
           Y1_CB_L = - (matmul(p%D1_141, m%F_L)) ! Uses non rotated loads
        endif
        if (p%Floating) then
           Y1_CB_L = matmul(RRb2g, Y1_CB_L) !>>> New
        endif
  
        Y1 = Y1_CB + Y1_Utp + Y1_CB_L+ Y1_Guy_L + Y1_Guy_R 
  
negative.code:          CC(1:nY,1:nCB )   = - p%C1_11
           CC(1:nY,nCB+1:nX) = - p%C1_12
           if (p%Floating .and. present(u)) then
              CC(1:3,:) = matmul(transpose(u%TPMesh%Orientation(:,:,1)), CC(1:3,:)) ! >>> New 
              CC(4:6,:) = matmul(transpose(u%TPMesh%Orientation(:,:,1)), CC(4:6,:)) ! >>> New 
           endif
        endif
     endif
  