     if(x<w && y<h)
     {
         size_t idx = x + (size_t)y*w;
-        float c = tau/theta;
+
         float d = dd[idx];
+        float p1 = p[idx] + (tau/theta)*d;
+        float p2 = p[idx+(size_t)h*w] + (tau/theta)*d;
+        float p3 = p[idx+(size_t)2*h*w] + (tau/theta)*d;
+        float maxDenom = fmax(1, sqrtf(powf(p1, 2) + powf(p2, 2) + powf(p3, 2)));
         
-        for(int j=0; j<3; j++)
-        {
-            size_t idxOffset = idx + (size_t)j*h*w;
-
-            float p1 = p[idxOffset] + c*d;
-            float p2 = p[idxOffset+(size_t)h*w] + c*d;
-            float p3 = p[idxOffset+(size_t)2*h*w] + c*d;
-            
-            p[idxOffset]                = p1/fmax(1, sqrtf(powf(p1, 2) + powf(p2, 2) + powf(p3, 2)));
-            p[idxOffset+(size_t)h*w]    = p2/fmax(1, sqrtf(powf(p1, 2) + powf(p2, 2) + powf(p3, 2)));
-            p[idxOffset+(size_t)2*h*w]  = p3/fmax(1, sqrtf(powf(p1, 2) + powf(p2, 2) + powf(p3, 2)));
-            /*
-            p[idxOffset]                = p1/fmax(1, fabs(p1));
-            p[idxOffset+(size_t)h*w]    = p2/fmax(1, fabs(p2));
-            p[idxOffset+(size_t)2*h*w]  = p3/fmax(1, fabs(p3));
-            */
-        }
+        p[idx]                = p1/maxDenom;
+        p[idx+(size_t)h*w]    = p2/maxDenom;
+        p[idx+(size_t)2*h*w]  = p3/maxDenom;
     }
 }
 

            ComputeDualVariable<<<grid, block>>>(dDiaD, dP, IMG_WIDTH, IMG_HEIGHT, theta, tau);
            cudaDeviceSynchronize();
        }

        theta *= decay;
    }

    DiamondDotProduct<<<grid, block>>>(dP, dD, IMG_WIDTH, IMG_HEIGHT);

    if(x<w && y<h)
    {
        size_t idx = x + (size_t)y*w;

        float pp = p[idx];
        float a1 = 0.0f;    float a2 = 0.0f;    float a3 = 0.0f;
        float b1 = 0.0f;    float b2 = 0.0f;
