                                             Real3* originalXSPH,
                                             uint* gridMarkerIndex,
                                             uint* activityIdentifierD,
+                                            uint* mapOriginalToSorted,
                                             const size_t numAllMarkers) {
     uint id = blockIdx.x * blockDim.x + threadIdx.x;
     if (id >= numAllMarkers)
         return;
 
-    uint index = gridMarkerIndex[id];
-
     // Check the activity of this particle
-    uint activity = activityIdentifierD[index];
+    uint activity = activityIdentifierD[id];
     if(activity == 0)
         return;
 
-    originalXSPH[index] = sortedXSPH[id];
+    uint index = mapOriginalToSorted[id];
+
+    originalXSPH[id] = sortedXSPH[index];
 }
 
 //--------------------------------------------------------------------------------------------------------------------------------


        CopySortedToOriginal_XSPH_D<<<numBlocks1, numThreads1>>>(
            mR3CAST(vel_XSPH_Sorted_D), mR3CAST(fsiGeneralData->vel_XSPH_D),
            U1CAST(markersProximityD->gridMarkerIndexD), 
            U1CAST(fsiGeneralData->activityIdentifierD), numObjectsH->numAllMarkers);
    }

    if (density_initialization % paramsH->densityReinit == 0)

        // 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),
            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));
