private void gather(double[] z, SparseVector v, double taui, int d) {
        // Number of entries in the lower and upper part of the original matrix
        int nl = 0, nu = 0;
        for (VectorEntry e : v) {
            if (e.index() < d)
                nl++;
            else if (e.index() > d)
                nu++;
        }
        v.zero();

        // Entries in the L part of the vector
        lower.clear();
        for (int i = 0; i < d; ++i)
            if (Math.abs(z[i]) > taui)
                lower.add(new IntDoubleEntry(i, z[i]));

        // Entries in the U part of the vector
        upper.clear();
        for (int i = d + 1; i < z.length; ++i)
            if (Math.abs(z[i]) > taui)
                upper.add(new IntDoubleEntry(i, z[i]));

        // Sort in descending order
        Collections.sort(lower);
        Collections.sort(upper);

        // Always keep the diagonal
        v.set(d, z[d]);

        // Keep at most nl+p lower entries
        for (int i = 0; i < Math.min(nl + p, lower.size()); ++i) {
            IntDoubleEntry e = lower.get(i);
            v.set(e.index, e.value);
        }

        // Keep at most nu+p upper entries
        for (int i = 0; i < Math.min(nu + p, upper.size()); ++i) {
            IntDoubleEntry e = upper.get(i);
            v.set(e.index, e.value);
        }
    }