             gradW = gradW_new;
         }
         // Calculate dv/dt
-        derivVelRho += DifVelocityRho_ElasticSPH(gradW, dist3, d, invd, sortedPosRad[index], sortedPosRad[j], velMasA, velMasB,
-                                                 rhoPresMuA, rhoPresMuB, TauXxYyZzA, TauXyXzYzA, TauXxYyZzB, TauXyXzYzB);
+        derivVelRho += DifVelocityRho_ElasticSPH(gradW, dist3, d, invd, sortedPosRad[index], sortedPosRad[j], 
+            velMasA, velMasB, rhoPresMuA, rhoPresMuB, TauXxYyZzA, TauXyXzYzA, TauXxYyZzB, TauXyXzYzB);
         // Calculate dsigma/dt
         if (sortedRhoPreMu[index].w < -0.5) {
             // start to calculate the stress rate

            // Integration of the kernel function
            sum_w_i += Wab * paramsD.volume0;
            // XSPH
            if (rhoPresMuB.w > -1.5 && rhoPresMuB.w < -0.5) {
                deltaV += paramsD.volume0 * (velMasB - velMasA) * Wab;
            }
            N_ = N_ + 1;
        }
        // Find particles that have contact with this particle

                        Real3 velMasB = sortedVelMas[j];
                        Real rho_bar = 0.5 * (rhoPreMuA.x + rhoPresMuB.x);
                        deltaV += paramsD.markerMass * (velMasB - velMasA) *
                            W3h(d, paramsD.HSML) / rho_bar;
                    }
                }
            }
        }
    }

    vel_XSPH_Sorted_D[index] = 
        paramsD.EPS_XSPH * deltaV + sortedXSPHandShift[index] * paramsD.INV_dT;

    if (!(isfinite(vel_XSPH_Sorted_D[index].x) && 
        isfinite(vel_XSPH_Sorted_D[index].y) && isfinite(vel_XSPH_Sorted_D[index].z))) {
        printf("Error! particle vXSPH is NAN: thrown from ChFsiForceExplicitSPH.cu, newVel_XSPH_D !\n");
        *isErrorD = true;
    }
}

//--------------------------------------------------------------------------------------------------------------------------------
__global__ void CopySortedToOriginal_D(Real4* sortedDerivVelRho,
                                       Real3* sortedDerivTauXxYyZz,
                                       Real3* sortedDerivTauXyXzYz,
