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