Main Page   Class Hierarchy   Alphabetical List   Compound List   File List   Compound Members   File Members  

panda/src/mathutil/fftCompressor.cxx

Go to the documentation of this file.
00001 // Filename: fftCompressor.cxx
00002 // Created by:  drose (11Dec00)
00003 //
00004 ////////////////////////////////////////////////////////////////////
00005 //
00006 // PANDA 3D SOFTWARE
00007 // Copyright (c) 2001, Disney Enterprises, Inc.  All rights reserved
00008 //
00009 // All use of this software is subject to the terms of the Panda 3d
00010 // Software license.  You should have received a copy of this license
00011 // along with this source code; you will also find a current copy of
00012 // the license at http://www.panda3d.org/license.txt .
00013 //
00014 // To contact the maintainers of this program write to
00015 // panda3d@yahoogroups.com .
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 // These FFTW support objects can only be defined if we actually have
00032 // the FFTW library available.
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 //     Function: FFTCompressor::Constructor
00044 //       Access: Public
00045 //  Description: Constructs a new compressor object with default
00046 //               parameters.
00047 ////////////////////////////////////////////////////////////////////
00048 FFTCompressor::
00049 FFTCompressor() {
00050   set_quality(-1);
00051   _transpose_quats = false;
00052 }
00053 
00054 ////////////////////////////////////////////////////////////////////
00055 //     Function: FFTCompressor::is_compression_available
00056 //       Access: Public, Static
00057 //  Description: Returns true if the FFTW library is compiled in, so
00058 //               that this class is actually capable of doing useful
00059 //               compression/decompression work.  Returns false
00060 //               otherwise, in which case any attempt to write a
00061 //               compressed stream will actually write an uncompressed
00062 //               stream, and any attempt to read a compressed stream
00063 //               will fail.
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 //     Function: FFTCompressor::set_quality
00076 //       Access: Public
00077 //  Description: Sets the quality factor for the compression.  This is
00078 //               an integer in the range 0 - 100 that roughly controls
00079 //               how aggressively the reals are compressed; lower
00080 //               numbers mean smaller output, and more data loss.
00081 //
00082 //               There are a few special cases.  Quality -1 means to
00083 //               use whatever individual parameters are set in the
00084 //               user's Configrc file, rather than the single quality
00085 //               dial.  Quality 101 or higher means to generate
00086 //               lossless output (this is the default if libfftw is
00087 //               not available).
00088 //
00089 //               Quality 102 writes all four components of quaternions
00090 //               to the output file, rather than just three, quality
00091 //               103 converts hpr to matrix (instead of quat) and
00092 //               writes a 9-component matrix, and quality 104 just
00093 //               writes out hpr directly.  Quality levels 102 and
00094 //               greater are strictly for debugging purposes, and are
00095 //               only available if NDEBUG is not defined.
00096 ////////////////////////////////////////////////////////////////////
00097 void FFTCompressor::
00098 set_quality(int quality) {
00099 #ifndef HAVE_FFTW
00100   // If we don't actually have FFTW, we can't really compress anything.
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     // A negative quality indicates we should read the various
00112     // parameters from individual config variables.
00113     _fft_offset = fft_offset;
00114     _fft_factor = fft_factor;
00115     _fft_exponent = fft_exponent;
00116 
00117   } else if (_quality < 40) {
00118     // 0 - 40 :
00119     //   fft-offset 1.0 - 0.001
00120     //   fft-factor 1.0
00121     //   fft-exponent 4.0
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     // 40 - 95:
00130     //   fft-offset 0.001
00131     //   fft-factor 1.0 - 0.1
00132     //   fft-exponent 4.0
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     // 95 - 100:
00141     //   fft-offset 0.001
00142     //   fft-factor 0.1 - 0.0
00143     //   fft-exponent 4.0
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 //     Function: FFTCompressor::get_quality
00155 //       Access: Public
00156 //  Description: Returns the quality number that was previously set
00157 //               via set_quality().
00158 ////////////////////////////////////////////////////////////////////
00159 int FFTCompressor::
00160 get_quality() const {
00161   return _quality;
00162 }
00163 
00164 ////////////////////////////////////////////////////////////////////
00165 //     Function: FFTCompressor::set_transpose_quats
00166 //       Access: Public
00167 //  Description: Sets the transpose_quats flag.  This is provided
00168 //               mainly for backward compatibility with old bam files
00169 //               that were written out with the quaternions
00170 //               inadvertently transposed.
00171 ////////////////////////////////////////////////////////////////////
00172 void FFTCompressor::
00173 set_transpose_quats(bool flag) {
00174   _transpose_quats = flag;
00175 }
00176 
00177 ////////////////////////////////////////////////////////////////////
00178 //     Function: FFTCompressor::get_transpose_quats
00179 //       Access: Public
00180 //  Description: Returns the transpose_quats flag.  See
00181 //               set_transpose_quats().
00182 ////////////////////////////////////////////////////////////////////
00183 bool FFTCompressor::
00184 get_transpose_quats() const {
00185   return _transpose_quats;
00186 }
00187 
00188 ////////////////////////////////////////////////////////////////////
00189 //     Function: FFTCompressor::write_header
00190 //       Access: Public
00191 //  Description: Writes the compression parameters to the indicated
00192 //               datagram.  It is necessary to call this before
00193 //               writing anything else to the datagram, since these
00194 //               parameters will be necessary to correctly decompress
00195 //               the data later.
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 //     Function: FFTCompressor::write_reals
00209 //       Access: Public
00210 //  Description: Writes an array of floating-point numbers to the
00211 //               indicated datagram.
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     // Special case: lossless output.
00219     for (int i = 0; i < length; i++) {
00220       datagram.add_float32(array[i]);
00221     }
00222     return;
00223   }
00224 
00225 #ifndef HAVE_FFTW
00226   // If we don't have FFTW, we shouldn't get here.
00227   nassertv(false);
00228 
00229 #else
00230 
00231   if (length == 0) {
00232     // Special case: do nothing.
00233     return;
00234   }
00235 
00236   if (length == 1) {
00237     // Special case: just write out the one number.
00238     datagram.add_float32(array[0]);
00239     return;
00240   }
00241 
00242   // Normal case: FFT the array, and write that out.
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   // Now encode the numbers, run-length encoded by size, so we only
00255   // write out the number of bits we need for each number.
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     // How many bits do we need to encode this integer?
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     // A special case: if we're writing a string of one-byters and we
00290     // come across a single intervening zero, don't interrupt the run
00291     // just for that.
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       // Now we need to flush the last run.
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 //     Function: FFTCompressor::write_hprs
00315 //       Access: Public
00316 //  Description: Writes an array of HPR angles to the indicated
00317 //               datagram.
00318 ////////////////////////////////////////////////////////////////////
00319 void FFTCompressor::
00320 write_hprs(Datagram &datagram, const LVecBase3f *array, int length) {
00321 #ifndef NDEBUG
00322   if (_quality >= 104) {
00323     // If quality level is at least 104, we don't even convert hpr at
00324     // all.  This is just for debugging.
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     // If quality level is 103, we convert hpr to a table of matrices.
00344     // This is just for debugging.
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   // First, convert the HPR's to quats.  We expect quats to have
00377   // better FFT consistency, and therefore compress better, even
00378   // though they have an extra component.
00379 
00380   // However, because the quaternion will be normalized, we don't even
00381   // have to write out all three components; any three can be used to
00382   // determine the fourth (provided we ensure consistency of sign).
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();  // This may not be necessary, but let's not take chances.
00400 
00401     if (rot.get_r() < 0) {
00402       // Since rot == -rot, we can flip the quarternion if need be to
00403       // keep the r component positive.  This has two advantages.
00404       // One, it makes it possible to infer r completely given i, j,
00405       // and k (since we know it must be >= 0), and two, it helps
00406       // protect against poor continuity caused by inadvertent
00407       // flipping of the quarternion's sign between frames.
00408 
00409       // The choice of leaving r implicit rather than any of the other
00410       // three seems to work the best in terms of guaranteeing
00411       // continuity.
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   // If quality is at least 102, we write all four quat components,
00457   // instead of just the three.  This is just for debugging.
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 //     Function: FFTCompressor::read_header
00470 //       Access: Public
00471 //  Description: Reads the compression header that was written
00472 //               previously.  This fills in the compression parameters
00473 //               necessary to correctly decompress the following data.
00474 //
00475 //               Returns true if the header is read successfully,
00476 //               false otherwise.
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 //     Function: FFTCompressor::read_reals
00508 //       Access: Public
00509 //  Description: Reads an array of floating-point numbers.  The result
00510 //               is pushed onto the end of the indicated vector, which
00511 //               is not cleared first; it is the user's responsibility
00512 //               to ensure that the array is initially empty.  Returns
00513 //               true if the data is read correctly, false if there is
00514 //               an error.
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     // Special case: lossless output.
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   // If we don't have FFTW, we shouldn't get here.
00532   return false;
00533 
00534 #else
00535 
00536   if (length == 0) {
00537     // Special case: do nothing.
00538     return true;
00539   }
00540 
00541   if (length == 1) {
00542     // Special case: just read in the one number.
00543     array.push_back(di.get_float32());
00544     return true;
00545   }
00546 
00547   // Normal case: read in the FFT array, and convert it back to
00548   // (nearly) the original numbers.
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 //     Function: FFTCompressor::read_hprs
00579 //       Access: Public
00580 //  Description: Reads an array of HPR angles.  The result is pushed
00581 //               onto the end of the indicated vector, which is not
00582 //               cleared first; it is the user's responsibility to
00583 //               ensure that the array is initially empty.
00584 ////////////////////////////////////////////////////////////////////
00585 bool FFTCompressor::
00586 read_hprs(DatagramIterator &di, vector_LVecBase3f &array) {
00587 #ifndef NDEBUG
00588   if (_quality >= 104) {
00589     // If quality level is at least 104, we don't even convert hpr to
00590     // quat.  This is just for debugging.
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     // If quality level is 103, we read in a table of 3x3 rotation
00609     // matrices.  This is just for debugging.
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       // Infer the r component from the remaining three.
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         // If we have written out all four components, use them.
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();      // Just for good measure.
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 //     Function: FFTCompressor::free_storage
00711 //       Access: Public, Static
00712 //  Description: Frees memory that has been allocated during past runs
00713 //               of the FFTCompressor.  This is an optional call, but
00714 //               it may be made from time to time to empty the global
00715 //               cache that the compressor objects keep to facilitate
00716 //               fast compression/decompression.
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 //     Function: FFTCompressor::write_run
00740 //       Access: Private
00741 //  Description: Writes a sequence of integers that all require the
00742 //               same number of bits.  Returns the number of integers
00743 //               written, i.e. run.size().
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     // If the width is anything other than RW_double, we write a
00755     // single byte indicating the width and length of the upcoming
00756     // run.
00757 
00758     if (run.size() <= RW_length_mask &&
00759         ((int)run_width | run.size()) != RW_double) {
00760       // If there are enough bits remaining in the byte, use them to
00761       // indicate the length of the run.  We have to be a little
00762       // careful, however, not to accidentally write a byte that looks
00763       // like an RW_double flag.
00764       datagram.add_uint8((int)run_width | run.size());
00765 
00766     } else {
00767       // Otherwise, write zero as the length, to indicate that we'll
00768       // write the actual length in the following 16-bit word.
00769       datagram.add_uint8(run_width);
00770 
00771       // Assuming, of course, that the length fits within 16 bits.
00772       nassertr(run.size() < 65536, 0);
00773       nassertr(run.size() != 0, 0);
00774 
00775       datagram.add_uint16(run.size());
00776     }
00777   }
00778 
00779   // Now write the data itself.
00780   vector_double::const_iterator ri;
00781   switch (run_width) {
00782   case RW_0:
00783     // If it's a string of zeroes, we're done!
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       // In the case of RW_double, we only write the numbers one at a
00807       // time, each time preceded by the RW_double flag.  Hopefully
00808       // this will happen only rarely.
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 //     Function: FFTCompressor::read_run
00823 //       Access: Private
00824 //  Description: Reads a sequence of integers that all require the
00825 //               same number of bits.  Returns the number of integers
00826 //               read.  It is the responsibility of the user to clear
00827 //               the vector before calling this function, or the
00828 //               numbers read will be appended to the end.
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     // RW_double is a special case, and requires the whole byte.  In
00838     // this case, we don't encode a length, but assume it's only one.
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     // If the length was zero, it means the actual length follows as a
00849     // 16-bit word.
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 //     Function: FFTCompressor::get_scale_factor
00897 //       Access: Private
00898 //  Description: Returns the appropriate scaling for the given
00899 //               position within the halfcomplex array.
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 //     Function: FFTCompressor::interpolate
00913 //       Access: Private, Static
00914 //  Description: Returns a number between a and b, inclusive,
00915 //               according to the value of t between 0 and 1,
00916 //               inclusive.
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 //     Function: get_real_compress_plan
00928 //  Description: Returns a FFTW plan suitable for compressing a float
00929 //               array of the indicated length.
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 //     Function: get_real_decompress_plan
00948 //  Description: Returns a FFTW plan suitable for decompressing a float
00949 //               array of the indicated length.
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

Generated on Fri May 2 00:40:26 2003 for Panda by doxygen1.3