How to optimize YUV to RGB color conversion code

Question

I have written a function to convert an image in YUV420P to RGB but it is taking 30 millisecond to convert an image (size: 1280 x 720) into RGB, but when I am using ffmpeg function ( as this) to convert YUV image into RGB its taking only 2 millisecond for the same image. What is the problem with my code ? How can I optimize the code that I have written ?? My code is given below

 int step = origImage->widthStep;
 uchar *data = (uchar *)origImage->imageData; 
 int size = origImage->width * origImage->height;
 IplImage* img1 = cvCreateImage(cvGetSize(origImage), IPL_DEPTH_8U, 3);

    for (int i = 0; i<origImage->height; i++)
    {
      for (int j=0; j<origImage->width; j++)
      {
        float Y = data[i*step + j];
        float U = data[ (int)(size + (i/2)*(step/2)  + j/2) ];
        float V = data[ (int)(size*1.25 + (i/2)*(step/2) + j/2)];

        float R = Y + 1.402 * (V - 128);
        float G = Y - 0.344 * (U - 128) - 0.714 * (V - 128);
        float B = Y + 1.772 * (U - 128);


        if (R < 0){ R = 0; } if (G < 0){ G = 0; } if (B < 0){ B = 0; }
        if (R > 255 ){ R = 255; } if (G > 255) { G = 255; } if (B > 255) { B = 255; }

        cvSet2D(img1, i, j,cvScalar(B,G,R));
      }
    }

Answer 1

Here, try this(should reduce to 25 milliseconds):

 int step = origImage->widthStep;
 uchar *data = (uchar *)origImage->imageData; 
 int size = origImage->width * origImage->height;
 IplImage* img1 = cvCreateImage(cvGetSize(origImage), IPL_DEPTH_8U, 3);

    int stepDb2=step /2;
    float sizeMb1d25=size*1.25 ;
    int origImagePTheight=origImage->height;
    int origImagePTwidth=origImage->width;
    for (int i = 0; i<origImagePTheight; i++)
    {
      float idb2=i/2;
      int iStep=i*step;
      for (int j=0; j<origImagePTwidth; j++)
      {
        float variable=idb2*stepDb2  + j/2;
        float Y = data[iStep + j];
        float U = -128 + data[ (int)(size + variable) ];
        float V = -128 + data[ (int)(sizeMb1d25 + variable)];

        float R = Y + 1.402 * V ;
        float G = Y - 0.344 * U - 0.714 * V;
        float B = Y + 1.772 * U;

        R= R * !(R<0);
        G= G * !(G<0);
        B= B * !(B<0);

        R=R*(!(R>255)) + 255 * (R>255);
        G=G*(!(G>255)) + 255 * (G>255);
        B=B*(!(B>255)) + 255 * (B>255);

        cvSet2D(img1, i, j,cvScalar(B,G,R));
      }
    }