chalk

kis_perspective_math.cpp
1/*
2 * This file is part of Chalk
3 *
4 * Copyright (c) 2006 Cyrille Berger <cberger@cberger.net>
5 *
6 * This program is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 *
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with this program; if not, write to the Free Software
18 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
19 */
20
21#include "kis_perspective_math.h"
22
23#include <tqrect.h>
24
25#if 1
26
27#include <iostream>
28#include <stdlib.h>
29#include <math.h>
30//#define NDEBUG // uncomment to remove checking of assert()
31#include <assert.h>
32#define DEFAULT_ALLOC 2
33
34namespace math { // TODO: use eigen
35
36template <class ElType> class matrix;
37
38template <class ElType>
39 class vector
40{
41 public:
42 friend class matrix<ElType>;
43 ElType * data;
44 int len;
45 int length()const;
46 vector();
47 vector(int n);
48 ~vector(){ delete [] data;}
49 //Copy operator
50 vector(const vector<ElType>& v) ;
51 //assignment operator
52 vector<ElType>& operator =(const vector<ElType> &original);
53 ElType& operator[](int i)const ;
54 void zero();
55 vector<ElType> operator+(const vector<ElType>& v);
56 vector<ElType> operator-(const vector<ElType>&v);
57 void rprint()const; //print entries on a single line
58 void resize(int n);
59 int operator==(const vector<ElType>& v)const;
60 friend vector<ElType> operator* (ElType c,vector<ElType>& v );
61 friend vector<ElType> operator*(vector<ElType>& v,ElType c );
62 friend std::ostream& operator<<(std::ostream& s,vector<ElType>& v);
63};
64template <class ElType>
65 void vector<ElType>::zero()
66{
67 for(int i=0;i<len;i++) data[i]=(ElType)0;
68}
69template <class ElType>
70 int vector<ElType>::length()const
71{
72 return len;
73}
74template <class ElType>
75 ElType& vector<ElType>::operator[](int i)const
76{
77 assert(i>=0 && i < len);
78 return data[i];
79}
80
81template <class ElType>
82 vector<ElType>::vector()
83{
84 data=new ElType[ DEFAULT_ALLOC];
85 assert(data!=0);
86 len= DEFAULT_ALLOC;
87}
88template <class ElType>
89 vector<ElType>::vector(int n)
90{
91 data = new ElType[len=n];
92 assert(data!=0);
93}
94template <class ElType>
95 vector<ElType>::vector(const vector<ElType>& v)
96{
97 data=new ElType[len=v.len];
98 assert(data!=0);
99 for(int i=0;i<len;i++) data[i]=v.data[i];
100}
101template <class ElType>
102 vector<ElType>& vector<ElType>::operator =(const vector<ElType> &original)
103{
104 if(this != &original)
105 {
106 delete [] data;
107 data= new ElType[len=original.len];
108 assert(data!=0);
109 for(int i=0;i<len;i++) data[i]=original.data[i];
110 }
111 return *this;
112}
113template <class ElType>
114 vector<ElType> vector<ElType>::operator+(const vector<ElType>& v)
115{
116 vector<ElType> sum(len);
117 for(int i=0;i<len;i++) sum[i] = data[i]+v.data[i];
118 return sum;
119
120}
121template <class ElType>
122 vector<ElType> vector<ElType>::operator-(const vector<ElType>& v)
123{
124 vector<ElType> sum(len);
125 for(int i=0;i<len;i++) sum[i] = data[i]-v.data[i];
126 return sum;
127}
128template <class ElType>
129 void vector<ElType>::rprint()const //print entries on a single line
130{
131 int i;
132 std::cout << "VECTOR: ";
133 std::cout << "(";
134 for(i=0;i<len-1;i++) std::cout << data[i] << ",";
135 std::cout << data[len-1] << ")" << std::endl;
136 return;
137}
138template <class ElType>
139 void vector<ElType>::resize(int n)
140{
141 delete[]data;
142 data = new ElType[len=n];
143 assert(data !=0);
144}
145template <class ElType>
146 int vector<ElType>::operator==(const vector<ElType>& v)const
147{
148 if(len != v.len) return 0;
149 for(int i=0;i<len;i++) if(data[i]!=v.data[i]) return 0;
150 return 1;
151}
152template <class ElType>
153 vector<ElType> operator*(ElType c,vector<ElType>& v )
154{
155 vector<ElType> ans(v.len);
156 for(int i=0;i<v.len;i++) ans[i]=c*v[i];
157 return ans;
158}
159template <class ElType>
160 vector<ElType> operator*(vector<ElType>& v,ElType c )
161{
162 vector<ElType> ans(v.len);
163 for(int i=0;i<v.len;i++) ans[i]=c*v[i];
164 return ans;
165}
166template <class ElType>
167 std::ostream& operator<<(std::ostream& s,vector<ElType>& v)
168{
169 s << "(";
170 for(int i=0;i<v.len-1;i++) s << v.data[i] << ", ";
171 s << v.data[v.len-1]<<")"<<endl;
172 return s;
173}
174
175template <class ElType>
176 class matrix
177{
178 public:
179 vector<ElType> *m;
180 int rows,cols;
181 matrix();
182 matrix( int r, int c);
183 matrix(const matrix<ElType> &s);
184 ~matrix();
185 matrix& operator =(const matrix<ElType>& s);
186 vector<ElType>& operator[](const int i);
187 vector<ElType> operator*(const vector<ElType>&);
188 friend matrix<ElType> operator*(const ElType&, const matrix<ElType>&);
189 friend matrix<ElType> operator*(const matrix<ElType>&, const ElType&);
190 matrix<ElType> operator*(const matrix<ElType>& a);
191 matrix<ElType> operator+(const matrix<ElType>& a);
192 matrix<ElType> operator-(const matrix<ElType>& a);
193 matrix<ElType> transpose();
194 //matrix<ElType> inverse();
195 friend std::ostream& operator<<(std::ostream& s,matrix<ElType>& m);
196 friend void ludcmp(matrix<ElType>& a,vector<int>& indx,double &d);
197 friend void lubksb(matrix<ElType>&a,vector<int>& indx,vector<ElType>&b);
198};
199template <class ElType>
200 matrix<ElType>::matrix()
201{
202 m = new vector<ElType>[DEFAULT_ALLOC];
203 assert(m !=0);
204 rows=cols=DEFAULT_ALLOC;
205 for(int i=0;i<rows;i++)
206 {
207 vector<ElType> v;
208 m[i]= v;
209 }
210}
211
212template <class ElType>
213 matrix<ElType>::matrix(int r, int c)
214{
215 m= new vector<ElType>[r];
216 assert(m != 0);
217 rows=r;
218 cols=c;
219 for(int i=0;i<r;i++)
220 {
221 vector<ElType> v(cols);
222 m[i]=v;
223 }
224}
225template <class ElType>
226 matrix<ElType>::matrix(const matrix<ElType> &s)
227{
228 int i;
229 rows=s.rows;
230 m = new vector<ElType>[rows];
231 assert(m!=0);
232 cols =s.cols;
233 for(i=0;i<rows;i++)
234 {
235 m[i]=s.m[i];
236 }
237}
238template <class ElType>
239 matrix<ElType>::~matrix()
240{
241 delete [] m;
242}
243
244template <class ElType>
245 matrix<ElType>& matrix<ElType>::operator =(const matrix<ElType> &s)
246{
247 if(this != &s)
248 {
249 delete []m;
250 rows= s.rows;
251 cols=s.cols;
252 m = new vector<ElType>[rows];
253 assert(m !=0);
254 for(int i=0;i<rows;i++) m[i]=s.m[i];
255 }
256 return *this;
257}
258template <class ElType>
259 vector<ElType>& matrix<ElType>::operator[](const int i)
260{
261 assert(i>=0 && i < rows);
262 return m[i];
263}
264template <class ElType>
265 vector<ElType> matrix<ElType>::operator*(const vector<ElType>& v)
266{
267 int i,j;
268 assert(cols == v.len);
269 vector<ElType> ans(rows);
270 for(i=0;i<rows;i++)
271 {
272 ans.data[i]=0.0;
273 for(j=0;j<cols;j++) ans.data[i] += m[i][j]*v.data[j];
274 }
275 return ans;
276}
277template <class ElType>
278 matrix<ElType> operator*(const ElType& x,const matrix<ElType>& s)
279{
280 matrix<ElType> ans(s.rows,s.cols);
281 for(int i=0;i<ans.rows;i++)
282 {
283 ans.m[i]= x*s.m[i];
284 }
285 return ans;
286}
287template <class ElType>
288 matrix<ElType> matrix<ElType>::transpose()
289{
290 matrix<ElType> ans(cols,rows);
291 for(int i=0;i<rows;i++)
292 {
293 for(int j=0;j<cols;j++) ans[j][i]=m[i][j];
294 }
295 return ans;
296}
297template <class ElType>
298 matrix<ElType> operator*(const matrix<ElType>& s,const ElType& x)
299{
300 matrix<ElType> ans(s.rows,s.cols);
301 for(int i=0;i<ans.rows;i++)
302 {
303 ans.m[i]= x*s.m[i];
304 }
305 return ans;
306}
307template <class ElType>
308 matrix<ElType> matrix<ElType> ::operator*(const matrix<ElType>& a)
309{
310
311 assert(cols == a.rows);
312
313 matrix<ElType> ans(rows,a.cols);
314 for(int i=0;i<rows;i++)
315 {
316 for(int j=0;j<a.cols;j++)
317 {
318 ans.m[i][j]=0.0;
319 for(int k=0;k<cols;k++)
320 {
321 ans.m[i][j] += m[i][k]*a.m[k][j];
322 }
323 }
324 }
325 return ans;
326}
327template <class ElType>
328 matrix<ElType> matrix<ElType> ::operator+(const matrix<ElType> & a)
329{
330 int i,j;
331
332 assert(rows== a.rows);
333 assert(cols== a.cols);
334
335 matrix<ElType> ans(a.rows,a.cols);
336 for(i=0;i<a.rows;i++)
337 {
338 for(j=0;j<a.cols;j++)
339 {
340 ans.m[i][j] = m[i][j] + a.m[i][j]; //faster than assigning vectors?
341 }
342 }
343 return ans;
344}
345template <class ElType>
346 matrix<ElType> matrix<ElType>::operator-(const matrix<ElType>& a)
347{
348 int i,j;
349 assert(rows == a.rows);
350 assert(cols == a.cols);
351 matrix ans(rows,cols);
352 for(i=0;i<rows;i++)
353 {
354 for(j=0;j<cols;j++)
355 ans.m[i][j] = m[i][j] - a.m[i][j];
356 }
357 return ans;
358}
359template <class ElType>
360 std::ostream& operator<<(std::ostream& s,matrix<ElType>& m)
361{
362 for(int i=0; i<m.rows;i++) s << m[i];
363 return s;
364}
365
366#define TINY 1.0e-20;
367//we assume fabs(ElType) is defined
368//assignment of doubles to ElType is defined
369template <class ElType>
370void ludcmp(matrix<ElType>& a, vector<int>& indx,double& d)
371{
372 int i,imax,j,k;
373 ElType big,dum,sum,temp;
374 int n=a.rows;
375 vector<ElType> vv(n);
376 assert(a.rows == a.cols);
377 d=1.0;
378 for (i=0;i<n;i++)
379 {
380 big=0.0;
381// kdDebug() << "new search" << endl;
382 for (j=0;j<n;j++) { if ((temp=fabs(a[i][j])) > big) big=temp;
383/* kdDebug() << temp << " " << fabs(a[i][j]) << " "<< big <<endl; */}
384 if (big == 0.0) { std::cerr << "Singular matrix in routine LUDCMP" << std::endl; big = TINY;}
385 vv[i]=1.0/big;
386 }
387 for (j=0;j<n;j++)
388 {
389 for (i=0;i<j;i++)
390 {
391 sum=a[i][j];
392 for (k=0;k<i;k++) sum -= a[i][k]*a[k][j];
393 a[i][j]=sum;
394 }
395 big=0.0;
396 for (i=j;i<n;i++)
397 {
398 sum=a[i][j];
399 for (k=0;k<j;k++) sum -= a[i][k]*a[k][j];
400 a[i][j]=sum;
401 if ( (dum=vv[i]*fabs(sum)) >= big)
402 {
403 big=dum;
404 imax=i;
405 }
406 }
407 if (j != imax)
408 {
409 for (k=0;k<n;k++)
410 {
411 dum=a[imax][k];
412 a[imax][k]=a[j][k];
413 a[j][k]=dum;
414 }
415 d = -(d);
416 vv[imax]=vv[j];
417 }
418 indx[j]=imax;
419 if (a[j][j] == 0.0) a[j][j]=TINY;
420 if (j != n-1) {
421 dum=1.0/(a[j][j]);
422 for (i=j+1;i<n;i++) a[i][j] *= dum;
423 }
424 }
425}
426#undef TINY
427template <class ElType>
428void lubksb(matrix<ElType>& a,vector<int>& indx,vector<ElType>& b)
429{
430 int i,ip,j;
431 ElType sum;
432 int n=a.rows;
433 for (i=0;i<n;i++)
434 {
435 ip=indx[i];
436 sum=b[ip];
437 b[ip]=b[i];
438 for (j=0;j<=i-1;j++) sum -= a[i][j]*b[j];
439 b[i]=sum;
440 }
441 for (i=n-1;i>=0;i--)
442 {
443 sum=b[i];
444 for (j=i+1;j<n;j++) sum -= a[i][j]*b[j];
445 b[i]=sum/a[i][i];
446 }
447}
448
449
450
451}
452#endif
453
454double* 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)
455{
456 double* matrix = new double[9];
457
458 math::matrix<double> a(10,10);
459 math::vector<double> b(10);
460 math::vector<int> indx(10);
461 double d = 0.;
462 for(int i = 0; i <= 9; i++)
463 {
464 for(int j = 0; j <= 9; j++)
465 {
466 a[i][j] = 0.;
467 }
468 b[i] = 0.;
469 indx[i] = 0;
470 }
471
472 // topLeft
473 a[0][0] = topLeft1.x();
474 a[0][1] = topLeft1.y();
475 a[0][2] = 1;
476 a[0][6] = -topLeft2.x() * topLeft1.x();
477 a[0][7] = -topLeft2.x() * topLeft1.y();
478 a[0][8] = -topLeft2.x();
479 a[1][3] = topLeft1.x();
480 a[1][4] = topLeft1.y();
481 a[1][5] = 1;
482 a[1][6] = -topLeft2.y() * topLeft1.x();
483 a[1][7] = -topLeft2.y() * topLeft1.y();
484 a[1][8] = -topLeft2.y();
485 // topRight
486 a[2][0] = topRight1.x();
487 a[2][1] = topRight1.y();
488 a[2][2] = 1;
489 a[2][6] = -topRight2.x() * topRight1.x();
490 a[2][7] = -topRight2.x() * topRight1.y();
491 a[2][8] = -topRight2.x();
492 a[3][3] = topRight1.x();
493 a[3][4] = topRight1.y();
494 a[3][5] = 1;
495 a[3][6] = -topRight2.y() * topRight1.x();
496 a[3][7] = -topRight2.y() * topRight1.y();
497 a[3][8] = -topRight2.y();
498 // bottomLeft1
499 a[4][0] = bottomLeft1.x();
500 a[4][1] = bottomLeft1.y();
501 a[4][2] = 1;
502 a[4][6] = -bottomLeft2.x() * bottomLeft1.x();
503 a[4][7] = -bottomLeft2.x() * bottomLeft1.y();
504 a[4][8] = -bottomLeft2.x();
505 a[5][3] = bottomLeft1.x();
506 a[5][4] = bottomLeft1.y();
507 a[5][5] = 1;
508 a[5][6] = -bottomLeft2.y() * bottomLeft1.x();
509 a[5][7] = -bottomLeft2.y() * bottomLeft1.y();
510 a[5][8] = -bottomLeft2.y();
511 // bottomRight
512 a[6][0] = bottomRight1.x();
513 a[6][1] = bottomRight1.y();
514 a[6][2] = 1;
515 a[6][6] = -bottomRight2.x() * bottomRight1.x();
516 a[6][7] = -bottomRight2.x() * bottomRight1.y();
517 a[6][8] = -bottomRight2.x();
518 a[7][3] = bottomRight1.x();
519 a[7][4] = bottomRight1.y();
520 a[7][5] = 1;
521 a[7][6] = -bottomRight2.y() * bottomRight1.x();
522 a[7][7] = -bottomRight2.y() * bottomRight1.y();
523 a[7][8] = -bottomRight2.y();
524 a[8][8] = 1;
525 b[8] = 1;
526// 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;
527 math::ludcmp<double>(a,indx,d);
528 math::lubksb<double>(a,indx,b);
529
530 for(int i = 0; i < 9; i++)
531 {
532 matrix[i] = b[i];
533 }
534 return matrix;
535}
536
537double* KisPerspectiveMath::computeMatrixTransfoToPerspective(const KisPoint& topLeft, const KisPoint& topRight, const KisPoint& bottomLeft, const KisPoint& bottomRight, const TQRect& r)
538{
539 return KisPerspectiveMath::computeMatrixTransfo(topLeft, topRight, bottomLeft, bottomRight, KisPoint(r.topLeft()), KisPoint(r.topRight()), KisPoint(r.bottomLeft()), KisPoint(r.bottomRight()));
540}
541
542double* KisPerspectiveMath::computeMatrixTransfoFromPerspective(const TQRect& r, const KisPoint& topLeft, const KisPoint& topRight, const KisPoint& bottomLeft, const KisPoint& bottomRight)
543{
544 return KisPerspectiveMath::computeMatrixTransfo(KisPoint(r.topLeft()), KisPoint(r.topRight()), KisPoint(r.bottomLeft()), KisPoint(r.bottomRight()), topLeft, topRight, bottomLeft, bottomRight);
545}
546
KisPoint
A double-based point class that can return it's coordinates approximated to integers.
Definition kis_point.h:28