00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include "kis_perspective_math.h"
00022
00023 #include <qrect.h>
00024
00025 #if 1
00026
00027 #include <iostream>
00028 #include <stdlib.h>
00029 #include <math.h>
00030
00031 #include <assert.h>
00032 #define DEFAULT_ALLOC 2
00033
00034 namespace math {
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
00050 vector(const vector<ElType>& v) ;
00051
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;
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 std::ostream& operator<<(std::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
00130 {
00131 int i;
00132 std::cout << "VECTOR: ";
00133 std::cout << "(";
00134 for(i=0;i<len-1;i++) std::cout << data[i] << ",";
00135 std::cout << data[len-1] << ")" << std::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 std::ostream& operator<<(std::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
00195 friend std::ostream& operator<<(std::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];
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 std::ostream& operator<<(std::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
00368
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
00382 for (j=0;j<n;j++) { if ((temp=fabs(a[i][j])) > big) big=temp;
00383 }
00384 if (big == 0.0) { std::cerr << "Singular matrix in routine LUDCMP" << std::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
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
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
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
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
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