Accord.Imaging.Filters.RotateNearestNeighbor.ProcessFilter16bpc C# (CSharp) Method

        private unsafe void ProcessFilter16bpc(UnmanagedImage sourceData, UnmanagedImage destinationData)
        {
            // get source image size
            int width = sourceData.Width;
            int height = sourceData.Height;
            double halfWidth = (double)width / 2;
            double halfHeight = (double)height / 2;

            // get destination image size
            int newWidth = destinationData.Width;
            int newHeight = destinationData.Height;
            double halfNewWidth = (double)newWidth / 2;
            double halfNewHeight = (double)newHeight / 2;

            // angle's sine and cosine
            double angleRad = -angle * Math.PI / 180;
            double angleCos = Math.Cos(angleRad);
            double angleSin = Math.Sin(angleRad);

            int srcStride = sourceData.Stride;
            int dstStride = destinationData.Stride;

            // fill values
            ushort fillR = (ushort)(fillColor.R << 8);
            ushort fillG = (ushort)(fillColor.G << 8);
            ushort fillB = (ushort)(fillColor.B << 8);

            // do the job
            byte* src = (byte*)sourceData.ImageData.ToPointer();
            byte* dstBase = (byte*)destinationData.ImageData.ToPointer();

            // destination pixel's coordinate relative to image center
            double cx, cy;
            // source pixel's coordinates
            int ox, oy;
            // temporary pointer
            ushort* p;

            // check pixel format
            if (destinationData.PixelFormat == PixelFormat.Format16bppGrayScale)
            {
                // grayscale
                cy = -halfNewHeight;
                for (int y = 0; y < newHeight; y++)
                {
                    ushort* dst = (ushort*)(dstBase + y * dstStride);

                    cx = -halfNewWidth;
                    for (int x = 0; x < newWidth; x++, dst++)
                    {
                        // coordinate of the nearest point
                        ox = (int)(angleCos * cx + angleSin * cy + halfWidth);
                        oy = (int)(-angleSin * cx + angleCos * cy + halfHeight);

                        // validate source pixel's coordinates
                        if ((ox < 0) || (oy < 0) || (ox >= width) || (oy >= height))
                        {
                            // fill destination image with filler
                            *dst = fillG;
                        }
                        else
                        {
                            // fill destination image with pixel from source image
                            p = (ushort*)(src + oy * srcStride + ox * 2);
                            *dst = *p;
                        }
                        cx++;
                    }
                    cy++;
                }
            }
            else
            {
                // RGB
                cy = -halfNewHeight;
                for (int y = 0; y < newHeight; y++)
                {
                    ushort* dst = (ushort*)(dstBase + y * dstStride);

                    cx = -halfNewWidth;
                    for (int x = 0; x < newWidth; x++, dst += 3)
                    {
                        // coordinate of the nearest point
                        ox = (int)(angleCos * cx + angleSin * cy + halfWidth);
                        oy = (int)(-angleSin * cx + angleCos * cy + halfHeight);

                        // validate source pixel's coordinates
                        if ((ox < 0) || (oy < 0) || (ox >= width) || (oy >= height))
                        {
                            // fill destination image with filler
                            dst[RGB.R] = fillR;
                            dst[RGB.G] = fillG;
                            dst[RGB.B] = fillB;
                        }
                        else
                        {
                            // fill destination image with pixel from source image
                            p = (ushort*)(src + oy * srcStride + ox * 6);

                            dst[RGB.R] = p[RGB.R];
                            dst[RGB.G] = p[RGB.G];
                            dst[RGB.B] = p[RGB.B];
                        }
                        cx++;
                    }
                    cy++;
                }
            }
        }
    }