/[opencvs]/eyes/square_to_quad.c
ViewVC logotype

Contents of /eyes/square_to_quad.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 1.1 - (show annotations)
Mon Nov 12 07:33:44 2012 UTC (5 years, 8 months ago) by hib
Branch: MAIN
CVS Tags: HEAD
File MIME type: text/plain
cleanup
1 #include <stdio.h>
2 #include <math.h>
3 #include "square_to_quad.h"
4
5 /* ripped off from http://code.google.com/p/wiimotetuio/source/browse/trunk/WiimoteTUIO/Warper.cs */
6
7
8
9
10 void setIdentity( struct Warper *w)
11 {
12 setSource(w,0.0 ,0.0,
13 1.0, 0.0,
14 0.0, 1.0,
15 1.0, 1.0);
16 setDestination(w,0.0, 0.0,
17 1.0, 0.0,
18 0.0, 1.0,
19 1.0, 1.0);
20 computeWarp(w);
21 }
22
23 void setSource(struct Warper *w, double x0,
24 double y0,
25 double x1,
26 double y1,
27 double x2,
28 double y2,
29 double x3,
30 double y3){
31 w->srcX[0] = x0;
32 w->srcY[0] = y0;
33 w->srcX[1] = x1;
34 w->srcY[1] = y1;
35 w->srcX[2] = x2;
36 w->srcY[2] = y2;
37 w->srcX[3] = x3;
38 w->srcY[3] = y3;
39 w->dirty = 1;
40 }
41
42 void setDestination(struct Warper *w,double x0,
43 double y0,
44 double x1,
45 double y1,
46 double x2,
47 double y2,
48 double x3,
49 double y3){
50 w->dstX[0] = x0;
51 w->dstY[0] = y0;
52 w->dstX[1] = x1;
53 w->dstY[1] = y1;
54 w->dstX[2] = x2;
55 w->dstY[2] = y2;
56 w->dstX[3] = x3;
57 w->dstY[3] = y3;
58 w->dirty = 1;
59 }
60
61
62 void computeWarp(struct Warper *w) {
63 computeQuadToSquare( w->srcX[0],w->srcY[0],
64 w->srcX[1],w->srcY[1],
65 w->srcX[2],w->srcY[2],
66 w->srcX[3],w->srcY[3],
67 w->srcMat);
68 computeSquareToQuad( w->dstX[0], w->dstY[0],
69 w->dstX[1], w->dstY[1],
70 w->dstX[2], w->dstY[2],
71 w->dstX[3], w->dstY[3],
72 w->dstMat);
73 multMats(w->srcMat, w->dstMat, w->warpMat);
74 w->dirty = 0;
75 }
76
77 void multMats(double * srcMat, double * dstMat, double * resMat) {
78 // DSTDO/CBB: could be faster, but not called often enough to matter
79 int r;
80 for ( r = 0; r < 4; r++) {
81 int ri = r * 4;
82 int c;
83 for ( c = 0; c < 4; c++) {
84 resMat[ri + c] = (srcMat[ri ] * dstMat[c ] +
85 srcMat[ri + 1] * dstMat[c + 4] +
86 srcMat[ri + 2] * dstMat[c + 8] +
87 srcMat[ri + 3] * dstMat[c + 12]);
88 }
89 }
90
91 }
92
93 void computeSquareToQuad( double x0,
94 double y0,
95 double x1,
96 double y1,
97 double x2,
98 double y2,
99 double x3,
100 double y3,
101 double* mat) {
102
103 double dx1 = x1 - x2; double dy1 = y1 - y2;
104 double dx2 = x3 - x2; double dy2 = y3 - y2;
105 double sx = x0 - x1 + x2 - x3;
106 double sy = y0 - y1 + y2 - y3;
107 double g = (sx * dy2 - dx2 * sy) / (dx1 * dy2 - dx2 * dy1);
108 double h = (dx1 * sy - sx * dy1) / (dx1 * dy2 - dx2 * dy1);
109 double a = x1 - x0 + g * x1;
110 double b = x3 - x0 + h * x3;
111 double c = x0;
112 double d = y1 - y0 + g * y1;
113 double e = y3 - y0 + h * y3;
114 double f = y0;
115
116 mat[ 0] = a; mat[ 1] = d; mat[ 2] = 0; mat[ 3] = g;
117 mat[ 4] = b; mat[ 5] = e; mat[ 6] = 0; mat[ 7] = h;
118 mat[ 8] = 0; mat[ 9] = 0; mat[10] = 1; mat[11] = 0;
119 mat[12] = c; mat[13] = f; mat[14] = 0; mat[15] = 1;
120 }
121
122 void computeQuadToSquare( double x0,
123 double y0,
124 double x1,
125 double y1,
126 double x2,
127 double y2,
128 double x3,
129 double y3,
130 double * mat) {
131 computeSquareToQuad(x0,y0,x1,y1,x2,y2,x3,y3, mat);
132
133 // invert through adjoint
134
135 double a = mat[ 0]; double d = mat[ 1]; double /* ignore */ g = mat[ 3];
136 double b = mat[ 4]; double e = mat[ 5]; double /* 3rd col*/ h = mat[ 7];
137 /* ignore 3rd row */
138 double c = mat[12]; double f = mat[13];
139
140 double A = e - f * h;
141 double B = c * h - b;
142 double C = b * f - c * e;
143 double D = f * g - d;
144 double E = a - c * g;
145 double F = c * d - a * f;
146 double G = d * h - e * g;
147 double H = b * g - a * h;
148 double I = a * e - b * d;
149
150 // Probably unnecessary since 'I' is also scaled by the determinant,
151 // and 'I' scales the homogeneous coordinate, which, in turn,
152 // scales the X,Y coordinates.
153 // Determinant = a * (e - f * h) + b * (f * g - d) + c * (d * h - e * g);
154 double idet = 1.0f / (a * A + b * D + c * G);
155
156 mat[ 0] = A * idet; mat[ 1] = D * idet; mat[ 2] = 0; mat[ 3] = G * idet;
157 mat[ 4] = B * idet; mat[ 5] = E * idet; mat[ 6] = 0; mat[ 7] = H * idet;
158 mat[ 8] = 0 ; mat[ 9] = 0 ; mat[10] = 1; mat[11] = 0 ;
159 mat[12] = C * idet; mat[13] = F * idet; mat[14] = 0; mat[15] = I * idet;
160 }
161
162 double * getWarpMatrix(struct Warper *w)
163 {
164 return w->warpMat;
165 }
166
167 void warp(struct Warper *w,double srcX, double srcY,double *dstX,double *dstY)
168 {
169 if (w->dirty)
170 computeWarp(w);
171 warp2(w->warpMat, srcX, srcY, dstX, dstY);
172 }
173
174 void warp2(double * mat, double srcX, double srcY, double *dstX, double *dstY){
175 double result[4];
176 double z = 0;
177 result[0] = (double)(srcX * mat[0] + srcY*mat[4] + z*mat[8] + 1*mat[12]);
178 result[1] = (double)(srcX * mat[1] + srcY*mat[5] + z*mat[9] + 1*mat[13]);
179 result[2] = (double)(srcX * mat[2] + srcY*mat[6] + z*mat[10] + 1*mat[14]);
180 result[3] = (double)(srcX * mat[3] + srcY*mat[7] + z*mat[11] + 1*mat[15]);
181 *dstX = result[0]/result[3];
182 *dstY = result[1]/result[3];
183 }
184
185
186
187
188
189 #ifdef example
190
191
192 #include <stdio.h>
193 #include <math.h>
194 #include "square_to_quad.h"
195
196 int main () {
197 struct Warper wr;
198 struct Warper *w;
199 w=&wr;
200
201 setIdentity(w);
202 setDestination(w,0.0,0.0,2.0,0.0,0.0,1.0,2.0,3.0);
203 double xi,yi;
204 for (xi=0.0;xi<=1.0;xi += 0.1) {
205 for (yi=0.;yi<=1.;yi += 0.1) {
206 double xo,yo;
207 warp(w,xi,yi,&xo,&yo);
208 fprintf(stdout,"%lf,%lf -> %lf,%lf\n",xi,yi,xo,yo);
209 }
210 }
211 }
212
213 #endif

  ViewVC Help
Powered by ViewVC 1.1.5