/// <summary>
        /// Fills the whole WriteableBitmap with a color.
        /// </summary>
        /// <param name="bmp">The WriteableBitmap.</param>
        /// <param name="color">The color used for filling.</param>
        public static void Clear(this WriteableBitmap bmp, Color color)
        {
            // Add one to use mul and cheap bit shift for multiplicaltion
            int a = color.A + 1;
            int col = (color.A << 24)
                      | ((byte) ((color.R*a) >> 8) << 16)
                      | ((byte) ((color.G*a) >> 8) << 8)
                      | ((byte) ((color.B*a) >> 8));

            int w = bmp.PixelWidth;
            int h = bmp.PixelHeight;
            #if SILVERLIGHT
             var pixels = bmp.Pixels;
             var len = w*SizeOfArgb;
            #else
            bmp.Lock();
            unsafe
            {
                var pixels = (int*) bmp.BackBuffer;
                int len = bmp.BackBufferStride;
            #endif

                // Fill first line
                for (int x = 0; x < w; x++)
                {
                    pixels[x] = col;
                }

                // Copy first line
                int blockHeight = 1;
                int y = 1;
                while (y < h)
                {
            #if SILVERLIGHT
                 Buffer.BlockCopy(pixels, 0, pixels, y*len, blockHeight*len);
            #else
                    NativeMethods.CopyUnmanagedMemory((IntPtr) pixels, 0, (IntPtr) pixels, y*len, blockHeight*len);
            #endif
                    y += blockHeight;
                    blockHeight = Math.Min(2*blockHeight, h - y);
                }

            #if !SILVERLIGHT
            }
            bmp.AddDirtyRect(new Int32Rect(0, 0, bmp.PixelWidth, bmp.PixelHeight));
            bmp.Unlock();
            #endif
        }
        /// <summary>
        /// Clear all pixel with given color.
        /// </summary>
        /// <param name="wBitmap">Must not be null, or pixel width, pixel height equal to zero</param>
        /// <exception>Exception should be expect with null writeablebitmap or pixel width, height equal to zero.</exception>
        internal static void Clear(this WriteableBitmap wBitmap, Color color)
        {
            if (!(wBitmap.Format == PixelFormats.Pbgra32 || wBitmap.Format == PixelFormats.Gray8))
                return;
            byte[] col = ConvertColor(wBitmap.Format, color);

            //Currently only support two PixelFormat.
            int sizeOfColor = wBitmap.Format == PixelFormats.Pbgra32 ? 4 : 1;

            int pixelW = wBitmap.Format == PixelFormats.Gray8 ? wBitmap.BackBufferStride : wBitmap.PixelWidth;
            int pixelH = wBitmap.PixelHeight;
            int totalPixels = pixelW * pixelH;

            wBitmap.Lock();
            unsafe
            {
                byte* pixels = (byte*)wBitmap.BackBuffer;
                int length = col.Length;
                //Draw first dot color. 
                for (int index = 0; index < length; index++)
                {
                    *(pixels + index) = col[index];
                }

                int pixelIndex = 1;
                int blockPixels = 1;
                //Expand to all other pixels with Log(n) process. 
                while (pixelIndex < totalPixels)
                {
                    CopyUnmanagedMemory(pixels, 0, pixels, pixelIndex * sizeOfColor, blockPixels * sizeOfColor);

                    pixelIndex += blockPixels;
                    blockPixels = Math.Min(2 * blockPixels, totalPixels - pixelIndex);
                }
            }
            wBitmap.AddDirtyRect(new Int32Rect(0, 0, wBitmap.PixelWidth, wBitmap.PixelHeight));
            wBitmap.Unlock();

        }
        /// <summary>
        /// Draws a cubic BeziƩr spline defined by start, end and two control points.
        /// </summary>
        /// <param name="bmp">The WriteableBitmap.</param>
        /// <param name="x1">The x-coordinate of the start point.</param>
        /// <param name="y1">The y-coordinate of the start point.</param>
        /// <param name="cx1">The x-coordinate of the 1st control point.</param>
        /// <param name="cy1">The y-coordinate of the 1st control point.</param>
        /// <param name="cx2">The x-coordinate of the 2nd control point.</param>
        /// <param name="cy2">The y-coordinate of the 2nd control point.</param>
        /// <param name="x2">The x-coordinate of the end point.</param>
        /// <param name="y2">The y-coordinate of the end point.</param>
        /// <param name="color">The color.</param>
        public static void DrawBezier(this WriteableBitmap bmp, int x1, int y1, int cx1, int cy1, int cx2, int cy2,
                                      int x2, int y2, int color)
        {
            // Determine distances between controls points (bounding rect) to find the optimal stepsize
            int minX = Math.Min(x1, Math.Min(cx1, Math.Min(cx2, x2)));
            int minY = Math.Min(y1, Math.Min(cy1, Math.Min(cy2, y2)));
            int maxX = Math.Max(x1, Math.Max(cx1, Math.Max(cx2, x2)));
            int maxY = Math.Max(y1, Math.Max(cy1, Math.Max(cy2, y2)));

            // Get slope
            int lenx = maxX - minX;
            int len = maxY - minY;
            if (lenx > len)
            {
                len = lenx;
            }

            // Prevent divison by zero
            if (len != 0)
            {
                // Use refs for faster access (really important!) speeds up a lot!
                int w = bmp.PixelWidth;
                int h = bmp.PixelHeight;
#if SILVERLIGHT
            int[] pixels = bmp.Pixels;
#else
                bmp.Lock();
                IntPtr pixels = bmp.BackBuffer;
#endif

                // Init vars
                float step = StepFactor/len;
                int tx1 = x1;
                int ty1 = y1;
                int tx2, ty2;

                // Interpolate
                for (float t = step; t <= 1; t += step)
                {
                    float tSq = t*t;
                    float t1 = 1 - t;
                    float t1Sq = t1*t1;

                    tx2 = (int) (t1*t1Sq*x1 + 3*t*t1Sq*cx1 + 3*t1*tSq*cx2 + t*tSq*x2);
                    ty2 = (int) (t1*t1Sq*y1 + 3*t*t1Sq*cy1 + 3*t1*tSq*cy2 + t*tSq*y2);

                    // Draw line
                    DrawLine(pixels, w, h, tx1, ty1, tx2, ty2, color);
                    tx1 = tx2;
                    ty1 = ty2;
                }

                // Prevent rounding gap
                DrawLine(pixels, w, h, tx1, ty1, x2, y2, color);
#if !SILVERLIGHT
                bmp.AddDirtyRect(new Int32Rect(0, 0, bmp.PixelWidth, bmp.PixelHeight));
                bmp.Unlock();
#endif
            }
        }
        /// <summary>
        /// Draws a closed Cardinal spline (cubic) defined by a point collection. 
        /// The cardinal spline passes through each point in the collection.
        /// </summary>
        /// <param name="bmp">The WriteableBitmap.</param>
        /// <param name="points">The points for the curve in x and y pairs, therefore the array is interpreted as (x1, y1, x2, y2, x3, y3, x4, y4, x1, x2 ..., xn, yn).</param>
        /// <param name="tension">The tension of the curve defines the shape. Usually between 0 and 1. 0 would be a straight line.</param>
        /// <param name="color">The color for the spline.</param>
        public static void DrawCurveClosed(this WriteableBitmap bmp, int[] points, float tension, int color)
        {
            // Use refs for faster access (really important!) speeds up a lot!
            int w = bmp.PixelWidth;
            int h = bmp.PixelHeight;
#if SILVERLIGHT
         int[] pixels = bmp.Pixels;
#else
            bmp.Lock();
            IntPtr pixels = bmp.BackBuffer;
#endif
            int pn = points.Length;

            // First segment
            DrawCurveSegment(points[pn - 2], points[pn - 1], points[0], points[1], points[2], points[3], points[4],
                             points[5], tension, color, pixels, w, h);

            // Middle segments
            int i;
            for (i = 2; i < pn - 4; i += 2)
            {
                DrawCurveSegment(points[i - 2], points[i - 1], points[i], points[i + 1], points[i + 2], points[i + 3],
                                 points[i + 4], points[i + 5], tension, color, pixels, w, h);
            }

            // Last segment
            DrawCurveSegment(points[i - 2], points[i - 1], points[i], points[i + 1], points[i + 2], points[i + 3],
                             points[0], points[1], tension, color, pixels, w, h);

            // Last-to-First segment
            DrawCurveSegment(points[i], points[i + 1], points[i + 2], points[i + 3], points[0], points[1], points[2],
                             points[3], tension, color, pixels, w, h);
#if !SILVERLIGHT
            bmp.AddDirtyRect(new Int32Rect(0, 0, w, h));
            bmp.Unlock();
#endif
        }
 /// <summary>
 /// Fills the whole WriteableBitmap with an empty color (0).
 /// </summary>
 /// <param name="bmp">The WriteableBitmap.</param>
 public static void Clear(this WriteableBitmap bmp)
 {
     #if SILVERLIGHT
       Array.Clear(bmp.Pixels, 0, bmp.Pixels.Length);
     #else
     bmp.Lock();
     NativeMethods.SetUnmanagedMemory(bmp.BackBuffer, 0, bmp.BackBufferStride*bmp.PixelHeight);
     bmp.AddDirtyRect(new Int32Rect(0, 0, bmp.PixelWidth, bmp.PixelHeight));
     bmp.Unlock();
     #endif
 }
        /// <summary>
        /// Draws a rectangle.
        /// x2 has to be greater than x1 and y2 has to be greater than y1.
        /// </summary>
        /// <param name="bmp">The WriteableBitmap.</param>
        /// <param name="x1">The x-coordinate of the bounding rectangle's left side.</param>
        /// <param name="y1">The y-coordinate of the bounding rectangle's top side.</param>
        /// <param name="x2">The x-coordinate of the bounding rectangle's right side.</param>
        /// <param name="y2">The y-coordinate of the bounding rectangle's bottom side.</param>
        /// <param name="color">The color.</param>
        public static void DrawRectangle(this WriteableBitmap bmp, int x1, int y1, int x2, int y2, int color)
        {
            // Use refs for faster access (really important!) speeds up a lot!
            int w = bmp.PixelWidth;
            int h = bmp.PixelHeight;
#if SILVERLIGHT
         int[] pixels = bmp.Pixels;
#else
            bmp.Lock();
            unsafe
            {
                var pixels = (int*) bmp.BackBuffer;
#endif
                // Check boundaries
                if (x1 < 0)
                {
                    x1 = 0;
                }
                if (y1 < 0)
                {
                    y1 = 0;
                }
                if (x2 < 0)
                {
                    x2 = 0;
                }
                if (y2 < 0)
                {
                    y2 = 0;
                }
                if (x1 >= w)
                {
                    x1 = w - 1;
                }
                if (y1 >= h)
                {
                    y1 = h - 1;
                }
                if (x2 >= w)
                {
                    x2 = w - 1;
                }
                if (y2 >= h)
                {
                    y2 = h - 1;
                }

                int startY = y1*w;
                int endY = y2*w;

                int offset2 = endY + x1;
                int endOffset = startY + x2;
                int startYPlusX1 = startY + x1;

                // top and bottom horizontal scanlines
                for (int x = startYPlusX1; x <= endOffset; x++)
                {
                    pixels[x] = color; // top horizontal line
                    pixels[offset2] = color; // bottom horizontal line
                    offset2++;
                }

                // offset2 == endY + x2

                // vertical scanlines
                endOffset = startYPlusX1 + w;
                offset2 -= w;

                for (int y = startY + x2 + w; y < offset2; y += w)
                {
                    pixels[y] = color; // right vertical line
                    pixels[endOffset] = color; // left vertical line
                    endOffset += w;
                }
#if !SILVERLIGHT
            }
            bmp.AddDirtyRect(new Int32Rect(x1, y1, x2 - x1, y2 - y1));
            bmp.Unlock();
#endif
        }
        /// <summary>
        /// Draws a polyline. Add the first point also at the end of the array if the line should be closed.
        /// </summary>
        /// <param name="bmp">The WriteableBitmap.</param>
        /// <param name="points">The points of the polyline in x and y pairs, therefore the array is interpreted as (x1, y1, x2, y2, ..., xn, yn).</param>
        /// <param name="color">The color for the line.</param>
        public static void DrawPolyline(this WriteableBitmap bmp, int[] points, int color)
        {
            // Use refs for faster access (really important!) speeds up a lot!
            int w = bmp.PixelWidth;
            int h = bmp.PixelHeight;
#if SILVERLIGHT         
          int[] pixels = bmp.Pixels;
#else
            bmp.Lock();
            IntPtr pixels = bmp.BackBuffer;
#endif
            int x1 = points[0];
            int y1 = points[1];
            int x2, y2;
            for (int i = 2; i < points.Length; i += 2)
            {
                x2 = points[i];
                y2 = points[i + 1];
                DrawLine(pixels, w, h, x1, y1, x2, y2, color);
                x1 = x2;
                y1 = y2;
            }
#if !SILVERLIGHT
            bmp.AddDirtyRect(new Int32Rect(0, 0, bmp.PixelWidth, bmp.PixelHeight));
            bmp.Unlock();
#endif
        }
        /// <summary>
        /// Draws a colored line by connecting two points using an optimized DDA.
        /// </summary>
        /// <param name="bmp">The WriteableBitmap.</param>
        /// <param name="x1">The x-coordinate of the start point.</param>
        /// <param name="y1">The y-coordinate of the start point.</param>
        /// <param name="x2">The x-coordinate of the end point.</param>
        /// <param name="y2">The y-coordinate of the end point.</param>
        /// <param name="color">The color for the line.</param>
        public static void DrawLine(this WriteableBitmap bmp, int x1, int y1, int x2, int y2, int color)
        {
#if SILVERLIGHT
         DrawLine(bmp.Pixels, bmp.PixelWidth, bmp.PixelHeight, x1, y1, x2, y2, color);
#else
            bmp.Lock();
            DrawLine(bmp.BackBuffer, bmp.BackBufferStride/SizeOfArgb, bmp.PixelHeight, x1, y1, x2, y2, color);
            bmp.AddDirtyRect(new Int32Rect(0, 0, bmp.PixelWidth, bmp.PixelHeight));
            bmp.Unlock();
#endif
        }
 /// <summary>
 /// Sets the color of the pixel using an extra alpha value.
 /// </summary>
 /// <param name="bmp">The WriteableBitmap.</param>
 /// <param name="x">The x coordinate (row).</param>
 /// <param name="y">The y coordinate (column).</param>
 /// <param name="a">The alpha value of the color.</param>
 /// <param name="color">The color.</param>
 public static void SetPixel(this WriteableBitmap bmp, int x, int y, byte a, Color color)
 {
     // Add one to use mul and cheap bit shift for multiplicaltion
     int ai = a + 1;
     int argb = (a << 24)
                | ((byte) ((color.R*ai) >> 8) << 16)
                | ((byte) ((color.G*ai) >> 8) << 8)
                | ((byte) ((color.B*ai) >> 8));
     #if SILVERLIGHT
       bmp.Pixels[y * bmp.PixelWidth + x] = argb;
     #else
     bmp.Lock();
     unsafe
     {
         var pixels = (int*) bmp.BackBuffer;
         pixels[y*bmp.BackBufferStride/SizeOfArgb + x] = argb;
     }
     bmp.AddDirtyRect(new Int32Rect(x, y, 1, 1));
     bmp.Unlock();
     #endif
 }
        /// <summary>
        /// Applies the given function to all the pixels of the bitmap in 
        /// order to set their color.
        /// </summary>
        /// <param name="bmp">The WriteableBitmap.</param>
        /// <param name="func">The function to apply. With parameters x, y, source color and a color as a result</param>
        public static void ForEach(this WriteableBitmap bmp, Func<int, int, Color, Color> func)
        {
            int w = bmp.PixelWidth;
            int h = bmp.PixelHeight;
            #if SILVERLIGHT
             int[] pixels = bmp.Pixels;
            #else
            bmp.Lock();
            unsafe
            {
                var pixels = (int*) bmp.BackBuffer;
            #endif
                int index = 0;

                for (int y = 0; y < h; y++)
                {
                    for (int x = 0; x < w; x++)
                    {
                        int c = pixels[index];
                        Color color = func(x, y,
                                           Color.FromArgb((byte) (c >> 24), (byte) (c >> 16), (byte) (c >> 8),
                                                          (byte) (c)));
                        // Add one to use mul and cheap bit shift for multiplicaltion
                        int a = color.A + 1;
                        pixels[index++] = (color.A << 24)
                                          | ((byte) ((color.R*a) >> 8) << 16)
                                          | ((byte) ((color.G*a) >> 8) << 8)
                                          | ((byte) ((color.B*a) >> 8));
                    }
                }
            #if !SILVERLIGHT
            }
            bmp.AddDirtyRect(new Int32Rect(0, 0, w, h));
            bmp.Unlock();
            #endif
        }
 /// <summary>
 /// Sets the color of the pixel including the alpha value.
 /// </summary>
 /// <param name="bmp">The WriteableBitmap.</param>
 /// <param name="x">The x coordinate (row).</param>
 /// <param name="y">The y coordinate (column).</param>
 /// <param name="a">The alpha value of the color.</param>
 /// <param name="r">The red value of the color.</param>
 /// <param name="g">The green value of the color.</param>
 /// <param name="b">The blue value of the color.</param>
 public static void SetPixel(this WriteableBitmap bmp, int x, int y, byte a, byte r, byte g, byte b)
 {
     int argb = (a << 24) | (r << 16) | (g << 8) | b;
     #if SILVERLIGHT
       bmp.Pixels[y * bmp.PixelWidth + x] = argb;
     #else
     bmp.Lock();
     unsafe
     {
         var pixels = (int*) bmp.BackBuffer;
         pixels[y*bmp.BackBufferStride/SizeOfArgb + x] = argb;
     }
     bmp.AddDirtyRect(new Int32Rect(x, y, 1, 1));
     bmp.Unlock();
     #endif
 }
        /// <summary>
        /// Draws a filled rectangle.
        /// x2 has to be greater than x1 and y2 has to be greater than y1.
        /// </summary>
        /// <param name="bmp">The WriteableBitmap.</param>
        /// <param name="x1">The x-coordinate of the bounding rectangle's left side.</param>
        /// <param name="y1">The y-coordinate of the bounding rectangle's top side.</param>
        /// <param name="x2">The x-coordinate of the bounding rectangle's right side.</param>
        /// <param name="y2">The y-coordinate of the bounding rectangle's bottom side.</param>
        /// <param name="color">The color.</param>
        public static void FillRectangle(this WriteableBitmap bmp, int x1, int y1, int x2, int y2, int color)
        {
            // Use refs for faster access (really important!) speeds up a lot!
            int w = bmp.PixelWidth;
            int h = bmp.PixelHeight;
            #if SILVERLIGHT
             int[] pixels = bmp.Pixels;
            #else
            bmp.Lock();
            unsafe
            {
                var pixels = (int*) bmp.BackBuffer;
            #endif

                // Check boundaries
                if (x1 < 0)
                {
                    x1 = 0;
                }
                if (y1 < 0)
                {
                    y1 = 0;
                }
                if (x2 < 0)
                {
                    x2 = 0;
                }
                if (y2 < 0)
                {
                    y2 = 0;
                }
                if (x1 >= w)
                {
                    x1 = w - 1;
                }
                if (y1 >= h)
                {
                    y1 = h - 1;
                }
                if (x2 >= w)
                {
                    x2 = w - 1;
                }
                if (y2 >= h)
                {
                    y2 = h - 1;
                }

                // Fill first line
                int startY = y1*w;
                int startYPlusX1 = startY + x1;
                int endOffset = startY + x2;
                for (int x = startYPlusX1; x < endOffset; x++)
                {
                    pixels[x] = color;
                }

                // Copy first line
                int len = (x2 - x1)*SizeOfArgb;
                int srcOffsetBytes = startYPlusX1*SizeOfArgb;
                int offset2 = y2*w + x1;
                for (int y = startYPlusX1 + w; y < offset2; y += w)
                {
            #if SILVERLIGHT
                  Buffer.BlockCopy(pixels, srcOffsetBytes, pixels, y*SizeOfArgb, len);
            #else
                    NativeMethods.CopyUnmanagedMemory((IntPtr) pixels, srcOffsetBytes, (IntPtr) pixels, y*SizeOfArgb,
                                                      len);
            #endif
                }
            #if !SILVERLIGHT
            }
            bmp.AddDirtyRect(new Int32Rect(x1, y1, x2 - x1, y2 - y1));
            bmp.Unlock();
            #endif
        }
        /// <summary>
        /// Draws a filled polygon. Add the first point also at the end of the array if the line should be closed.
        /// </summary>
        /// <param name="bmp">The WriteableBitmap.</param>
        /// <param name="points">The points of the polygon in x and y pairs, therefore the array is interpreted as (x1, y1, x2, y2, ..., xn, yn).</param>
        /// <param name="color">The color for the line.</param>
        public static void FillPolygon(this WriteableBitmap bmp, int[] points, int color)
        {
            // Use refs for faster access (really important!) speeds up a lot!
            int w = bmp.PixelWidth;
            int h = bmp.PixelHeight;
            #if SILVERLIGHT
             int[] pixels = bmp.Pixels;
            #else
            bmp.Lock();
            unsafe
            {
                var pixels = (int*) bmp.BackBuffer;
            #endif
                int pn = points.Length;
                int pnh = points.Length >> 1;
                var intersectionsX = new int[pnh];

                // Find y min and max (slightly faster than scanning from 0 to height)
                int yMin = h;
                int yMax = 0;
                for (int i = 1; i < pn; i += 2)
                {
                    int py = points[i];
                    if (py < yMin) yMin = py;
                    if (py > yMax) yMax = py;
                }
                if (yMin < 0) yMin = 0;
                if (yMax >= h) yMax = h - 1;

                // Scan line from min to max
                for (int y = yMin; y <= yMax; y++)
                {
                    // Initial point x, y
                    int vxi = points[0];
                    int vyi = points[1];

                    // Find all intersections
                    // Based on http://alienryderflex.com/polygon_fill/
                    int intersectionCount = 0;
                    for (int i = 2; i < pn; i += 2)
                    {
                        // Next point x, y
                        int vxj = points[i];
                        int vyj = points[i + 1];

                        // Is the scanline between the two points
                        if (vyi < y && vyj >= y
                            || vyj < y && vyi >= y)
                        {
                            // Compute the intersection of the scanline with the edge (line between two points)
                            // NOTE: only division has to be floating point
                            intersectionsX[intersectionCount++] =
                                (int) (vxi + (y - vyi)/(float) (vyj - vyi)*(vxj - vxi));
                        }
                        vxi = vxj;
                        vyi = vyj;
                    }

                    // Sort the intersections from left to right using Insertion sort
                    // It's faster than Array.Sort for this small data set
                    int t, j;
                    for (int i = 1; i < intersectionCount; i++)
                    {
                        t = intersectionsX[i];
                        j = i;
                        while (j > 0 && intersectionsX[j - 1] > t)
                        {
                            intersectionsX[j] = intersectionsX[j - 1];
                            j = j - 1;
                        }
                        intersectionsX[j] = t;
                    }

                    // Fill the pixels between the intersections
                    for (int i = 0; i < intersectionCount - 1; i += 2)
                    {
                        int x0 = intersectionsX[i];
                        int x1 = intersectionsX[i + 1];

                        // Check boundary
                        if (x1 > 0 && x0 < w)
                        {
                            if (x0 < 0) x0 = 0;
                            if (x1 >= w) x1 = w - 1;

                            // Fill the pixels
                            for (int x = x0; x <= x1; x++)
                            {
                                pixels[y*w + x] = color;
                            }
                        }
                    }
                }
            #if !SILVERLIGHT
            }
            bmp.AddDirtyRect(new Int32Rect(0, 0, w, h));
            bmp.Unlock();
            #endif
        }
        /// <summary>
        /// Draw rectangle with given color.
        /// </summary>
        /// <param name="wBitmap">Must not be null, or pixel width, pixel height equal to zero</param>
        /// <exception>Exception should be expect with null writeablebitmap or pixel width, height equal to zero.</exception>
        internal static void FillRectangle(this WriteableBitmap wBitmap, Int32Rect rectangle, Color color)
        {
            if (!(wBitmap.Format == PixelFormats.Pbgra32 || wBitmap.Format == PixelFormats.Gray8))
                return;
            byte[] col = ConvertColor(wBitmap.Format, color);

            //Currently only support two PixelFormat.
            int sizeOfColor = wBitmap.Format == PixelFormats.Pbgra32 ? 4 : 1;

            int pixelW = wBitmap.Format == PixelFormats.Gray8 ? wBitmap.BackBufferStride : wBitmap.PixelWidth;
            int pixelH = wBitmap.PixelHeight;

            if (rectangle.X >= pixelW
                || rectangle.Y >= pixelH
                || rectangle.X + rectangle.Width - 1 < 0
                || rectangle.Y + rectangle.Height - 1 < 0)
                return;

            if (rectangle.X < 0) rectangle.X = 0;
            if (rectangle.Y < 0) rectangle.Y = 0;
            if (rectangle.X + rectangle.Width >= pixelW) rectangle.Width = pixelW - rectangle.X;
            if (rectangle.Y + rectangle.Height >= pixelH) rectangle.Height = pixelH - rectangle.Y;

            wBitmap.Lock();
            unsafe
            {
                byte* pixels = (byte*)wBitmap.BackBuffer;

                int startPoint = rectangle.Y * pixelW + rectangle.X;
                int endBoundry = startPoint + rectangle.Width;
                int srcOffsetBytes = startPoint * sizeOfColor;

                int length = col.Length;
                //Draw first dot color.
                for (int index = 0; index < length; index++)
                {
                    *(pixels + srcOffsetBytes + index) = col[index];
                }

                int pixelIndex = startPoint + 1;
                int blockPixels = 1;

                //Use first pixel color at (x, y) offset to draw first line.
                while (pixelIndex < endBoundry)
                {
                    CopyUnmanagedMemory(pixels, srcOffsetBytes, pixels, pixelIndex * sizeOfColor, blockPixels * sizeOfColor);

                    pixelIndex += blockPixels;
                    blockPixels = Math.Min(2 * blockPixels, endBoundry - pixelIndex);
                }

                int bottomLeft = (rectangle.Y + rectangle.Height - 1) * pixelW + rectangle.X;
                //Use first line of pixel to fill up rest of rectangle.
                for (pixelIndex = startPoint + pixelW; pixelIndex <= bottomLeft; pixelIndex += pixelW)
                {
                    CopyUnmanagedMemory(pixels, srcOffsetBytes, pixels, pixelIndex * sizeOfColor, rectangle.Width * sizeOfColor);
                }

            }
            wBitmap.AddDirtyRect(rectangle);
            wBitmap.Unlock();
        }
        /// <summary>
        /// Draws a colored line by connecting two points using the Bresenham algorithm.
        /// </summary>
        /// <param name="bmp">The WriteableBitmap.</param>
        /// <param name="x1">The x-coordinate of the start point.</param>
        /// <param name="y1">The y-coordinate of the start point.</param>
        /// <param name="x2">The x-coordinate of the end point.</param>
        /// <param name="y2">The y-coordinate of the end point.</param>
        /// <param name="color">The color for the line.</param>
        public static void DrawLineBresenham(this WriteableBitmap bmp, int x1, int y1, int x2, int y2, int color)
        {
            // Use refs for faster access (really important!) speeds up a lot!
            int w = bmp.PixelWidth;
            int h = bmp.PixelHeight;
#if SILVERLIGHT
         int[] pixels = bmp.Pixels;
#else
            bmp.Lock();
            unsafe
            {
                var pixels = (int*) bmp.BackBuffer;
#endif
                // Distance start and end point
                int dx = x2 - x1;
                int dy = y2 - y1;

                // Determine sign for direction x
                int incx = 0;
                if (dx < 0)
                {
                    dx = -dx;
                    incx = -1;
                }
                else if (dx > 0)
                {
                    incx = 1;
                }

                // Determine sign for direction y
                int incy = 0;
                if (dy < 0)
                {
                    dy = -dy;
                    incy = -1;
                }
                else if (dy > 0)
                {
                    incy = 1;
                }

                // Which gradient is larger
                int pdx, pdy, odx, ody, es, el;
                if (dx > dy)
                {
                    pdx = incx;
                    pdy = 0;
                    odx = incx;
                    ody = incy;
                    es = dy;
                    el = dx;
                }
                else
                {
                    pdx = 0;
                    pdy = incy;
                    odx = incx;
                    ody = incy;
                    es = dx;
                    el = dy;
                }

                // Init start
                int x = x1;
                int y = y1;
                int error = el >> 1;
                if (y < h && y >= 0 && x < w && x >= 0)
                {
                    pixels[y*w + x] = color;
                }

                // Walk the line!
                for (int i = 0; i < el; i++)
                {
                    // Update error term
                    error -= es;

                    // Decide which coord to use
                    if (error < 0)
                    {
                        error += el;
                        x += odx;
                        y += ody;
                    }
                    else
                    {
                        x += pdx;
                        y += pdy;
                    }

                    // Set pixel
                    if (y < h && y >= 0 && x < w && x >= 0)
                    {
                        pixels[y*w + x] = color;
                    }
                }
#if !SILVERLIGHT
            }
            bmp.AddDirtyRect(new Int32Rect(0, 0, bmp.PixelWidth, bmp.PixelHeight));
            bmp.Unlock();
#endif
        }
 /// <summary>
 /// Sets the color of the pixel.
 /// </summary>
 /// <param name="bmp">The WriteableBitmap.</param>
 /// <param name="x">The x coordinate (row).</param>
 /// <param name="y">The y coordinate (column).</param>
 /// <param name="color">The color.</param>
 public static void SetPixel(this WriteableBitmap bmp, int x, int y, int color)
 {
     #if SILVERLIGHT
       bmp.Pixels[y * bmp.PixelWidth + x] = color;
     #else
     bmp.Lock();
     unsafe
     {
         var pixels = (int*) bmp.BackBuffer;
         pixels[y*bmp.BackBufferStride/SizeOfArgb + x] = color;
     }
     bmp.AddDirtyRect(new Int32Rect(x, y, 1, 1));
     bmp.Unlock();
     #endif
 }
        /// <summary>
        /// Draws a colored line by connecting two points using a DDA algorithm (Digital Differential Analyzer).
        /// </summary>
        /// <param name="bmp">The WriteableBitmap.</param>
        /// <param name="x1">The x-coordinate of the start point.</param>
        /// <param name="y1">The y-coordinate of the start point.</param>
        /// <param name="x2">The x-coordinate of the end point.</param>
        /// <param name="y2">The y-coordinate of the end point.</param>
        /// <param name="color">The color for the line.</param>
        public static void DrawLineDDA(this WriteableBitmap bmp, int x1, int y1, int x2, int y2, int color)
        {
            // Use refs for faster access (really important!) speeds up a lot!
            int w = bmp.PixelWidth;
            int h = bmp.PixelHeight;
#if SILVERLIGHT
         int[] pixels = bmp.Pixels;
#else
            bmp.Lock();
            unsafe
            {
                var pixels = (int*) bmp.BackBuffer;
#endif
                // Distance start and end point
                int dx = x2 - x1;
                int dy = y2 - y1;

                // Determine slope (absoulte value)
                int len = dy >= 0 ? dy : -dy;
                int lenx = dx >= 0 ? dx : -dx;
                if (lenx > len)
                {
                    len = lenx;
                }

                // Prevent divison by zero
                if (len != 0)
                {
                    // Init steps and start
                    float incx = dx/(float) len;
                    float incy = dy/(float) len;
                    float x = x1;
                    float y = y1;

                    // Walk the line!
                    for (int i = 0; i < len; i++)
                    {
                        if (y < h && y >= 0 && x < w && x >= 0)
                        {
                            pixels[(int) y*w + (int) x] = color;
                        }
                        x += incx;
                        y += incy;
                    }
                }
#if !SILVERLIGHT
            }
            bmp.Unlock();
            bmp.AddDirtyRect(new Int32Rect(0, 0, bmp.PixelWidth, bmp.PixelHeight));
#endif
        }
 /// <summary>
 /// Sets the color of the pixel including the alpha value and using a precalculated index (faster).
 /// </summary>
 /// <param name="bmp">The WriteableBitmap.</param>
 /// <param name="index">The coordinate index.</param>
 /// <param name="a">The alpha value of the color.</param>
 /// <param name="r">The red value of the color.</param>
 /// <param name="g">The green value of the color.</param>
 /// <param name="b">The blue value of the color.</param>
 public static void SetPixeli(this WriteableBitmap bmp, int index, byte a, byte r, byte g, byte b)
 {
     int argb = (a << 24) | (r << 16) | (g << 8) | b;
     #if SILVERLIGHT
       bmp.Pixels[index] = argb;
     #else
     bmp.Lock();
     unsafe
     {
         var pixels = (int*) bmp.BackBuffer;
         pixels[index] = argb;
     }
     //TODO: compute dirty rect
     bmp.AddDirtyRect(new Int32Rect(0, 0, bmp.PixelWidth, bmp.PixelHeight));
     bmp.Unlock();
     #endif
 }
        public static void DrawLineEFLA(this WriteableBitmap bmp, int x1, int y1, int x2, int y2, int color, EFLAMode mode)
        {
            int w = bmp.PixelWidth;
            int h = bmp.PixelHeight;
            bmp.Lock();

            bool yLonger;
            int incrementVal, endVal;
            int shortLen;
            int longLen;

            unsafe
            {
                var pixels = (int*) bmp.BackBuffer;
                int x, y;

                switch (mode)
                {
                    case EFLAMode.Div:
                        yLonger = false;

                        shortLen = y2 - y1;
                        longLen = x2 - x1;

                        if (Math.Abs(shortLen) > Math.Abs(longLen))
                        {
                            int swap = shortLen;
                            shortLen = longLen;
                            longLen = swap;
                            yLonger = true;
                        }

                        if (longLen < 0) incrementVal = -1;
                        else incrementVal = 1;

                        double divDiff;
                        if (shortLen == 0) divDiff = longLen;
                        else divDiff = longLen/(double) shortLen;
                        if (yLonger)
                        {
                            for (int i = 0; i != longLen; i += incrementVal)
                            {
                                x = x1 + (int) (i/divDiff);
                                y = y1 + i;
                                pixels[y*w + x] = color;
                            }
                        }
                        else
                        {
                            for (int i = 0; i != longLen; i += incrementVal)
                            {
                                x = x1 + i;
                                y = y1 + (int) (i/divDiff);
                                pixels[y*w + x] = color;
                            }
                        }
                        break;
                    case EFLAMode.Mul:
                        yLonger = false;
                        shortLen = y2 - y1;
                        longLen = x2 - x1;

                        if (Math.Abs(shortLen) > Math.Abs(longLen))
                        {
                            int swap = shortLen;
                            shortLen = longLen;
                            longLen = swap;
                            yLonger = true;
                        }

                        if (longLen < 0) incrementVal = -1;
                        else incrementVal = 1;

                        double multDiff;
                        if (longLen == 0.0) multDiff = shortLen;
                        else multDiff = shortLen/(double) longLen;
                        if (yLonger)
                        {
                            for (int i = 0; i != longLen; i += incrementVal)
                            {
                                x = x1 + (int) (i*multDiff);
                                y = y1 + i;
                                pixels[y*w + x] = color;
                            }
                        }
                        else
                        {
                            for (int i = 0; i != longLen; i += incrementVal)
                            {
                                x = x1 + i;
                                y = y1 + (int) (i*multDiff);
                                pixels[y*w + x] = color;
                            }
                        }
                        break;
                    case EFLAMode.Add:
                        yLonger = false;

                        shortLen = y2 - y1;
                        longLen = x2 - x1;
                        if (Math.Abs(shortLen) > Math.Abs(longLen))
                        {
                            int swap = shortLen;
                            shortLen = longLen;
                            longLen = swap;
                            yLonger = true;
                        }

                        endVal = longLen;
                        if (longLen < 0)
                        {
                            incrementVal = -1;
                            longLen = -longLen;
                        }
                        else incrementVal = 1;

                        double decInc;
                        if (longLen == 0) decInc = shortLen;
                        else decInc = (shortLen/(double) longLen);
                        double j = 0.0;
                        if (yLonger)
                        {
                            for (int i = 0; i != endVal; i += incrementVal)
                            {
                                x = x1 + (int) j;
                                y = y1 + i;
                                pixels[y*w + x] = color;
                                j += decInc;
                            }
                        }
                        else
                        {
                            for (int i = 0; i != endVal; i += incrementVal)
                            {
                                x = x1 + i;
                                y = y1 + (int) j;
                                pixels[y*w + x] = color;
                                j += decInc;
                            }
                        }
                        break;
                    case EFLAMode.AddFixed:
                        yLonger = false;
                        shortLen = y2 - y1;
                        longLen = x2 - x1;
                        if (Math.Abs(shortLen) > Math.Abs(longLen))
                        {
                            int swap = shortLen;
                            shortLen = longLen;
                            longLen = swap;
                            yLonger = true;
                        }
                        endVal = longLen;
                        if (longLen < 0)
                        {
                            incrementVal = -1;
                            longLen = -longLen;
                        }
                        else incrementVal = 1;

                        int decIncI = 0;
                        if (longLen == 0) decInc = 0;
                        else decIncI = (shortLen << 16)/longLen;
                        int jI = 0;
                        if (yLonger)
                        {
                            for (int i = 0; i != endVal; i += incrementVal)
                            {
                                x = x1 + (jI >> 16);
                                y = y1 + i;
                                pixels[y*w + x] = color;
                                jI += decIncI;
                            }
                        }
                        else
                        {
                            for (int i = 0; i != endVal; i += incrementVal)
                            {
                                x = x1 + i;
                                y = y1 + (jI >> 16);
                                pixels[y*w + x] = color;
                                jI += decIncI;
                            }
                        }

                        break;
                    case EFLAMode.AddFixedPreCal:
                        break;
                    default:
                        throw new ArgumentOutOfRangeException("mode");
                }
            }
            bmp.AddDirtyRect(new Int32Rect(0, 0, bmp.PixelWidth, bmp.PixelHeight));
            bmp.Unlock();
        }
 /// <summary>
 /// Sets the color of the pixel using an extra alpha value and a precalculated index (faster).
 /// </summary>
 /// <param name="bmp">The WriteableBitmap.</param>
 /// <param name="index">The coordinate index.</param>
 /// <param name="a">The alpha value of the color.</param>
 /// <param name="color">The color.</param>
 public static void SetPixeli(this WriteableBitmap bmp, int index, byte a, Color color)
 {
     // Add one to use mul and cheap bit shift for multiplicaltion
     int ai = a + 1;
     int argb = (a << 24)
                | ((byte) ((color.R*ai) >> 8) << 16)
                | ((byte) ((color.G*ai) >> 8) << 8)
                | ((byte) ((color.B*ai) >> 8));
     #if SILVERLIGHT
       bmp.Pixels[index] = argb;
     #else
     bmp.Lock();
     unsafe
     {
         var pixels = (int*) bmp.BackBuffer;
         pixels[index] = argb;
     }
     //TODO: compute dirty rect
     bmp.AddDirtyRect(new Int32Rect(0, 0, bmp.PixelWidth, bmp.PixelHeight));
     bmp.Unlock();
     #endif
 }
        /// <summary>
        /// Draws a quad.
        /// </summary>
        /// <param name="bmp">The WriteableBitmap.</param>
        /// <param name="x1">The x-coordinate of the 1st point.</param>
        /// <param name="y1">The y-coordinate of the 1st point.</param>
        /// <param name="x2">The x-coordinate of the 2nd point.</param>
        /// <param name="y2">The y-coordinate of the 2nd point.</param>
        /// <param name="x3">The x-coordinate of the 3rd point.</param>
        /// <param name="y3">The y-coordinate of the 3rd point.</param>
        /// <param name="x4">The x-coordinate of the 4th point.</param>
        /// <param name="y4">The y-coordinate of the 4th point.</param>
        /// <param name="color">The color.</param>
        public static void DrawQuad(this WriteableBitmap bmp, int x1, int y1, int x2, int y2, int x3, int y3, int x4,
                                    int y4, int color)
        {
            // Use refs for faster access (really important!) speeds up a lot!
            int w = bmp.PixelWidth;
            int h = bmp.PixelHeight;
#if SILVERLIGHT
         int[] pixels = bmp.Pixels;
#else
            bmp.Lock();
            IntPtr pixels = bmp.BackBuffer;
#endif

            DrawLine(pixels, w, h, x1, y1, x2, y2, color);
            DrawLine(pixels, w, h, x2, y2, x3, y3, color);
            DrawLine(pixels, w, h, x3, y3, x4, y4, color);
            DrawLine(pixels, w, h, x4, y4, x1, y1, color);

#if !SILVERLIGHT
            bmp.AddDirtyRect(new Int32Rect(0, 0, bmp.PixelWidth, bmp.PixelHeight));
            bmp.Unlock();
#endif
        }
 /// <summary>
 /// Sets the color of the pixel using a precalculated index (faster).
 /// </summary>
 /// <param name="bmp">The WriteableBitmap.</param>
 /// <param name="index">The coordinate index.</param>
 /// <param name="color">The color.</param>
 public static void SetPixeli(this WriteableBitmap bmp, int index, int color)
 {
     #if SILVERLIGHT
       bmp.Pixels[index] = color;
     #else
     bmp.Lock();
     unsafe
     {
         var pixels = (int*) bmp.BackBuffer;
         pixels[index] = color;
     }
     //TODO: compute dirty rect
     bmp.AddDirtyRect(new Int32Rect(0, 0, bmp.PixelWidth, bmp.PixelHeight));
     bmp.Unlock();
     #endif
 }
        /// <summary>
        /// A Fast Bresenham Type Algorithm For Drawing Ellipses http://homepage.smc.edu/kennedy_john/belipse.pdf 
        /// Uses a different parameter representation than DrawEllipse().
        /// </summary>
        /// <param name="bmp">The WriteableBitmap.</param>
        /// <param name="xc">The x-coordinate of the ellipses center.</param>
        /// <param name="yc">The y-coordinate of the ellipses center.</param>
        /// <param name="xr">The radius of the ellipse in x-direction.</param>
        /// <param name="yr">The radius of the ellipse in y-direction.</param>
        /// <param name="color">The color for the line.</param>
        public static void DrawEllipseCentered(this WriteableBitmap bmp, int xc, int yc, int xr, int yr, int color)
        {
            // Use refs for faster access (really important!) speeds up a lot!
            int w = bmp.PixelWidth;
            int h = bmp.PixelHeight;
#if SILVERLIGHT
          int[] pixels = bmp.Pixels;
#else
            bmp.Lock();
            unsafe
            {
                var pixels = (int*) bmp.BackBuffer;
#endif
                // Init vars
                int uh, lh, uy, ly, lx, rx;
                int x = xr;
                int y = 0;
                int xrSqTwo = (xr*xr) << 1;
                int yrSqTwo = (yr*yr) << 1;
                int xChg = yr*yr*(1 - (xr << 1));
                int yChg = xr*xr;
                int err = 0;
                int xStopping = yrSqTwo*xr;
                int yStopping = 0;

                // Draw first set of points counter clockwise where tangent line slope > -1.
                while (xStopping >= yStopping)
                {
                    // Draw 4 quadrant points at once
                    uy = yc + y; // Upper half
                    ly = yc - y; // Lower half
                    if (uy < 0) uy = 0; // Clip
                    if (uy >= h) uy = h - 1; // ...
                    if (ly < 0) ly = 0;
                    if (ly >= h) ly = h - 1;
                    uh = uy*w; // Upper half
                    lh = ly*w; // Lower half

                    rx = xc + x;
                    lx = xc - x;
                    if (rx < 0) rx = 0; // Clip
                    if (rx >= w) rx = w - 1; // ...
                    if (lx < 0) lx = 0;
                    if (lx >= w) lx = w - 1;
                    pixels[rx + uh] = color; // Quadrant I (Actually an octant)
                    pixels[lx + uh] = color; // Quadrant II
                    pixels[lx + lh] = color; // Quadrant III
                    pixels[rx + lh] = color; // Quadrant IV

                    y++;
                    yStopping += xrSqTwo;
                    err += yChg;
                    yChg += xrSqTwo;
                    if ((xChg + (err << 1)) > 0)
                    {
                        x--;
                        xStopping -= yrSqTwo;
                        err += xChg;
                        xChg += yrSqTwo;
                    }
                }

                // ReInit vars
                x = 0;
                y = yr;
                uy = yc + y; // Upper half
                ly = yc - y; // Lower half
                if (uy < 0) uy = 0; // Clip
                if (uy >= h) uy = h - 1; // ...
                if (ly < 0) ly = 0;
                if (ly >= h) ly = h - 1;
                uh = uy*w; // Upper half
                lh = ly*w; // Lower half
                xChg = yr*yr;
                yChg = xr*xr*(1 - (yr << 1));
                err = 0;
                xStopping = 0;
                yStopping = xrSqTwo*yr;

                // Draw second set of points clockwise where tangent line slope < -1.
                while (xStopping <= yStopping)
                {
                    // Draw 4 quadrant points at once
                    rx = xc + x;
                    lx = xc - x;
                    if (rx < 0) rx = 0; // Clip
                    if (rx >= w) rx = w - 1; // ...
                    if (lx < 0) lx = 0;
                    if (lx >= w) lx = w - 1;
                    pixels[rx + uh] = color; // Quadrant I (Actually an octant)
                    pixels[lx + uh] = color; // Quadrant II
                    pixels[lx + lh] = color; // Quadrant III
                    pixels[rx + lh] = color; // Quadrant IV

                    x++;
                    xStopping += yrSqTwo;
                    err += xChg;
                    xChg += yrSqTwo;
                    if ((yChg + (err << 1)) > 0)
                    {
                        y--;
                        uy = yc + y; // Upper half
                        ly = yc - y; // Lower half
                        if (uy < 0) uy = 0; // Clip
                        if (uy >= h) uy = h - 1; // ...
                        if (ly < 0) ly = 0;
                        if (ly >= h) ly = h - 1;
                        uh = uy*w; // Upper half
                        lh = ly*w; // Lower half
                        yStopping -= xrSqTwo;
                        err += yChg;
                        yChg += xrSqTwo;
                    }
                }
#if !SILVERLIGHT
            }
            bmp.AddDirtyRect(new Int32Rect(0, 0, bmp.PixelWidth, bmp.PixelHeight));
            bmp.Unlock();
#endif
        }
        public static void SetPixel(this WriteableBitmap wb, int x, int y, Color c)
        {
            if (y > wb.PixelHeight - 1 || x > wb.PixelWidth - 1)
            {
                return;
            }

            if (y < 0 || x < 0)
            {
                return;
            }

            if (!wb.Format.Equals(PixelFormats.Bgra32))
            {
                return;
            }

            wb.Lock();
            IntPtr buff = wb.BackBuffer;
            int stride = wb.BackBufferStride;

            unsafe
            {
                byte* pbuff = (byte*)buff.ToPointer();
                int loc = y * stride + x * 4;
                pbuff[loc]     = c.B;
                pbuff[loc + 1] = c.G;
                pbuff[loc + 2] = c.R;
                pbuff[loc + 3] = c.A;
            }

            wb.AddDirtyRect(new Int32Rect(x, y, 1, 1));
            wb.Unlock();
        }