anchor.diff:        if (p%nDOFM>0) then
            if (p%Floating .and. present(u)) then
               ! TODO TODO rotate it A MBmmB A^t
  -            !DD(1:3,:) = DD(1:3,:) + matmul(transpose(u%TPMesh%Orientation(:,:,1)), p%MBmmB(1:3,:) ! >>> New 
  +            !DD(1:3,:) = DD(1:3,:) + matmul(transpose(u%TPMesh%Orientation(:,:,1)), p%MBmmB(1:3,:) ! >>> Rotate All
               DD(1:nY,13:18 ) = DD(1:nY,13:18 )+ p%MBmmB
            else
               DD(1:nY,13:18 ) = DD(1:nY,13:18 )+ p%MBmmB
  
positive.code:       m%UL_dotdot     = 0.0_ReKi
        ! --- CB modes contribution to motion (L-DOF only)
        if ( p%nDOFM > 0) then
           if (p%Floating) then ! >>> New
              udotdot_TP(1:3) = matmul(Rg2b, u%TPMesh%TranslationAcc( :,1))
              udotdot_TP(4:6) = matmul(Rg2b, u%TPMesh%RotationAcc(:,1)    )
           else
              udotdot_TP = (/u%TPMesh%TranslationAcc( :,1), u%TPMesh%RotationAcc(:,1)/)
           endif
           m%UL            = matmul( p%PhiM,  x%qm    )
           m%UL_dot        = matmul( p%PhiM,  x%qmdot )
           m%UL_dotdot     = matmul( p%C2_61, x%qm    )    + matmul( p%C2_62   , x%qmdot )    & 
  
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 )
  