1 /*
2 * Copyright (C) 2011 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 /*
18 * Contains implementation of a class EmulatedFakeRotatingCameraDevice that encapsulates
19 * fake camera device.
20 */
21
22 #define GL_GLEXT_PROTOTYPES
23 #define LOG_NDEBUG 0
24 #define LOG_TAG "EmulatedCamera_FakeDevice"
25 #define FAKE_CAMERA_SENSOR "FakeRotatingCameraSensor"
26 #include <log/log.h>
27 #include "EmulatedFakeCamera.h"
28 #include "EmulatedFakeRotatingCameraDevice.h"
29 #include <qemu_pipe_bp.h>
30
31 #include <EGL/egl.h>
32 #include <GLES/gl.h>
33 #include <GLES/glext.h>
34 #include <stdio.h>
35 #include <stdlib.h>
36 #include <math.h>
37 #include <fcntl.h>
38
39 #undef min
40 #undef max
41 #include <algorithm>
42
43 namespace android {
44
45 // include the dots pattern directly, it is NV21 format
46 #include "acircles_pattern_1280_720.c"
47
48 // ----------------------------------------------------------------------------
49
checkEglError(const char * op,EGLBoolean returnVal=EGL_TRUE)50 static void checkEglError(const char* op, EGLBoolean returnVal = EGL_TRUE) {
51 if (returnVal != EGL_TRUE) {
52 ALOGE("%s() returned %d\n", op, returnVal);
53 }
54
55 for (EGLint error = eglGetError(); error != EGL_SUCCESS; error
56 = eglGetError()) {
57 ALOGE("after %s() eglError (0x%x)\n", op, error);
58 }
59 }
60
clamp_rgb(signed value)61 static signed clamp_rgb(signed value) {
62 if (value > 255) {
63 value = 255;
64 } else if (value < 0) {
65 value = 0;
66 }
67 return value;
68 }
69
rgba8888_to_nv21(uint8_t * input,uint8_t * output,int width,int height)70 static void rgba8888_to_nv21(uint8_t* input, uint8_t* output, int width, int height) {
71 int align = 16;
72 int yStride = (width + (align -1)) & ~(align-1);
73 uint8_t* outputVU = output + height*yStride;
74 for (int j = 0; j < height; ++j) {
75 uint8_t* outputY = output + j*yStride;
76 for (int i = 0; i < width; ++i) {
77 uint8_t R = input[j*width*4 + i*4];
78 uint8_t G = input[j*width*4 + i*4 + 1];
79 uint8_t B = input[j*width*4 + i*4 + 2];
80 uint8_t Y = clamp_rgb((77 * R + 150 * G + 29 * B) >> 8);
81 *outputY++ = Y;
82 bool jeven = (j & 1) == 0;
83 bool ieven = (i & 1) == 0;
84 if (jeven && ieven) {
85 uint8_t V = clamp_rgb((( 128 * R - 107 * G - 21 * B) >> 8) + 128);
86 uint8_t U = clamp_rgb((( -43 * R - 85 * G + 128 * B) >> 8) + 128);
87 *outputVU++ = V;
88 *outputVU++ = U;
89 }
90 }
91 }
92 }
93
nv21_to_rgba8888(uint8_t * input,uint32_t * output,int width,int height)94 static void nv21_to_rgba8888(uint8_t* input, uint32_t * output, int width, int height) {
95 int align = 16;
96 int yStride = (width + (align -1)) & ~(align-1);
97 uint8_t* inputVU = input + height*yStride;
98 uint8_t Y, U, V;
99 for (int j = 0; j < height; ++j) {
100 uint8_t* inputY = input + j*yStride;
101 for (int i = 0; i < width; ++i) {
102 Y = *inputY++;
103 bool jeven = (j & 1) == 0;
104 bool ieven = (i & 1) == 0;
105 if (jeven && ieven) {
106 V = *inputVU++;
107 U = *inputVU++;
108 }
109 *output++ = YUVToRGB32(Y,U,V);
110 }
111 }
112 }
113
render(int width,int height)114 void EmulatedFakeRotatingCameraDevice::render(int width, int height)
115 {
116 update_scene((float)width, (float)height);
117 create_texture_dotx(1280, 720);
118
119 int w= 992/2;
120 int h = 1280/2;
121 const GLfloat verticesfloat[] = {
122 -w, -h, 0,
123 w, -h, 0,
124 w, h, 0,
125 -w, h, 0
126 };
127
128 const GLfloat texCoordsfloat[] = {
129 0, 0,
130 1.0f, 0,
131 1.0f, 1.0f,
132 0, 1.0f
133 };
134
135 const GLushort indices[] = { 0, 1, 2, 0, 2, 3 };
136
137 glVertexPointer(3, GL_FLOAT, 0, verticesfloat);
138 glTexCoordPointer(2, GL_FLOAT, 0, texCoordsfloat);
139 glClearColor(0.5, 0.5, 0.5, 1.0);
140 int nelem = sizeof(indices)/sizeof(indices[0]);
141 glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
142 glDrawElements(GL_TRIANGLES, nelem, GL_UNSIGNED_SHORT, indices);
143 glFinish();
144 glReadPixels(0, 0, width, height, GL_RGBA, GL_UNSIGNED_BYTE, mPixelBuf);
145 }
146
get_color(uint32_t * img,int i,int j,int w,int h,int dw,uint32_t * color)147 static void get_color(uint32_t* img, int i, int j, int w, int h, int dw, uint32_t * color) {
148 int mini = dw/2 - w/2;
149 int minj = dw/2 - h/2;
150 int maxi = mini + w -1;
151 int maxj = minj + h -1;
152
153 if ( i >= mini && i <= maxi && j >= minj && j <= maxj) {
154 *color = img[i-mini + dw*(j-minj)];
155 }
156 }
157
convert_to_square(uint32_t * src,uint32_t * dest,int sw,int sh,int dw)158 static void convert_to_square(uint32_t* src, uint32_t* dest, int sw, int sh, int dw) {
159 for (int i=0; i < dw; ++i) {
160 for (int j=0; j < dw; ++j) {
161 uint32_t color=0;
162 get_color(src, i, j, sw, sh, dw, &color);
163 dest[i+j*dw] = color;
164 }
165 }
166 }
167
create_texture_dotx(int width,int height)168 void EmulatedFakeRotatingCameraDevice::create_texture_dotx(int width, int height) {
169 uint32_t* myrgba = new uint32_t[width * height];
170 nv21_to_rgba8888(rawData, myrgba, width, height);
171 uint32_t* myrgba2 = new uint32_t[width * width];
172 convert_to_square(myrgba, myrgba2, width, height, width);
173
174 glGenTextures(1, &mTexture);
175 glBindTexture(GL_TEXTURE_2D, mTexture);
176 glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, width, 0, GL_RGBA, GL_UNSIGNED_BYTE, myrgba2);
177 //glGenerateMipmapOES does not work on mac, dont use it.
178 //glGenerateMipmapOES(GL_TEXTURE_2D);
179 // need to use linear, otherwise the dots will have sharp edges
180 glTexParameterx(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
181 glTexParameterx(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
182 glTexEnvx(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE);
183 delete[] myrgba;
184 delete[] myrgba2;
185 }
186
187
gluLookAt(float eyeX,float eyeY,float eyeZ,float centerX,float centerY,float centerZ,float upX,float upY,float upZ)188 static void gluLookAt(float eyeX, float eyeY, float eyeZ,
189 float centerX, float centerY, float centerZ, float upX, float upY,
190 float upZ)
191 {
192 // See the OpenGL GLUT documentation for gluLookAt for a description
193 // of the algorithm. We implement it in a straightforward way:
194
195 float fx = centerX - eyeX;
196 float fy = centerY - eyeY;
197 float fz = centerZ - eyeZ;
198 float flf = 1.0f / sqrt(fx * fx + fy * fy + fz * fz);
199 fx *= flf;
200 fy *= flf;
201 fz *= flf;
202
203 // compute s = f x up (x means "cross product")
204
205 float sx = fy * upZ - fz * upY;
206 float sy = fz * upX - fx * upZ;
207 float sz = fx * upY - fy * upX;
208 float slf = 1.0f / sqrt(sx * sx + sy * sy + sz * sz);
209 sx *= slf;
210 sy *= slf;
211 sz *= slf;
212
213 // compute u = s x f
214 float ux = sy * fz - sz * fy;
215 float uy = sz * fx - sx * fz;
216 float uz = sx * fy - sy * fx;
217 float ulf = 1.0f / sqrt(ux * ux + uy * uy + uz * uz);
218 ux *= ulf;
219 uy *= ulf;
220 uz *= ulf;
221
222 float m[16] ;
223 m[0] = sx;
224 m[1] = ux;
225 m[2] = -fx;
226 m[3] = 0.0f;
227
228 m[4] = sy;
229 m[5] = uy;
230 m[6] = -fy;
231 m[7] = 0.0f;
232
233 m[8] = sz;
234 m[9] = uz;
235 m[10] = -fz;
236 m[11] = 0.0f;
237
238 m[12] = 0.0f;
239 m[13] = 0.0f;
240 m[14] = 0.0f;
241 m[15] = 1.0f;
242
243 glMultMatrixf(m);
244 glTranslatef(-eyeX, -eyeY, -eyeZ);
245 }
246
update_scene(float width,float height)247 void EmulatedFakeRotatingCameraDevice::update_scene(float width, float height)
248 {
249 float ratio = width / height;
250 glViewport(0, 0, width, height);
251 glMatrixMode(GL_PROJECTION);
252 glLoadIdentity();
253 glFrustumf(-ratio/2.0, ratio/2.0, -1/2.0, 1/2.0, 1, 40000);
254 glMatrixMode(GL_MODELVIEW);
255 glLoadIdentity();
256 float up_x=-1;
257 float up_y=0;
258 float up_z=0;
259 get_yawing(&up_x, &up_y, &up_z);
260 float eye_x=0;
261 float eye_y=0;
262 float eye_z=2000;
263 get_eye_x_y_z(&eye_x, &eye_y, &eye_z);
264 gluLookAt( eye_x, eye_y, eye_z, 0, 0, 0, up_x, up_y, up_z);
265 glEnable(GL_TEXTURE_2D);
266 glEnableClientState(GL_VERTEX_ARRAY);
267 glEnableClientState(GL_TEXTURE_COORD_ARRAY);
268 }
269
free_gl_surface(void)270 void EmulatedFakeRotatingCameraDevice::free_gl_surface(void)
271 {
272 if (mEglDisplay != EGL_NO_DISPLAY)
273 {
274 eglMakeCurrent( EGL_NO_DISPLAY, EGL_NO_SURFACE,
275 EGL_NO_SURFACE, EGL_NO_CONTEXT );
276 eglDestroyContext( mEglDisplay, mEglContext );
277 eglDestroySurface( mEglDisplay, mEglSurface );
278 eglTerminate( mEglDisplay );
279 mEglDisplay = EGL_NO_DISPLAY;
280 }
281 }
282
init_sensor()283 void EmulatedFakeRotatingCameraDevice::init_sensor() {
284 if (mSensorPipe >=0) return;
285 // create a sensor pipe
286 mSensorPipe = qemu_pipe_open_ns(NULL, FAKE_CAMERA_SENSOR, O_RDWR);
287 if (mSensorPipe < 0) {
288 ALOGE("cannot open %s", FAKE_CAMERA_SENSOR);
289 } else {
290 ALOGD("successfully opened %s", FAKE_CAMERA_SENSOR);
291 }
292 }
293
read_sensor()294 void EmulatedFakeRotatingCameraDevice::read_sensor() {
295 if (mSensorPipe < 0) return;
296 char get[] = "get";
297 int pipe_command_length = sizeof(get);
298 qemu_pipe_write_fully(mSensorPipe, &pipe_command_length, sizeof(pipe_command_length));
299 qemu_pipe_write_fully(mSensorPipe, get, pipe_command_length);
300 qemu_pipe_read_fully(mSensorPipe, &pipe_command_length, sizeof(pipe_command_length));
301 qemu_pipe_read_fully(mSensorPipe, &mSensorValues, pipe_command_length);
302 assert(pipe_command_length == 9*sizeof(float));
303 ALOGD("accel: %g %g %g; magnetic %g %g %g orientation %g %g %g",
304 mSensorValues[SENSOR_VALUE_ACCEL_X], mSensorValues[SENSOR_VALUE_ACCEL_Y],
305 mSensorValues[SENSOR_VALUE_ACCEL_Z],
306 mSensorValues[SENSOR_VALUE_MAGNETIC_X], mSensorValues[SENSOR_VALUE_MAGNETIC_Y],
307 mSensorValues[SENSOR_VALUE_MAGNETIC_Y],
308 mSensorValues[SENSOR_VALUE_ROTATION_X], mSensorValues[SENSOR_VALUE_ROTATION_Y],
309 mSensorValues[SENSOR_VALUE_ROTATION_Z]);
310 }
311
read_rotation_vector(double * yaw,double * pitch,double * roll)312 void EmulatedFakeRotatingCameraDevice::read_rotation_vector(double *yaw, double* pitch, double* roll) {
313 read_sensor();
314 *yaw = mSensorValues[SENSOR_VALUE_ROTATION_Z];
315 *pitch = mSensorValues[SENSOR_VALUE_ROTATION_X];
316 *roll = mSensorValues[SENSOR_VALUE_ROTATION_Y];
317 return;
318 }
319
get_yawing(float * x,float * y,float * z)320 void EmulatedFakeRotatingCameraDevice::get_yawing(float* x, float* y, float*z) {
321 double yaw, pitch, roll;
322 read_rotation_vector(&yaw, &pitch, &roll);
323 *x = sin((180+yaw)*3.14/180);
324 *y = cos((180+yaw)*3.14/180);
325 *z = 0;
326 ALOGD("%s: yaw is %g, x %g y %g z %g", __func__, yaw, *x, *y, *z);
327 }
328
get_eye_x_y_z(float * x,float * y,float * z)329 void EmulatedFakeRotatingCameraDevice::get_eye_x_y_z(float* x, float* y, float*z) {
330 const float R=3500;
331 //the coordinate of real camera is rotated (x-y swap)
332 //and reverted (+/- swap)
333 //
334 //so rotation y is clockwise around x axis;
335 //and rotation x is clockwise around y axis.
336 const float theta_around_x = -mSensorValues[SENSOR_VALUE_ROTATION_Y];
337 const float theta_around_y = -mSensorValues[SENSOR_VALUE_ROTATION_X];
338 //apply x rotation first
339 float y1 = -R*sin(theta_around_x*3.14/180);
340 float z1 = R*cos(theta_around_x*3.14/180);
341 //apply y rotation second
342 float xz2 = z1 * sin(theta_around_y*3.14/180);
343 float zz2 = z1 * cos(theta_around_y*3.14/180);
344 *x = xz2;
345 *y = y1;
346 *z = zz2;
347
348 }
349
init_gl_surface(int width,int height)350 int EmulatedFakeRotatingCameraDevice::init_gl_surface(int width, int height)
351 {
352 EGLint numConfigs = 1;
353 EGLConfig myConfig = {0};
354
355 if ( (mEglDisplay = eglGetDisplay(EGL_DEFAULT_DISPLAY)) == EGL_NO_DISPLAY )
356 {
357 ALOGE("eglGetDisplay failed\n");
358 return 0;
359 }
360
361 if ( eglInitialize(mEglDisplay, NULL, NULL) != EGL_TRUE )
362 {
363 ALOGE("eglInitialize failed\n");
364 return 0;
365 }
366
367 {
368 EGLint s_configAttribs[] = {
369 EGL_SURFACE_TYPE, EGL_PBUFFER_BIT|EGL_WINDOW_BIT,
370 EGL_RED_SIZE, 5,
371 EGL_GREEN_SIZE, 6,
372 EGL_BLUE_SIZE, 5,
373 EGL_NONE
374 };
375 eglChooseConfig(mEglDisplay, s_configAttribs, &myConfig, 1, &numConfigs);
376 EGLint attribs[] = { EGL_WIDTH, width, EGL_HEIGHT, height, EGL_NONE };
377 mEglSurface = eglCreatePbufferSurface(mEglDisplay, myConfig, attribs);
378 if (mEglSurface == EGL_NO_SURFACE) {
379 ALOGE("eglCreatePbufferSurface error %x\n", eglGetError());
380 }
381 }
382
383 if ( (mEglContext = eglCreateContext(mEglDisplay, myConfig, 0, 0)) == EGL_NO_CONTEXT )
384 {
385 ALOGE("eglCreateContext failed\n");
386 return 0;
387 }
388
389 if ( eglMakeCurrent(mEglDisplay, mEglSurface, mEglSurface, mEglContext) != EGL_TRUE )
390 {
391 ALOGE("eglMakeCurrent failed\n");
392 return 0;
393 }
394
395 int w, h;
396
397 eglQuerySurface(mEglDisplay, mEglSurface, EGL_WIDTH, &w);
398 checkEglError("eglQuerySurface");
399 eglQuerySurface(mEglDisplay, mEglSurface, EGL_HEIGHT, &h);
400 checkEglError("eglQuerySurface");
401
402 ALOGD("Window dimensions: %d x %d\n", w, h);
403
404 glDisable(GL_DITHER);
405 glEnable(GL_CULL_FACE);
406
407 return 1;
408 }
409
EmulatedFakeRotatingCameraDevice()410 EmulatedFakeRotatingCameraDevice::EmulatedFakeRotatingCameraDevice():
411 mObjectLock(),
412 mOpenglReady(false),
413 mState(ECDS_CONNECTED)
414 {
415 // not much to initialize
416 mState = ECDS_INITIALIZED;
417 }
418
~EmulatedFakeRotatingCameraDevice()419 EmulatedFakeRotatingCameraDevice::~EmulatedFakeRotatingCameraDevice()
420 {
421 mState = ECDS_INVALID;
422 }
423
424 /****************************************************************************
425 * Emulated camera device abstract interface implementation.
426 ***************************************************************************/
427
connectDevice()428 status_t EmulatedFakeRotatingCameraDevice::connectDevice()
429 {
430 ALOGV("%s", __FUNCTION__);
431
432 Mutex::Autolock locker(&mObjectLock);
433 if (!isInitialized()) {
434 ALOGE("%s: Fake camera device is not initialized.", __FUNCTION__);
435 return EINVAL;
436 }
437 if (isConnected()) {
438 ALOGW("%s: Fake camera device is already connected.", __FUNCTION__);
439 return NO_ERROR;
440 }
441
442 /* There is no device to connect to. */
443 mState = ECDS_CONNECTED;
444
445 return NO_ERROR;
446 }
447
disconnectDevice()448 status_t EmulatedFakeRotatingCameraDevice::disconnectDevice()
449 {
450 ALOGV("%s", __FUNCTION__);
451
452 Mutex::Autolock locker(&mObjectLock);
453 if (!isConnected()) {
454 ALOGW("%s: Fake camera device is already disconnected.", __FUNCTION__);
455 return NO_ERROR;
456 }
457 if (isStarted()) {
458 ALOGE("%s: Cannot disconnect from the started device.", __FUNCTION__);
459 return EINVAL;
460 }
461
462 /* There is no device to disconnect from. */
463 mState = ECDS_INITIALIZED;
464
465 return NO_ERROR;
466 }
467
startDevice(int width,int height,uint32_t pix_fmt)468 status_t EmulatedFakeRotatingCameraDevice::startDevice(int width,
469 int height,
470 uint32_t pix_fmt)
471 {
472 ALOGE("%s width %d height %d", __FUNCTION__, width, height);
473
474 Mutex::Autolock locker(&mObjectLock);
475 if (!isConnected()) {
476 ALOGE("%s: Fake camera device is not connected.", __FUNCTION__);
477 return EINVAL;
478 }
479 if (isStarted()) {
480 ALOGE("%s: Fake camera device is already started.", __FUNCTION__);
481 return EINVAL;
482 }
483
484 mFrameWidth = width;
485 mFrameHeight = height;
486 mPixelFormat = pix_fmt;
487 mState = ECDS_STARTED;
488
489 return NO_ERROR;
490 }
491
stopDevice()492 status_t EmulatedFakeRotatingCameraDevice::stopDevice()
493 {
494 ALOGV("%s", __FUNCTION__);
495
496 Mutex::Autolock locker(&mObjectLock);
497 if (!isStarted()) {
498 ALOGW("%s: Fake camera device is not started.", __FUNCTION__);
499 return NO_ERROR;
500 }
501
502 mState = ECDS_CONNECTED;
503
504 if (mOpenglReady) {
505 free_gl_surface();
506 delete mPixelBuf;
507 mOpenglReady=false;
508 }
509 if (mSensorPipe >= 0) {
510 close(mSensorPipe);
511 mSensorPipe = -1;
512 }
513
514 return NO_ERROR;
515 }
516
517 /****************************************************************************
518 * Worker thread management overrides.
519 ***************************************************************************/
520
produceFrame(void * buffer,int64_t * timestamp)521 bool EmulatedFakeRotatingCameraDevice::produceFrame(void* buffer,
522 int64_t* timestamp)
523 {
524 if (mOpenglReady == false) {
525 init_gl_surface(mFrameWidth, mFrameHeight);
526 mOpenglReady = true;
527 int width=mFrameWidth;
528 int height = mFrameHeight;
529 int kGlBytesPerPixel = 4;
530 mPixelBuf = new uint8_t[width * height * kGlBytesPerPixel];
531 init_sensor();
532 }
533 render(mFrameWidth, mFrameHeight);
534 fillBuffer(buffer);
535 return true;
536 }
537
538 /****************************************************************************
539 * Fake camera device private API
540 ***************************************************************************/
541
fillBuffer(void * buffer)542 void EmulatedFakeRotatingCameraDevice::fillBuffer(void* buffer)
543 {
544 uint8_t* currentFrame = reinterpret_cast<uint8_t*>(buffer);
545 rgba8888_to_nv21(mPixelBuf, currentFrame, mFrameWidth, mFrameHeight);
546 return;
547 }
548
549 }; /* namespace android */
550