#include <iostream>
#include<opencv2/opencv.hpp>
using namespace cv;
using namespace std;

void coordinateTransform(Point2d*p4Corner,Point2d*np4Corner,double rotAngle,double gamma,Point2d center)
{
    double cx=center.x,cy=center.y;
    double rad=rotAngle*CV_PI/180;
    double alpha=std::cos(rad)*gamma;
    double beta=std::sin(rad)*gamma;
    for(int i=0;i<4;i++)
    {
        np4Corner[i].x= alpha*p4Corner[i].x+beta*p4Corner[i].y+(1-alpha)*cx-beta*cy;
        np4Corner[i].y=-beta*p4Corner[i].x+alpha*p4Corner[i].y+beta*cx+(1-alpha)*cy;
    }
}
void find4Borders(Point2d* np4Corner,double borders[4])
{
    borders[0]=np4Corner[0].y;
    borders[1]=np4Corner[0].y;
    borders[2]=np4Corner[0].x;
    borders[3]=np4Corner[0].x;
    for(int i=1;i<4;i++)
    {
        borders[0]=np4Corner[i].y<borders[0]?np4Corner[i].y:borders[0];//上邊界
        borders[1]=np4Corner[i].y>borders[1]?np4Corner[i].y:borders[1];//下邊界
        borders[2]=np4Corner[i].x<borders[2]?np4Corner[i].x:borders[2];//左邊界
        borders[3]=np4Corner[i].x>borders[3]?np4Corner[i].x:borders[3];//右邊界
    }
    for(int i=0;i<4;i++)
    {
        if(borders[i]<0)
            borders[i]=floor(borders[i]);
        else
            borders[i]=ceil(borders[i]);

    }
}
//    Mat mat=(Mat_<double>(2,3)<<alpha,  beta,  (1-alpha)*x+beta*y,
//                                -beta,  alpha,   beta*x+(1-alpha)*y);
void rotImg(const Mat&src,Mat&dst,double rotAngle,double gamma,Point2d center)
{
    double rad=rotAngle*CV_PI/180;
    double cosRad=std::cos(rad) ;
    double sinRad=std::sin(rad);
    int oldRows=src.rows, oldCols=src.cols;
    //逆向旋轉的矩陣描述
    double x=center.x,y=center.y;
    double alpha=cosRad*gamma,beta=sinRad*gamma;//正向縮放
    //計算新圖像的大小
    //1、獲取原圖像的四個角點坐標p4Corner，以及變換后的四個角點坐標np4Corner
    Point2d p4Corner[]={ Point2d(0,0),              Point2d(src.cols,0),
                      Point2d(0,src.rows) ,  Point2d(src.cols,src.rows)};
    Point2d np4Corner[4];
    coordinateTransform(p4Corner,np4Corner,rotAngle,gamma,center);//對原圖像四個角點做坐標正向變換
    //根據np4Corner，找到變換后新圖像的上、下、左、右邊界坐標
    double borders[4];
    find4Borders(np4Corner,borders);
    //計算新圖的行數、列數
    int newRows=borders[1]-borders[0];
    int newCols=borders[3]-borders[2];
    //為目標圖像分配內存
    dst=Mat::zeros(newRows,newCols,src.type());

    //計算二維插值
    int step0=src.step[0],step1=src.step[1];
    int channels=src.channels();
    alpha =cosRad/gamma;//逆向縮放
    beta=sinRad/gamma;//逆向縮放
    for(int i=0;i<newRows;i++)
        for(int j=0;j<newCols;j++)
        {
            //注意：在新圖中，原點坐標偏移了（borders[2],borders[0]）。所以要把每個像素坐標偏移回來(j+borders[2])、(i+borders[0])
            double x_inv=alpha*(j+borders[2])-beta*(i+borders[0])+(1-alpha)*x+beta*y;//逆映射回原圖坐標
            double y_inv=beta*(j+borders[2])+alpha*(i+borders[0])-beta*x+(1-alpha)*y;//逆映射回原圖坐標
            int j_inv=floor(x_inv);
            int i_inv=floor(y_inv);
            if(j_inv>=0&&j_inv<oldCols&&i_inv>=0&&i_inv<oldRows)
            {
                double a=x_inv-j_inv, b=y_inv-i_inv;
                //找到四個坐標點
                uchar* p1=src.data+step0*i_inv+step1*j_inv;
                uchar* p2=src.data+step0*i_inv+step1*(j_inv+1);
                uchar* p3=src.data+step0*(i_inv+1)+step1*j_inv;
                uchar* p4=src.data+step0*(i_inv+1)+step1*(j_inv+1);
                //獲取目標圖像中的第i、j坐標
                uchar* pdst=dst.data+dst.step[0]*i+dst.step[1]*j;
                for(int k=0;k<channels;k++)
                {
                    pdst[k]=static_cast<uchar>((1-a)*(1-b)*p1[k]+a*(1-b)*p2[k]+b*(1-a)*p3[k]+a*b*p4[k]) ;
                }
            }
        }
}

int main()
{
    Mat src=imread("D:/Qt/MyImage/baboon.jpg",1);

    Mat dst;
    rotImg(src,dst,315,0.5,Point(100,100));
    imshow("rotated image",dst);
    imshow("original image",src);
    waitKey();

    return 0;
}