app/src/main/jni/libuvc/src/frame_original.c

   1 /*********************************************************************
   2 * Software License Agreement (BSD License)
   3 *
   4 *  Copyright (C) 2010-2012 Ken Tossell
   5 *  All rights reserved.
   6 *
   7 *  Redistribution and use in source and binary forms, with or without
   8 *  modification, are permitted provided that the following conditions
   9 *  are met:
  10 *
  11 *   * Redistributions of source code must retain the above copyright
  12 *     notice, this list of conditions and the following disclaimer.
  13 *   * Redistributions in binary form must reproduce the above
  14 *     copyright notice, this list of conditions and the following
  15 *     disclaimer in the documentation and/or other materials provided
  16 *     with the distribution.
  17 *   * Neither the name of the author nor other contributors may be
  18 *     used to endorse or promote products derived from this software
  19 *     without specific prior written permission.
  20 *
  21 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  22 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  23 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  24 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  25 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  26 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  27 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  28 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  29 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  30 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  31 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  32 *  POSSIBILITY OF SUCH DAMAGE.
  33 *********************************************************************/
  34 /**
  35  * @defgroup frame Frame processing
  36  */
  37 #include "libuvc/libuvc.h"
  38 #include "libuvc/libuvc_internal.h"
  39
  40 /** @internal */
  41 uvc_error_t uvc_ensure_frame_size(uvc_frame_t *frame, size_t need_bytes) {
  42   if (frame->library_owns_data) {
  43     if (!frame->data || frame->data_bytes != need_bytes) {
  44       frame->data_bytes = need_bytes;
  45       frame->data = realloc(frame->data, frame->data_bytes);
  46     }
  47     if (!frame->data)
  48       return UVC_ERROR_NO_MEM;
  49     return UVC_SUCCESS;
  50   } else {
  51     if (!frame->data || frame->data_bytes < need_bytes)
  52       return UVC_ERROR_NO_MEM;
  53     return UVC_SUCCESS;
  54   }
  55 }
  56
  57 /** @brief Allocate a frame structure
  58  * @ingroup frame
  59  *
  60  * @param data_bytes Number of bytes to allocate, or zero
  61  * @return New frame, or NULL on error
  62  */
  63 uvc_frame_t *uvc_allocate_frame(size_t data_bytes) {
  64   uvc_frame_t *frame = malloc(sizeof(*frame));
  65
  66   if (!frame)
  67     return NULL;
  68
  69   bzero(frame, sizeof(*frame));
  70
  71   frame->library_owns_data = 1;
  72
  73   if (data_bytes > 0) {
  74     frame->data_bytes = data_bytes;
  75     frame->data = malloc(data_bytes);
  76
  77     if (!frame->data) {
  78       free(frame);
  79       return NULL;
  80     }
  81   }
  82
  83   return frame;
  84 }
  85
  86 /** @brief Free a frame structure
  87  * @ingroup frame
  88  *
  89  * @param frame Frame to destroy
  90  */
  91 void uvc_free_frame(uvc_frame_t *frame) {
  92   if (frame->data_bytes > 0 && frame->library_owns_data)
  93     free(frame->data);
  94
  95   free(frame);
  96 }
  97
  98 static inline unsigned char sat(int i) {
  99   return (unsigned char)( i >= 255 ? 255 : (i < 0 ? 0 : i));
 100 }
 101
 102 /** @brief Duplicate a frame, preserving color format
 103  * @ingroup frame
 104  *
 105  * @param in Original frame
 106  * @param out Duplicate frame
 107  */
 108 uvc_error_t uvc_duplicate_frame(uvc_frame_t *in, uvc_frame_t *out) {
 109   if (uvc_ensure_frame_size(out, in->data_bytes) < 0)
 110     return UVC_ERROR_NO_MEM;
 111
 112   out->width = in->width;
 113   out->height = in->height;
 114   out->frame_format = in->frame_format;
 115   out->step = in->step;
 116   out->sequence = in->sequence;
 117   out->capture_time = in->capture_time;
 118   out->source = in->source;
 119
 120   memcpy(out->data, in->data, in->data_bytes);
 121
 122   return UVC_SUCCESS;
 123 }
 124
 125 #define YUYV2RGB_2(pyuv, prgb) { \
 126     float r = 1.402f * ((pyuv)[3]-128); \
 127     float g = -0.34414f * ((pyuv)[1]-128) - 0.71414f * ((pyuv)[3]-128); \
 128     float b = 1.772f * ((pyuv)[1]-128); \
 129     (prgb)[0] = sat(pyuv[0] + r); \
 130     (prgb)[1] = sat(pyuv[0] + g); \
 131     (prgb)[2] = sat(pyuv[0] + b); \
 132     (prgb)[3] = sat(pyuv[2] + r); \
 133     (prgb)[4] = sat(pyuv[2] + g); \
 134     (prgb)[5] = sat(pyuv[2] + b); \
 135     }
 136 #define IYUYV2RGB_2(pyuv, prgb) { \
 137     int r = (22987 * ((pyuv)[3] - 128)) >> 14; \
 138     int g = (-5636 * ((pyuv)[1] - 128) - 11698 * ((pyuv)[3] - 128)) >> 14; \
 139     int b = (29049 * ((pyuv)[1] - 128)) >> 14; \
 140     (prgb)[0] = sat(*(pyuv) + r); \
 141     (prgb)[1] = sat(*(pyuv) + g); \
 142     (prgb)[2] = sat(*(pyuv) + b); \
 143     (prgb)[3] = sat((pyuv)[2] + r); \
 144     (prgb)[4] = sat((pyuv)[2] + g); \
 145     (prgb)[5] = sat((pyuv)[2] + b); \
 146     }
 147 #define IYUYV2RGB_16(pyuv, prgb) IYUYV2RGB_8(pyuv, prgb); IYUYV2RGB_8(pyuv + 16, prgb + 24);
 148 #define IYUYV2RGB_8(pyuv, prgb) IYUYV2RGB_4(pyuv, prgb); IYUYV2RGB_4(pyuv + 8, prgb + 12);
 149 #define IYUYV2RGB_4(pyuv, prgb) IYUYV2RGB_2(pyuv, prgb); IYUYV2RGB_2(pyuv + 4, prgb + 6);
 150
 151 /** @brief Convert a frame from YUYV to RGB
 152  * @ingroup frame
 153  *
 154  * @param in YUYV frame
 155  * @param out RGB frame
 156  */
 157 uvc_error_t uvc_yuyv2rgb(uvc_frame_t *in, uvc_frame_t *out) {
 158   if (in->frame_format != UVC_FRAME_FORMAT_YUYV)
 159     return UVC_ERROR_INVALID_PARAM;
 160
 161   if (uvc_ensure_frame_size(out, in->width * in->height * 3) < 0)
 162     return UVC_ERROR_NO_MEM;
 163
 164   out->width = in->width;
 165   out->height = in->height;
 166   out->frame_format = UVC_FRAME_FORMAT_RGB;
 167   out->step = in->width * 3;
 168   out->sequence = in->sequence;
 169   out->capture_time = in->capture_time;
 170   out->source = in->source;
 171
 172   uint8_t *pyuv = in->data;
 173   uint8_t *prgb = out->data;
 174   uint8_t *prgb_end = prgb + out->data_bytes;
 175
 176   while (prgb < prgb_end) {
 177     IYUYV2RGB_8(pyuv, prgb);
 178
 179     prgb += 3 * 8;
 180     pyuv += 2 * 8;
 181   }
 182
 183   return UVC_SUCCESS;
 184 }
 185
 186 #define IYUYV2BGR_2(pyuv, pbgr) { \
 187     int r = (22987 * ((pyuv)[3] - 128)) >> 14; \
 188     int g = (-5636 * ((pyuv)[1] - 128) - 11698 * ((pyuv)[3] - 128)) >> 14; \
 189     int b = (29049 * ((pyuv)[1] - 128)) >> 14; \
 190     (pbgr)[0] = sat(*(pyuv) + b); \
 191     (pbgr)[1] = sat(*(pyuv) + g); \
 192     (pbgr)[2] = sat(*(pyuv) + r); \
 193     (pbgr)[3] = sat((pyuv)[2] + b); \
 194     (pbgr)[4] = sat((pyuv)[2] + g); \
 195     (pbgr)[5] = sat((pyuv)[2] + r); \
 196     }
 197 #define IYUYV2BGR_16(pyuv, pbgr) IYUYV2BGR_8(pyuv, pbgr); IYUYV2BGR_8(pyuv + 16, pbgr + 24);
 198 #define IYUYV2BGR_8(pyuv, pbgr) IYUYV2BGR_4(pyuv, pbgr); IYUYV2BGR_4(pyuv + 8, pbgr + 12);
 199 #define IYUYV2BGR_4(pyuv, pbgr) IYUYV2BGR_2(pyuv, pbgr); IYUYV2BGR_2(pyuv + 4, pbgr + 6);
 200
 201 /** @brief Convert a frame from YUYV to BGR
 202  * @ingroup frame
 203  *
 204  * @param in YUYV frame
 205  * @param out BGR frame
 206  */
 207 uvc_error_t uvc_yuyv2bgr(uvc_frame_t *in, uvc_frame_t *out) {
 208   if (in->frame_format != UVC_FRAME_FORMAT_YUYV)
 209     return UVC_ERROR_INVALID_PARAM;
 210
 211   if (uvc_ensure_frame_size(out, in->width * in->height * 3) < 0)
 212     return UVC_ERROR_NO_MEM;
 213
 214   out->width = in->width;
 215   out->height = in->height;
 216   out->frame_format = UVC_FRAME_FORMAT_BGR;
 217   out->step = in->width * 3;
 218   out->sequence = in->sequence;
 219   out->capture_time = in->capture_time;
 220   out->source = in->source;
 221
 222   uint8_t *pyuv = in->data;
 223   uint8_t *pbgr = out->data;
 224   uint8_t *pbgr_end = pbgr + out->data_bytes;
 225
 226   while (pbgr < pbgr_end) {
 227     IYUYV2BGR_8(pyuv, pbgr);
 228
 229     pbgr += 3 * 8;
 230     pyuv += 2 * 8;
 231   }
 232
 233   return UVC_SUCCESS;
 234 }
 235
 236 #define IUYVY2RGB_2(pyuv, prgb) { \
 237     int r = (22987 * ((pyuv)[2] - 128)) >> 14; \
 238     int g = (-5636 * ((pyuv)[0] - 128) - 11698 * ((pyuv)[2] - 128)) >> 14; \
 239     int b = (29049 * ((pyuv)[0] - 128)) >> 14; \
 240     (prgb)[0] = sat((pyuv)[1] + r); \
 241     (prgb)[1] = sat((pyuv)[1] + g); \
 242     (prgb)[2] = sat((pyuv)[1] + b); \
 243     (prgb)[3] = sat((pyuv)[3] + r); \
 244     (prgb)[4] = sat((pyuv)[3] + g); \
 245     (prgb)[5] = sat((pyuv)[3] + b); \
 246     }
 247 #define IUYVY2RGB_16(pyuv, prgb) IUYVY2RGB_8(pyuv, prgb); IUYVY2RGB_8(pyuv + 16, prgb + 24);
 248 #define IUYVY2RGB_8(pyuv, prgb) IUYVY2RGB_4(pyuv, prgb); IUYVY2RGB_4(pyuv + 8, prgb + 12);
 249 #define IUYVY2RGB_4(pyuv, prgb) IUYVY2RGB_2(pyuv, prgb); IUYVY2RGB_2(pyuv + 4, prgb + 6);
 250
 251 /** @brief Convert a frame from UYVY to RGB
 252  * @ingroup frame
 253  * @param ini UYVY frame
 254  * @param out RGB frame
 255  */
 256 uvc_error_t uvc_uyvy2rgb(uvc_frame_t *in, uvc_frame_t *out) {
 257   if (in->frame_format != UVC_FRAME_FORMAT_UYVY)
 258     return UVC_ERROR_INVALID_PARAM;
 259
 260   if (uvc_ensure_frame_size(out, in->width * in->height * 3) < 0)
 261     return UVC_ERROR_NO_MEM;
 262
 263   out->width = in->width;
 264   out->height = in->height;
 265   out->frame_format = UVC_FRAME_FORMAT_RGB;
 266   out->step = in->width *3;
 267   out->sequence = in->sequence;
 268   out->capture_time = in->capture_time;
 269   out->source = in->source;
 270
 271   uint8_t *pyuv = in->data;
 272   uint8_t *prgb = out->data;
 273   uint8_t *prgb_end = prgb + out->data_bytes;
 274
 275   while (prgb < prgb_end) {
 276     IUYVY2RGB_8(pyuv, prgb);
 277
 278     prgb += 3 * 8;
 279     pyuv += 2 * 8;
 280   }
 281
 282   return UVC_SUCCESS;
 283 }
 284
 285 #define IUYVY2BGR_2(pyuv, pbgr) { \
 286     int r = (22987 * ((pyuv)[2] - 128)) >> 14; \
 287     int g = (-5636 * ((pyuv)[0] - 128) - 11698 * ((pyuv)[2] - 128)) >> 14; \
 288     int b = (29049 * ((pyuv)[0] - 128)) >> 14; \
 289     (pbgr)[0] = sat((pyuv)[1] + b); \
 290     (pbgr)[1] = sat((pyuv)[1] + g); \
 291     (pbgr)[2] = sat((pyuv)[1] + r); \
 292     (pbgr)[3] = sat((pyuv)[3] + b); \
 293     (pbgr)[4] = sat((pyuv)[3] + g); \
 294     (pbgr)[5] = sat((pyuv)[3] + r); \
 295     }
 296 #define IUYVY2BGR_16(pyuv, pbgr) IUYVY2BGR_8(pyuv, pbgr); IUYVY2BGR_8(pyuv + 16, pbgr + 24);
 297 #define IUYVY2BGR_8(pyuv, pbgr) IUYVY2BGR_4(pyuv, pbgr); IUYVY2BGR_4(pyuv + 8, pbgr + 12);
 298 #define IUYVY2BGR_4(pyuv, pbgr) IUYVY2BGR_2(pyuv, pbgr); IUYVY2BGR_2(pyuv + 4, pbgr + 6);
 299
 300 /** @brief Convert a frame from UYVY to BGR
 301  * @ingroup frame
 302  * @param ini UYVY frame
 303  * @param out BGR frame
 304  */
 305 uvc_error_t uvc_uyvy2bgr(uvc_frame_t *in, uvc_frame_t *out) {
 306   if (in->frame_format != UVC_FRAME_FORMAT_UYVY)
 307     return UVC_ERROR_INVALID_PARAM;
 308
 309   if (uvc_ensure_frame_size(out, in->width * in->height * 3) < 0)
 310     return UVC_ERROR_NO_MEM;
 311
 312   out->width = in->width;
 313   out->height = in->height;
 314   out->frame_format = UVC_FRAME_FORMAT_BGR;
 315   out->step = in->width *3;
 316   out->sequence = in->sequence;
 317   out->capture_time = in->capture_time;
 318   out->source = in->source;
 319
 320   uint8_t *pyuv = in->data;
 321   uint8_t *pbgr = out->data;
 322   uint8_t *pbgr_end = pbgr + out->data_bytes;
 323
 324   while (pbgr < pbgr_end) {
 325     IUYVY2BGR_8(pyuv, pbgr);
 326
 327     pbgr += 3 * 8;
 328     pyuv += 2 * 8;
 329   }
 330
 331   return UVC_SUCCESS;
 332 }
 333
 334 /** @brief Convert a frame to RGB
 335  * @ingroup frame
 336  *
 337  * @param in non-RGB frame
 338  * @param out RGB frame
 339  */
 340 uvc_error_t uvc_any2rgb(uvc_frame_t *in, uvc_frame_t *out) {
 341   switch (in->frame_format) {
 342     case UVC_FRAME_FORMAT_YUYV:
 343       return uvc_yuyv2rgb(in, out);
 344     case UVC_FRAME_FORMAT_UYVY:
 345       return uvc_uyvy2rgb(in, out);
 346     case UVC_FRAME_FORMAT_RGB:
 347       return uvc_duplicate_frame(in, out);
 348     default:
 349       return UVC_ERROR_NOT_SUPPORTED;
 350   }
 351 }
 352
 353 /** @brief Convert a frame to BGR
 354  * @ingroup frame
 355  *
 356  * @param in non-BGR frame
 357  * @param out BGR frame
 358  */
 359 uvc_error_t uvc_any2bgr(uvc_frame_t *in, uvc_frame_t *out) {
 360   switch (in->frame_format) {
 361     case UVC_FRAME_FORMAT_YUYV:
 362       return uvc_yuyv2bgr(in, out);
 363     case UVC_FRAME_FORMAT_UYVY:
 364       return uvc_uyvy2bgr(in, out);
 365     case UVC_FRAME_FORMAT_BGR:
 366       return uvc_duplicate_frame(in, out);
 367     default:
 368       return UVC_ERROR_NOT_SUPPORTED;
 369   }
 370 }