Panda3D
fftCompressor.cxx
1 // Filename: fftCompressor.cxx
2 // Created by: drose (11Dec00)
3 //
4 ////////////////////////////////////////////////////////////////////
5 //
6 // PANDA 3D SOFTWARE
7 // Copyright (c) Carnegie Mellon University. All rights reserved.
8 //
9 // All use of this software is subject to the terms of the revised BSD
10 // license. You should have received a copy of this license along
11 // with this source code in a file named "LICENSE."
12 //
13 ////////////////////////////////////////////////////////////////////
14 
15 #include "fftCompressor.h"
16 #include "config_mathutil.h"
17 #include "config_linmath.h"
18 
19 #include "datagram.h"
20 #include "datagramIterator.h"
21 #include "compose_matrix.h"
22 #include "pmap.h"
23 #include <math.h>
24 
25 #ifdef HAVE_FFTW
26 
27 // hack.....
28 // this is a hack to help interrogate sort out a macro
29 // in the system poll and select definitions
30 //
31 #ifdef howmany
32 #undef howmany
33 #endif
34 
35 #ifdef PHAVE_DRFFTW_H
36  #include "drfftw.h"
37 #else
38  #include "rfftw.h"
39 #endif
40 
41 // These FFTW support objects can only be defined if we actually have
42 // the FFTW library available.
43 static rfftw_plan get_real_compress_plan(int length);
44 static rfftw_plan get_real_decompress_plan(int length);
45 
46 typedef pmap<int, rfftw_plan> RealPlans;
47 static RealPlans _real_compress_plans;
48 static RealPlans _real_decompress_plans;
49 
50 #endif
51 
52 ////////////////////////////////////////////////////////////////////
53 // Function: FFTCompressor::Constructor
54 // Access: Public
55 // Description: Constructs a new compressor object with default
56 // parameters.
57 ////////////////////////////////////////////////////////////////////
60  _bam_minor_version = 0;
61  set_quality(-1);
62  _use_error_threshold = false;
63  _transpose_quats = false;
64 }
65 
66 ////////////////////////////////////////////////////////////////////
67 // Function: FFTCompressor::is_compression_available
68 // Access: Public, Static
69 // Description: Returns true if the FFTW library is compiled in, so
70 // that this class is actually capable of doing useful
71 // compression/decompression work. Returns false
72 // otherwise, in which case any attempt to write a
73 // compressed stream will actually write an uncompressed
74 // stream, and any attempt to read a compressed stream
75 // will fail.
76 ////////////////////////////////////////////////////////////////////
77 bool FFTCompressor::
79 #ifndef HAVE_FFTW
80  return false;
81 #else
82  return true;
83 #endif
84 }
85 
86 ////////////////////////////////////////////////////////////////////
87 // Function: FFTCompressor::set_quality
88 // Access: Public
89 // Description: Sets the quality factor for the compression. This is
90 // an integer in the range 0 - 100 that roughly controls
91 // how aggressively the reals are compressed; lower
92 // numbers mean smaller output, and more data loss.
93 //
94 // There are a few special cases. Quality -1 means to
95 // use whatever individual parameters are set in the
96 // user's Configrc file, rather than the single quality
97 // dial. Quality 101 or higher means to generate
98 // lossless output (this is the default if libfftw is
99 // not available).
100 //
101 // Quality 102 writes all four components of quaternions
102 // to the output file, rather than just three, quality
103 // 103 converts hpr to matrix (instead of quat) and
104 // writes a 9-component matrix, and quality 104 just
105 // writes out hpr directly. Quality levels 102 and
106 // greater are strictly for debugging purposes, and are
107 // only available if NDEBUG is not defined.
108 ////////////////////////////////////////////////////////////////////
109 void FFTCompressor::
110 set_quality(int quality) {
111 #ifndef HAVE_FFTW
112  // If we don't actually have FFTW, we can't really compress anything.
113  if (_quality <= 100) {
114  mathutil_cat.warning()
115  << "FFTW library is not available; generating uncompressed output.\n";
116  }
117  _quality = 101;
118 
119 #else
120  _quality = quality;
121 
122  if (_quality < 0) {
123  // A negative quality indicates we should read the various
124  // parameters from individual config variables.
125  _fft_offset = fft_offset;
126  _fft_factor = fft_factor;
127  _fft_exponent = fft_exponent;
128 
129  } else if (_quality < 40) {
130  // 0 - 40 :
131  // fft-offset 1.0 - 0.001
132  // fft-factor 1.0
133  // fft-exponent 4.0
134 
135  double t = (double)_quality / 40.0;
136  _fft_offset = interpolate(t, 1.0, 0.001);
137  _fft_factor = 1.0;
138  _fft_exponent = 4.0;
139 
140  } else if (_quality < 95) {
141  // 40 - 95:
142  // fft-offset 0.001
143  // fft-factor 1.0 - 0.1
144  // fft-exponent 4.0
145 
146  double t = (double)(_quality - 40) / 55.0;
147  _fft_offset = 0.001;
148  _fft_factor = interpolate(t, 1.0, 0.1);
149  _fft_exponent = 4.0;
150 
151  } else {
152  // 95 - 100:
153  // fft-offset 0.001
154  // fft-factor 0.1 - 0.0
155  // fft-exponent 4.0
156 
157  double t = (double)(_quality - 95) / 5.0;
158  _fft_offset = 0.001;
159  _fft_factor = interpolate(t, 0.1, 0.0);
160  _fft_exponent = 4.0;
161  }
162 #endif
163 }
164 
165 ////////////////////////////////////////////////////////////////////
166 // Function: FFTCompressor::get_quality
167 // Access: Public
168 // Description: Returns the quality number that was previously set
169 // via set_quality().
170 ////////////////////////////////////////////////////////////////////
171 int FFTCompressor::
172 get_quality() const {
173  return _quality;
174 }
175 
176 ////////////////////////////////////////////////////////////////////
177 // Function: FFTCompressor::set_use_error_threshold
178 // Access: Public
179 // Description: Enables or disables the use of the error threshold
180 // measurement to put a cap on the amount of damage done
181 // by lossy compression. When this is enabled, the
182 // potential results of the compression are analyzed
183 // before the data is written; if it is determined that
184 // the compression will damage a particular string of
185 // reals too much, that particular string of reals is
186 // written uncompressed.
187 ////////////////////////////////////////////////////////////////////
188 void FFTCompressor::
189 set_use_error_threshold(bool use_error_threshold) {
190  _use_error_threshold = use_error_threshold;
191 }
192 
193 ////////////////////////////////////////////////////////////////////
194 // Function: FFTCompressor::get_use_error_threshold
195 // Access: Public
196 // Description: Returns whether the error threshold measurement is
197 // enabled. See set_use_error_threshold().
198 ////////////////////////////////////////////////////////////////////
199 bool FFTCompressor::
201  return _use_error_threshold;
202 }
203 
204 ////////////////////////////////////////////////////////////////////
205 // Function: FFTCompressor::set_transpose_quats
206 // Access: Public
207 // Description: Sets the transpose_quats flag. This is provided
208 // mainly for backward compatibility with old bam files
209 // that were written out with the quaternions
210 // inadvertently transposed.
211 ////////////////////////////////////////////////////////////////////
212 void FFTCompressor::
214  _transpose_quats = flag;
215 }
216 
217 ////////////////////////////////////////////////////////////////////
218 // Function: FFTCompressor::get_transpose_quats
219 // Access: Public
220 // Description: Returns the transpose_quats flag. See
221 // set_transpose_quats().
222 ////////////////////////////////////////////////////////////////////
223 bool FFTCompressor::
225  return _transpose_quats;
226 }
227 
228 ////////////////////////////////////////////////////////////////////
229 // Function: FFTCompressor::write_header
230 // Access: Public
231 // Description: Writes the compression parameters to the indicated
232 // datagram. It is necessary to call this before
233 // writing anything else to the datagram, since these
234 // parameters will be necessary to correctly decompress
235 // the data later.
236 ////////////////////////////////////////////////////////////////////
237 void FFTCompressor::
238 write_header(Datagram &datagram) {
239  datagram.add_int8(_quality);
240  if (_quality < 0) {
241  datagram.add_float64(_fft_offset);
242  datagram.add_float64(_fft_factor);
243  datagram.add_float64(_fft_exponent);
244  }
245 }
246 
247 ////////////////////////////////////////////////////////////////////
248 // Function: FFTCompressor::write_reals
249 // Access: Public
250 // Description: Writes an array of floating-point numbers to the
251 // indicated datagram.
252 ////////////////////////////////////////////////////////////////////
253 void FFTCompressor::
254 write_reals(Datagram &datagram, const PN_stdfloat *array, int length) {
255  datagram.add_int32(length);
256 
257  if (_quality > 100) {
258  // Special case: lossless output.
259  for (int i = 0; i < length; i++) {
260  datagram.add_stdfloat(array[i]);
261  }
262  return;
263  }
264 
265 #ifndef HAVE_FFTW
266  // If we don't have FFTW, we shouldn't get here.
267  nassertv(false);
268 
269 #else
270 
271  if (length == 0) {
272  // Special case: do nothing.
273  return;
274  }
275 
276  if (length == 1) {
277  // Special case: just write out the one number.
278  datagram.add_stdfloat(array[0]);
279  return;
280  }
281 
282  // Normal case: FFT the array, and write that out.
283 
284  // First, check the compressability.
285  bool reject_compression = false;
286 
287  // This logic needs a closer examination. Not sure it's useful
288  // as-is.
289  /*
290  if (_use_error_threshold) {
291  // Don't encode the data if it moves too erratically.
292  PN_stdfloat error = get_compressability(array, length);
293  if (error > fft_error_threshold) {
294  // No good: the data probably won't compress well. Just write
295  // out lossless data.
296  reject_compression = true;
297  }
298  }
299  */
300 
301  datagram.add_bool(reject_compression);
302  if (reject_compression) {
303  if (mathutil_cat.is_debug()) {
304  mathutil_cat.debug()
305  << "Writing stream of " << length << " numbers uncompressed.\n";
306  }
307  for (int i = 0; i < length; i++) {
308  datagram.add_stdfloat(array[i]);
309  }
310  return;
311  }
312 
313  // Now generate the Fourier transform.
314  double *data = (double *)alloca(length * sizeof(double));
315  int i;
316  for (i = 0; i < length; i++) {
317  data[i] = array[i];
318  }
319 
320  double *half_complex = (double *)alloca(length * sizeof(double));
321 
322  rfftw_plan plan = get_real_compress_plan(length);
323  rfftw_one(plan, data, half_complex);
324 
325 
326  // Now encode the numbers, run-length encoded by size, so we only
327  // write out the number of bits we need for each number.
328 
329  vector_double run;
330  RunWidth run_width = RW_invalid;
331  int num_written = 0;
332 
333  for (i = 0; i < length; i++) {
334  static const double max_range_32 = 2147483647.0;
335  static const double max_range_16 = 32767.0;
336  static const double max_range_8 = 127.0;
337 
338  double scale_factor = get_scale_factor(i, length);
339  double num = cfloor(half_complex[i] / scale_factor + 0.5);
340 
341  // How many bits do we need to encode this integer?
342  double a = fabs(num);
343  RunWidth num_width;
344 
345  if (a == 0.0) {
346  num_width = RW_0;
347 
348  } else if (a <= max_range_8) {
349  num_width = RW_8;
350 
351  } else if (a <= max_range_16) {
352  num_width = RW_16;
353 
354  } else if (a <= max_range_32) {
355  num_width = RW_32;
356 
357  } else {
358  num_width = RW_double;
359  }
360 
361  // A special case: if we're writing a string of one-byters and we
362  // come across a single intervening zero, don't interrupt the run
363  // just for that.
364  if (run_width == RW_8 && num_width == RW_0) {
365  if (i + 1 >= length || half_complex[i + 1] != 0.0) {
366  num_width = RW_8;
367  }
368  }
369 
370  if (num_width != run_width) {
371  // Now we need to flush the last run.
372  num_written += write_run(datagram, run_width, run);
373  run.clear();
374  run_width = num_width;
375  }
376 
377  run.push_back(num);
378  }
379 
380  num_written += write_run(datagram, run_width, run);
381  nassertv(num_written == length);
382 #endif
383 }
384 
385 ////////////////////////////////////////////////////////////////////
386 // Function: FFTCompressor::write_hprs
387 // Access: Public
388 // Description: Writes an array of HPR angles to the indicated
389 // datagram.
390 ////////////////////////////////////////////////////////////////////
391 void FFTCompressor::
392 write_hprs(Datagram &datagram, const LVecBase3 *array, int length) {
393 #ifndef NDEBUG
394  if (_quality >= 104) {
395  // If quality level is at least 104, we don't even convert hpr at
396  // all. This is just for debugging.
397  vector_stdfloat h, p, r;
398 
399  h.reserve(length);
400  p.reserve(length);
401  r.reserve(length);
402 
403  for (int i = 0; i < length; i++) {
404  h.push_back(array[i][0]);
405  p.push_back(array[i][1]);
406  r.push_back(array[i][2]);
407  }
408 
409  if (length == 0) {
410  write_reals(datagram, NULL, length);
411  write_reals(datagram, NULL, length);
412  write_reals(datagram, NULL, length);
413  } else {
414  write_reals(datagram, &h[0], length);
415  write_reals(datagram, &p[0], length);
416  write_reals(datagram, &r[0], length);
417  }
418  return;
419  }
420  if (_quality >= 103) {
421  // If quality level is 103, we convert hpr to a table of matrices.
422  // This is just for debugging.
423  vector_stdfloat
424  m00, m01, m02,
425  m10, m11, m12,
426  m20, m21, m22;
427  for (int i = 0; i < length; i++) {
428  LMatrix3 mat;
429  compose_matrix(mat, LVecBase3(1.0, 1.0, 1.0), LVecBase3(0.0, 0.0, 0.0),
430  array[i]);
431  m00.push_back(mat(0, 0));
432  m01.push_back(mat(0, 1));
433  m02.push_back(mat(0, 2));
434  m10.push_back(mat(1, 0));
435  m11.push_back(mat(1, 1));
436  m12.push_back(mat(1, 2));
437  m20.push_back(mat(2, 0));
438  m21.push_back(mat(2, 1));
439  m22.push_back(mat(2, 2));
440  }
441 
442  if (length == 0) {
443  write_reals(datagram, NULL, length);
444  write_reals(datagram, NULL, length);
445  write_reals(datagram, NULL, length);
446  write_reals(datagram, NULL, length);
447  write_reals(datagram, NULL, length);
448  write_reals(datagram, NULL, length);
449  write_reals(datagram, NULL, length);
450  write_reals(datagram, NULL, length);
451  write_reals(datagram, NULL, length);
452  } else {
453  write_reals(datagram, &m00[0], length);
454  write_reals(datagram, &m01[0], length);
455  write_reals(datagram, &m02[0], length);
456  write_reals(datagram, &m10[0], length);
457  write_reals(datagram, &m11[0], length);
458  write_reals(datagram, &m12[0], length);
459  write_reals(datagram, &m20[0], length);
460  write_reals(datagram, &m21[0], length);
461  write_reals(datagram, &m22[0], length);
462  }
463  return;
464  }
465 #endif
466 
467  // First, convert the HPR's to quats. We expect quats to have
468  // better FFT consistency, and therefore compress better, even
469  // though they have an extra component.
470 
471  // However, because the quaternion will be normalized, we don't even
472  // have to write out all three components; any three can be used to
473  // determine the fourth (provided we ensure consistency of sign).
474 
475  vector_stdfloat qr, qi, qj, qk;
476 
477  qr.reserve(length);
478  qi.reserve(length);
479  qj.reserve(length);
480  qk.reserve(length);
481 
482  for (int i = 0; i < length; i++) {
483  LMatrix3 mat;
484  compose_matrix(mat, LVecBase3(1.0, 1.0, 1.0), LVecBase3(0.0, 0.0, 0.0),
485  array[i]);
486  if (_transpose_quats) {
487  mat.transpose_in_place();
488  }
489 
490  LOrientation rot(mat);
491  rot.normalize(); // This may not be necessary, but let's not take chances.
492 
493  if (rot.get_r() < 0) {
494  // Since rot == -rot, we can flip the quarternion if need be to
495  // keep the r component positive. This has two advantages.
496  // One, it makes it possible to infer r completely given i, j,
497  // and k (since we know it must be >= 0), and two, it helps
498  // protect against poor continuity caused by inadvertent
499  // flipping of the quarternion's sign between frames.
500 
501  // The choice of leaving r implicit rather than any of the other
502  // three seems to work the best in terms of guaranteeing
503  // continuity.
504  rot.set(-rot.get_r(), -rot.get_i(), -rot.get_j(), -rot.get_k());
505  }
506 
507 #ifdef NOTIFY_DEBUG
508  if (mathutil_cat.is_warning()) {
509  LMatrix3 mat2;
510  rot.extract_to_matrix(mat2);
511  if (!mat.almost_equal(mat2, 0.0001)) {
512  LVecBase3 hpr1, hpr2;
513  LVecBase3 scale, shear;
514  decompose_matrix(mat, scale, shear, hpr1);
515  decompose_matrix(mat2, scale, shear, hpr2);
516  mathutil_cat.warning()
517  << "Converted hpr to quaternion incorrectly!\n"
518  << " Source hpr: " << array[i] << ", or " << hpr1 << "\n";
519  mathutil_cat.warning(false)
520  << " Quaternion: " << rot << "\n"
521  << " Which represents: hpr " << hpr2 << " scale "
522  << scale << "\n";
523  }
524  }
525 #endif
526 
527  qr.push_back(rot.get_r());
528  qi.push_back(rot.get_i());
529  qj.push_back(rot.get_j());
530  qk.push_back(rot.get_k());
531  }
532 
533  // If quality is at least 102, we write all four quat components,
534  // instead of just the three. This is just for debugging.
535 #ifndef NDEBUG
536  if (_quality >= 102) {
537  if (length == 0) {
538  write_reals(datagram, NULL, length);
539  } else {
540  write_reals(datagram, &qr[0], length);
541  }
542  }
543 #endif
544  if (length == 0) {
545  write_reals(datagram, NULL, length);
546  write_reals(datagram, NULL, length);
547  write_reals(datagram, NULL, length);
548  } else {
549  write_reals(datagram, &qi[0], length);
550  write_reals(datagram, &qj[0], length);
551  write_reals(datagram, &qk[0], length);
552  }
553 }
554 
555 ////////////////////////////////////////////////////////////////////
556 // Function: FFTCompressor::read_header
557 // Access: Public
558 // Description: Reads the compression header that was written
559 // previously. This fills in the compression parameters
560 // necessary to correctly decompress the following data.
561 //
562 // Returns true if the header is read successfully,
563 // false otherwise.
564 ////////////////////////////////////////////////////////////////////
565 bool FFTCompressor::
566 read_header(DatagramIterator &di, int bam_minor_version) {
567  _bam_minor_version = bam_minor_version;
568  _quality = di.get_int8();
569 
570  if (mathutil_cat.is_debug()) {
571  mathutil_cat.debug()
572  << "Found compressed data at quality level " << _quality << "\n";
573  }
574 
575 #ifndef HAVE_FFTW
576  if (_quality <= 100) {
577  mathutil_cat.error()
578  << "FFTW library is not available; cannot read compressed data.\n";
579  return false;
580  }
581 #endif
582 
583  set_quality(_quality);
584 
585  if (_quality < 0) {
586  _fft_offset = di.get_float64();
587  _fft_factor = di.get_float64();
588  _fft_exponent = di.get_float64();
589  }
590 
591  return true;
592 }
593 
594 ////////////////////////////////////////////////////////////////////
595 // Function: FFTCompressor::read_reals
596 // Access: Public
597 // Description: Reads an array of floating-point numbers. The result
598 // is pushed onto the end of the indicated vector, which
599 // is not cleared first; it is the user's responsibility
600 // to ensure that the array is initially empty. Returns
601 // true if the data is read correctly, false if there is
602 // an error.
603 ////////////////////////////////////////////////////////////////////
604 bool FFTCompressor::
605 read_reals(DatagramIterator &di, vector_stdfloat &array) {
606  int length = di.get_int32();
607 
608  if (_quality > 100) {
609  array.reserve(array.size() + length);
610 
611  // Special case: lossless output.
612  for (int i = 0; i < length; i++) {
613  array.push_back(di.get_stdfloat());
614  }
615  return true;
616  }
617 
618 #ifndef HAVE_FFTW
619  // If we don't have FFTW, we shouldn't get here.
620  return false;
621 
622 #else
623 
624  if (length == 0) {
625  // Special case: do nothing.
626  return true;
627  }
628 
629  if (length == 1) {
630  // Special case: just read in the one number.
631  array.push_back(di.get_stdfloat());
632  return true;
633  }
634 
635  // Normal case: read in the FFT array, and convert it back to
636  // (nearly) the original numbers.
637 
638  // First, check the reject_compression flag. If it's set, we
639  // decided to just write out the stream uncompressed.
640  bool reject_compression = di.get_bool();
641  if (reject_compression) {
642  array.reserve(array.size() + length);
643  for (int i = 0; i < length; i++) {
644  array.push_back(di.get_stdfloat());
645  }
646  return true;
647  }
648 
649  vector_double half_complex;
650  half_complex.reserve(length);
651  int num_read = 0;
652  while (num_read < length) {
653  num_read += read_run(di, half_complex);
654  }
655  nassertr(num_read == length, false);
656  nassertr((int)half_complex.size() == length, false);
657 
658  int i;
659  for (i = 0; i < length; i++) {
660  half_complex[i] *= get_scale_factor(i, length);
661  }
662 
663  double *data = (double *)alloca(length * sizeof(double));
664  rfftw_plan plan = get_real_decompress_plan(length);
665  rfftw_one(plan, &half_complex[0], data);
666 
667  double scale = 1.0 / (double)length;
668  array.reserve(array.size() + length);
669  for (i = 0; i < length; i++) {
670  array.push_back(data[i] * scale);
671  }
672 
673  return true;
674 #endif
675 }
676 
677 ////////////////////////////////////////////////////////////////////
678 // Function: FFTCompressor::read_hprs
679 // Access: Public
680 // Description: Reads an array of HPR angles. The result is pushed
681 // onto the end of the indicated vector, which is not
682 // cleared first; it is the user's responsibility to
683 // ensure that the array is initially empty.
684 //
685 // new_hpr is a temporary, transitional parameter. If
686 // it is set false, the hprs are decompressed according
687 // to the old, broken hpr calculation; if true, the hprs
688 // are decompressed according to the new, correct hpr
689 // calculation. See temp_hpr_fix.
690 ////////////////////////////////////////////////////////////////////
691 bool FFTCompressor::
692 read_hprs(DatagramIterator &di, pvector<LVecBase3> &array, bool new_hpr) {
693 #ifndef NDEBUG
694  if (_quality >= 104) {
695  // If quality level is at least 104, we don't even convert hpr to
696  // quat. This is just for debugging.
697  vector_stdfloat h, p, r;
698  bool okflag = true;
699  okflag =
700  read_reals(di, h) &&
701  read_reals(di, p) &&
702  read_reals(di, r);
703 
704  if (okflag) {
705  nassertr(h.size() == p.size() && p.size() == r.size(), false);
706  for (int i = 0; i < (int)h.size(); i++) {
707  array.push_back(LVecBase3(h[i], p[i], r[i]));
708  }
709  }
710 
711  return okflag;
712  }
713  if (_quality >= 103) {
714  // If quality level is 103, we read in a table of 3x3 rotation
715  // matrices. This is just for debugging.
716  vector_stdfloat
717  m00, m01, m02,
718  m10, m11, m12,
719  m20, m21, m22;
720  bool okflag = true;
721  okflag =
722  read_reals(di, m00) &&
723  read_reals(di, m01) &&
724  read_reals(di, m02) &&
725  read_reals(di, m10) &&
726  read_reals(di, m11) &&
727  read_reals(di, m12) &&
728  read_reals(di, m20) &&
729  read_reals(di, m21) &&
730  read_reals(di, m22);
731 
732  if (okflag) {
733  for (int i = 0; i < (int)m00.size(); i++) {
734  LMatrix3 mat(m00[i], m01[i], m02[i],
735  m10[i], m11[i], m12[i],
736  m20[i], m21[i], m22[i]);
737  LVecBase3 scale, shear, hpr;
738  if (new_hpr) {
739  decompose_matrix_new_hpr(mat, scale, shear, hpr);
740  } else {
741  decompose_matrix_old_hpr(mat, scale, shear, hpr);
742  }
743  array.push_back(hpr);
744  }
745  }
746 
747  return okflag;
748  }
749 #endif
750 
751  vector_stdfloat qr, qi, qj, qk;
752 
753  bool okflag = true;
754 
755 #ifndef NDEBUG
756  if (_quality >= 102) {
757  okflag = read_reals(di, qr);
758  }
759 #endif
760 
761  okflag =
762  okflag &&
763  read_reals(di, qi) &&
764  read_reals(di, qj) &&
765  read_reals(di, qk);
766 
767  if (okflag) {
768  nassertr(qi.size() == qj.size() && qj.size() == qk.size(), false);
769 
770  array.reserve(array.size() + qi.size());
771  for (int i = 0; i < (int)qi.size(); i++) {
772  LOrientation rot;
773 
774  // Infer the r component from the remaining three.
775  PN_stdfloat qr2 = 1.0 - (qi[i] * qi[i] + qj[i] * qj[i] + qk[i] * qk[i]);
776  PN_stdfloat qr1 = qr2 < 0.0 ? 0.0 : sqrtf(qr2);
777 
778  rot.set(qr1, qi[i], qj[i], qk[i]);
779 
780 #ifndef NDEBUG
781  if (_quality >= 102) {
782  // If we have written out all four components, use them.
783  rot[0] = qr[i];
784 
785  if (!IS_THRESHOLD_EQUAL(qr[i], qr1, 0.001)) {
786  mathutil_cat.warning()
787  << "qr[" << i << "] = " << qr[i] << ", qr1 = " << qr1
788  << ", diff is " << qr1 - qr[i] << "\n";
789  }
790  } else
791 #endif
792 
793  rot.normalize(); // Just for good measure.
794 
795  LMatrix3 mat;
796  rot.extract_to_matrix(mat);
797  if (_transpose_quats) {
798  mat.transpose_in_place();
799  }
800  LVecBase3 scale, shear, hpr;
801  if (new_hpr) {
802  decompose_matrix_new_hpr(mat, scale, shear, hpr);
803  } else {
804  decompose_matrix_old_hpr(mat, scale, shear, hpr);
805  }
806  array.push_back(hpr);
807  }
808  }
809 
810  return okflag;
811 }
812 
813 ////////////////////////////////////////////////////////////////////
814 // Function: FFTCompressor::read_hprs
815 // Access: Public
816 // Description: Reads an array of HPR angles. The result is pushed
817 // onto the end of the indicated vector, which is not
818 // cleared first; it is the user's responsibility to
819 // ensure that the array is initially empty.
820 ////////////////////////////////////////////////////////////////////
821 bool FFTCompressor::
823  return read_hprs(di, array, temp_hpr_fix);
824 }
825 
826 
827 ////////////////////////////////////////////////////////////////////
828 // Function: FFTCompressor::free_storage
829 // Access: Public, Static
830 // Description: Frees memory that has been allocated during past runs
831 // of the FFTCompressor. This is an optional call, but
832 // it may be made from time to time to empty the global
833 // cache that the compressor objects keep to facilitate
834 // fast compression/decompression.
835 ////////////////////////////////////////////////////////////////////
836 void FFTCompressor::
838 #ifdef HAVE_FFTW
839  RealPlans::iterator pi;
840  for (pi = _real_compress_plans.begin();
841  pi != _real_compress_plans.end();
842  ++pi) {
843  rfftw_destroy_plan((*pi).second);
844  }
845  _real_compress_plans.clear();
846 
847  for (pi = _real_decompress_plans.begin();
848  pi != _real_decompress_plans.end();
849  ++pi) {
850  rfftw_destroy_plan((*pi).second);
851  }
852  _real_decompress_plans.clear();
853 #endif
854 }
855 
856 ////////////////////////////////////////////////////////////////////
857 // Function: FFTCompressor::write_run
858 // Access: Private
859 // Description: Writes a sequence of integers that all require the
860 // same number of bits. Returns the number of integers
861 // written, i.e. run.size().
862 ////////////////////////////////////////////////////////////////////
863 int FFTCompressor::
864 write_run(Datagram &datagram, FFTCompressor::RunWidth run_width,
865  const vector_double &run) {
866  if (run.empty()) {
867  return 0;
868  }
869  nassertr(run_width != RW_invalid, 0);
870 
871  if (run_width != RW_double) {
872  // If the width is anything other than RW_double, we write a
873  // single byte indicating the width and length of the upcoming
874  // run.
875 
876  if (run.size() <= RW_length_mask &&
877  ((int)run_width | run.size()) != RW_double) {
878  // If there are enough bits remaining in the byte, use them to
879  // indicate the length of the run. We have to be a little
880  // careful, however, not to accidentally write a byte that looks
881  // like an RW_double flag.
882  datagram.add_uint8((int)run_width | run.size());
883 
884  } else {
885  // Otherwise, write zero as the length, to indicate that we'll
886  // write the actual length in the following 16-bit word.
887  datagram.add_uint8(run_width);
888 
889  // Assuming, of course, that the length fits within 16 bits.
890  nassertr(run.size() < 65536, 0);
891  nassertr(run.size() != 0, 0);
892 
893  datagram.add_uint16(run.size());
894  }
895  }
896 
897  // Now write the data itself.
898  vector_double::const_iterator ri;
899  switch (run_width) {
900  case RW_0:
901  // If it's a string of zeroes, we're done!
902  break;
903 
904  case RW_8:
905  for (ri = run.begin(); ri != run.end(); ++ri) {
906  datagram.add_int8((int)*ri);
907  }
908  break;
909 
910  case RW_16:
911  for (ri = run.begin(); ri != run.end(); ++ri) {
912  datagram.add_int16((int)*ri);
913  }
914  break;
915 
916  case RW_32:
917  for (ri = run.begin(); ri != run.end(); ++ri) {
918  datagram.add_int32((int)*ri);
919  }
920  break;
921 
922  case RW_double:
923  for (ri = run.begin(); ri != run.end(); ++ri) {
924  // In the case of RW_double, we only write the numbers one at a
925  // time, each time preceded by the RW_double flag. Hopefully
926  // this will happen only rarely.
927  datagram.add_int8((PN_int8)RW_double);
928  datagram.add_float64(*ri);
929  }
930  break;
931 
932  default:
933  break;
934  }
935 
936  return run.size();
937 }
938 
939 ////////////////////////////////////////////////////////////////////
940 // Function: FFTCompressor::read_run
941 // Access: Private
942 // Description: Reads a sequence of integers that all require the
943 // same number of bits. Returns the number of integers
944 // read. It is the responsibility of the user to clear
945 // the vector before calling this function, or the
946 // numbers read will be appended to the end.
947 ////////////////////////////////////////////////////////////////////
948 int FFTCompressor::
949 read_run(DatagramIterator &di, vector_double &run) {
950  PN_uint8 start = di.get_uint8();
951  RunWidth run_width;
952  int length;
953 
954  if ((start & 0xff) == RW_double) {
955  // RW_double is a special case, and requires the whole byte. In
956  // this case, we don't encode a length, but assume it's only one.
957  run_width = RW_double;
958  length = 1;
959 
960  } else {
961  run_width = (RunWidth)(start & RW_width_mask);
962  length = start & RW_length_mask;
963  }
964 
965  if (length == 0) {
966  // If the length was zero, it means the actual length follows as a
967  // 16-bit word.
968  length = di.get_uint16();
969  }
970  nassertr(length != 0, 0);
971 
972  run.reserve(run.size() + length);
973 
974  int i;
975  switch (run_width) {
976  case RW_0:
977  for (i = 0; i < length; i++) {
978  run.push_back(0.0);
979  }
980  break;
981 
982  case RW_8:
983  for (i = 0; i < length; i++) {
984  run.push_back((double)(int)di.get_int8());
985  }
986  break;
987 
988  case RW_16:
989  for (i = 0; i < length; i++) {
990  run.push_back((double)(int)di.get_int16());
991  }
992  break;
993 
994  case RW_32:
995  for (i = 0; i < length; i++) {
996  run.push_back((double)(int)di.get_int32());
997  }
998  break;
999 
1000  case RW_double:
1001  for (i = 0; i < length; i++) {
1002  run.push_back(di.get_float64());
1003  }
1004  break;
1005 
1006  default:
1007  break;
1008  }
1009 
1010  return length;
1011 }
1012 
1013 ////////////////////////////////////////////////////////////////////
1014 // Function: FFTCompressor::get_scale_factor
1015 // Access: Private
1016 // Description: Returns the appropriate scaling for the given
1017 // position within the halfcomplex array.
1018 ////////////////////////////////////////////////////////////////////
1019 double FFTCompressor::
1020 get_scale_factor(int i, int length) const {
1021  int m = (length / 2) + 1;
1022  int k = (i < m) ? i : length - i;
1023  nassertr(k >= 0 && k < m, 1.0);
1024 
1025  return _fft_offset +
1026  _fft_factor * pow((double)(m-1 - k) / (double)(m-1), _fft_exponent);
1027 }
1028 
1029 ////////////////////////////////////////////////////////////////////
1030 // Function: FFTCompressor::interpolate
1031 // Access: Private, Static
1032 // Description: Returns a number between a and b, inclusive,
1033 // according to the value of t between 0 and 1,
1034 // inclusive.
1035 ////////////////////////////////////////////////////////////////////
1036 double FFTCompressor::
1037 interpolate(double t, double a, double b) {
1038  return a + t * (b - a);
1039 }
1040 
1041 ////////////////////////////////////////////////////////////////////
1042 // Function: FFTCompressor::get_compressability
1043 // Access: Private
1044 // Description: Returns a factor that indicates how erratically the
1045 // values are changing. The lower the result, the
1046 // calmer the numbers, and the greater its likelihood of
1047 // being successfully compressed.
1048 ////////////////////////////////////////////////////////////////////
1049 PN_stdfloat FFTCompressor::
1050 get_compressability(const PN_stdfloat *data, int length) const {
1051  // The result returned is actually the standard deviation of the
1052  // table of deltas between consecutive frames. This number is
1053  // larger if the frames have wildly different values.
1054 
1055  if (length <= 2) {
1056  return 0.0;
1057  }
1058 
1059  PN_stdfloat sum = 0.0;
1060  PN_stdfloat sum2 = 0.0;
1061  for (int i = 1; i < length; i++) {
1062  PN_stdfloat delta = data[i] - data[i - 1];
1063 
1064  sum += delta;
1065  sum2 += delta * delta;
1066  }
1067  PN_stdfloat variance = (sum2 - (sum * sum) / (length - 1)) / (length - 2);
1068  if (variance < 0.0) {
1069  // This can only happen due to tiny roundoff error.
1070  return 0.0;
1071  }
1072 
1073  PN_stdfloat std_deviation = csqrt(variance);
1074 
1075  return std_deviation;
1076 }
1077 
1078 
1079 
1080 #ifdef HAVE_FFTW
1081 
1082 ////////////////////////////////////////////////////////////////////
1083 // Function: get_real_compress_plan
1084 // Description: Returns a FFTW plan suitable for compressing a float
1085 // array of the indicated length.
1086 ////////////////////////////////////////////////////////////////////
1087 static rfftw_plan
1088 get_real_compress_plan(int length) {
1089  RealPlans::iterator pi;
1090  pi = _real_compress_plans.find(length);
1091  if (pi != _real_compress_plans.end()) {
1092  return (*pi).second;
1093  }
1094 
1095  rfftw_plan plan;
1096  plan = rfftw_create_plan(length, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE);
1097  _real_compress_plans.insert(RealPlans::value_type(length, plan));
1098 
1099  return plan;
1100 }
1101 
1102 ////////////////////////////////////////////////////////////////////
1103 // Function: get_real_decompress_plan
1104 // Description: Returns a FFTW plan suitable for decompressing a float
1105 // array of the indicated length.
1106 ////////////////////////////////////////////////////////////////////
1107 static rfftw_plan
1108 get_real_decompress_plan(int length) {
1109  RealPlans::iterator pi;
1110  pi = _real_decompress_plans.find(length);
1111  if (pi != _real_decompress_plans.end()) {
1112  return (*pi).second;
1113  }
1114 
1115  rfftw_plan plan;
1116  plan = rfftw_create_plan(length, FFTW_COMPLEX_TO_REAL, FFTW_ESTIMATE);
1117  _real_decompress_plans.insert(RealPlans::value_type(length, plan));
1118 
1119  return plan;
1120 }
1121 
1122 #endif
PN_int8 get_int8()
Extracts a signed 8-bit integer.
void add_uint8(PN_uint8 value)
Adds an unsigned 8-bit integer to the datagram.
Definition: datagram.I:138
This is the base class for all three-component vectors and points.
Definition: lvecBase3.h:105
bool read_reals(DatagramIterator &di, vector_stdfloat &array)
Reads an array of floating-point numbers.
bool read_hprs(DatagramIterator &di, pvector< LVecBase3 > &array, bool new_hpr)
Reads an array of HPR angles.
This is our own Panda specialization on the default STL map.
Definition: pmap.h:52
void set_transpose_quats(bool flag)
Sets the transpose_quats flag.
void set_quality(int quality)
Sets the quality factor for the compression.
bool get_bool()
Extracts a boolean value.
PN_stdfloat get_stdfloat()
Extracts either a 32-bit or a 64-bit floating-point number, according to Datagram::set_stdfloat_doubl...
void add_float64(PN_float64 value)
Adds a 64-bit floating-point number to the datagram.
Definition: datagram.I:228
void add_int8(PN_int8 value)
Adds a signed 8-bit integer to the datagram.
Definition: datagram.I:128
bool read_header(DatagramIterator &di, int bam_minor_version)
Reads the compression header that was written previously.
This is a unit quaternion representing an orientation.
Definition: lorientation.h:92
static void free_storage()
Frees memory that has been allocated during past runs of the FFTCompressor.
void extract_to_matrix(LMatrix3f &m) const
Based on the quat lib from VRPN.
bool get_use_error_threshold() const
Returns whether the error threshold measurement is enabled.
PN_int32 get_int32()
Extracts a signed 32-bit integer.
PN_uint8 get_uint8()
Extracts an unsigned 8-bit integer.
PN_int16 get_int16()
Extracts a signed 16-bit integer.
PN_uint16 get_uint16()
Extracts an unsigned 16-bit integer.
void add_stdfloat(PN_stdfloat value)
Adds either a 32-bit or a 64-bit floating-point number, according to set_stdfloat_double().
Definition: datagram.I:240
This is our own Panda specialization on the default STL vector.
Definition: pvector.h:39
void add_int16(PN_int16 value)
Adds a signed 16-bit integer to the datagram.
Definition: datagram.I:148
void add_bool(bool value)
Adds a boolean value to the datagram.
Definition: datagram.I:118
void write_header(Datagram &datagram)
Writes the compression parameters to the indicated datagram.
int get_quality() const
Returns the quality number that was previously set via set_quality().
void add_uint16(PN_uint16 value)
Adds an unsigned 16-bit integer to the datagram.
Definition: datagram.I:181
void write_reals(Datagram &datagram, const PN_stdfloat *array, int length)
Writes an array of floating-point numbers to the indicated datagram.
PN_float64 get_float64()
Extracts a 64-bit floating-point number.
bool get_transpose_quats() const
Returns the transpose_quats flag.
void write_hprs(Datagram &datagram, const LVecBase3 *array, int length)
Writes an array of HPR angles to the indicated datagram.
FFTCompressor()
Constructs a new compressor object with default parameters.
void add_int32(PN_int32 value)
Adds a signed 32-bit integer to the datagram.
Definition: datagram.I:159
A class to retrieve the individual data elements previously stored in a Datagram. ...
void set_use_error_threshold(bool use_error_threshold)
Enables or disables the use of the error threshold measurement to put a cap on the amount of damage d...
This is a 3-by-3 transform matrix.
Definition: lmatrix.h:110
An ordered list of data elements, formatted in memory for transmission over a socket or writing to a ...
Definition: datagram.h:43
bool almost_equal(const LMatrix3f &other, float threshold) const
Returns true if two matrices are memberwise equal within a specified tolerance.
Definition: lmatrix.cxx:419
static bool is_compression_available()
Returns true if the FFTW library is compiled in, so that this class is actually capable of doing usef...