Panda3D
|
00001 // Filename: fftCompressor.cxx 00002 // Created by: drose (11Dec00) 00003 // 00004 //////////////////////////////////////////////////////////////////// 00005 // 00006 // PANDA 3D SOFTWARE 00007 // Copyright (c) Carnegie Mellon University. All rights reserved. 00008 // 00009 // All use of this software is subject to the terms of the revised BSD 00010 // license. You should have received a copy of this license along 00011 // with this source code in a file named "LICENSE." 00012 // 00013 //////////////////////////////////////////////////////////////////// 00014 00015 #include "fftCompressor.h" 00016 #include "config_mathutil.h" 00017 #include "config_linmath.h" 00018 00019 #include "datagram.h" 00020 #include "datagramIterator.h" 00021 #include "compose_matrix.h" 00022 #include "pmap.h" 00023 #include <math.h> 00024 00025 #ifdef HAVE_FFTW 00026 00027 // hack..... 00028 // this is a hack to help interigate sort out a macro 00029 // in the system poll and select definitions 00030 // 00031 #ifdef howmany 00032 #undef howmany 00033 #endif 00034 00035 #ifdef PHAVE_DRFFTW_H 00036 #include "drfftw.h" 00037 #else 00038 #include "rfftw.h" 00039 #endif 00040 00041 // These FFTW support objects can only be defined if we actually have 00042 // the FFTW library available. 00043 static rfftw_plan get_real_compress_plan(int length); 00044 static rfftw_plan get_real_decompress_plan(int length); 00045 00046 typedef pmap<int, rfftw_plan> RealPlans; 00047 static RealPlans _real_compress_plans; 00048 static RealPlans _real_decompress_plans; 00049 00050 #endif 00051 00052 //////////////////////////////////////////////////////////////////// 00053 // Function: FFTCompressor::Constructor 00054 // Access: Public 00055 // Description: Constructs a new compressor object with default 00056 // parameters. 00057 //////////////////////////////////////////////////////////////////// 00058 FFTCompressor:: 00059 FFTCompressor() { 00060 _bam_minor_version = 0; 00061 set_quality(-1); 00062 _use_error_threshold = false; 00063 _transpose_quats = false; 00064 } 00065 00066 //////////////////////////////////////////////////////////////////// 00067 // Function: FFTCompressor::is_compression_available 00068 // Access: Public, Static 00069 // Description: Returns true if the FFTW library is compiled in, so 00070 // that this class is actually capable of doing useful 00071 // compression/decompression work. Returns false 00072 // otherwise, in which case any attempt to write a 00073 // compressed stream will actually write an uncompressed 00074 // stream, and any attempt to read a compressed stream 00075 // will fail. 00076 //////////////////////////////////////////////////////////////////// 00077 bool FFTCompressor:: 00078 is_compression_available() { 00079 #ifndef HAVE_FFTW 00080 return false; 00081 #else 00082 return true; 00083 #endif 00084 } 00085 00086 //////////////////////////////////////////////////////////////////// 00087 // Function: FFTCompressor::set_quality 00088 // Access: Public 00089 // Description: Sets the quality factor for the compression. This is 00090 // an integer in the range 0 - 100 that roughly controls 00091 // how aggressively the reals are compressed; lower 00092 // numbers mean smaller output, and more data loss. 00093 // 00094 // There are a few special cases. Quality -1 means to 00095 // use whatever individual parameters are set in the 00096 // user's Configrc file, rather than the single quality 00097 // dial. Quality 101 or higher means to generate 00098 // lossless output (this is the default if libfftw is 00099 // not available). 00100 // 00101 // Quality 102 writes all four components of quaternions 00102 // to the output file, rather than just three, quality 00103 // 103 converts hpr to matrix (instead of quat) and 00104 // writes a 9-component matrix, and quality 104 just 00105 // writes out hpr directly. Quality levels 102 and 00106 // greater are strictly for debugging purposes, and are 00107 // only available if NDEBUG is not defined. 00108 //////////////////////////////////////////////////////////////////// 00109 void FFTCompressor:: 00110 set_quality(int quality) { 00111 #ifndef HAVE_FFTW 00112 // If we don't actually have FFTW, we can't really compress anything. 00113 if (_quality <= 100) { 00114 mathutil_cat.warning() 00115 << "FFTW library is not available; generating uncompressed output.\n"; 00116 } 00117 _quality = 101; 00118 00119 #else 00120 _quality = quality; 00121 00122 if (_quality < 0) { 00123 // A negative quality indicates we should read the various 00124 // parameters from individual config variables. 00125 _fft_offset = fft_offset; 00126 _fft_factor = fft_factor; 00127 _fft_exponent = fft_exponent; 00128 00129 } else if (_quality < 40) { 00130 // 0 - 40 : 00131 // fft-offset 1.0 - 0.001 00132 // fft-factor 1.0 00133 // fft-exponent 4.0 00134 00135 double t = (double)_quality / 40.0; 00136 _fft_offset = interpolate(t, 1.0, 0.001); 00137 _fft_factor = 1.0; 00138 _fft_exponent = 4.0; 00139 00140 } else if (_quality < 95) { 00141 // 40 - 95: 00142 // fft-offset 0.001 00143 // fft-factor 1.0 - 0.1 00144 // fft-exponent 4.0 00145 00146 double t = (double)(_quality - 40) / 55.0; 00147 _fft_offset = 0.001; 00148 _fft_factor = interpolate(t, 1.0, 0.1); 00149 _fft_exponent = 4.0; 00150 00151 } else { 00152 // 95 - 100: 00153 // fft-offset 0.001 00154 // fft-factor 0.1 - 0.0 00155 // fft-exponent 4.0 00156 00157 double t = (double)(_quality - 95) / 5.0; 00158 _fft_offset = 0.001; 00159 _fft_factor = interpolate(t, 0.1, 0.0); 00160 _fft_exponent = 4.0; 00161 } 00162 #endif 00163 } 00164 00165 //////////////////////////////////////////////////////////////////// 00166 // Function: FFTCompressor::get_quality 00167 // Access: Public 00168 // Description: Returns the quality number that was previously set 00169 // via set_quality(). 00170 //////////////////////////////////////////////////////////////////// 00171 int FFTCompressor:: 00172 get_quality() const { 00173 return _quality; 00174 } 00175 00176 //////////////////////////////////////////////////////////////////// 00177 // Function: FFTCompressor::set_use_error_threshold 00178 // Access: Public 00179 // Description: Enables or disables the use of the error threshold 00180 // measurement to put a cap on the amount of damage done 00181 // by lossy compression. When this is enabled, the 00182 // potential results of the compression are analyzed 00183 // before the data is written; if it is determined that 00184 // the compression will damage a particular string of 00185 // reals too much, that particular string of reals is 00186 // written uncompressed. 00187 //////////////////////////////////////////////////////////////////// 00188 void FFTCompressor:: 00189 set_use_error_threshold(bool use_error_threshold) { 00190 _use_error_threshold = use_error_threshold; 00191 } 00192 00193 //////////////////////////////////////////////////////////////////// 00194 // Function: FFTCompressor::get_use_error_threshold 00195 // Access: Public 00196 // Description: Returns whether the error threshold measurement is 00197 // enabled. See set_use_error_threshold(). 00198 //////////////////////////////////////////////////////////////////// 00199 bool FFTCompressor:: 00200 get_use_error_threshold() const { 00201 return _use_error_threshold; 00202 } 00203 00204 //////////////////////////////////////////////////////////////////// 00205 // Function: FFTCompressor::set_transpose_quats 00206 // Access: Public 00207 // Description: Sets the transpose_quats flag. This is provided 00208 // mainly for backward compatibility with old bam files 00209 // that were written out with the quaternions 00210 // inadvertently transposed. 00211 //////////////////////////////////////////////////////////////////// 00212 void FFTCompressor:: 00213 set_transpose_quats(bool flag) { 00214 _transpose_quats = flag; 00215 } 00216 00217 //////////////////////////////////////////////////////////////////// 00218 // Function: FFTCompressor::get_transpose_quats 00219 // Access: Public 00220 // Description: Returns the transpose_quats flag. See 00221 // set_transpose_quats(). 00222 //////////////////////////////////////////////////////////////////// 00223 bool FFTCompressor:: 00224 get_transpose_quats() const { 00225 return _transpose_quats; 00226 } 00227 00228 //////////////////////////////////////////////////////////////////// 00229 // Function: FFTCompressor::write_header 00230 // Access: Public 00231 // Description: Writes the compression parameters to the indicated 00232 // datagram. It is necessary to call this before 00233 // writing anything else to the datagram, since these 00234 // parameters will be necessary to correctly decompress 00235 // the data later. 00236 //////////////////////////////////////////////////////////////////// 00237 void FFTCompressor:: 00238 write_header(Datagram &datagram) { 00239 datagram.add_int8(_quality); 00240 if (_quality < 0) { 00241 datagram.add_float64(_fft_offset); 00242 datagram.add_float64(_fft_factor); 00243 datagram.add_float64(_fft_exponent); 00244 } 00245 } 00246 00247 //////////////////////////////////////////////////////////////////// 00248 // Function: FFTCompressor::write_reals 00249 // Access: Public 00250 // Description: Writes an array of floating-point numbers to the 00251 // indicated datagram. 00252 //////////////////////////////////////////////////////////////////// 00253 void FFTCompressor:: 00254 write_reals(Datagram &datagram, const PN_stdfloat *array, int length) { 00255 datagram.add_int32(length); 00256 00257 if (_quality > 100) { 00258 // Special case: lossless output. 00259 for (int i = 0; i < length; i++) { 00260 datagram.add_stdfloat(array[i]); 00261 } 00262 return; 00263 } 00264 00265 #ifndef HAVE_FFTW 00266 // If we don't have FFTW, we shouldn't get here. 00267 nassertv(false); 00268 00269 #else 00270 00271 if (length == 0) { 00272 // Special case: do nothing. 00273 return; 00274 } 00275 00276 if (length == 1) { 00277 // Special case: just write out the one number. 00278 datagram.add_stdfloat(array[0]); 00279 return; 00280 } 00281 00282 // Normal case: FFT the array, and write that out. 00283 00284 // First, check the compressability. 00285 bool reject_compression = false; 00286 00287 // This logic needs a closer examination. Not sure it's useful 00288 // as-is. 00289 /* 00290 if (_use_error_threshold) { 00291 // Don't encode the data if it moves too erratically. 00292 PN_stdfloat error = get_compressability(array, length); 00293 if (error > fft_error_threshold) { 00294 // No good: the data probably won't compress well. Just write 00295 // out lossless data. 00296 reject_compression = true; 00297 } 00298 } 00299 */ 00300 00301 datagram.add_bool(reject_compression); 00302 if (reject_compression) { 00303 if (mathutil_cat.is_debug()) { 00304 mathutil_cat.debug() 00305 << "Writing stream of " << length << " numbers uncompressed.\n"; 00306 } 00307 for (int i = 0; i < length; i++) { 00308 datagram.add_stdfloat(array[i]); 00309 } 00310 return; 00311 } 00312 00313 // Now generate the Fourier transform. 00314 double *data = (double *)alloca(length * sizeof(double)); 00315 int i; 00316 for (i = 0; i < length; i++) { 00317 data[i] = array[i]; 00318 } 00319 00320 double *half_complex = (double *)alloca(length * sizeof(double)); 00321 00322 rfftw_plan plan = get_real_compress_plan(length); 00323 rfftw_one(plan, data, half_complex); 00324 00325 00326 // Now encode the numbers, run-length encoded by size, so we only 00327 // write out the number of bits we need for each number. 00328 00329 vector_double run; 00330 RunWidth run_width = RW_invalid; 00331 int num_written = 0; 00332 00333 for (i = 0; i < length; i++) { 00334 static const double max_range_32 = 2147483647.0; 00335 static const double max_range_16 = 32767.0; 00336 static const double max_range_8 = 127.0; 00337 00338 double scale_factor = get_scale_factor(i, length); 00339 double num = cfloor(half_complex[i] / scale_factor + 0.5); 00340 00341 // How many bits do we need to encode this integer? 00342 double a = fabs(num); 00343 RunWidth num_width; 00344 00345 if (a == 0.0) { 00346 num_width = RW_0; 00347 00348 } else if (a <= max_range_8) { 00349 num_width = RW_8; 00350 00351 } else if (a <= max_range_16) { 00352 num_width = RW_16; 00353 00354 } else if (a <= max_range_32) { 00355 num_width = RW_32; 00356 00357 } else { 00358 num_width = RW_double; 00359 } 00360 00361 // A special case: if we're writing a string of one-byters and we 00362 // come across a single intervening zero, don't interrupt the run 00363 // just for that. 00364 if (run_width == RW_8 && num_width == RW_0) { 00365 if (i + 1 >= length || half_complex[i + 1] != 0.0) { 00366 num_width = RW_8; 00367 } 00368 } 00369 00370 if (num_width != run_width) { 00371 // Now we need to flush the last run. 00372 num_written += write_run(datagram, run_width, run); 00373 run.clear(); 00374 run_width = num_width; 00375 } 00376 00377 run.push_back(num); 00378 } 00379 00380 num_written += write_run(datagram, run_width, run); 00381 nassertv(num_written == length); 00382 #endif 00383 } 00384 00385 //////////////////////////////////////////////////////////////////// 00386 // Function: FFTCompressor::write_hprs 00387 // Access: Public 00388 // Description: Writes an array of HPR angles to the indicated 00389 // datagram. 00390 //////////////////////////////////////////////////////////////////// 00391 void FFTCompressor:: 00392 write_hprs(Datagram &datagram, const LVecBase3 *array, int length) { 00393 #ifndef NDEBUG 00394 if (_quality >= 104) { 00395 // If quality level is at least 104, we don't even convert hpr at 00396 // all. This is just for debugging. 00397 vector_stdfloat h, p, r; 00398 00399 h.reserve(length); 00400 p.reserve(length); 00401 r.reserve(length); 00402 00403 for (int i = 0; i < length; i++) { 00404 h.push_back(array[i][0]); 00405 p.push_back(array[i][1]); 00406 r.push_back(array[i][2]); 00407 } 00408 00409 if (length == 0) { 00410 write_reals(datagram, NULL, length); 00411 write_reals(datagram, NULL, length); 00412 write_reals(datagram, NULL, length); 00413 } else { 00414 write_reals(datagram, &h[0], length); 00415 write_reals(datagram, &p[0], length); 00416 write_reals(datagram, &r[0], length); 00417 } 00418 return; 00419 } 00420 if (_quality >= 103) { 00421 // If quality level is 103, we convert hpr to a table of matrices. 00422 // This is just for debugging. 00423 vector_stdfloat 00424 m00, m01, m02, 00425 m10, m11, m12, 00426 m20, m21, m22; 00427 for (int i = 0; i < length; i++) { 00428 LMatrix3 mat; 00429 compose_matrix(mat, LVecBase3(1.0, 1.0, 1.0), LVecBase3(0.0, 0.0, 0.0), 00430 array[i]); 00431 m00.push_back(mat(0, 0)); 00432 m01.push_back(mat(0, 1)); 00433 m02.push_back(mat(0, 2)); 00434 m10.push_back(mat(1, 0)); 00435 m11.push_back(mat(1, 1)); 00436 m12.push_back(mat(1, 2)); 00437 m20.push_back(mat(2, 0)); 00438 m21.push_back(mat(2, 1)); 00439 m22.push_back(mat(2, 2)); 00440 } 00441 00442 if (length == 0) { 00443 write_reals(datagram, NULL, length); 00444 write_reals(datagram, NULL, length); 00445 write_reals(datagram, NULL, length); 00446 write_reals(datagram, NULL, length); 00447 write_reals(datagram, NULL, length); 00448 write_reals(datagram, NULL, length); 00449 write_reals(datagram, NULL, length); 00450 write_reals(datagram, NULL, length); 00451 write_reals(datagram, NULL, length); 00452 } else { 00453 write_reals(datagram, &m00[0], length); 00454 write_reals(datagram, &m01[0], length); 00455 write_reals(datagram, &m02[0], length); 00456 write_reals(datagram, &m10[0], length); 00457 write_reals(datagram, &m11[0], length); 00458 write_reals(datagram, &m12[0], length); 00459 write_reals(datagram, &m20[0], length); 00460 write_reals(datagram, &m21[0], length); 00461 write_reals(datagram, &m22[0], length); 00462 } 00463 return; 00464 } 00465 #endif 00466 00467 // First, convert the HPR's to quats. We expect quats to have 00468 // better FFT consistency, and therefore compress better, even 00469 // though they have an extra component. 00470 00471 // However, because the quaternion will be normalized, we don't even 00472 // have to write out all three components; any three can be used to 00473 // determine the fourth (provided we ensure consistency of sign). 00474 00475 vector_stdfloat qr, qi, qj, qk; 00476 00477 qr.reserve(length); 00478 qi.reserve(length); 00479 qj.reserve(length); 00480 qk.reserve(length); 00481 00482 for (int i = 0; i < length; i++) { 00483 LMatrix3 mat; 00484 compose_matrix(mat, LVecBase3(1.0, 1.0, 1.0), LVecBase3(0.0, 0.0, 0.0), 00485 array[i]); 00486 if (_transpose_quats) { 00487 mat.transpose_in_place(); 00488 } 00489 00490 LOrientation rot(mat); 00491 rot.normalize(); // This may not be necessary, but let's not take chances. 00492 00493 if (rot.get_r() < 0) { 00494 // Since rot == -rot, we can flip the quarternion if need be to 00495 // keep the r component positive. This has two advantages. 00496 // One, it makes it possible to infer r completely given i, j, 00497 // and k (since we know it must be >= 0), and two, it helps 00498 // protect against poor continuity caused by inadvertent 00499 // flipping of the quarternion's sign between frames. 00500 00501 // The choice of leaving r implicit rather than any of the other 00502 // three seems to work the best in terms of guaranteeing 00503 // continuity. 00504 rot.set(-rot.get_r(), -rot.get_i(), -rot.get_j(), -rot.get_k()); 00505 } 00506 00507 #ifdef NOTIFY_DEBUG 00508 if (mathutil_cat.is_warning()) { 00509 LMatrix3 mat2; 00510 rot.extract_to_matrix(mat2); 00511 if (!mat.almost_equal(mat2, 0.0001)) { 00512 LVecBase3 hpr1, hpr2; 00513 LVecBase3 scale, shear; 00514 decompose_matrix(mat, scale, shear, hpr1); 00515 decompose_matrix(mat2, scale, shear, hpr2); 00516 mathutil_cat.warning() 00517 << "Converted hpr to quaternion incorrectly!\n" 00518 << " Source hpr: " << array[i] << ", or " << hpr1 << "\n"; 00519 mathutil_cat.warning(false) 00520 << " Quaternion: " << rot << "\n" 00521 << " Which represents: hpr " << hpr2 << " scale " 00522 << scale << "\n"; 00523 } 00524 } 00525 #endif 00526 00527 qr.push_back(rot.get_r()); 00528 qi.push_back(rot.get_i()); 00529 qj.push_back(rot.get_j()); 00530 qk.push_back(rot.get_k()); 00531 } 00532 00533 // If quality is at least 102, we write all four quat components, 00534 // instead of just the three. This is just for debugging. 00535 #ifndef NDEBUG 00536 if (_quality >= 102) { 00537 if (length == 0) { 00538 write_reals(datagram, NULL, length); 00539 } else { 00540 write_reals(datagram, &qr[0], length); 00541 } 00542 } 00543 #endif 00544 if (length == 0) { 00545 write_reals(datagram, NULL, length); 00546 write_reals(datagram, NULL, length); 00547 write_reals(datagram, NULL, length); 00548 } else { 00549 write_reals(datagram, &qi[0], length); 00550 write_reals(datagram, &qj[0], length); 00551 write_reals(datagram, &qk[0], length); 00552 } 00553 } 00554 00555 //////////////////////////////////////////////////////////////////// 00556 // Function: FFTCompressor::read_header 00557 // Access: Public 00558 // Description: Reads the compression header that was written 00559 // previously. This fills in the compression parameters 00560 // necessary to correctly decompress the following data. 00561 // 00562 // Returns true if the header is read successfully, 00563 // false otherwise. 00564 //////////////////////////////////////////////////////////////////// 00565 bool FFTCompressor:: 00566 read_header(DatagramIterator &di, int bam_minor_version) { 00567 _bam_minor_version = bam_minor_version; 00568 _quality = di.get_int8(); 00569 00570 if (mathutil_cat.is_debug()) { 00571 mathutil_cat.debug() 00572 << "Found compressed data at quality level " << _quality << "\n"; 00573 } 00574 00575 #ifndef HAVE_FFTW 00576 if (_quality <= 100) { 00577 mathutil_cat.error() 00578 << "FFTW library is not available; cannot read compressed data.\n"; 00579 return false; 00580 } 00581 #endif 00582 00583 set_quality(_quality); 00584 00585 if (_quality < 0) { 00586 _fft_offset = di.get_float64(); 00587 _fft_factor = di.get_float64(); 00588 _fft_exponent = di.get_float64(); 00589 } 00590 00591 return true; 00592 } 00593 00594 //////////////////////////////////////////////////////////////////// 00595 // Function: FFTCompressor::read_reals 00596 // Access: Public 00597 // Description: Reads an array of floating-point numbers. The result 00598 // is pushed onto the end of the indicated vector, which 00599 // is not cleared first; it is the user's responsibility 00600 // to ensure that the array is initially empty. Returns 00601 // true if the data is read correctly, false if there is 00602 // an error. 00603 //////////////////////////////////////////////////////////////////// 00604 bool FFTCompressor:: 00605 read_reals(DatagramIterator &di, vector_stdfloat &array) { 00606 int length = di.get_int32(); 00607 00608 if (_quality > 100) { 00609 array.reserve(array.size() + length); 00610 00611 // Special case: lossless output. 00612 for (int i = 0; i < length; i++) { 00613 array.push_back(di.get_stdfloat()); 00614 } 00615 return true; 00616 } 00617 00618 #ifndef HAVE_FFTW 00619 // If we don't have FFTW, we shouldn't get here. 00620 return false; 00621 00622 #else 00623 00624 if (length == 0) { 00625 // Special case: do nothing. 00626 return true; 00627 } 00628 00629 if (length == 1) { 00630 // Special case: just read in the one number. 00631 array.push_back(di.get_stdfloat()); 00632 return true; 00633 } 00634 00635 // Normal case: read in the FFT array, and convert it back to 00636 // (nearly) the original numbers. 00637 00638 // First, check the reject_compression flag. If it's set, we 00639 // decided to just write out the stream uncompressed. 00640 bool reject_compression = di.get_bool(); 00641 if (reject_compression) { 00642 array.reserve(array.size() + length); 00643 for (int i = 0; i < length; i++) { 00644 array.push_back(di.get_stdfloat()); 00645 } 00646 return true; 00647 } 00648 00649 vector_double half_complex; 00650 half_complex.reserve(length); 00651 int num_read = 0; 00652 while (num_read < length) { 00653 num_read += read_run(di, half_complex); 00654 } 00655 nassertr(num_read == length, false); 00656 nassertr((int)half_complex.size() == length, false); 00657 00658 int i; 00659 for (i = 0; i < length; i++) { 00660 half_complex[i] *= get_scale_factor(i, length); 00661 } 00662 00663 double *data = (double *)alloca(length * sizeof(double)); 00664 rfftw_plan plan = get_real_decompress_plan(length); 00665 rfftw_one(plan, &half_complex[0], data); 00666 00667 double scale = 1.0 / (double)length; 00668 array.reserve(array.size() + length); 00669 for (i = 0; i < length; i++) { 00670 array.push_back(data[i] * scale); 00671 } 00672 00673 return true; 00674 #endif 00675 } 00676 00677 //////////////////////////////////////////////////////////////////// 00678 // Function: FFTCompressor::read_hprs 00679 // Access: Public 00680 // Description: Reads an array of HPR angles. The result is pushed 00681 // onto the end of the indicated vector, which is not 00682 // cleared first; it is the user's responsibility to 00683 // ensure that the array is initially empty. 00684 // 00685 // new_hpr is a temporary, transitional parameter. If 00686 // it is set false, the hprs are decompressed according 00687 // to the old, broken hpr calculation; if true, the hprs 00688 // are decompressed according to the new, correct hpr 00689 // calculation. See temp_hpr_fix. 00690 //////////////////////////////////////////////////////////////////// 00691 bool FFTCompressor:: 00692 read_hprs(DatagramIterator &di, pvector<LVecBase3> &array, bool new_hpr) { 00693 #ifndef NDEBUG 00694 if (_quality >= 104) { 00695 // If quality level is at least 104, we don't even convert hpr to 00696 // quat. This is just for debugging. 00697 vector_stdfloat h, p, r; 00698 bool okflag = true; 00699 okflag = 00700 read_reals(di, h) && 00701 read_reals(di, p) && 00702 read_reals(di, r); 00703 00704 if (okflag) { 00705 nassertr(h.size() == p.size() && p.size() == r.size(), false); 00706 for (int i = 0; i < (int)h.size(); i++) { 00707 array.push_back(LVecBase3(h[i], p[i], r[i])); 00708 } 00709 } 00710 00711 return okflag; 00712 } 00713 if (_quality >= 103) { 00714 // If quality level is 103, we read in a table of 3x3 rotation 00715 // matrices. This is just for debugging. 00716 vector_stdfloat 00717 m00, m01, m02, 00718 m10, m11, m12, 00719 m20, m21, m22; 00720 bool okflag = true; 00721 okflag = 00722 read_reals(di, m00) && 00723 read_reals(di, m01) && 00724 read_reals(di, m02) && 00725 read_reals(di, m10) && 00726 read_reals(di, m11) && 00727 read_reals(di, m12) && 00728 read_reals(di, m20) && 00729 read_reals(di, m21) && 00730 read_reals(di, m22); 00731 00732 if (okflag) { 00733 for (int i = 0; i < (int)m00.size(); i++) { 00734 LMatrix3 mat(m00[i], m01[i], m02[i], 00735 m10[i], m11[i], m12[i], 00736 m20[i], m21[i], m22[i]); 00737 LVecBase3 scale, shear, hpr; 00738 if (new_hpr) { 00739 decompose_matrix_new_hpr(mat, scale, shear, hpr); 00740 } else { 00741 decompose_matrix_old_hpr(mat, scale, shear, hpr); 00742 } 00743 array.push_back(hpr); 00744 } 00745 } 00746 00747 return okflag; 00748 } 00749 #endif 00750 00751 vector_stdfloat qr, qi, qj, qk; 00752 00753 bool okflag = true; 00754 00755 #ifndef NDEBUG 00756 if (_quality >= 102) { 00757 okflag = read_reals(di, qr); 00758 } 00759 #endif 00760 00761 okflag = 00762 okflag && 00763 read_reals(di, qi) && 00764 read_reals(di, qj) && 00765 read_reals(di, qk); 00766 00767 if (okflag) { 00768 nassertr(qi.size() == qj.size() && qj.size() == qk.size(), false); 00769 00770 array.reserve(array.size() + qi.size()); 00771 for (int i = 0; i < (int)qi.size(); i++) { 00772 LOrientation rot; 00773 00774 // Infer the r component from the remaining three. 00775 PN_stdfloat qr2 = 1.0 - (qi[i] * qi[i] + qj[i] * qj[i] + qk[i] * qk[i]); 00776 PN_stdfloat qr1 = qr2 < 0.0 ? 0.0 : sqrtf(qr2); 00777 00778 rot.set(qr1, qi[i], qj[i], qk[i]); 00779 00780 #ifndef NDEBUG 00781 if (_quality >= 102) { 00782 // If we have written out all four components, use them. 00783 rot[0] = qr[i]; 00784 00785 if (!IS_THRESHOLD_EQUAL(qr[i], qr1, 0.001)) { 00786 mathutil_cat.warning() 00787 << "qr[" << i << "] = " << qr[i] << ", qr1 = " << qr1 00788 << ", diff is " << qr1 - qr[i] << "\n"; 00789 } 00790 } else 00791 #endif 00792 00793 rot.normalize(); // Just for good measure. 00794 00795 LMatrix3 mat; 00796 rot.extract_to_matrix(mat); 00797 if (_transpose_quats) { 00798 mat.transpose_in_place(); 00799 } 00800 LVecBase3 scale, shear, hpr; 00801 if (new_hpr) { 00802 decompose_matrix_new_hpr(mat, scale, shear, hpr); 00803 } else { 00804 decompose_matrix_old_hpr(mat, scale, shear, hpr); 00805 } 00806 array.push_back(hpr); 00807 } 00808 } 00809 00810 return okflag; 00811 } 00812 00813 //////////////////////////////////////////////////////////////////// 00814 // Function: FFTCompressor::read_hprs 00815 // Access: Public 00816 // Description: Reads an array of HPR angles. The result is pushed 00817 // onto the end of the indicated vector, which is not 00818 // cleared first; it is the user's responsibility to 00819 // ensure that the array is initially empty. 00820 //////////////////////////////////////////////////////////////////// 00821 bool FFTCompressor:: 00822 read_hprs(DatagramIterator &di, pvector<LVecBase3> &array) { 00823 return read_hprs(di, array, temp_hpr_fix); 00824 } 00825 00826 00827 //////////////////////////////////////////////////////////////////// 00828 // Function: FFTCompressor::free_storage 00829 // Access: Public, Static 00830 // Description: Frees memory that has been allocated during past runs 00831 // of the FFTCompressor. This is an optional call, but 00832 // it may be made from time to time to empty the global 00833 // cache that the compressor objects keep to facilitate 00834 // fast compression/decompression. 00835 //////////////////////////////////////////////////////////////////// 00836 void FFTCompressor:: 00837 free_storage() { 00838 #ifdef HAVE_FFTW 00839 RealPlans::iterator pi; 00840 for (pi = _real_compress_plans.begin(); 00841 pi != _real_compress_plans.end(); 00842 ++pi) { 00843 rfftw_destroy_plan((*pi).second); 00844 } 00845 _real_compress_plans.clear(); 00846 00847 for (pi = _real_decompress_plans.begin(); 00848 pi != _real_decompress_plans.end(); 00849 ++pi) { 00850 rfftw_destroy_plan((*pi).second); 00851 } 00852 _real_decompress_plans.clear(); 00853 #endif 00854 } 00855 00856 //////////////////////////////////////////////////////////////////// 00857 // Function: FFTCompressor::write_run 00858 // Access: Private 00859 // Description: Writes a sequence of integers that all require the 00860 // same number of bits. Returns the number of integers 00861 // written, i.e. run.size(). 00862 //////////////////////////////////////////////////////////////////// 00863 int FFTCompressor:: 00864 write_run(Datagram &datagram, FFTCompressor::RunWidth run_width, 00865 const vector_double &run) { 00866 if (run.empty()) { 00867 return 0; 00868 } 00869 nassertr(run_width != RW_invalid, 0); 00870 00871 if (run_width != RW_double) { 00872 // If the width is anything other than RW_double, we write a 00873 // single byte indicating the width and length of the upcoming 00874 // run. 00875 00876 if (run.size() <= RW_length_mask && 00877 ((int)run_width | run.size()) != RW_double) { 00878 // If there are enough bits remaining in the byte, use them to 00879 // indicate the length of the run. We have to be a little 00880 // careful, however, not to accidentally write a byte that looks 00881 // like an RW_double flag. 00882 datagram.add_uint8((int)run_width | run.size()); 00883 00884 } else { 00885 // Otherwise, write zero as the length, to indicate that we'll 00886 // write the actual length in the following 16-bit word. 00887 datagram.add_uint8(run_width); 00888 00889 // Assuming, of course, that the length fits within 16 bits. 00890 nassertr(run.size() < 65536, 0); 00891 nassertr(run.size() != 0, 0); 00892 00893 datagram.add_uint16(run.size()); 00894 } 00895 } 00896 00897 // Now write the data itself. 00898 vector_double::const_iterator ri; 00899 switch (run_width) { 00900 case RW_0: 00901 // If it's a string of zeroes, we're done! 00902 break; 00903 00904 case RW_8: 00905 for (ri = run.begin(); ri != run.end(); ++ri) { 00906 datagram.add_int8((int)*ri); 00907 } 00908 break; 00909 00910 case RW_16: 00911 for (ri = run.begin(); ri != run.end(); ++ri) { 00912 datagram.add_int16((int)*ri); 00913 } 00914 break; 00915 00916 case RW_32: 00917 for (ri = run.begin(); ri != run.end(); ++ri) { 00918 datagram.add_int32((int)*ri); 00919 } 00920 break; 00921 00922 case RW_double: 00923 for (ri = run.begin(); ri != run.end(); ++ri) { 00924 // In the case of RW_double, we only write the numbers one at a 00925 // time, each time preceded by the RW_double flag. Hopefully 00926 // this will happen only rarely. 00927 datagram.add_int8((PN_int8)RW_double); 00928 datagram.add_float64(*ri); 00929 } 00930 break; 00931 00932 default: 00933 break; 00934 } 00935 00936 return run.size(); 00937 } 00938 00939 //////////////////////////////////////////////////////////////////// 00940 // Function: FFTCompressor::read_run 00941 // Access: Private 00942 // Description: Reads a sequence of integers that all require the 00943 // same number of bits. Returns the number of integers 00944 // read. It is the responsibility of the user to clear 00945 // the vector before calling this function, or the 00946 // numbers read will be appended to the end. 00947 //////////////////////////////////////////////////////////////////// 00948 int FFTCompressor:: 00949 read_run(DatagramIterator &di, vector_double &run) { 00950 PN_uint8 start = di.get_uint8(); 00951 RunWidth run_width; 00952 int length; 00953 00954 if ((start & 0xff) == RW_double) { 00955 // RW_double is a special case, and requires the whole byte. In 00956 // this case, we don't encode a length, but assume it's only one. 00957 run_width = RW_double; 00958 length = 1; 00959 00960 } else { 00961 run_width = (RunWidth)(start & RW_width_mask); 00962 length = start & RW_length_mask; 00963 } 00964 00965 if (length == 0) { 00966 // If the length was zero, it means the actual length follows as a 00967 // 16-bit word. 00968 length = di.get_uint16(); 00969 } 00970 nassertr(length != 0, 0); 00971 00972 run.reserve(run.size() + length); 00973 00974 int i; 00975 switch (run_width) { 00976 case RW_0: 00977 for (i = 0; i < length; i++) { 00978 run.push_back(0.0); 00979 } 00980 break; 00981 00982 case RW_8: 00983 for (i = 0; i < length; i++) { 00984 run.push_back((double)(int)di.get_int8()); 00985 } 00986 break; 00987 00988 case RW_16: 00989 for (i = 0; i < length; i++) { 00990 run.push_back((double)(int)di.get_int16()); 00991 } 00992 break; 00993 00994 case RW_32: 00995 for (i = 0; i < length; i++) { 00996 run.push_back((double)(int)di.get_int32()); 00997 } 00998 break; 00999 01000 case RW_double: 01001 for (i = 0; i < length; i++) { 01002 run.push_back(di.get_float64()); 01003 } 01004 break; 01005 01006 default: 01007 break; 01008 } 01009 01010 return length; 01011 } 01012 01013 //////////////////////////////////////////////////////////////////// 01014 // Function: FFTCompressor::get_scale_factor 01015 // Access: Private 01016 // Description: Returns the appropriate scaling for the given 01017 // position within the halfcomplex array. 01018 //////////////////////////////////////////////////////////////////// 01019 double FFTCompressor:: 01020 get_scale_factor(int i, int length) const { 01021 int m = (length / 2) + 1; 01022 int k = (i < m) ? i : length - i; 01023 nassertr(k >= 0 && k < m, 1.0); 01024 01025 return _fft_offset + 01026 _fft_factor * pow((double)(m-1 - k) / (double)(m-1), _fft_exponent); 01027 } 01028 01029 //////////////////////////////////////////////////////////////////// 01030 // Function: FFTCompressor::interpolate 01031 // Access: Private, Static 01032 // Description: Returns a number between a and b, inclusive, 01033 // according to the value of t between 0 and 1, 01034 // inclusive. 01035 //////////////////////////////////////////////////////////////////// 01036 double FFTCompressor:: 01037 interpolate(double t, double a, double b) { 01038 return a + t * (b - a); 01039 } 01040 01041 //////////////////////////////////////////////////////////////////// 01042 // Function: FFTCompressor::get_compressability 01043 // Access: Private 01044 // Description: Returns a factor that indicates how erratically the 01045 // values are changing. The lower the result, the 01046 // calmer the numbers, and the greater its likelihood of 01047 // being successfully compressed. 01048 //////////////////////////////////////////////////////////////////// 01049 PN_stdfloat FFTCompressor:: 01050 get_compressability(const PN_stdfloat *data, int length) const { 01051 // The result returned is actually the standard deviation of the 01052 // table of deltas between consecutive frames. This number is 01053 // larger if the frames have wildly different values. 01054 01055 if (length <= 2) { 01056 return 0.0; 01057 } 01058 01059 PN_stdfloat sum = 0.0; 01060 PN_stdfloat sum2 = 0.0; 01061 for (int i = 1; i < length; i++) { 01062 PN_stdfloat delta = data[i] - data[i - 1]; 01063 01064 sum += delta; 01065 sum2 += delta * delta; 01066 } 01067 PN_stdfloat variance = (sum2 - (sum * sum) / (length - 1)) / (length - 2); 01068 if (variance < 0.0) { 01069 // This can only happen due to tiny roundoff error. 01070 return 0.0; 01071 } 01072 01073 PN_stdfloat std_deviation = csqrt(variance); 01074 01075 return std_deviation; 01076 } 01077 01078 01079 01080 #ifdef HAVE_FFTW 01081 01082 //////////////////////////////////////////////////////////////////// 01083 // Function: get_real_compress_plan 01084 // Description: Returns a FFTW plan suitable for compressing a float 01085 // array of the indicated length. 01086 //////////////////////////////////////////////////////////////////// 01087 static rfftw_plan 01088 get_real_compress_plan(int length) { 01089 RealPlans::iterator pi; 01090 pi = _real_compress_plans.find(length); 01091 if (pi != _real_compress_plans.end()) { 01092 return (*pi).second; 01093 } 01094 01095 rfftw_plan plan; 01096 plan = rfftw_create_plan(length, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE); 01097 _real_compress_plans.insert(RealPlans::value_type(length, plan)); 01098 01099 return plan; 01100 } 01101 01102 //////////////////////////////////////////////////////////////////// 01103 // Function: get_real_decompress_plan 01104 // Description: Returns a FFTW plan suitable for decompressing a float 01105 // array of the indicated length. 01106 //////////////////////////////////////////////////////////////////// 01107 static rfftw_plan 01108 get_real_decompress_plan(int length) { 01109 RealPlans::iterator pi; 01110 pi = _real_decompress_plans.find(length); 01111 if (pi != _real_decompress_plans.end()) { 01112 return (*pi).second; 01113 } 01114 01115 rfftw_plan plan; 01116 plan = rfftw_create_plan(length, FFTW_COMPLEX_TO_REAL, FFTW_ESTIMATE); 01117 _real_decompress_plans.insert(RealPlans::value_type(length, plan)); 01118 01119 return plan; 01120 } 01121 01122 #endif