//Transform images according to transform matrix
        // From Line 130-239
        public static ReturnColorImg transformColor(Mat img1, Mat mask1, Mat img2, Mat mask2,
           Mat dst, Mat dst_mask,
                      Point centroid1, Point centroid2, double angle, Point tweak1, Point tweak2, bool mode = true)
        {
            Mat E = img2.Clone();
            Mat E_mask = mask2.Clone();//Don't ruin original images

            double intersections = 0;
            double x = centroid2.X;
            double y = centroid2.Y;
            double _x, _y, _y2;
            double y2;

            LineSegment2D centerLine = new LineSegment2D(new Point((int)x, (int)y), new Point(img2.Width - (int)x, img2.Height - (int)y));
            //Rectangle r=new Rectangle((int)x,(int)y,2*(img2.Width-(int)x),2*(img2.Height-(int)y));

            Mat ri = new Mat(2 * (img2.Width - (int)x), 2 * (img2.Height - (int)y),DepthType.Cv8U, 3);
            ri.SetTo(new MCvScalar(255, 255, 255));
            Point oldc = new Point((int)x, (int)y);
            bool success = false; // if the tweaking is not successful, return false
            // inverse y axis
            // y2 = -y;
            // rotation of centeroid

            x -= img2.Width / 2;
            y -= img2.Height / 2;//shift origin to (w/2,h/2)
            _x = x * Math.Cos(angle / (180 / Math.PI)) - y * Math.Sin(angle / (180 / Math.PI));//rotate by theta
            _y = x * Math.Sin(angle / (180 / Math.PI)) + y * Math.Cos(angle / (180 / Math.PI));

            _x += img2.Width / 2;
            _y += img2.Height / 2;//back to origin

            //_x = x+img2.Width/2;
            //_y = y+img2.Height/2;

            // inverse y axis
            //_y = -_y2;

            centroid2.X = (int)_x;
            centroid2.Y = (int)_y;
            Point shift = new Point();
            shift.X = centroid1.X - centroid2.X;
            shift.Y = centroid1.Y - centroid2.Y;
            MatImage m1 = new MatImage(E);
            m1.Rotate(angle, new Bgr(255, 255, 255));
            E = m1.Out();

            MatImage m2 = new MatImage(E_mask);
            m1.Rotate(angle, new Bgr(255, 255, 255));
            E_mask = m2.Out();

            //Find optimal size of canvas to hold both images and appropriate transformations
            Point t1, t2;//transformation 1 and 2
            t1 = new Point();
            t2 = new Point();
            int optimal_h = 0, optimal_w = 0;//of canvas(IplImage* dst)
            switch (quadrant(shift))
            {
                case 1:
                    t1.X = 0;
                    t1.Y = 0;
                    t2 = shift;
                    optimal_h = Math.Max(img1.Height, img2.Height + shift.Y);
                    optimal_w = Math.Max(img1.Width, img2.Width + shift.X);
                    break;
                case 2:
                    t1.X = -shift.X;
                    t1.Y = 0;
                    t2.X = 0;
                    t2.Y = shift.Y;
                    optimal_h = Math.Max(img1.Height, img2.Height + shift.Y);
                    optimal_w = Math.Max(img2.Width, img1.Width - shift.X);
                    break;
                case 3:
                    t1.X = -shift.X;
                    t1.Y = -shift.Y;
                    t2.X = 0;
                    t2.Y = 0;
                    optimal_h = Math.Max(img1.Height - shift.Y, img2.Height);
                    optimal_w = Math.Max(img1.Width - shift.X, img2.Width);
                    break;
                case 4:
                    t1.X = 0;
                    t1.Y = -shift.Y;
                    t2.X = shift.X;
                    t2.Y = 0;
                    optimal_h = Math.Max(img1.Height - shift.Y, img2.Height);
                    optimal_w = Math.Max(img2.Width + shift.X, img1.Width);
                    break;
            }

            // add tweak factor
            t1.X += tweak1.X;
            t1.Y += tweak1.Y;
            t2.X += tweak2.X;
            t2.Y += tweak2.Y;

            //optimal_h = 1000;
            //optimal_w = 1000;
            dst = new Mat(optimal_h, optimal_w,DepthType.Cv8U,3);
            dst_mask = new Mat(optimal_h, optimal_w,DepthType.Cv8U,3);

            if (mode)
            {
                dst.SetTo(new MCvScalar(255, 255, 255)); // white background=255, black background=0

            }
            else
            {
                dst.SetTo(new MCvScalar(0, 0, 0)); // white background=255, black background=0

            }

            dst_mask.SetTo(new MCvScalar(0, 0, 0));

            /*if (BKG_WHITE)
                cvSet(dst, cvScalar(255));//make it white
            else
                cvSet(dst, cvScalar(0));//make it black*/

            for (int i = 0; i < img1.Height; ++i)
            {
                for (int j = 0; j < img1.Width; ++j)
                {
                    // if black background
                    if (mode)
                    {
                        if (mask1.GetData(i, j)[0] != 255)
                        {
                            int i_new = i + t1.Y;
                            int j_new = j + t1.X;
                            try
                            {
                                dst.SetValue(i_new, j_new, img1.GetData(i, j));
                                int[] vals = { 255, 255, 255 };
                                dst_mask.SetValue(i_new, j_new, vals);

                            }
                            catch(IndexOutOfRangeException e)
                            {
                                //MessageBox.Show("You cannot tweak in that direction further");
                                success = false;
                                goto ret;

                            }

                        }
                    }
                    // if white background
                    else
                    {
                        if (mask1.GetData(i, j) [0] != 0)
                        {
                            int i_new = i + t1.Y;
                            int j_new = j + t1.X;
                            try
                            {
                                dst.SetValue(i_new, j_new, img1.GetData(i, j));
                                int[] vals = { 0, 0, 0 };
                                dst_mask.SetValue(i_new, j_new, vals);

                            }
                            catch(IndexOutOfRangeException e)
                            {
                                //MessageBox.Show("You cannot tweak in that direction further");
                                success = false;
                                goto ret;

                            }

                        }
                    }

                }
            }

            //Apply transformation to image2

            for (int i = 0; i < img2.Height; ++i)
            {
                for (int j = 0; j < img2.Width; ++j)
                {
                    // if black background
                    if (mode)
                    {
                        if (E_mask.GetData(i, j) [0] != 255)
                        {
                            int i_new = i + t2.Y;
                            int j_new = j + t2.X;
                            try
                            {
                                if (dst_mask.GetData(i_new,j_new)[0] != 0)
                                {
                                    intersections++;
                                }
                                else
                                {
                                    dst.SetValue(i_new, j_new, E.GetData(i, j));
                                    int[] vals = { 255, 255, 255 };
                                    dst_mask.SetValue(i_new, j_new, vals);

                                }
                            }
                            catch (IndexOutOfRangeException e)
                            {
                                //MessageBox.Show("You cannot tweak in that direction further");
                                success = false;
                                goto ret;
                            }

                        }
                    }
                    // else if white background
                    else
                    {
                        if (E_mask.GetData(i, j)[0] != 0)
                        {
                            int i_new = i + t2.Y;
                            int j_new = j + t2.X;
                            try
                            {
                                if (dst_mask.GetData(i_new, j_new)[0] != 0)
                                {
                                    intersections++;
                                }
                                else
                                {
                                    dst.SetValue(i_new, j_new, E.GetData(i, j));
                                    int[] vals = { 0, 0, 0 };
                                    dst_mask.SetValue(i_new, j_new, vals);

                                }
                            }
                            catch (IndexOutOfRangeException e)
                            {
                                //MessageBox.Show("You cannot tweak in that direction further");
                                success = false;
                                goto ret;
                            }

                        }
                    }
                }
            }

            /*for (int i = 0; i < Adst.Length; i++)
            {
                for(int j=0; j < Adst[0].Length; j++)
                {
                    dst.SetValue(i, j, Adst[i][j]);
                }
            }
            for (int i = 0; i < Adst_mask.Length; i++)
            {
                for (int j = 0; j < Adst_mask[0].Length; j++)
                {
                    dst_mask.SetValue(i, j, Adst_mask[i][j]);
                }
            }*/
            // dst.SetTo(Adst);
            //dst_mask.SetTo(Adst_mask);

            success = true;

            /*cvReleaseImage(&E);
            cvReleaseImage(&E_mask);*/ // should not need these two lines because of garbage collection

            // threshold detection is meaningless for 2-piece case, always success

            ret:
            if (intersections > Constants.THRESHOLD)
            {
                /*cvReleaseImage(&dst);//In case of failure in joining
                cvReleaseImage(&dst_mask);//release memory*/
                ReturnColorImg img = new ReturnColorImg();
                img.img = dst;
                img.img_mask = dst_mask;
                img.source1 = img1;
                img.source2 = E_mask;
                img.center1 = centroid1;
                img.center2old = oldc;
                img.center2new = centroid2;
                img.centerLinee = centerLine;
                img.returnbool = false; // for determining if the image is matched or not
                img.translate1 = t1;
                img.translate2 = t2;
                img.overlap = intersections;
                img.success = success; // for tweak only
                return img;
            }
            else
            {
                ReturnColorImg img = new ReturnColorImg();
                img.img = dst;
                img.img_mask = dst_mask;
                img.source1 = img1;
                img.source2 = E_mask;
                img.center1 = centroid1;
                img.center2old = oldc;
                img.center2new = centroid2;
                img.centerLinee = centerLine;
                img.returnbool = true; // for determining if the image is matched or not
                img.translate1 = t1;
                img.translate2 = t2;
                img.overlap = intersections;
                img.success = success; // for tweak only
                return img;
            }
        }