00001 #include "urbi/uconversion.hh"
00002
00003 #ifndef NO_IMAGE_CONVERSION
00004 # include <setjmp.h>
00005
00006 # include "jpeg-6b/jpeglib.h"
00007
00008 namespace urbi
00009 {
00010
00011 namespace
00012 {
00013 void *read_jpeg(const char *jpgbuffer, int jpgbuffer_size,
00014 bool RGB, int &output_size);
00015
00016 int write_jpeg(const unsigned char* src, int w, int h, bool ycrcb,
00017 unsigned char* dst, int &sz, int quality);
00018
00019
00020 inline unsigned char clamp(float v)
00021 {
00022 if (v < 0)
00023 return 0;
00024 if (v > 255)
00025 return 255;
00026 return (unsigned char) v;
00027 }
00028 }
00029
00030 int
00031 convertRGBtoYCrCb(const byte * sourceImage,
00032 int bufferSize,
00033 byte * destinationImage)
00034 {
00035 unsigned char *in = (unsigned char *) sourceImage;
00036 unsigned char *out = (unsigned char *) destinationImage;
00037 for (int i = 0; i < bufferSize - 2; i += 3)
00038 {
00039 float r = in[i];
00040 float g = in[i + 1];
00041 float b = in[i + 2];
00042
00043
00044
00045
00046
00047 out[i] = clamp((0.257 * r) + (0.504 * g) + (0.098 * b) + 16);
00048 out[i + 1] = clamp((0.439 * r) - (0.368 * g) - (0.071 * b) + 128);
00049 out[i + 2] = clamp(-(0.148 * r) - (0.291 * g) + (0.439 * b) + 128);
00050 }
00051 return 1;
00052 }
00053
00054 int
00055 convertYCrCbtoYCbCr(const byte * sourceImage,
00056 int bufferSize,
00057 byte * destinationImage)
00058 {
00059 unsigned char *in = (unsigned char *) sourceImage;
00060 unsigned char *out = (unsigned char *) destinationImage;
00061 for (int i = 0; i < bufferSize - 2; i += 3)
00062 {
00063 out[i] = in[i];
00064 out[i + 1] = in[i + 2];
00065 out[i + 2] = in[i + 1];
00066 }
00067 return 1;
00068 }
00069
00070
00071 int
00072 convertYCrCbtoRGB(const byte * sourceImage,
00073 int bufferSize,
00074 byte * destinationImage)
00075 {
00076 unsigned char *in = (unsigned char *) sourceImage;
00077 unsigned char *out = (unsigned char *) destinationImage;
00078 for (int i = 0; i < bufferSize - 2; i += 3)
00079 {
00080 float y = in[i];
00081 float cb = in[i + 1];
00082 float cr = in[i + 2];
00083
00084
00085
00086
00087
00088 out[i] = clamp(1.164 * (y - 16) + 1.596 * (cr - 128));
00089 out[i + 1] = clamp(1.164 * (y - 16) - 0.813 * (cr - 128) -
00090 0.392 * (cb - 128));
00091 out[i + 2] = clamp(1.164 * (y - 16) + 2.017 * (cb - 128));
00092 }
00093 return 1;
00094 }
00095
00096
00097 int
00098 convertJPEGtoYCrCb(const byte * source, int sourcelen, byte * dest,
00099 int &size)
00100 {
00101 int sz;
00102 void *destination = read_jpeg((const char *) source, sourcelen, false, sz);
00103 if (!destination)
00104 {
00105 size = 0;
00106 return 0;
00107 }
00108 int cplen = sz > size ? size : sz;
00109 memcpy(dest, destination, cplen);
00110 free(destination);
00111 size = sz;
00112 return 1;
00113 }
00114
00115 int
00116 convertJPEGtoRGB(const byte * source, int sourcelen, byte * dest, int &size)
00117 {
00118 int sz;
00119 void *destination = read_jpeg((const char *) source, sourcelen, true, sz);
00120 if (!destination)
00121 {
00122 size = 0;
00123 return 0;
00124 }
00125 int cplen = sz > size ? size : sz;
00126 memcpy(dest, destination, cplen);
00127 free(destination);
00128 size = sz;
00129 return 1;
00130 }
00131
00132 int convertRGBtoJPEG(const byte* source,
00133 int w, int h, byte* dest, int &size, int quality)
00134 {
00135 return write_jpeg(source, w, h, false, dest, size, quality);
00136 }
00137
00138
00139 int convertYCrCbtoJPEG(const byte* source,
00140 int w, int h, byte* dest, int &size, int quality)
00141 {
00142 return write_jpeg(source, w, h, true, dest, size, quality);
00143 }
00144
00145 struct mem_source_mgr
00146 {
00147 struct jpeg_source_mgr pub;
00148 JOCTET eoi[2];
00149 };
00150
00151
00152 namespace
00153 {
00154 void init_source(j_decompress_ptr)
00155 {
00156 }
00157
00158 boolean fill_input_buffer(j_decompress_ptr cinfo)
00159 {
00160 mem_source_mgr *src = (mem_source_mgr *) cinfo->src;
00161 if (src->pub.bytes_in_buffer != 0)
00162 return TRUE;
00163 src->eoi[0] = 0xFF;
00164 src->eoi[1] = JPEG_EOI;
00165 src->pub.bytes_in_buffer = 2;
00166 src->pub.next_input_byte = src->eoi;
00167 return TRUE;
00168 }
00169
00170 void term_source(j_decompress_ptr)
00171 {
00172 }
00173
00174 void skip_input_data(j_decompress_ptr cinfo, long num_bytes)
00175 {
00176 mem_source_mgr* src = (mem_source_mgr*) cinfo->src;
00177 if (num_bytes <= 0)
00178 return;
00179 if (static_cast<unsigned long> (num_bytes) > src->pub.bytes_in_buffer)
00180 num_bytes = src->pub.bytes_in_buffer;
00181 src->pub.bytes_in_buffer -= num_bytes;
00182 src->pub.next_input_byte += num_bytes;
00183 }
00184
00185 }
00186
00187 struct urbi_jpeg_error_mgr
00188 {
00189 struct jpeg_error_mgr pub;
00190 jmp_buf setjmp_buffer;
00191 };
00192
00193 METHODDEF(void)
00194 urbi_jpeg_error_exit (j_common_ptr cinfo)
00195 {
00196
00197 urbi_jpeg_error_mgr * myerr = ( urbi_jpeg_error_mgr *) cinfo->err;
00198
00199
00200
00201 (*cinfo->err->output_message) (cinfo);
00202
00203
00204 longjmp(myerr->setjmp_buffer, 1);
00205 }
00206
00207
00208 struct mem_destination_mgr
00209 {
00210 struct jpeg_destination_mgr pub;
00211 };
00212
00213 void init_destination(j_compress_ptr)
00214 {
00215 }
00216
00217 boolean empty_output_buffer(j_compress_ptr)
00218 {
00219 return FALSE;
00220 }
00221
00222 void term_destination(j_compress_ptr)
00223 {
00224 }
00225
00226 namespace
00227 {
00228 int
00229 write_jpeg(const unsigned char* src, int w, int h, bool ycrcb,
00230 unsigned char* dst, int &sz, int quality)
00231 {
00232 struct jpeg_compress_struct cinfo;
00233 struct jpeg_error_mgr jerr;
00234
00235 JSAMPROW row_pointer[1];
00236 int row_stride;
00237
00238 cinfo.err = jpeg_std_error(&jerr);
00239 jpeg_create_compress(&cinfo);
00240 mem_destination_mgr *dest = (struct mem_destination_mgr *)
00241 (*cinfo.mem->alloc_small) ((j_common_ptr) & cinfo, JPOOL_PERMANENT,
00242 sizeof (mem_destination_mgr));
00243
00244 cinfo.dest = (jpeg_destination_mgr*)dest;
00245 dest->pub.init_destination=&init_destination;
00246 dest->pub.empty_output_buffer = &empty_output_buffer;
00247 dest->pub.term_destination = term_destination;
00248 dest->pub.free_in_buffer = sz;
00249 dest->pub.next_output_byte = dst;
00250 cinfo.image_width = w;
00251 cinfo.image_height = h;
00252 cinfo.input_components = 3;
00253
00254 cinfo.in_color_space = ycrcb ? JCS_YCbCr : JCS_RGB;
00255
00256 jpeg_set_defaults(&cinfo);
00257
00258 jpeg_set_quality(&cinfo,
00259 quality, TRUE );
00260
00261 jpeg_start_compress(&cinfo, TRUE);
00262
00263 row_stride = w * 3;
00264
00265 while (cinfo.next_scanline < cinfo.image_height)
00266 {
00267 row_pointer[0] = (JSAMPLE *)& src[cinfo.next_scanline * row_stride];
00268 (void) jpeg_write_scanlines(&cinfo, row_pointer, 1);
00269 }
00270
00271 jpeg_finish_compress(&cinfo);
00272 sz -= dest->pub.free_in_buffer ;
00273 jpeg_destroy_compress(&cinfo);
00274
00275 return sz;
00276 }
00277
00280 void *read_jpeg(const char *jpgbuffer, int jpgbuffer_size, bool RGB,
00281 int &output_size)
00282 {
00283 struct jpeg_decompress_struct cinfo;
00284 struct urbi_jpeg_error_mgr jerr;
00285 cinfo.err = jpeg_std_error(&jerr.pub);
00286 jerr.pub.error_exit = urbi_jpeg_error_exit;
00287 if (setjmp(jerr.setjmp_buffer))
00288 {
00289
00290
00291
00292 jpeg_destroy_decompress(&cinfo);
00293 printf( "JPEG error!\n");
00294 return 0;
00295 }
00296 jpeg_create_decompress(&cinfo);
00297 mem_source_mgr *source = (struct mem_source_mgr *)
00298 (*cinfo.mem->alloc_small) ((j_common_ptr) & cinfo, JPOOL_PERMANENT,
00299 sizeof (mem_source_mgr));
00300
00301 cinfo.src = (jpeg_source_mgr *) source;
00302 source->pub.skip_input_data = skip_input_data;
00303 source->pub.term_source = term_source;
00304 source->pub.init_source = init_source;
00305 source->pub.fill_input_buffer = fill_input_buffer;
00306 source->pub.resync_to_restart = jpeg_resync_to_restart;
00307 source->pub.bytes_in_buffer = jpgbuffer_size;
00308 source->pub.next_input_byte = (JOCTET *) jpgbuffer;
00309 cinfo.out_color_space = (RGB ? JCS_RGB : JCS_YCbCr);
00310 jpeg_read_header(&cinfo, TRUE);
00311 cinfo.out_color_space = (RGB ? JCS_RGB : JCS_YCbCr);
00312 jpeg_start_decompress(&cinfo);
00313 output_size = cinfo.output_width *
00314 cinfo.output_components *
00315 cinfo.output_height;
00316 void *buffer = malloc(output_size);
00317
00318 while (cinfo.output_scanline < cinfo.output_height)
00319 {
00320
00321
00322
00323
00324 JSAMPROW row_pointer[1];
00325 row_pointer[0] = (JOCTET *) & ((char *) buffer)[cinfo.output_scanline *
00326 cinfo.output_components *
00327 cinfo.output_width];
00328 jpeg_read_scanlines(&cinfo, row_pointer, 1);
00329 }
00330 jpeg_finish_decompress(&cinfo);
00331 jpeg_destroy_decompress(&cinfo);
00332
00333 return buffer;
00334 }
00335
00336
00337
00338
00339 void scaleColorImage(unsigned char * src, int sw, int sh,
00340 int scx, int scy, unsigned char * dst,
00341 int dw, int dh, float sx, float sy)
00342 {
00343 for (int x = 0; x < dw; ++x)
00344 for (int y = 0; y < dh; ++y)
00345 {
00346
00347 float fsrcx = (float) (x-dw/2) / sx + (float) scx;
00348 float fsrcy = (float) (y-dh/2) / sy + (float) scy;
00349 int srcx = (int) fsrcx;
00350 int srcy = (int) fsrcy;
00351 if (srcx <= 0 || srcx >= sw - 1 || srcy <= 0 || srcy >= sh - 1)
00352 memset(dst + (x + y * dw) * 3, 0, 3);
00353 else
00354 {
00355 float xfactor = fsrcx - (float) srcx;
00356 float yfactor = fsrcy - (float) srcy;
00357 for (int color = 0; color < 3; ++color)
00358 {
00359 float up = (float) src[(srcx + srcy * sw) * 3 + color]
00360 * (1.0 - xfactor)
00361 + (float) src[(srcx + 1 + srcy * sw) * 3 + color] * xfactor;
00362 float down = (float) src[(srcx + (srcy + 1) * sw) * 3 + color]
00363 * (1.0 - xfactor)
00364 + (float) src[(srcx + 1 + (srcy + 1) * sw) * 3 + color]
00365 * xfactor;
00366 float result = up * (1.0 - yfactor) + down * yfactor;
00367 dst[(x + y * dw) * 3 + color] = (unsigned char) result;
00368 }
00369 }
00370 }
00371 }
00372
00373 }
00374
00377 int convert(const UImage& src, UImage& dest)
00378 {
00379 if (dest.width == 0)
00380 dest.width = src.width;
00381 if (dest.height == 0)
00382 dest.height = src.height;
00383
00384 void* uncompressedData = malloc(src.width * src.height * 3);
00385 int usz = src.width * src.height * 3;
00386 int format = 42;
00387 int targetformat = 42;
00388
00389 switch (dest.imageFormat)
00390 {
00391 case IMAGE_RGB:
00392 case IMAGE_PPM:
00393 targetformat = 1;
00394 break;
00395 case IMAGE_YCbCr:
00396 targetformat = 0;
00397 break;
00398 case IMAGE_JPEG:
00399 targetformat = -1;
00400 break;
00401 case IMAGE_UNKNOWN:
00402 break;
00403 }
00404 int p = 0;
00405 int c = 0;
00406 switch (src.imageFormat)
00407 {
00408 case IMAGE_YCbCr:
00409 format = 1;
00410 memcpy(uncompressedData, src.data, src.width * src.height * 3);
00411 break;
00412 case IMAGE_RGB:
00413 format = 0;
00414 memcpy(uncompressedData, src.data, src.width * src.height * 3);
00415 break;
00416 case IMAGE_PPM:
00417 format = 0;
00418
00419 p = 0;
00420 c = 0;
00421 while (c < 3)
00422 if (src.data[p++] == '\n')
00423 ++c;
00424 memcpy(src.data + p, uncompressedData, src.width * src.height * 3);
00425 break;
00426 case IMAGE_JPEG:
00427 if (targetformat == 0)
00428 {
00429 convertJPEGtoRGB((byte*) src.data, src.size,
00430 (byte*) uncompressedData, usz);
00431 format = 0;
00432 }
00433 else
00434 {
00435 convertJPEGtoYCrCb((byte*) src.data, src.size,
00436 (byte*) uncompressedData, usz);
00437 format = 1;
00438 }
00439 break;
00440 case IMAGE_UNKNOWN:
00441 break;
00442 }
00443
00444
00445 if (src.width != dest.width || src.height != dest.height)
00446 {
00447 void* scaled = malloc(dest.width * dest.height * 3);
00448 scaleColorImage((unsigned char*) uncompressedData, src.width,
00449 src.height, src.width/2, src.height/2,
00450 (unsigned char*) scaled, dest.width, dest.height,
00451 (float) dest.width / (float) src.width,
00452 (float) dest.height / (float) src.height);
00453 free(uncompressedData);
00454 uncompressedData = scaled;
00455 }
00456
00457
00458 dest.size = dest.width * dest.height * 3 + 20;
00459 dest.data = static_cast<unsigned char*> (realloc(dest.data, dest.size));
00460
00461 switch (dest.imageFormat)
00462 {
00463 case IMAGE_RGB:
00464 if (format == 1)
00465 convertYCrCbtoRGB((byte*) uncompressedData,
00466 dest.width * dest.height * 3, (byte*) dest.data);
00467 else
00468 memcpy(dest.data, uncompressedData, dest.width * dest.height * 3);
00469 break;
00470 case IMAGE_YCbCr:
00471 if (format == 0)
00472 convertRGBtoYCrCb((byte*) uncompressedData,
00473 dest.width * dest.height * 3, (byte*) dest.data);
00474 else
00475 memcpy(dest.data, uncompressedData, dest.width * dest.height * 3);
00476 break;
00477 case IMAGE_PPM:
00478 sprintf((char*) dest.data, "P6\n%zd %zd\n255\n",
00479 dest.width, dest.height);
00480 if (format == 1)
00481 convertYCrCbtoRGB((byte*) uncompressedData,
00482 dest.width * dest.height * 3,
00483 (byte*) dest.data + strlen((char*) dest.data));
00484 else
00485 memcpy(dest.data + strlen((char*) dest.data),
00486 uncompressedData, dest.width * dest.height * 3);
00487 break;
00488 case IMAGE_JPEG:
00489
00490
00491
00492
00493
00494
00495
00496
00497
00498 fprintf(stderr,
00499 "unsupported conversion requested: can't compress to jpeg\n");
00500 free(uncompressedData);
00501 return 0;
00502 break;
00503 case IMAGE_UNKNOWN:
00504 break;
00505 }
00506
00507 free(uncompressedData);
00508 return 1;
00509 }
00510
00511 }
00512
00513 #endif // !NO_IMAGE_CONVERSION
00514
00515 namespace urbi
00516 {
00517 struct wavheader
00518 {
00519 char riff[4];
00520 int length;
00521 char wave[4];
00522 char fmt[4];
00523 int lnginfo;
00524 short one;
00525 short channels;
00526 int freqechant;
00527 int bytespersec;
00528 short bytesperechant;
00529 short bitperchannel;
00530 char data[4];
00531 int datalength;
00532 };
00533
00534 template<class S, class D>
00535 void copy(S* src, D* dst,
00536 int sc, int dc, int sr, int dr, int count, bool sf, bool df)
00537 {
00538 int shift = 8 * (sizeof (S) - sizeof (D));
00539 for (int i = 0; i < count; ++i)
00540 {
00541 float soffset = (float)i * ((float)sr / (float)dr);
00542 int so = (int)soffset;
00543 float factor = soffset - (float)so;
00544 S s1, s2;
00545 s1 = src[so * sc];
00546 if (i != count - 1)
00547 s2 = src[(so + 1) * sc];
00548 else
00549 s2 = s1;
00550 if (!sf)
00551 {
00552 s1 = s1 ^ (1<<(sizeof (S)*8-1));
00553 s2 = s2 ^ (1<<(sizeof (S)*8-1));
00554 }
00555 int v1 = (int) ((float)(s1)*(1.0-factor) + (float)(s2)*factor);
00556 int v2;
00557 if (sc==1)
00558 v2 = v1;
00559 else
00560 {
00561 s1 = src[so*sc+1];
00562 if (i != count - 1)
00563 s2 = src[(so+1)*sc+1];
00564 else
00565 s2 = s1;
00566 if (!sf)
00567 {
00568 s1 = s1 ^ (1<<(sizeof (S)*8-1));
00569 s2 = s2 ^ (1<<(sizeof (S)*8-1));
00570 }
00571 v2 = (int) ((float)(s1)*(1.0-factor) + (float)(s2)*factor);
00572 }
00573 D d1, d2;
00574 if (shift>=0)
00575 {
00576 d1 = (D)(v1 >>shift);
00577 d2 = (D)(v2 >>shift);
00578 }
00579 else
00580 {
00581 d1 = (D)(v1) * (1<< (-shift));
00582 d2 = (D)(v2) * (1<< (-shift));
00583 }
00584 if (!df)
00585 {
00586 d1 = d1 ^ (1<<(sizeof (D)*8-1));
00587 d2 = d2 ^ (1<<(sizeof (D)*8-1));
00588 }
00589 if (dc==2)
00590 {
00591 dst[i*2] = d1;
00592 dst[i*2+1] = d2;
00593 }
00594 else
00595 dst[i] = (D) (((int)d1+(int)d2) /2);
00596 }
00597 }
00598
00605 int
00606 convert (const USound &source, USound &dest)
00607 {
00608 if ((source.soundFormat != SOUND_RAW
00609 && source.soundFormat != SOUND_WAV)
00610 || (dest.soundFormat != SOUND_RAW
00611 && dest.soundFormat != SOUND_WAV))
00612 return 1;
00613
00614
00615 int schannels, srate, ssampleSize;
00616 USoundSampleFormat ssampleFormat;
00617 if (source.soundFormat == SOUND_WAV)
00618 {
00619 wavheader * wh = (wavheader *)source.data;
00620 schannels = wh->channels;
00621 srate = wh->freqechant;
00622 ssampleSize = wh->bitperchannel;
00623 ssampleFormat = (ssampleSize>8)?SAMPLE_SIGNED:SAMPLE_UNSIGNED;
00624 }
00625 else
00626 {
00627 schannels = source.channels;
00628 srate = source.rate;
00629 ssampleSize = source.sampleSize;
00630 ssampleFormat = source.sampleFormat;
00631 }
00632 if (!dest.channels)
00633 dest.channels = schannels;
00634 if (!dest.rate)
00635 dest.rate = srate;
00636 if (!dest.sampleSize)
00637 dest.sampleSize = ssampleSize;
00638 if (!(int)dest.sampleFormat)
00639 dest.sampleFormat = ssampleFormat;
00640 if (dest.soundFormat == SOUND_WAV)
00641 dest.sampleFormat = dest.sampleSize > 8 ? SAMPLE_SIGNED
00642 : SAMPLE_UNSIGNED;
00643
00644 unsigned destSize =
00645 ((long long)(source.size
00646 - ((source.soundFormat == SOUND_WAV)?44:0))
00647 * (long long)dest.channels
00648 * (long long)dest.rate
00649 * (long long)(dest.sampleSize/8))
00650 / ((long long)schannels
00651 *(long long)srate
00652 *(long long)(ssampleSize/8));
00653 if (dest.soundFormat == SOUND_WAV)
00654 destSize += sizeof (wavheader);
00655 if (dest.size<destSize)
00656 dest.data = static_cast<char*> (realloc (dest.data, destSize));
00657 dest.size = destSize;
00658
00659 if (dest.soundFormat == SOUND_WAV)
00660 {
00661 wavheader* wh = (wavheader*) dest.data;
00662 memcpy(wh->riff, "RIFF", 4);
00663 wh->length = dest.size - 8;
00664 memcpy(wh->wave, "WAVE", 4);
00665 memcpy(wh->fmt, "fmt ", 4);
00666 wh->lnginfo = 16;
00667 wh->one = 1;
00668 wh->channels = dest.channels;
00669 wh->freqechant = dest.rate;
00670 wh->bytespersec = dest.rate * dest.channels * (dest.sampleSize/8);
00671 wh->bytesperechant = (dest.sampleSize/8)*dest.channels;
00672 wh->bitperchannel = dest.sampleSize;
00673 memcpy(wh->data, "data", 4);
00674 wh->datalength = destSize - sizeof (wavheader);
00675 }
00676
00677
00678 char * sbuffer = source.data;
00679 if (source.soundFormat == SOUND_WAV)
00680 sbuffer += sizeof (wavheader);
00681 char * dbuffer = dest.data;
00682 if (dest.soundFormat == SOUND_WAV)
00683 dbuffer += sizeof (wavheader);
00684 int elementCount = dest.size - (dest.soundFormat == SOUND_WAV ?
00685 sizeof (wavheader) : 0);
00686 elementCount /= (dest.channels * (dest.sampleSize / 8));
00687 switch (ssampleSize * 1000 + dest.sampleSize)
00688 {
00689 case 8008:
00690 copy(sbuffer, dbuffer, schannels, dest.channels, srate, dest.rate,
00691 elementCount, ssampleFormat==SAMPLE_SIGNED, dest.sampleFormat ==
00692 SAMPLE_SIGNED);
00693 break;
00694 case 16008:
00695 copy((short *)sbuffer, dbuffer, schannels, dest.channels, srate,
00696 dest.rate, elementCount, ssampleFormat==SAMPLE_SIGNED,
00697 dest.sampleFormat == SAMPLE_SIGNED);
00698 break;
00699 case 16016:
00700 copy((short *)sbuffer, (short *)dbuffer, schannels, dest.channels,
00701 srate, dest.rate, elementCount, ssampleFormat==SAMPLE_SIGNED,
00702 dest.sampleFormat == SAMPLE_SIGNED);
00703 break;
00704 case 8016:
00705 copy((char *)sbuffer, (short *)dbuffer, schannels, dest.channels,
00706 srate, dest.rate, elementCount, ssampleFormat==SAMPLE_SIGNED,
00707 dest.sampleFormat == SAMPLE_SIGNED);
00708 break;
00709 }
00710 return 0;
00711 }
00712
00713 }