15 #include "fftCompressor.h"
16 #include "config_mathutil.h"
17 #include "config_linmath.h"
20 #include "datagramIterator.h"
21 #include "compose_matrix.h"
43 static rfftw_plan get_real_compress_plan(
int length);
44 static rfftw_plan get_real_decompress_plan(
int length);
47 static RealPlans _real_compress_plans;
48 static RealPlans _real_decompress_plans;
60 _bam_minor_version = 0;
62 _use_error_threshold =
false;
63 _transpose_quats =
false;
113 if (_quality <= 100) {
114 mathutil_cat.warning()
115 <<
"FFTW library is not available; generating uncompressed output.\n";
125 _fft_offset = fft_offset;
126 _fft_factor = fft_factor;
127 _fft_exponent = fft_exponent;
129 }
else if (_quality < 40) {
135 double t = (double)_quality / 40.0;
136 _fft_offset = interpolate(t, 1.0, 0.001);
140 }
else if (_quality < 95) {
146 double t = (double)(_quality - 40) / 55.0;
148 _fft_factor = interpolate(t, 1.0, 0.1);
157 double t = (double)(_quality - 95) / 5.0;
159 _fft_factor = interpolate(t, 0.1, 0.0);
190 _use_error_threshold = use_error_threshold;
201 return _use_error_threshold;
214 _transpose_quats = flag;
225 return _transpose_quats;
257 if (_quality > 100) {
259 for (
int i = 0; i < length; i++) {
285 bool reject_compression =
false;
301 datagram.
add_bool(reject_compression);
302 if (reject_compression) {
303 if (mathutil_cat.is_debug()) {
305 <<
"Writing stream of " << length <<
" numbers uncompressed.\n";
307 for (
int i = 0; i < length; i++) {
314 double *data = (
double *)alloca(length *
sizeof(
double));
316 for (i = 0; i < length; i++) {
320 double *half_complex = (
double *)alloca(length *
sizeof(
double));
322 rfftw_plan plan = get_real_compress_plan(length);
323 rfftw_one(plan, data, half_complex);
330 RunWidth run_width = RW_invalid;
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;
338 double scale_factor = get_scale_factor(i, length);
339 double num = cfloor(half_complex[i] / scale_factor + 0.5);
342 double a = fabs(num);
348 }
else if (a <= max_range_8) {
351 }
else if (a <= max_range_16) {
354 }
else if (a <= max_range_32) {
358 num_width = RW_double;
364 if (run_width == RW_8 && num_width == RW_0) {
365 if (i + 1 >= length || half_complex[i + 1] != 0.0) {
370 if (num_width != run_width) {
372 num_written += write_run(datagram, run_width, run);
374 run_width = num_width;
380 num_written += write_run(datagram, run_width, run);
381 nassertv(num_written == length);
394 if (_quality >= 104) {
397 vector_stdfloat h, p, r;
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]);
420 if (_quality >= 103) {
427 for (
int i = 0; i < length; 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));
475 vector_stdfloat qr, qi, qj, qk;
482 for (
int i = 0; i < length; i++) {
486 if (_transpose_quats) {
487 mat.transpose_in_place();
493 if (rot.get_r() < 0) {
504 rot.set(-rot.get_r(), -rot.get_i(), -rot.get_j(), -rot.get_k());
508 if (mathutil_cat.is_warning()) {
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 "
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());
536 if (_quality >= 102) {
567 _bam_minor_version = bam_minor_version;
570 if (mathutil_cat.is_debug()) {
572 <<
"Found compressed data at quality level " << _quality <<
"\n";
576 if (_quality <= 100) {
578 <<
"FFTW library is not available; cannot read compressed data.\n";
608 if (_quality > 100) {
609 array.reserve(array.size() + length);
612 for (
int i = 0; i < length; i++) {
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++) {
649 vector_double half_complex;
650 half_complex.reserve(length);
652 while (num_read < length) {
653 num_read += read_run(di, half_complex);
655 nassertr(num_read == length,
false);
656 nassertr((
int)half_complex.size() == length,
false);
659 for (i = 0; i < length; i++) {
660 half_complex[i] *= get_scale_factor(i, length);
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);
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);
694 if (_quality >= 104) {
697 vector_stdfloat h, p, r;
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]));
713 if (_quality >= 103) {
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]);
739 decompose_matrix_new_hpr(mat, scale, shear, hpr);
741 decompose_matrix_old_hpr(mat, scale, shear, hpr);
743 array.push_back(hpr);
751 vector_stdfloat qr, qi, qj, qk;
756 if (_quality >= 102) {
768 nassertr(qi.size() == qj.size() && qj.size() == qk.size(),
false);
770 array.reserve(array.size() + qi.size());
771 for (
int i = 0; i < (int)qi.size(); i++) {
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);
778 rot.set(qr1, qi[i], qj[i], qk[i]);
781 if (_quality >= 102) {
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";
797 if (_transpose_quats) {
798 mat.transpose_in_place();
802 decompose_matrix_new_hpr(mat, scale, shear, hpr);
804 decompose_matrix_old_hpr(mat, scale, shear, hpr);
806 array.push_back(hpr);
823 return read_hprs(di, array, temp_hpr_fix);
839 RealPlans::iterator pi;
840 for (pi = _real_compress_plans.begin();
841 pi != _real_compress_plans.end();
843 rfftw_destroy_plan((*pi).second);
845 _real_compress_plans.clear();
847 for (pi = _real_decompress_plans.begin();
848 pi != _real_decompress_plans.end();
850 rfftw_destroy_plan((*pi).second);
852 _real_decompress_plans.clear();
864 write_run(
Datagram &datagram, FFTCompressor::RunWidth run_width,
865 const vector_double &run) {
869 nassertr(run_width != RW_invalid, 0);
871 if (run_width != RW_double) {
876 if (run.size() <= RW_length_mask &&
877 ((int)run_width | run.size()) != RW_double) {
882 datagram.
add_uint8((
int)run_width | run.size());
890 nassertr(run.size() < 65536, 0);
891 nassertr(run.size() != 0, 0);
898 vector_double::const_iterator ri;
905 for (ri = run.begin(); ri != run.end(); ++ri) {
911 for (ri = run.begin(); ri != run.end(); ++ri) {
917 for (ri = run.begin(); ri != run.end(); ++ri) {
923 for (ri = run.begin(); ri != run.end(); ++ri) {
927 datagram.
add_int8((PN_int8)RW_double);
954 if ((start & 0xff) == RW_double) {
957 run_width = RW_double;
961 run_width = (RunWidth)(start & RW_width_mask);
962 length = start & RW_length_mask;
970 nassertr(length != 0, 0);
972 run.reserve(run.size() + length);
977 for (i = 0; i < length; i++) {
983 for (i = 0; i < length; i++) {
984 run.push_back((
double)(
int)di.
get_int8());
989 for (i = 0; i < length; i++) {
990 run.push_back((
double)(
int)di.
get_int16());
995 for (i = 0; i < length; i++) {
996 run.push_back((
double)(
int)di.
get_int32());
1001 for (i = 0; i < length; i++) {
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);
1025 return _fft_offset +
1026 _fft_factor * pow((
double)(m-1 - k) / (
double)(m-1), _fft_exponent);
1036 double FFTCompressor::
1037 interpolate(
double t,
double a,
double b) {
1038 return a + t * (b - a);
1049 PN_stdfloat FFTCompressor::
1050 get_compressability(
const PN_stdfloat *data,
int length)
const {
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];
1065 sum2 += delta * delta;
1067 PN_stdfloat variance = (sum2 - (sum * sum) / (length - 1)) / (length - 2);
1068 if (variance < 0.0) {
1073 PN_stdfloat std_deviation = csqrt(variance);
1075 return std_deviation;
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;
1096 plan = rfftw_create_plan(length, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE);
1097 _real_compress_plans.insert(RealPlans::value_type(length, 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;
1116 plan = rfftw_create_plan(length, FFTW_COMPLEX_TO_REAL, FFTW_ESTIMATE);
1117 _real_decompress_plans.insert(RealPlans::value_type(length, plan));
int get_quality() const
Returns the quality number that was previously set via set_quality().
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.
This is the base class for all three-component vectors and points.
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.
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.
void add_int8(PN_int8 value)
Adds a signed 8-bit integer to the datagram.
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.
void extract_to_matrix(LMatrix3f &m) const
Based on the quat lib from VRPN.
static void free_storage()
Frees memory that has been allocated during past runs of the FFTCompressor.
PN_int32 get_int32()
Extracts a signed 32-bit integer.
PN_uint8 get_uint8()
Extracts an unsigned 8-bit integer.
bool almost_equal(const LMatrix3f &other, float threshold) const
Returns true if two matrices are memberwise equal within a specified tolerance.
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().
This is our own Panda specialization on the default STL vector.
void add_int16(PN_int16 value)
Adds a signed 16-bit integer to the datagram.
void add_bool(bool value)
Adds a boolean value to the datagram.
void write_header(Datagram &datagram)
Writes the compression parameters to the indicated datagram.
bool get_transpose_quats() const
Returns the transpose_quats flag.
void add_uint16(PN_uint16 value)
Adds an unsigned 16-bit integer to the datagram.
bool get_use_error_threshold() const
Returns whether the error threshold measurement is enabled.
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.
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.
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.
An ordered list of data elements, formatted in memory for transmission over a socket or writing to a ...
static bool is_compression_available()
Returns true if the FFTW library is compiled in, so that this class is actually capable of doing usef...