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