diff --git a/zUtil_Cxx/include/gt6DUpdatePrimalOps.h b/zUtil_Cxx/include/gt6DUpdatePrimalOps.h
index fc3bf58b4571fbb9fb3c652cc905ae419bd9834a..b6a9e8a443cba7b9c15be7c21509a0255f06143e 100644
--- a/zUtil_Cxx/include/gt6DUpdatePrimalOps.h
+++ b/zUtil_Cxx/include/gt6DUpdatePrimalOps.h
@@ -187,9 +187,10 @@ namespace GT6D {
     const mwSize num_elems = mxGetNumberOfElements(mat_old_solution);
     const mwSize elem_size = mxGetElementSize(mat_old_solution);
 
-    mat_new_solution = mxCreateNumericArray(2, zero_dims, type, mxREAL);
-    mxSetDimensions(mat_new_solution, dims, numDims);
-    mxSetData(mat_new_solution, mxMalloc(elem_size * num_elems));
+    mat_new_solution = mxCreateSharedDataCopy(mat_old_solution);
+//    mat_new_solution = mxCreateNumericArray(2, zero_dims, type, mxREAL);
+//    mxSetDimensions(mat_new_solution, dims, numDims);
+//    mxSetData(mat_new_solution, mxMalloc(elem_size * num_elems));
 
     mat_new_enh_solution = mxCreateNumericArray(2, zero_dims, type, mxREAL);
     mxSetDimensions(mat_new_enh_solution, dims, numDims);
diff --git a/zUtil_Deformation/gt6DUpdatePrimal.m b/zUtil_Deformation/gt6DUpdatePrimal.m
index b1cbec86d4b7bd546fa183b3a4b5068d4f570d4a..8693ae79b94c7664b248c1c2e2587fc886045462 100644
--- a/zUtil_Deformation/gt6DUpdatePrimal.m
+++ b/zUtil_Deformation/gt6DUpdatePrimal.m
@@ -1,21 +1,24 @@
-function [new_sol, new_enh_sol, app_time] = gt6DUpdatePrimal(old_sol, corr_tomo, corr_l1, tau, use_c_function)
+function [curr_sol, new_enh_sol, app_time] = gt6DUpdatePrimal(curr_sol, corr_tomo, corr_l1, tau, use_c_function, num_threads)
 % function [new_sol, new_enh_sol] = gt6DUpdatePrimal(old_sol, corr_tomo, corr_l1, tau, use_c_function)
 %
 
     if (~exist('use_c_function', 'var'))
         use_c_function = true;
     end
+    if (~exist('num_threads', 'var'))
+        num_threads = 1;
+    end
 
     c = tic();
     if (use_c_function)
-        [new_sol, new_enh_sol] = gt6DUpdatePrimal_c(old_sol, corr_tomo, corr_l1, tau);
+        [curr_sol, new_enh_sol] = gt6DUpdatePrimal_c(curr_sol, corr_tomo, corr_l1, tau, num_threads);
     else
         theta = 1;
 
-        v = old_sol + (corr_tomo + corr_l1) .* tau;
+        v = curr_sol + (corr_tomo + corr_l1) .* tau;
         v(v < 0) = 0;
-        new_enh_sol = v + theta .* (v - old_sol);
-        new_sol = v;
+        new_enh_sol = v + theta .* (v - curr_sol);
+        curr_sol = v;
     end
     app_time = toc(c);
 end
\ No newline at end of file