anchor.diff:        udotdot_TP = (/u%TPMesh%TranslationAcc(:,1), u%TPMesh%RotationAcc(:,1)/)
         if (p%Floating) then
            ! >>> New udotdot_TP to body coordinates
  -         !udotdot_TP(1:3) = matmul( u%TPMesh%Orientation(:,:,1), udotdot_TP(1:3) ) 
  -         !udotdot_TP(4:6) = matmul( u%TPMesh%Orientation(:,:,1), udotdot_TP(4:6) ) 
  +         udotdot_TP(1:3) = matmul( u%TPMesh%Orientation(:,:,1), udotdot_TP(1:3) ) 
  +         udotdot_TP(4:6) = matmul( u%TPMesh%Orientation(:,:,1), udotdot_TP(4:6) ) 
         endif
         
         ! State equation
  
positive.code:       call AllocAry(BB, nX, nU, 'BB',    ErrStat2, ErrMsg2 ); if(Failed()) return; BB(:,:) = 0.0_ReKi
        if(nCB>0) then
           BB(nCB+1:nX, 1 :6  ) = 0.0_ReKi
           BB(nCB+1:nX, 13:18 ) = -p%MMB(1:nCB,1:6)
           call AllocAry(dFext_dFmeshk, p%nDOF              , 'dFext',    ErrStat2, ErrMsg2 ); if(Failed()) return
           call AllocAry(dFred_dFmeshk, p%nDOF_red          , 'dFred',    ErrStat2, ErrMsg2 ); if(Failed()) return
           call AllocAry(dFL_dFmeshk  , p%nDOF__L           , 'dFl'  ,    ErrStat2, ErrMsg2 ); if(Failed()) return
  
negative.code:       if (nCB>0) then
           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
  