00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include "fftCompressor.h"
00020 #include "config_mathutil.h"
00021
00022 #include <datagram.h>
00023 #include <datagramIterator.h>
00024 #include <compose_matrix.h>
00025 #include <math.h>
00026
00027 #ifdef HAVE_FFTW
00028
00029 #include <rfftw.h>
00030
00031
00032
00033 static rfftw_plan get_real_compress_plan(int length);
00034 static rfftw_plan get_real_decompress_plan(int length);
00035
00036 typedef pmap<int, rfftw_plan> RealPlans;
00037 static RealPlans _real_compress_plans;
00038 static RealPlans _real_decompress_plans;
00039
00040 #endif
00041
00042
00043
00044
00045
00046
00047
00048 FFTCompressor::
00049 FFTCompressor() {
00050 set_quality(-1);
00051 _transpose_quats = false;
00052 }
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065 bool FFTCompressor::
00066 is_compression_available() {
00067 #ifndef HAVE_FFTW
00068 return false;
00069 #else
00070 return true;
00071 #endif
00072 }
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097 void FFTCompressor::
00098 set_quality(int quality) {
00099 #ifndef HAVE_FFTW
00100
00101 if (_quality <= 100) {
00102 mathutil_cat.warning()
00103 << "FFTW library is not available; generating uncompressed output.\n";
00104 }
00105 _quality = 101;
00106
00107 #else
00108 _quality = quality;
00109
00110 if (_quality < 0) {
00111
00112
00113 _fft_offset = fft_offset;
00114 _fft_factor = fft_factor;
00115 _fft_exponent = fft_exponent;
00116
00117 } else if (_quality < 40) {
00118
00119
00120
00121
00122
00123 double t = (double)_quality / 40.0;
00124 _fft_offset = interpolate(t, 1.0, 0.001);
00125 _fft_factor = 1.0;
00126 _fft_exponent = 4.0;
00127
00128 } else if (_quality < 95) {
00129
00130
00131
00132
00133
00134 double t = (double)(_quality - 40) / 55.0;
00135 _fft_offset = 0.001;
00136 _fft_factor = interpolate(t, 1.0, 0.1);
00137 _fft_exponent = 4.0;
00138
00139 } else {
00140
00141
00142
00143
00144
00145 double t = (double)(_quality - 95) / 5.0;
00146 _fft_offset = 0.001;
00147 _fft_factor = interpolate(t, 0.1, 0.0);
00148 _fft_exponent = 4.0;
00149 }
00150 #endif
00151 }
00152
00153
00154
00155
00156
00157
00158
00159 int FFTCompressor::
00160 get_quality() const {
00161 return _quality;
00162 }
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172 void FFTCompressor::
00173 set_transpose_quats(bool flag) {
00174 _transpose_quats = flag;
00175 }
00176
00177
00178
00179
00180
00181
00182
00183 bool FFTCompressor::
00184 get_transpose_quats() const {
00185 return _transpose_quats;
00186 }
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197 void FFTCompressor::
00198 write_header(Datagram &datagram) {
00199 datagram.add_int8(_quality);
00200 if (_quality < 0) {
00201 datagram.add_float64(_fft_offset);
00202 datagram.add_float64(_fft_factor);
00203 datagram.add_float64(_fft_exponent);
00204 }
00205 }
00206
00207
00208
00209
00210
00211
00212
00213 void FFTCompressor::
00214 write_reals(Datagram &datagram, const float *array, int length) {
00215 datagram.add_int32(length);
00216
00217 if (_quality > 100) {
00218
00219 for (int i = 0; i < length; i++) {
00220 datagram.add_float32(array[i]);
00221 }
00222 return;
00223 }
00224
00225 #ifndef HAVE_FFTW
00226
00227 nassertv(false);
00228
00229 #else
00230
00231 if (length == 0) {
00232
00233 return;
00234 }
00235
00236 if (length == 1) {
00237
00238 datagram.add_float32(array[0]);
00239 return;
00240 }
00241
00242
00243 double *data = (double *)alloca(length * sizeof(double));
00244 int i;
00245 for (i = 0; i < length; i++) {
00246 data[i] = array[i];
00247 }
00248
00249 double *half_complex = (double *)alloca(length * sizeof(double));
00250
00251 rfftw_plan plan = get_real_compress_plan(length);
00252 rfftw_one(plan, data, half_complex);
00253
00254
00255
00256
00257 vector_double run;
00258 RunWidth run_width = RW_invalid;
00259 int num_written = 0;
00260
00261 for (i = 0; i < length; i++) {
00262 static const double max_range_32 = 2147483647.0;
00263 static const double max_range_16 = 32767.0;
00264 static const double max_range_8 = 127.0;
00265
00266 double scale_factor = get_scale_factor(i, length);
00267 double num = cfloor(half_complex[i] / scale_factor + 0.5);
00268
00269
00270 double a = fabs(num);
00271 RunWidth num_width;
00272
00273 if (a == 0.0) {
00274 num_width = RW_0;
00275
00276 } else if (a <= max_range_8) {
00277 num_width = RW_8;
00278
00279 } else if (a <= max_range_16) {
00280 num_width = RW_16;
00281
00282 } else if (a <= max_range_32) {
00283 num_width = RW_32;
00284
00285 } else {
00286 num_width = RW_double;
00287 }
00288
00289
00290
00291
00292 if (run_width == RW_8 && num_width == RW_0) {
00293 if (i + 1 >= length || half_complex[i + 1] != 0.0) {
00294 num_width = RW_8;
00295 }
00296 }
00297
00298 if (num_width != run_width) {
00299
00300 num_written += write_run(datagram, run_width, run);
00301 run.clear();
00302 run_width = num_width;
00303 }
00304
00305 run.push_back(num);
00306 }
00307
00308 num_written += write_run(datagram, run_width, run);
00309 nassertv(num_written == length);
00310 #endif
00311 }
00312
00313
00314
00315
00316
00317
00318
00319 void FFTCompressor::
00320 write_hprs(Datagram &datagram, const LVecBase3f *array, int length) {
00321 #ifndef NDEBUG
00322 if (_quality >= 104) {
00323
00324
00325 vector_float h, p, r;
00326
00327 h.reserve(length);
00328 p.reserve(length);
00329 r.reserve(length);
00330
00331 for (int i = 0; i < length; i++) {
00332 h.push_back(array[i][0]);
00333 p.push_back(array[i][1]);
00334 r.push_back(array[i][2]);
00335 }
00336
00337 write_reals(datagram, &h[0], length);
00338 write_reals(datagram, &p[0], length);
00339 write_reals(datagram, &r[0], length);
00340 return;
00341 }
00342 if (_quality >= 103) {
00343
00344
00345 vector_float
00346 m00, m01, m02,
00347 m10, m11, m12,
00348 m20, m21, m22;
00349 for (int i = 0; i < length; i++) {
00350 LMatrix3f mat;
00351 compose_matrix(mat, LVecBase3f(1.0, 1.0, 1.0), array[i]);
00352 m00.push_back(mat(0, 0));
00353 m01.push_back(mat(0, 1));
00354 m02.push_back(mat(0, 2));
00355 m10.push_back(mat(1, 0));
00356 m11.push_back(mat(1, 1));
00357 m12.push_back(mat(1, 2));
00358 m20.push_back(mat(2, 0));
00359 m21.push_back(mat(2, 1));
00360 m22.push_back(mat(2, 2));
00361 }
00362
00363 write_reals(datagram, &m00[0], length);
00364 write_reals(datagram, &m01[0], length);
00365 write_reals(datagram, &m02[0], length);
00366 write_reals(datagram, &m10[0], length);
00367 write_reals(datagram, &m11[0], length);
00368 write_reals(datagram, &m12[0], length);
00369 write_reals(datagram, &m20[0], length);
00370 write_reals(datagram, &m21[0], length);
00371 write_reals(datagram, &m22[0], length);
00372 return;
00373 }
00374 #endif
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384 vector_float qr, qi, qj, qk;
00385
00386 qr.reserve(length);
00387 qi.reserve(length);
00388 qj.reserve(length);
00389 qk.reserve(length);
00390
00391 for (int i = 0; i < length; i++) {
00392 LMatrix3f mat;
00393 compose_matrix(mat, LVecBase3f(1.0, 1.0, 1.0), array[i]);
00394 if (_transpose_quats) {
00395 mat.transpose_in_place();
00396 }
00397
00398 LOrientationf rot(mat);
00399 rot.normalize();
00400
00401 if (rot.get_r() < 0) {
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412 rot.set(-rot.get_r(), -rot.get_i(), -rot.get_j(), -rot.get_k());
00413 }
00414
00415 #ifdef NOTIFY_DEBUG
00416 if (mathutil_cat.is_warning()) {
00417 LMatrix3f mat2;
00418 rot.extract_to_matrix(mat2);
00419 if (!mat.almost_equal(mat2, 0.0001)) {
00420 LVecBase3f hpr1, hpr2;
00421 LVecBase3f scale;
00422 bool d1 = decompose_matrix(mat, scale, hpr1);
00423 bool d2 = decompose_matrix(mat2, scale, hpr2);
00424 mathutil_cat.warning()
00425 << "Converted hpr to quaternion incorrectly!\n"
00426 << " Source hpr: " << array[i];
00427 if (d1) {
00428 mathutil_cat.warning(false)
00429 << ", or " << hpr1 << "\n";
00430 } else {
00431 mathutil_cat.warning(false)
00432 << ", incorrectly converted to the non-ortho matrix:\n";
00433 mat2.write(mathutil_cat.warning(false), 4);
00434 }
00435 mathutil_cat.warning(false)
00436 << " Quaternion: " << rot << "\n";
00437 if (d2) {
00438 mathutil_cat.warning(false)
00439 << " Which represents: hpr " << hpr2 << " scale "
00440 << scale << "\n";
00441 } else {
00442 mathutil_cat.warning(false)
00443 << " Which is incorrectly converted to the following non-ortho matrix:\n";
00444 mat2.write(mathutil_cat.warning(false), 4);
00445 }
00446 }
00447 }
00448 #endif
00449
00450 qr.push_back(rot.get_r());
00451 qi.push_back(rot.get_i());
00452 qj.push_back(rot.get_j());
00453 qk.push_back(rot.get_k());
00454 }
00455
00456
00457
00458 #ifndef NDEBUG
00459 if (_quality >= 102) {
00460 write_reals(datagram, &qr[0], length);
00461 }
00462 #endif
00463 write_reals(datagram, &qi[0], length);
00464 write_reals(datagram, &qj[0], length);
00465 write_reals(datagram, &qk[0], length);
00466 }
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478 bool FFTCompressor::
00479 read_header(DatagramIterator &di) {
00480 _quality = di.get_int8();
00481
00482 if (mathutil_cat.is_debug()) {
00483 mathutil_cat.debug()
00484 << "Found compressed data at quality level " << _quality << "\n";
00485 }
00486
00487 #ifndef HAVE_FFTW
00488 if (_quality <= 100) {
00489 mathutil_cat.error()
00490 << "FFTW library is not available; cannot read compressed data.\n";
00491 return false;
00492 }
00493 #endif
00494
00495 set_quality(_quality);
00496
00497 if (_quality < 0) {
00498 _fft_offset = di.get_float64();
00499 _fft_factor = di.get_float64();
00500 _fft_exponent = di.get_float64();
00501 }
00502
00503 return true;
00504 }
00505
00506
00507
00508
00509
00510
00511
00512
00513
00514
00515
00516 bool FFTCompressor::
00517 read_reals(DatagramIterator &di, vector_float &array) {
00518 int length = di.get_int32();
00519
00520 if (_quality > 100) {
00521 array.reserve(array.size() + length);
00522
00523
00524 for (int i = 0; i < length; i++) {
00525 array.push_back(di.get_float32());
00526 }
00527 return true;
00528 }
00529
00530 #ifndef HAVE_FFTW
00531
00532 return false;
00533
00534 #else
00535
00536 if (length == 0) {
00537
00538 return true;
00539 }
00540
00541 if (length == 1) {
00542
00543 array.push_back(di.get_float32());
00544 return true;
00545 }
00546
00547
00548
00549 vector_double half_complex;
00550 half_complex.reserve(length);
00551 int num_read = 0;
00552 while (num_read < length) {
00553 num_read += read_run(di, half_complex);
00554 }
00555 nassertr(num_read == length, false);
00556 nassertr((int)half_complex.size() == length, false);
00557
00558 int i;
00559 for (i = 0; i < length; i++) {
00560 half_complex[i] *= get_scale_factor(i, length);
00561 }
00562
00563 double *data = (double *)alloca(length * sizeof(double));
00564 rfftw_plan plan = get_real_decompress_plan(length);
00565 rfftw_one(plan, &half_complex[0], data);
00566
00567 double scale = 1.0 / (double)length;
00568 array.reserve(array.size() + length);
00569 for (i = 0; i < length; i++) {
00570 array.push_back(data[i] * scale);
00571 }
00572
00573 return true;
00574 #endif
00575 }
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585 bool FFTCompressor::
00586 read_hprs(DatagramIterator &di, vector_LVecBase3f &array) {
00587 #ifndef NDEBUG
00588 if (_quality >= 104) {
00589
00590
00591 vector_float h, p, r;
00592 bool okflag = true;
00593 okflag =
00594 read_reals(di, h) &&
00595 read_reals(di, p) &&
00596 read_reals(di, r);
00597
00598 if (okflag) {
00599 nassertr(h.size() == p.size() && p.size() == r.size(), false);
00600 for (int i = 0; i < (int)h.size(); i++) {
00601 array.push_back(LVecBase3f(h[i], p[i], r[i]));
00602 }
00603 }
00604
00605 return okflag;
00606 }
00607 if (_quality >= 103) {
00608
00609
00610 vector_float
00611 m00, m01, m02,
00612 m10, m11, m12,
00613 m20, m21, m22;
00614 bool okflag = true;
00615 okflag =
00616 read_reals(di, m00) &&
00617 read_reals(di, m01) &&
00618 read_reals(di, m02) &&
00619 read_reals(di, m10) &&
00620 read_reals(di, m11) &&
00621 read_reals(di, m12) &&
00622 read_reals(di, m20) &&
00623 read_reals(di, m21) &&
00624 read_reals(di, m22);
00625
00626 if (okflag) {
00627 for (int i = 0; i < (int)m00.size(); i++) {
00628 LMatrix3f mat(m00[i], m01[i], m02[i],
00629 m10[i], m11[i], m12[i],
00630 m20[i], m21[i], m22[i]);
00631 LVecBase3f scale, hpr;
00632 bool success = decompose_matrix(mat, scale, hpr);
00633 if (success) {
00634 array.push_back(hpr);
00635 } else {
00636 mathutil_cat.error()
00637 << "Unable to decompose matrix:\n";
00638 mat.write(mathutil_cat.error(false), 2);
00639 array.push_back(LVecBase3f(0.0, 0.0, 0.0));
00640 }
00641 }
00642 }
00643
00644 return okflag;
00645 }
00646 #endif
00647
00648 vector_float qr, qi, qj, qk;
00649
00650 bool okflag = true;
00651
00652 #ifndef NDEBUG
00653 if (_quality >= 102) {
00654 okflag = read_reals(di, qr);
00655 }
00656 #endif
00657
00658 okflag =
00659 okflag &&
00660 read_reals(di, qi) &&
00661 read_reals(di, qj) &&
00662 read_reals(di, qk);
00663
00664 if (okflag) {
00665 nassertr(qi.size() == qj.size() && qj.size() == qk.size(), false);
00666
00667 array.reserve(array.size() + qi.size());
00668 for (int i = 0; i < (int)qi.size(); i++) {
00669 LOrientationf rot;
00670
00671
00672 float qr2 = 1.0 - (qi[i] * qi[i] + qj[i] * qj[i] + qk[i] * qk[i]);
00673 float qr1 = qr2 < 0.0 ? 0.0 : sqrtf(qr2);
00674
00675 rot.set(qr1, qi[i], qj[i], qk[i]);
00676
00677 #ifndef NDEBUG
00678 if (_quality >= 102) {
00679
00680 rot[0] = qr[i];
00681
00682 if (!IS_THRESHOLD_EQUAL(qr[i], qr1, 0.001)) {
00683 mathutil_cat.warning()
00684 << "qr[" << i << "] = " << qr[i] << ", qr1 = " << qr1
00685 << ", diff is " << qr1 - qr[i] << "\n";
00686 }
00687 } else
00688 #endif
00689
00690 rot.normalize();
00691
00692 LMatrix3f mat;
00693 rot.extract_to_matrix(mat);
00694 if (_transpose_quats) {
00695 mat.transpose_in_place();
00696 }
00697 LVecBase3f scale, hpr;
00698 bool success = decompose_matrix(mat, scale, hpr);
00699 nassertr(success, false);
00700
00701 array.push_back(hpr);
00702 }
00703 }
00704
00705 return okflag;
00706 }
00707
00708
00709
00710
00711
00712
00713
00714
00715
00716
00717
00718 void FFTCompressor::
00719 free_storage() {
00720 #ifdef HAVE_FFTW
00721 RealPlans::iterator pi;
00722 for (pi = _real_compress_plans.begin();
00723 pi != _real_compress_plans.end();
00724 ++pi) {
00725 rfftw_destroy_plan((*pi).second);
00726 }
00727 _real_compress_plans.clear();
00728
00729 for (pi = _real_decompress_plans.begin();
00730 pi != _real_decompress_plans.end();
00731 ++pi) {
00732 rfftw_destroy_plan((*pi).second);
00733 }
00734 _real_decompress_plans.clear();
00735 #endif
00736 }
00737
00738
00739
00740
00741
00742
00743
00744
00745 int FFTCompressor::
00746 write_run(Datagram &datagram, FFTCompressor::RunWidth run_width,
00747 const vector_double &run) {
00748 if (run.empty()) {
00749 return 0;
00750 }
00751 nassertr(run_width != RW_invalid, 0);
00752
00753 if (run_width != RW_double) {
00754
00755
00756
00757
00758 if (run.size() <= RW_length_mask &&
00759 ((int)run_width | run.size()) != RW_double) {
00760
00761
00762
00763
00764 datagram.add_uint8((int)run_width | run.size());
00765
00766 } else {
00767
00768
00769 datagram.add_uint8(run_width);
00770
00771
00772 nassertr(run.size() < 65536, 0);
00773 nassertr(run.size() != 0, 0);
00774
00775 datagram.add_uint16(run.size());
00776 }
00777 }
00778
00779
00780 vector_double::const_iterator ri;
00781 switch (run_width) {
00782 case RW_0:
00783
00784 break;
00785
00786 case RW_8:
00787 for (ri = run.begin(); ri != run.end(); ++ri) {
00788 datagram.add_int8((int)*ri);
00789 }
00790 break;
00791
00792 case RW_16:
00793 for (ri = run.begin(); ri != run.end(); ++ri) {
00794 datagram.add_int16((int)*ri);
00795 }
00796 break;
00797
00798 case RW_32:
00799 for (ri = run.begin(); ri != run.end(); ++ri) {
00800 datagram.add_int32((int)*ri);
00801 }
00802 break;
00803
00804 case RW_double:
00805 for (ri = run.begin(); ri != run.end(); ++ri) {
00806
00807
00808
00809 datagram.add_int8((PN_int8)RW_double);
00810 datagram.add_float64(*ri);
00811 }
00812 break;
00813
00814 default:
00815 break;
00816 }
00817
00818 return run.size();
00819 }
00820
00821
00822
00823
00824
00825
00826
00827
00828
00829
00830 int FFTCompressor::
00831 read_run(DatagramIterator &di, vector_double &run) {
00832 PN_uint8 start = di.get_uint8();
00833 RunWidth run_width;
00834 int length;
00835
00836 if ((start & 0xff) == RW_double) {
00837
00838
00839 run_width = RW_double;
00840 length = 1;
00841
00842 } else {
00843 run_width = (RunWidth)(start & RW_width_mask);
00844 length = start & RW_length_mask;
00845 }
00846
00847 if (length == 0) {
00848
00849
00850 length = di.get_uint16();
00851 }
00852 nassertr(length != 0, 0);
00853
00854 run.reserve(run.size() + length);
00855
00856 int i;
00857 switch (run_width) {
00858 case RW_0:
00859 for (i = 0; i < length; i++) {
00860 run.push_back(0.0);
00861 }
00862 break;
00863
00864 case RW_8:
00865 for (i = 0; i < length; i++) {
00866 run.push_back((double)(int)di.get_int8());
00867 }
00868 break;
00869
00870 case RW_16:
00871 for (i = 0; i < length; i++) {
00872 run.push_back((double)(int)di.get_int16());
00873 }
00874 break;
00875
00876 case RW_32:
00877 for (i = 0; i < length; i++) {
00878 run.push_back((double)(int)di.get_int32());
00879 }
00880 break;
00881
00882 case RW_double:
00883 for (i = 0; i < length; i++) {
00884 run.push_back(di.get_float64());
00885 }
00886 break;
00887
00888 default:
00889 break;
00890 }
00891
00892 return length;
00893 }
00894
00895
00896
00897
00898
00899
00900
00901 double FFTCompressor::
00902 get_scale_factor(int i, int length) const {
00903 int m = (length / 2) + 1;
00904 int k = (i < m) ? i : length - i;
00905 nassertr(k >= 0 && k < m, 1.0);
00906
00907 return _fft_offset +
00908 _fft_factor * pow((double)(m-1 - k) / (double)(m-1), _fft_exponent);
00909 }
00910
00911
00912
00913
00914
00915
00916
00917
00918 double FFTCompressor::
00919 interpolate(double t, double a, double b) {
00920 return a + t * (b - a);
00921 }
00922
00923
00924 #ifdef HAVE_FFTW
00925
00926
00927
00928
00929
00930
00931 static rfftw_plan
00932 get_real_compress_plan(int length) {
00933 RealPlans::iterator pi;
00934 pi = _real_compress_plans.find(length);
00935 if (pi != _real_compress_plans.end()) {
00936 return (*pi).second;
00937 }
00938
00939 rfftw_plan plan;
00940 plan = rfftw_create_plan(length, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE);
00941 _real_compress_plans.insert(RealPlans::value_type(length, plan));
00942
00943 return plan;
00944 }
00945
00946
00947
00948
00949
00950
00951 static rfftw_plan
00952 get_real_decompress_plan(int length) {
00953 RealPlans::iterator pi;
00954 pi = _real_decompress_plans.find(length);
00955 if (pi != _real_decompress_plans.end()) {
00956 return (*pi).second;
00957 }
00958
00959 rfftw_plan plan;
00960 plan = rfftw_create_plan(length, FFTW_COMPLEX_TO_REAL, FFTW_ESTIMATE);
00961 _real_decompress_plans.insert(RealPlans::value_type(length, plan));
00962
00963 return plan;
00964 }
00965
00966 #endif