krita

kis_perspective_math.cpp

00001 /*
00002  * This file is part of Krita
00003  *
00004  *  Copyright (c) 2006 Cyrille Berger <cberger@cberger.net>
00005  *
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
00019  */
00020 
00021 #include "kis_perspective_math.h"
00022 
00023 #include <qrect.h>
00024 
00025 #if 1
00026 
00027 #include <iostream.h>
00028 #include <stdlib.h>
00029 #include <math.h>
00030 //#define NDEBUG // uncomment to remove checking of assert()
00031 #include <assert.h>
00032 #define DEFAULT_ALLOC 2
00033 
00034 namespace math { // TODO: use eigen
00035 
00036 template <class ElType> class matrix;
00037 
00038 template <class ElType>
00039         class vector
00040 {
00041     public:
00042         friend class matrix<ElType>;
00043         ElType * data;
00044         int len;
00045         int length()const;
00046         vector();
00047         vector(int n);
00048         ~vector(){ delete [] data;}
00049      //Copy operator
00050         vector(const vector<ElType>& v) ;
00051      //assignment operator
00052         vector<ElType>& operator =(const vector<ElType> &original);
00053         ElType& operator[](int i)const  ;
00054         void zero();
00055         vector<ElType> operator+(const vector<ElType>& v);
00056         vector<ElType> operator-(const vector<ElType>&v);
00057         void  rprint()const;  //print entries on a single line
00058         void resize(int n);
00059         int operator==(const vector<ElType>& v)const;
00060         friend   vector<ElType> operator*(ElType c,vector<ElType>& v );
00061         friend   vector<ElType> operator*(vector<ElType>& v,ElType c );
00062         friend ostream& operator<<(ostream& s,vector<ElType>& v);
00063 };
00064 template <class ElType>
00065         void vector<ElType>::zero()
00066 {
00067     for(int i=0;i<len;i++) data[i]=(ElType)0;
00068 }
00069 template <class ElType>
00070         int vector<ElType>::length()const
00071 {
00072     return len;
00073 }
00074 template <class ElType>
00075         ElType& vector<ElType>::operator[](int i)const
00076 {
00077     assert(i>=0 && i < len);
00078     return data[i];
00079 }
00080 
00081 template <class ElType>
00082         vector<ElType>::vector()
00083 {
00084     data=new ElType[ DEFAULT_ALLOC];
00085     assert(data!=0);
00086     len=  DEFAULT_ALLOC;
00087 }
00088 template <class ElType>
00089         vector<ElType>::vector(int n)
00090 {
00091     data = new ElType[len=n];
00092     assert(data!=0);
00093 }
00094 template <class ElType>
00095         vector<ElType>::vector(const vector<ElType>& v)
00096 {
00097     data=new ElType[len=v.len];
00098     assert(data!=0);
00099     for(int i=0;i<len;i++) data[i]=v.data[i];
00100 }
00101 template <class ElType>
00102         vector<ElType>& vector<ElType>::operator =(const vector<ElType> &original)
00103 {
00104     if(this != &original)
00105     {
00106         delete [] data;
00107         data= new ElType[len=original.len];
00108         assert(data!=0);
00109         for(int i=0;i<len;i++) data[i]=original.data[i];
00110     }
00111     return *this;
00112 }
00113 template <class ElType>
00114         vector<ElType> vector<ElType>::operator+(const vector<ElType>& v)
00115 {
00116     vector<ElType> sum(len);
00117     for(int i=0;i<len;i++) sum[i] = data[i]+v.data[i];
00118     return sum;
00119 
00120 }
00121 template <class ElType>
00122         vector<ElType> vector<ElType>::operator-(const vector<ElType>& v)
00123 {
00124     vector<ElType> sum(len);
00125     for(int i=0;i<len;i++) sum[i] = data[i]-v.data[i];
00126     return sum;
00127 }
00128 template <class ElType>
00129         void  vector<ElType>::rprint()const  //print entries on a single line
00130 {
00131     int i;
00132     cout << "VECTOR: ";
00133     cout << "(";
00134     for(i=0;i<len-1;i++) cout << data[i] << ",";
00135     cout << data[len-1] << ")" << endl;
00136     return;
00137 }
00138 template <class ElType>
00139         void vector<ElType>::resize(int n)
00140 {
00141     delete[]data;
00142     data = new ElType[len=n];
00143     assert(data !=0);
00144 }
00145 template <class ElType>
00146         int vector<ElType>::operator==(const vector<ElType>& v)const
00147 {
00148     if(len != v.len) return 0;
00149     for(int i=0;i<len;i++) if(data[i]!=v.data[i]) return 0;
00150     return 1;
00151 }
00152 template <class ElType>
00153         vector<ElType> operator*(ElType c,vector<ElType>& v )
00154 {
00155     vector<ElType> ans(v.len);
00156     for(int i=0;i<v.len;i++) ans[i]=c*v[i];
00157     return ans;
00158 }
00159 template <class ElType>
00160         vector<ElType> operator*(vector<ElType>& v,ElType c )
00161 {
00162     vector<ElType> ans(v.len);
00163     for(int i=0;i<v.len;i++) ans[i]=c*v[i];
00164     return ans;
00165 }
00166 template <class ElType>
00167         ostream& operator<<(ostream& s,vector<ElType>& v)
00168 {
00169     s << "(";
00170     for(int i=0;i<v.len-1;i++) s << v.data[i] << ", ";
00171     s << v.data[v.len-1]<<")"<<endl;
00172     return s;
00173 }
00174 
00175 template <class ElType>
00176         class matrix
00177 {
00178     public:
00179         vector<ElType> *m;
00180         int rows,cols;
00181         matrix();
00182         matrix( int r, int c);
00183         matrix(const matrix<ElType> &s);
00184         ~matrix();
00185         matrix& operator =(const matrix<ElType>& s);
00186         vector<ElType>& operator[](const int i);
00187         vector<ElType> operator*(const vector<ElType>&);
00188         friend matrix<ElType>  operator*(const ElType&, const matrix<ElType>&);
00189         friend matrix<ElType> operator*(const matrix<ElType>&, const ElType&);
00190         matrix<ElType> operator*(const matrix<ElType>& a);
00191         matrix<ElType> operator+(const matrix<ElType>& a);
00192         matrix<ElType> operator-(const matrix<ElType>& a);
00193         matrix<ElType> transpose();
00194     //matrix<ElType> inverse();
00195         friend ostream& operator<<(ostream& s,matrix<ElType>& m);
00196         friend void ludcmp(matrix<ElType>& a,vector<int>& indx,double &d);
00197         friend void lubksb(matrix<ElType>&a,vector<int>& indx,vector<ElType>&b);
00198 };
00199 template <class ElType>
00200         matrix<ElType>::matrix()
00201 {
00202     m = new vector<ElType>[DEFAULT_ALLOC];
00203     assert(m !=0);
00204     rows=cols=DEFAULT_ALLOC;
00205     for(int i=0;i<rows;i++)
00206     {
00207         vector<ElType> v;
00208         m[i]= v;
00209     }
00210 }
00211 
00212 template <class ElType>
00213         matrix<ElType>::matrix(int r, int c)
00214 {
00215     m= new vector<ElType>[r];
00216     assert(m != 0);
00217     rows=r;
00218     cols=c;
00219     for(int i=0;i<r;i++)
00220     {
00221         vector<ElType> v(cols);
00222         m[i]=v;
00223     }
00224 }
00225 template <class ElType>
00226         matrix<ElType>::matrix(const matrix<ElType> &s)
00227 {
00228     int i;
00229     rows=s.rows;
00230     m = new vector<ElType>[rows];
00231     assert(m!=0);
00232     cols =s.cols;
00233     for(i=0;i<rows;i++)
00234     {
00235         m[i]=s.m[i];
00236     }
00237 }
00238 template <class ElType>
00239         matrix<ElType>::~matrix()
00240 {
00241     delete [] m;
00242 }
00243 
00244 template <class ElType>
00245         matrix<ElType>& matrix<ElType>::operator =(const matrix<ElType> &s)
00246 {
00247     if(this != &s)
00248     {
00249         delete []m;
00250         rows= s.rows;
00251         cols=s.cols;
00252         m = new vector<ElType>[rows];
00253         assert(m !=0);
00254         for(int i=0;i<rows;i++) m[i]=s.m[i];
00255     }
00256     return *this;
00257 }
00258 template <class ElType>
00259         vector<ElType>& matrix<ElType>::operator[](const int i)
00260 {
00261     assert(i>=0 && i < rows);
00262     return m[i];
00263 }
00264 template <class ElType>
00265         vector<ElType> matrix<ElType>::operator*(const vector<ElType>& v)
00266 {
00267     int i,j;
00268     assert(cols == v.len);
00269     vector<ElType> ans(rows);
00270     for(i=0;i<rows;i++)
00271     {
00272         ans.data[i]=0.0;
00273         for(j=0;j<cols;j++) ans.data[i] += m[i][j]*v.data[j];
00274     }
00275     return ans;
00276 }
00277 template <class ElType>
00278         matrix<ElType> operator*(const ElType& x,const matrix<ElType>& s)
00279 {
00280     matrix<ElType> ans(s.rows,s.cols);
00281     for(int i=0;i<ans.rows;i++)
00282     {
00283         ans.m[i]= x*s.m[i];
00284     }
00285     return ans;
00286 }
00287 template <class ElType>
00288         matrix<ElType> matrix<ElType>::transpose()
00289 {
00290     matrix<ElType> ans(cols,rows);
00291     for(int i=0;i<rows;i++)
00292     {
00293         for(int j=0;j<cols;j++) ans[j][i]=m[i][j];
00294     }
00295     return ans;
00296 }
00297 template <class ElType>
00298         matrix<ElType> operator*(const matrix<ElType>& s,const ElType& x)
00299 {
00300     matrix<ElType> ans(s.rows,s.cols);
00301     for(int i=0;i<ans.rows;i++)
00302     {
00303         ans.m[i]= x*s.m[i];
00304     }
00305     return ans;
00306 }
00307 template <class ElType>
00308         matrix<ElType>  matrix<ElType> ::operator*(const matrix<ElType>&  a)
00309 {
00310 
00311     assert(cols == a.rows);
00312 
00313     matrix<ElType>  ans(rows,a.cols);
00314     for(int i=0;i<rows;i++)
00315     {
00316         for(int j=0;j<a.cols;j++)
00317         {
00318             ans.m[i][j]=0.0;
00319             for(int k=0;k<cols;k++)
00320             {
00321                 ans.m[i][j] += m[i][k]*a.m[k][j];
00322             }
00323         }
00324     }
00325     return ans;
00326 }
00327 template <class ElType>
00328         matrix<ElType>  matrix<ElType> ::operator+(const matrix<ElType> & a)
00329 {
00330     int i,j;
00331 
00332     assert(rows== a.rows);
00333     assert(cols== a.cols);
00334 
00335     matrix<ElType>  ans(a.rows,a.cols);
00336     for(i=0;i<a.rows;i++)
00337     {
00338         for(j=0;j<a.cols;j++)
00339         {
00340             ans.m[i][j] = m[i][j] + a.m[i][j];  //faster than assigning vectors?
00341         }
00342     }
00343     return ans;
00344 }
00345 template <class ElType>
00346         matrix<ElType> matrix<ElType>::operator-(const matrix<ElType>& a)
00347 {
00348     int i,j;
00349     assert(rows == a.rows);
00350     assert(cols == a.cols);
00351     matrix ans(rows,cols);
00352     for(i=0;i<rows;i++)
00353     {
00354         for(j=0;j<cols;j++)
00355             ans.m[i][j] = m[i][j] - a.m[i][j];
00356     }
00357     return ans;
00358 }
00359 template <class ElType>
00360         ostream& operator<<(ostream& s,matrix<ElType>& m)
00361 {
00362     for(int i=0; i<m.rows;i++) s << m[i];
00363     return s;
00364 }
00365 
00366 #define TINY 1.0e-20;
00367 //we assume fabs(ElType) is defined
00368 //assignment of doubles to ElType is defined
00369 template <class ElType>
00370 void ludcmp(matrix<ElType>& a, vector<int>& indx,double& d)
00371 {
00372     int i,imax,j,k;
00373     ElType  big,dum,sum,temp;
00374     int n=a.rows;
00375     vector<ElType> vv(n);
00376     assert(a.rows == a.cols);
00377     d=1.0;
00378     for (i=0;i<n;i++)
00379     {
00380         big=0.0;
00381 //         kdDebug() << "new search" << endl;
00382         for (j=0;j<n;j++) { if ((temp=fabs(a[i][j])) > big) big=temp;
00383 /*            kdDebug() << temp << " " << fabs(a[i][j]) << " "<< big <<endl; */}
00384             if (big == 0.0) { cerr << "Singular matrix in routine LUDCMP" << endl; big = TINY;}
00385             vv[i]=1.0/big;
00386     }
00387     for (j=0;j<n;j++)
00388     {
00389         for (i=0;i<j;i++)
00390         {
00391             sum=a[i][j];
00392             for (k=0;k<i;k++) sum -= a[i][k]*a[k][j];
00393             a[i][j]=sum;
00394         }
00395         big=0.0;
00396         for (i=j;i<n;i++)
00397         {
00398             sum=a[i][j];
00399             for (k=0;k<j;k++) sum -= a[i][k]*a[k][j];
00400             a[i][j]=sum;
00401             if ( (dum=vv[i]*fabs(sum)) >= big)
00402             {
00403                 big=dum;
00404                 imax=i;
00405             }
00406         }
00407         if (j != imax)
00408         {
00409             for (k=0;k<n;k++)
00410             {
00411                 dum=a[imax][k];
00412                 a[imax][k]=a[j][k];
00413                 a[j][k]=dum;
00414             }
00415             d = -(d);
00416             vv[imax]=vv[j];
00417         }
00418         indx[j]=imax;
00419         if (a[j][j] == 0.0) a[j][j]=TINY;
00420         if (j != n-1) {
00421             dum=1.0/(a[j][j]);
00422             for (i=j+1;i<n;i++) a[i][j] *= dum;
00423         }
00424     }
00425 }
00426 #undef TINY
00427 template <class ElType>
00428 void lubksb(matrix<ElType>& a,vector<int>& indx,vector<ElType>& b)
00429 {
00430     int i,ip,j;
00431     ElType sum;
00432     int n=a.rows;
00433     for (i=0;i<n;i++)
00434     {
00435         ip=indx[i];
00436         sum=b[ip];
00437         b[ip]=b[i];
00438         for (j=0;j<=i-1;j++) sum -= a[i][j]*b[j];
00439         b[i]=sum;
00440     }
00441     for (i=n-1;i>=0;i--)
00442     {
00443         sum=b[i];
00444         for (j=i+1;j<n;j++) sum -= a[i][j]*b[j];
00445         b[i]=sum/a[i][i];
00446     }
00447 }
00448 
00449 
00450 
00451 }
00452 #endif
00453 
00454 double* KisPerspectiveMath::computeMatrixTransfo( const KisPoint& topLeft1, const KisPoint& topRight1, const KisPoint& bottomLeft1, const KisPoint& bottomRight1 , const KisPoint& topLeft2, const KisPoint& topRight2, const KisPoint& bottomLeft2, const KisPoint& bottomRight2)
00455 {
00456     double* matrix = new double[9];
00457 
00458     math::matrix<double> a(10,10);
00459     math::vector<double> b(10);
00460     math::vector<int> indx(10);
00461     double d = 0.;
00462     for(int i = 0; i <= 9; i++)
00463     {
00464         for(int j = 0; j <= 9; j++)
00465         {
00466             a[i][j] = 0.;
00467         }
00468         b[i] = 0.;
00469         indx[i] = 0;
00470     }
00471 
00472         // topLeft
00473     a[0][0] = topLeft1.x();
00474     a[0][1] = topLeft1.y();
00475     a[0][2] = 1;
00476     a[0][6] = -topLeft2.x() * topLeft1.x();
00477     a[0][7] = -topLeft2.x() * topLeft1.y();
00478     a[0][8] = -topLeft2.x();
00479     a[1][3] = topLeft1.x();
00480     a[1][4] = topLeft1.y();
00481     a[1][5] = 1;
00482     a[1][6] = -topLeft2.y() * topLeft1.x();
00483     a[1][7] = -topLeft2.y() * topLeft1.y();
00484     a[1][8] = -topLeft2.y();
00485         // topRight
00486     a[2][0] = topRight1.x();
00487     a[2][1] = topRight1.y();
00488     a[2][2] = 1;
00489     a[2][6] = -topRight2.x() * topRight1.x();
00490     a[2][7] = -topRight2.x() * topRight1.y();
00491     a[2][8] = -topRight2.x();
00492     a[3][3] = topRight1.x();
00493     a[3][4] = topRight1.y();
00494     a[3][5] = 1;
00495     a[3][6] = -topRight2.y() * topRight1.x();
00496     a[3][7] = -topRight2.y() * topRight1.y();
00497     a[3][8] = -topRight2.y();
00498         // bottomLeft1
00499     a[4][0] = bottomLeft1.x();
00500     a[4][1] = bottomLeft1.y();
00501     a[4][2] = 1;
00502     a[4][6] = -bottomLeft2.x() * bottomLeft1.x();
00503     a[4][7] = -bottomLeft2.x() * bottomLeft1.y();
00504     a[4][8] = -bottomLeft2.x();
00505     a[5][3] = bottomLeft1.x();
00506     a[5][4] = bottomLeft1.y();
00507     a[5][5] = 1;
00508     a[5][6] = -bottomLeft2.y() * bottomLeft1.x();
00509     a[5][7] = -bottomLeft2.y() * bottomLeft1.y();
00510     a[5][8] = -bottomLeft2.y();
00511         // bottomRight
00512     a[6][0] = bottomRight1.x();
00513     a[6][1] = bottomRight1.y();
00514     a[6][2] = 1;
00515     a[6][6] = -bottomRight2.x() * bottomRight1.x();
00516     a[6][7] = -bottomRight2.x() * bottomRight1.y();
00517     a[6][8] = -bottomRight2.x();
00518     a[7][3] = bottomRight1.x();
00519     a[7][4] = bottomRight1.y();
00520     a[7][5] = 1;
00521     a[7][6] = -bottomRight2.y() * bottomRight1.x();
00522     a[7][7] = -bottomRight2.y() * bottomRight1.y();
00523     a[7][8] = -bottomRight2.y();
00524     a[8][8] = 1;
00525     b[8] = 1;
00526 //     kdDebug() << " a := { { " << a[0][0] << " , " << a[0][1] << " , " << a[0][2] << " , " << a[0][3] << " , " << a[0][4] << " , " << a[0][5] << " , " << a[0][6] << " , " << a[0][7] << " , " << a[0][8] << " } , { " << a[1][0] << " , " << a[1][1] << " , " << a[1][2] << " , " << a[1][3] << " , " << a[1][4] << " , " << a[1][5] << " , " << a[1][6] << " , " << a[1][7] << " , " << a[1][8] << " } , { " << a[2][0] << " , " << a[2][1] << " , " << a[2][2] << " , " << a[2][3] << " , " << a[2][4] << " , " << a[2][5] << " , " << a[2][6] << " , " << a[2][7] << " , " << a[2][8] << " } , { " << a[3][0] << " , " << a[3][1] << " , " << a[3][2] << " , " << a[3][3] << " , " << a[3][4] << " , " << a[3][5] << " , " << a[3][6] << " , " << a[3][7] << " , " << a[3][8] << " } , { " << a[4][0] << " , " << a[4][1] << " , " << a[4][2] << " , " << a[4][3] << " , " << a[4][4] << " , " << a[4][5] << " , " << a[4][6] << " , " << a[4][7] << " , " << a[4][8] << " } , { " << a[5][0] << " , " << a[5][1] << " , " << a[5][2] << " , " << a[5][3] << " , " << a[5][4] << " , " << a[5][5] << " , " << a[5][6] << " , " << a[5][7] << " , " << a[5][8] << " } , { " << a[6][0] << " , " << a[6][1] << " , " << a[6][2] << " , " << a[6][3] << " , " << a[6][4] << " , " << a[6][5] << " , " << a[6][6] << " , " << a[6][7] << " , " << a[6][8] << " } , { "<< a[7][0] << " , " << a[7][1] << " , " << a[7][2] << " , " << a[7][3] << " , " << a[7][4] << " , " << a[7][5] << " , " << a[7][6] << " , " << a[7][7] << " , " << a[7][8] << " } , { "<< a[8][0] << " , " << a[8][1] << " , " << a[8][2] << " , " << a[8][3] << " , " << a[8][4] << " , " << a[8][5] << " , " << a[8][6] << " , " << a[8][7] << " , " << a[8][8] << " } }; " << endl;
00527     math::ludcmp<double>(a,indx,d);
00528     math::lubksb<double>(a,indx,b);
00529 
00530     for(int i = 0; i < 9; i++)
00531     {
00532         matrix[i] = b[i];
00533     }
00534     return matrix;
00535 }
00536 
00537 double* KisPerspectiveMath::computeMatrixTransfoToPerspective(const KisPoint& topLeft, const KisPoint& topRight, const KisPoint& bottomLeft, const KisPoint& bottomRight, const QRect& r)
00538 {
00539     return KisPerspectiveMath::computeMatrixTransfo(topLeft, topRight, bottomLeft, bottomRight, r.topLeft(), r.topRight(), r.bottomLeft(), r.bottomRight());
00540 }
00541 
00542 double* KisPerspectiveMath::computeMatrixTransfoFromPerspective(const QRect& r, const KisPoint& topLeft, const KisPoint& topRight, const KisPoint& bottomLeft, const KisPoint& bottomRight)
00543 {
00544     return KisPerspectiveMath::computeMatrixTransfo(r.topLeft(), r.topRight(), r.bottomLeft(), r.bottomRight(), topLeft, topRight, bottomLeft, bottomRight);
00545 }
00546 
KDE Home | KDE Accessibility Home | Description of Access Keys