         // The XSPH vector already included in the shifting vector
         CopySortedToOriginal_XSPH_D<<<numBlocks1, numThreads1>>>(
             mR3CAST(sortedXSPHandShift), mR3CAST(fsiGeneralData->vel_XSPH_D),
-            U1CAST(markersProximityD->gridMarkerIndexD), 
-            U1CAST(fsiGeneralData->activityIdentifierD), numObjectsH->numAllMarkers);
+            U1CAST(markersProximityD->gridMarkerIndexD),
+            U1CAST(fsiGeneralData->activityIdentifierD),
+            U1CAST(markersProximityD->mapOriginalToSorted),
+            numObjectsH->numAllMarkers);
     }else{
         thrust::device_vector<Real4> sortedPosRad_old = sortedSphMarkersD->posRadD;
         thrust::fill(vel_XSPH_Sorted_D.begin(), vel_XSPH_Sorted_D.end(), mR3(0.0));

        mR3CAST(sortedDerivTauXyXzYz), mR4CAST(fsiGeneralData->derivVelRhoD_old),
        mR3CAST(fsiGeneralData->derivTauXxYyZzD), mR3CAST(fsiGeneralData->derivTauXyXzYzD),
        U1CAST(markersProximityD->gridMarkerIndexD), U1CAST(fsiGeneralData->activityIdentifierD),
        numObjectsH->numAllMarkers);

    sortedDerivVelRho.clear();
    sortedDerivTauXxYyZz.clear();

                                            Real3* originalXSPH,
                                            uint* gridMarkerIndex,
                                            uint* activityIdentifierD,
                                            uint* mapOriginalToSorted,
                                            const size_t numAllMarkers) {
    uint id = blockIdx.x * blockDim.x + threadIdx.x;
    if (id >= numAllMarkers)
        return;

    // Check the activity of this particle
    uint activity = activityIdentifierD[id];
    if(activity == 0)
        return;

    uint index = mapOriginalToSorted[id];

    originalXSPH[id] = sortedXSPH[index];
}

//--------------------------------------------------------------------------------------------------------------------------------
