Panda3D
 All Classes Functions Variables Enumerations
fftCompressor.cxx
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
 All Classes Functions Variables Enumerations