1 /* Quantum Minigolf, a computer game illustrating quantum mechanics
2 Copyright (C) 2007 Friedemann Reinhard <friedemann.reinhard@gmail.com>
3
4 This program is free software; you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation; either version 2 of the License, or
7 (at your option) any later version.
8
9 This program is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
13
14 You should have received a copy of the GNU General Public License
15 along with this program; if not, write to the Free Software
16 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
17 */
18
19
20 #include "WebcamTracker.h"
21
22 //WebcamTracker
23 //the constructor, copy arguments, load the calibration file and init the camera
WebcamTracker(int w,int h,int ix,int iy,int rball,float vmax,Renderer * renderer)24 WebcamTracker::WebcamTracker( int w, int h,
25 int ix, int iy,int rball,
26 float vmax,
27 Renderer *renderer)
28 : Tracker(w, h, ix, iy, rball, vmax, renderer)
29 {
30 cam = 0;
31
32 bool colour = false;
33 int fps=30, sourceDepth=8;
34 this->cw = this->ch = 0;
35
36 ifstream calfile;
37 calfile.open("result.dat");
38 if(calfile.fail()){
39 cam = 0;
40 return;
41 }
42 float tx, ty;
43 calfile >> cx; calfile >> cy;
44 calfile >> tx; calfile >> ty;
45 calfile >> tx; calfile >> ty;
46 calfile >> xx; calfile >> xy;
47 calfile >> yx; calfile >> yy;
48 calfile.close();
49
50 camera = cameraTool::findCamera();
51 if (camera == NULL) {cam=0; return;}
52
53 if (sourceDepth==24) colour = true;
54 bool success = camera->initCamera(cw, ch, colour);
55
56 if(success){
57
58 this->cw = cw = camera->getWidth();
59 this->ch = ch = camera->getHeight();
60 fps = camera->getFps();
61
62 printf("camera: %s\n",camera->getName());
63 printf("format: %dx%d, %dfps\n\n",cw,ch,fps);
64 } else {
65 printf("could not initialize camera.\n");
66 camera->closeCamera();
67 delete camera;
68 cam=0; camera = NULL;
69 return;
70 }
71
72 success = camera->startCamera();
73 if(!success){
74 printf("cannot start camera.\n");
75 camera->closeCamera();
76 delete camera;
77 cam=0; camera=NULL; return;
78 }
79
80 cam = 1;
81 camera->pauseCamera();
82 }
83
~WebcamTracker(void)84 WebcamTracker::~WebcamTracker(void)
85 {
86 if(cam){
87 camera->closeCamera();
88 delete camera;
89 }
90 }
91
92 // GetHit
93 // extract a hit: Track the motion of the club until it swings over
94 // the driving point
95 // The motion is tracked by searching the brightest pixel of the camera image
96 // and then taking the intensity-weihgted position average
97 // in a square around this pixel
98 //
GetHit(float * v,float * phi)99 void WebcamTracker::GetHit(float *v, float *phi){
100 Uint32 clock = 0, clocklast=0;
101 SDL_Event dummyevent;
102
103 // do nothing if no camera is present
104 if(cam==0)return;
105
106 unsigned char *image; // the frame captured from the webcam
107 float pos[2], poslast[2]; // position of the club in the current and last frame
108 int maxpos[2]; // position of the brightest pixel
109 unsigned char max =0; // brightness of the brightest pixel
110 float sum = 0; // brightness integral in the integrating square
111 //around the brightest pixel
112 int mw=5; //halfwidth of the integrating square
113 int i,j;
114
115 bool skipped=true; // flag indicating whether the last frame was skipped
116 // because the club was not visible
117
118 camera->startCamera();
119
120 while(SDL_PollEvent(&dummyevent)==0){ // SDL_PollEvent : emergency exit if the user presses any key
121 sum=0;
122 poslast[0] = pos[0];
123 poslast[1] = pos[1];
124
125 // acquire an image from the camera
126 if((image = camera->getFrame())==NULL){continue;}
127 clocklast = clock;
128 clock = SDL_GetTicks();
129
130 // find the brightest pixel
131 max = 0;
132 for(i=0; i<cw; i++){
133 for(j=0; j<ch; j++){
134 unsigned char pixel = image[j*cw+i];
135 if(pixel >= max){
136 max = pixel;
137 maxpos[0] = i;
138 maxpos[1] = j;
139 }
140 }
141 }
142
143 //skip the frame if it is clear that the brightest point cannot be the club
144 if(max < 50) {skipped=true; continue;}
145 if(sqrt((maxpos[0] - cx)*(maxpos[0] - cx) +
146 (maxpos[1] - cy)*(maxpos[1] - cy)) > 10*10)
147 {skipped=true; continue;}
148
149 // now compute the club position by taking the intnsity-weighted
150 // position average
151 // of the area around the brightest pixel
152 pos[0] = pos[1] = 0;
153 int xlower = maxpos[0]-mw; if(xlower < 0) xlower=0;
154 int xupper = maxpos[0]+mw; if(xupper > cw) xupper=cw;
155 int ylower = maxpos[1]-mw; if(ylower < 0) ylower=0;
156 int yupper = maxpos[1]+mw; if(yupper > ch) yupper=ch;
157 for(int i=xlower; i<xupper; i++){
158 for(int j=ylower; j<yupper; j++){
159 unsigned char pixel = image[j*cw+i];
160 sum += (float)pixel;
161 pos[0] += i*(float)pixel;
162 pos[1] += j*(float)pixel;
163 }
164 }
165 pos[0] = pos[0] / sum;
166 pos[1] = pos[1] / sum;
167 Uint32 clock_after_pos = SDL_GetTicks();
168
169 //cout << pos[0] << " " << pos[1] << " " << (float)max << endl;
170
171 // do not look for a hit if the last frame had been skipped
172 if(skipped)
173 {
174 skipped=false; continue;
175 }
176
177 // float target[2]; target[0] = cx; target[1] = cy;
178 // float lastnow[2]; lastnow[0]=pos[0]-poslast[0]; lastnow[1]=pos[1]-poslast[1];
179 // float tarlast[2]; tarlast[0]=target[0]-poslast[0];tarlast[1]=target[1]-poslast[1];
180 // float tarnow[2]; tarnow[0]=target[0]-pos[0];tarnow[1]=target[1]-pos[1];
181
182 // create float values for screen coordinates of
183 //club position in the current frame
184 //club position in the last frame
185 //target position
186 float fn[2]; fn[0]=pos[0];fn[1]=pos[1];
187 float fl[2]; fl[0]=poslast[0];fl[1]=poslast[1];
188 float ft[2]; ft[0]=cx;ft[1]=cy;
189 // float ft[2]; ft[0]=target[0];ft[1]=target[1];
190 cam2scr(fn, fn+1);
191 cam2scr(fl, fl+1);
192 cam2scr(ft, ft+1);
193
194 // compute difference vectors between
195 // fln: last and current position
196 // ftl: last position and target
197 // ftn: cur. position and target
198 float fln[2]; fln[0]=fn[0]-fl[0];fln[1]=fn[1]-fl[1];
199 float ftl[2]; ftl[0]=fl[0]-ft[0];ftl[1]=fl[1]-ft[1];
200 float ftn[2]; ftn[0]=fn[0]-ft[0];ftn[1]=fn[1]-ft[1];
201 float nfln = sqrt(fln[0]*fln[0]+fln[1]*fln[1]);
202 // float flny[2];
203 // compute projection of tl onto ln and its orthognal rest
204 float nflnx = -(ftl[0]*fln[0]+ftl[1]*fln[1])/nfln;
205 float nflny = sqrt(ftl[0]*ftl[0]+ftl[1]*ftl[1] - nflnx*nflnx);
206
207 // finally, look for a hit
208 if(ftl[0]*fln[0]+ftl[1]*fln[1] < 0 &&
209 ftn[0]*fln[0]+ftn[1]*fln[1] > 0 && nflny < 50){
210 camera->pauseCamera();
211 *v = sqrt(fln[0]*fln[0]+ fln[1]*fln[1])/(clock-clocklast);
212 *phi = atan(fln[1]/fln[0]);
213 if(*v > vmax)
214 *v=vmax;
215 return;
216 }
217 }
218 }
219
220 // Dummy animator - if we are playing with a real club, we do not need
221 // to animate the hit.
AnimateHit(Uint32 duration,float v,float phi)222 void WebcamTracker::AnimateHit(Uint32 duration, float v, float phi){
223 }
224
225 //cam2scr
226 // convert from camera coordinates to screen coordinates.
cam2scr(float * x,float * y)227 void WebcamTracker::cam2scr(float *x, float *y){
228 float tx = *x-cx;
229 float ty = *y-cy;
230 *x = (float)ix + xx*tx + xy*ty;
231 *y = (float)iy + yx*tx + yy*ty;
232 }
233
234 //cam2scr
235 // convert from camera coordinates to screen coordinates.
236 // not operational yet
scr2cam(float * x,float * y)237 void WebcamTracker::scr2cam(float *x, float *y){
238 *x -= 320;
239 }