TheCloud/panda3d
1
0
Fork 0
mirror of https://github.com/panda3d/panda3d.git synced 2025-09-30 08:44:19 -04:00
Code Issues Packages Projects Releases Wiki Activity

separate compose_matrix into old_hpr and new_hpr variants

Browse Source
This commit is contained in:
David Rose 2004-11-21 19:51:58 +00:00
parent e2c525d201
commit 8747959c7b
7 changed files with 698 additions and 509 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
panda/src/chan/animChannelMatrixXfmTable.cxx
View File

@ -28,6 +28,7 @@
#include "bamReader.h"
#include "bamWriter.h"
#include "fftCompressor.h"
#include "config_linmath.h"
TypeHandle AnimChannelMatrixXfmTable::_type_handle;
@ -267,6 +268,7 @@ write_datagram(BamWriter *manager, Datagram &me) {
FFTCompressor compressor;
compressor.set_quality(compress_chan_quality);
compressor.set_use_error_threshold(true);
me.add_bool(temp_hpr_fix);
compressor.write_header(me);
// First, write out the scales and shears.
@ -336,6 +338,14 @@ fillin(DatagramIterator &scan, BamReader *manager) {
return;
}
bool new_hpr = false;
if (manager->get_file_minor_ver() >= 14) {
// Beginning in bam 4.14, we encode a bool to indicate whether
// we used the old hpr or the new hpr calculation. Previously,
// we assume all bams used the old hpr calculation.
new_hpr = scan.get_bool();
}
FFTCompressor compressor;
compressor.read_header(scan, manager->get_file_minor_ver());
@ -362,7 +372,7 @@ fillin(DatagramIterator &scan, BamReader *manager) {
// Read in the HPR array and store it back in the joint angles.
vector_LVecBase3f hprs;
compressor.read_hprs(scan, hprs);
compressor.read_hprs(scan, hprs, new_hpr);
PTA_float h_table = PTA_float::empty_array(hprs.size());
PTA_float p_table = PTA_float::empty_array(hprs.size());
PTA_float r_table = PTA_float::empty_array(hprs.size());

314
panda/src/linmath/compose_matrix_src.I
View File

@ -17,6 +17,24 @@
////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////
// Function: compose_matrix
// Description: Computes the 3x3 matrix from scale, shear, and
// rotation.
////////////////////////////////////////////////////////////////////
INLINE_LINMATH void
compose_matrix(FLOATNAME(LMatrix3) &mat,
const FLOATNAME(LVecBase3) &scale,
const FLOATNAME(LVecBase3) &shear,
const FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs) {
if (temp_hpr_fix) {
compose_matrix_new_hpr(mat, scale, shear, hpr, cs);
} else {
compose_matrix_old_hpr(mat, scale, shear, hpr, cs);
}
}
////////////////////////////////////////////////////////////////////
// Function: compose_matrix
// Description: Computes the 4x4 matrix according to scale, shear,
@ -29,30 +47,49 @@ compose_matrix(FLOATNAME(LMatrix4) &mat,
const FLOATNAME(LVecBase3) &hpr,
const FLOATNAME(LVecBase3) &translate,
CoordinateSystem cs) {
FLOATNAME(LMatrix3) upper3;
compose_matrix(upper3, scale, shear, hpr, cs);
mat = FLOATNAME(LMatrix4)(upper3, translate);
if (temp_hpr_fix) {
compose_matrix_new_hpr(mat, scale, shear, hpr, translate, cs);
} else {
compose_matrix_old_hpr(mat, scale, shear, hpr, translate, cs);
}
}
////////////////////////////////////////////////////////////////////
// Function: compose_matrix
// Description: Computes the 4x4 matrix according to scale, shear,
// rotation, and translation.
////////////////////////////////////////////////////////////////////
INLINE_LINMATH void
compose_matrix(FLOATNAME(LMatrix4) &mat,
const FLOATTYPE components[num_matrix_components],
CoordinateSystem cs) {
FLOATNAME(LVector3) scale(components[0],
components[1],
components[2]);
FLOATNAME(LVector3) shear(components[3],
components[4],
components[5]);
FLOATNAME(LVector3) hpr(components[6],
components[7],
components[8]);
FLOATNAME(LVector3) translate(components[9],
components[10],
components[11]);
compose_matrix(mat, scale, shear, hpr, translate, cs);
if (temp_hpr_fix) {
compose_matrix_new_hpr(mat, components, cs);
} else {
compose_matrix_old_hpr(mat, components, cs);
}
}
////////////////////////////////////////////////////////////////////
// Function: decompose_matrix
// Description: Extracts out the components of a 3x3 rotation matrix.
// Returns true if successful, or false if there was an
// error. Since a 3x3 matrix always contains an affine
// transform, this should succeed in the normal case;
// singular transforms are not treated as an error.
////////////////////////////////////////////////////////////////////
INLINE_LINMATH bool
decompose_matrix(const FLOATNAME(LMatrix3) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &shear,
FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs) {
if (temp_hpr_fix) {
return decompose_matrix_new_hpr(mat, scale, shear, hpr, cs);
} else {
return decompose_matrix_old_hpr(mat, scale, shear, hpr, cs);
}
}
////////////////////////////////////////////////////////////////////
// Function: decompose_matrix
@ -68,9 +105,131 @@ decompose_matrix(const FLOATNAME(LMatrix4) &mat,
FLOATNAME(LVecBase3) &hpr,
FLOATNAME(LVecBase3) &translate,
CoordinateSystem cs) {
if (temp_hpr_fix) {
return decompose_matrix_new_hpr(mat, scale, shear, hpr, translate, cs);
} else {
return decompose_matrix_old_hpr(mat, scale, shear, hpr, translate, cs);
}
}
////////////////////////////////////////////////////////////////////
// Function: decompose_matrix
// Description: Extracts out the components of an affine matrix.
// Returns true if the scale, shear, hpr, and translate
// completely describe the matrix, or false if the
// matrix is not affine.
////////////////////////////////////////////////////////////////////
INLINE_LINMATH bool
decompose_matrix(const FLOATNAME(LMatrix4) &mat,
FLOATTYPE components[num_matrix_components],
CoordinateSystem cs) {
if (temp_hpr_fix) {
return decompose_matrix_new_hpr(mat, components, cs);
} else {
return decompose_matrix_old_hpr(mat, components, cs);
}
}
// The following functions are deprecated; they have been replaced
// with new versions, above, that accept a shear component as well.
// Deprecated function.
INLINE EXPCL_PANDA void
compose_matrix(FLOATNAME(LMatrix3) &mat,
const FLOATNAME(LVecBase3) &scale,
const FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs) {
compose_matrix(mat, scale, FLOATNAME(LVecBase3)(0, 0, 0), hpr, cs);
}
// Deprecated function.
INLINE_LINMATH void
compose_matrix(FLOATNAME(LMatrix4) &mat,
const FLOATNAME(LVecBase3) &scale,
const FLOATNAME(LVecBase3) &hpr,
const FLOATNAME(LVecBase3) &translate,
CoordinateSystem cs) {
FLOATNAME(LMatrix3) upper3;
compose_matrix(upper3, scale, hpr, cs);
mat = FLOATNAME(LMatrix4)(upper3, translate);
}
// Deprecated function.
INLINE_LINMATH bool
decompose_matrix(const FLOATNAME(LMatrix3) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs) {
FLOATNAME(LVecBase3) shear;
if (!decompose_matrix(mat, scale, shear, hpr, cs)) {
return false;
}
return shear.almost_equal(FLOATNAME(LVecBase3)::zero());
}
// Deprecated function.
INLINE_LINMATH bool
decompose_matrix(const FLOATNAME(LMatrix4) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &hpr,
FLOATNAME(LVecBase3) &translate,
CoordinateSystem cs) {
// Get the translation first.
mat.get_row3(translate,3);
if (!decompose_matrix(mat.get_upper_3(), scale, shear, hpr, cs)) {
return decompose_matrix(mat.get_upper_3(), scale, hpr, cs);
}
// The following functions are transitional and serve only to migrate
// code from the old, incorrect hpr calculations that Panda used to
// use. New code should not call these functions directly; use the
// unqualified functions, above, instead.
// Transitional function.
INLINE_LINMATH void
compose_matrix_old_hpr(FLOATNAME(LMatrix4) &mat,
const FLOATNAME(LVecBase3) &scale,
const FLOATNAME(LVecBase3) &shear,
const FLOATNAME(LVecBase3) &hpr,
const FLOATNAME(LVecBase3) &translate,
CoordinateSystem cs) {
FLOATNAME(LMatrix3) upper3;
compose_matrix_old_hpr(upper3, scale, shear, hpr, cs);
mat = FLOATNAME(LMatrix4)(upper3, translate);
}
// Transitional function.
INLINE_LINMATH void
compose_matrix_old_hpr(FLOATNAME(LMatrix4) &mat,
const FLOATTYPE components[num_matrix_components],
CoordinateSystem cs) {
FLOATNAME(LVector3) scale(components[0],
components[1],
components[2]);
FLOATNAME(LVector3) shear(components[3],
components[4],
components[5]);
FLOATNAME(LVector3) hpr(components[6],
components[7],
components[8]);
FLOATNAME(LVector3) translate(components[9],
components[10],
components[11]);
compose_matrix_old_hpr(mat, scale, shear, hpr, translate, cs);
}
// Transitional function.
INLINE_LINMATH bool
decompose_matrix_old_hpr(const FLOATNAME(LMatrix4) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &shear,
FLOATNAME(LVecBase3) &hpr,
FLOATNAME(LVecBase3) &translate,
CoordinateSystem cs) {
// Get the translation first.
mat.get_row3(translate,3);
if (!decompose_matrix_old_hpr(mat.get_upper_3(), scale, shear, hpr, cs)) {
return false;
}
#ifndef NDEBUG
@ -80,12 +239,13 @@ decompose_matrix(const FLOATNAME(LMatrix4) &mat,
#endif
}
// Transitional function.
INLINE_LINMATH bool
decompose_matrix(const FLOATNAME(LMatrix4) &mat,
FLOATTYPE components[num_matrix_components],
CoordinateSystem cs) {
decompose_matrix_old_hpr(const FLOATNAME(LMatrix4) &mat,
FLOATTYPE components[num_matrix_components],
CoordinateSystem cs) {
FLOATNAME(LVector3) scale, shear, hpr, translate;
bool result = decompose_matrix(mat, scale, shear, hpr, translate, cs);
bool result = decompose_matrix_old_hpr(mat, scale, shear, hpr, translate, cs);
components[0] = scale[0];
components[1] = scale[1];
components[2] = scale[2];
@ -101,63 +261,77 @@ decompose_matrix(const FLOATNAME(LMatrix4) &mat,
return result;
}
// The following functions are deprecated; they have been replaced
// with new versions, above, that accept a shear component as well.
INLINE EXPCL_PANDA void
compose_matrix(FLOATNAME(LMatrix3) &mat,
const FLOATNAME(LVecBase3) &scale,
const FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs) {
compose_matrix(mat, scale, FLOATNAME(LVecBase3)(0, 0, 0), hpr, cs);
}
////////////////////////////////////////////////////////////////////
// Function: compose_matrix
// Description: Computes the 4x4 matrix according to scale, rotation,
// and translation.
////////////////////////////////////////////////////////////////////
// Transitional function.
INLINE_LINMATH void
compose_matrix(FLOATNAME(LMatrix4) &mat,
const FLOATNAME(LVecBase3) &scale,
const FLOATNAME(LVecBase3) &hpr,
const FLOATNAME(LVecBase3) &translate,
CoordinateSystem cs) {
compose_matrix_new_hpr(FLOATNAME(LMatrix4) &mat,
const FLOATNAME(LVecBase3) &scale,
const FLOATNAME(LVecBase3) &shear,
const FLOATNAME(LVecBase3) &hpr,
const FLOATNAME(LVecBase3) &translate,
CoordinateSystem cs) {
FLOATNAME(LMatrix3) upper3;
compose_matrix(upper3, scale, hpr, cs);
compose_matrix_new_hpr(upper3, scale, shear, hpr, cs);
mat = FLOATNAME(LMatrix4)(upper3, translate);
}
INLINE EXPCL_PANDA bool
decompose_matrix(const FLOATNAME(LMatrix3) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs) {
FLOATNAME(LVecBase3) shear;
if (!decompose_matrix(mat, scale, shear, hpr, cs)) {
return false;
}
return shear.almost_equal(FLOATNAME(LVecBase3)::zero());
// Transitional function.
INLINE_LINMATH void
compose_matrix_new_hpr(FLOATNAME(LMatrix4) &mat,
const FLOATTYPE components[num_matrix_components],
CoordinateSystem cs) {
FLOATNAME(LVector3) scale(components[0],
components[1],
components[2]);
FLOATNAME(LVector3) shear(components[3],
components[4],
components[5]);
FLOATNAME(LVector3) hpr(components[6],
components[7],
components[8]);
FLOATNAME(LVector3) translate(components[9],
components[10],
components[11]);
compose_matrix_new_hpr(mat, scale, shear, hpr, translate, cs);
}
////////////////////////////////////////////////////////////////////
// Function: decompose_matrix
// Description: Extracts out the components of an affine matrix.
// Returns true if the scale, hpr, translate
// completely describe the matrix, or false if there is
// also a shear component or if the matrix is not
// affine.
////////////////////////////////////////////////////////////////////
// Transitional function.
INLINE_LINMATH bool
decompose_matrix(const FLOATNAME(LMatrix4) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &hpr,
FLOATNAME(LVecBase3) &translate,
CoordinateSystem cs) {
decompose_matrix_new_hpr(const FLOATNAME(LMatrix4) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &shear,
FLOATNAME(LVecBase3) &hpr,
FLOATNAME(LVecBase3) &translate,
CoordinateSystem cs) {
// Get the translation first.
mat.get_row3(translate,3);
return decompose_matrix(mat.get_upper_3(), scale, hpr, cs);
if (!decompose_matrix_new_hpr(mat.get_upper_3(), scale, shear, hpr, cs)) {
return false;
}
#ifndef NDEBUG
return mat.get_col(3).almost_equal(FLOATNAME(LVecBase4)(0.0, 0.0, 0.0, 1.0));
#else
return true;
#endif
}
// Transitional function.
INLINE_LINMATH bool
decompose_matrix_new_hpr(const FLOATNAME(LMatrix4) &mat,
FLOATTYPE components[num_matrix_components],
CoordinateSystem cs) {
FLOATNAME(LVector3) scale, shear, hpr, translate;
bool result = decompose_matrix_new_hpr(mat, scale, shear, hpr, translate, cs);
components[0] = scale[0];
components[1] = scale[1];
components[2] = scale[2];
components[3] = shear[0];
components[4] = shear[1];
components[5] = shear[2];
components[6] = hpr[0];
components[7] = hpr[1];
components[8] = hpr[2];
components[9] = translate[0];
components[10] = translate[1];
components[11] = translate[2];
return result;
}

735
panda/src/linmath/compose_matrix_src.cxx
View File

@ -18,38 +18,25 @@
////////////////////////////////////////////////////////////////////
// Function: compose_matrix
// Function: compose_matrix_old_hpr
// Description: Computes the 3x3 matrix from scale, shear, and
// rotation.
////////////////////////////////////////////////////////////////////
void
compose_matrix(FLOATNAME(LMatrix3) &mat,
const FLOATNAME(LVecBase3) &scale,
const FLOATNAME(LVecBase3) &shear,
const FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs) {
// temp_hpr_fix blocks use the correct way. need to keep other way
// as default until legacy tools are fixed to work with correct way
if (temp_hpr_fix) {
mat =
FLOATNAME(LMatrix3)::scale_shear_mat(scale, shear, cs) *
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[2], FLOATNAME(LVector3)::forward(cs), cs) *
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[1], FLOATNAME(LVector3)::right(cs), cs) *
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[0], FLOATNAME(LVector3)::up(cs), cs);
} else {
mat =
FLOATNAME(LMatrix3)::scale_shear_mat(scale, shear, cs) *
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[1], FLOATNAME(LVector3)::right(cs), cs) *
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[0], FLOATNAME(LVector3)::up(cs), cs) *
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[2], FLOATNAME(LVector3)::back(cs), cs);
}
compose_matrix_old_hpr(FLOATNAME(LMatrix3) &mat,
const FLOATNAME(LVecBase3) &scale,
const FLOATNAME(LVecBase3) &shear,
const FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs) {
mat =
FLOATNAME(LMatrix3)::scale_shear_mat(scale, shear, cs) *
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[1], FLOATNAME(LVector3)::right(cs), cs) *
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[0], FLOATNAME(LVector3)::up(cs), cs) *
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[2], FLOATNAME(LVector3)::back(cs), cs);
}
////////////////////////////////////////////////////////////////////
// Function: unwind_yup_rotation
// Function: unwind_yup_rotation_old_hpr
// Description: Extracts the rotation about the x, y, and z axes from
// the given hpr & scale matrix. Adjusts the matrix
// to eliminate the rotation.
@ -58,161 +45,7 @@ compose_matrix(FLOATNAME(LMatrix3) &mat,
// right-handed Y-up coordinate system.
////////////////////////////////////////////////////////////////////
static void
unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
typedef FLOATNAME(LMatrix3) Matrix;
if (temp_hpr_fix) {
// Extract the axes from the matrix.
FLOATNAME(LVector3) x, y, z;
mat.get_row(x,0);
mat.get_row(y,1);
mat.get_row(z,2);
// Project Z into the XZ plane.
FLOATNAME(LVector2) xz(z[0], z[2]);
xz = normalize(xz);
// Compute the rotation about the +Y (up) axis. This is yaw, or
// "heading".
FLOATTYPE heading = rad_2_deg(((FLOATTYPE)atan2(xz[0], xz[1])));
// Unwind the heading, and continue.
Matrix rot_y;
rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
CS_yup_right);
x = x * rot_y;
y = y * rot_y;
z = z * rot_y;
// Project the rotated Z into the YZ plane.
FLOATNAME(LVector2) yz(z[1], z[2]);
yz = normalize(yz);
// Compute the rotation about the +X (right) axis. This is pitch.
FLOATTYPE pitch = rad_2_deg((FLOATTYPE)(-atan2(yz[0], yz[1])));
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
CS_yup_right);
x = x * rot_x;
y = y * rot_x;
z = z * rot_x;
// Project the rotated X onto the XY plane.
FLOATNAME(LVector2) xy(x[0], x[1]);
xy = normalize(xy);
// Compute the rotation about the +Z (back) axis. This is roll.
FLOATTYPE roll = -rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0])));
// Unwind the roll from the axes, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
CS_yup_right);
x = x * rot_z;
y = y * rot_z;
z = z * rot_z;
// Reset the matrix to reflect the unwinding.
mat.set_row(0, x);
mat.set_row(1, y);
mat.set_row(2, z);
// Return the three rotation components.
hpr[0] = heading;
hpr[1] = pitch;
hpr[2] = roll;
} else {
// Extract the axes from the matrix.
FLOATNAME(LVector3) x, y, z;
mat.get_row(x,0);
mat.get_row(y,1);
mat.get_row(z,2);
// Project X onto the XY plane.
FLOATNAME(LVector2) xy(x[0], x[1]);
xy = normalize(xy);
// Compute the rotation about the +Z (back) axis. This is roll.
FLOATTYPE roll = rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0])));
// Unwind the roll from the axes, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
CS_yup_right);
x = x * rot_z;
y = y * rot_z;
z = z * rot_z;
// Project the rotated X into the XZ plane.
FLOATNAME(LVector2) xz(x[0], x[2]);
xz = normalize(xz);
// Compute the rotation about the +Y (up) axis. This is yaw, or
// "heading".
FLOATTYPE heading = rad_2_deg(((FLOATTYPE)-atan2(xz[1], xz[0])));
// Unwind the heading, and continue.
Matrix rot_y;
rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
CS_yup_right);
x = x * rot_y;
y = y * rot_y;
z = z * rot_y;
// Project the rotated Z into the YZ plane.
FLOATNAME(LVector2) yz(z[1], z[2]);
yz = normalize(yz);
// Compute the rotation about the +X (right) axis. This is pitch.
FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)-atan2(yz[0], yz[1])));
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
CS_yup_right);
x = x * rot_x;
y = y * rot_x;
z = z * rot_x;
// Reset the matrix to reflect the unwinding.
mat.set_row(0, x);
mat.set_row(1, y);
mat.set_row(2, z);
// Return the three rotation components.
hpr[0] = heading;
hpr[1] = pitch;
hpr[2] = roll;
}
}
////////////////////////////////////////////////////////////////////
// Function: unwind_yup_rotation
// Description: Extracts the rotation about the x, y, and z axes from
// the given hpr & scale matrix, given the indicated
// roll amount as a hint. Adjusts the matrix to
// eliminate the rotation.
//
// This function assumes the matrix is stored in a
// right-handed Y-up coordinate system.
////////////////////////////////////////////////////////////////////
static void
unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
FLOATTYPE roll) {
if (temp_hpr_fix) {
unwind_yup_rotation(mat, hpr);
return;
}
unwind_yup_rotation_old_hpr(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
typedef FLOATNAME(LMatrix3) Matrix;
@ -221,54 +54,61 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
mat.get_row(x,0);
mat.get_row(y,1);
mat.get_row(z,2);
// Project X onto the XY plane.
FLOATNAME(LVector2) xy(x[0], x[1]);
xy = normalize(xy);
// Compute the rotation about the +Z (back) axis. This is roll.
FLOATTYPE roll = rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0])));
// Unwind the roll from the axes, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
CS_yup_right);
x = x * rot_z;
y = y * rot_z;
z = z * rot_z;
// Project the rotated X into the XZ plane.
FLOATNAME(LVector2) xz(x[0], x[2]);
xz = normalize(xz);
// Compute the rotation about the +Y (up) axis. This is yaw, or
// "heading".
FLOATTYPE heading = rad_2_deg(((FLOATTYPE)-atan2(xz[1], xz[0])));
// Unwind the heading, and continue.
Matrix rot_y;
rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
CS_yup_right);
x = x * rot_y;
y = y * rot_y;
z = z * rot_y;
// Project the rotated Z into the YZ plane.
FLOATNAME(LVector2) yz(z[1], z[2]);
yz = normalize(yz);
// Compute the rotation about the +X (right) axis. This is pitch.
FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)-atan2(yz[0], yz[1])));
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
CS_yup_right);
x = x * rot_x;
y = y * rot_x;
z = z * rot_x;
// Reset the matrix to reflect the unwinding.
mat.set_row(0, x);
mat.set_row(1, y);
mat.set_row(2, z);
// Return the three rotation components.
hpr[0] = heading;
hpr[1] = pitch;
@ -276,7 +116,7 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
}
////////////////////////////////////////////////////////////////////
// Function: unwind_zup_rotation
// Function: unwind_zup_rotation_old_hpr
// Description: Extracts the rotation about the x, y, and z axes from
// the given hpr & scale matrix. Adjusts the matrix
// to eliminate the rotation.
@ -285,227 +125,78 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
// right-handed Z-up coordinate system.
////////////////////////////////////////////////////////////////////
static void
unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
if (temp_hpr_fix) {
typedef FLOATNAME(LMatrix3) Matrix;
// Extract the axes from the matrix.
FLOATNAME(LVector3) x, y, z;
mat.get_row(x,0);
mat.get_row(y,1);
mat.get_row(z,2);
// Project Y into the XY plane.
FLOATNAME(LVector2) xy(y[0], y[1]);
xy = normalize(xy);
// Compute the rotation about the +Z (up) axis. This is yaw, or
// "heading".
FLOATTYPE heading = -rad_2_deg(((FLOATTYPE)atan2(xy[0], xy[1])));
// Unwind the heading, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
CS_zup_right);
x = x * rot_z;
y = y * rot_z;
z = z * rot_z;
// Project the rotated Y into the YZ plane.
FLOATNAME(LVector2) yz(y[1], y[2]);
yz = normalize(yz);
// Compute the rotation about the +X (right) axis. This is pitch.
FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)atan2(yz[1], yz[0])));
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
CS_zup_right);
x = x * rot_x;
y = y * rot_x;
z = z * rot_x;
// Project X into the XZ plane.
FLOATNAME(LVector2) xz(x[0], x[2]);
xz = normalize(xz);
// Compute the rotation about the -Y (back) axis. This is roll.
FLOATTYPE roll = -rad_2_deg(((FLOATTYPE)atan2(xz[1], xz[0])));
// Unwind the roll from the axes, and continue.
Matrix rot_y;
rot_y = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
CS_zup_right);
x = x * rot_y;
y = y * rot_y;
z = z * rot_y;
// Reset the matrix to reflect the unwinding.
mat.set_row(0, x);
mat.set_row(1, y);
mat.set_row(2, z);
// Return the three rotation components.
hpr[0] = heading;
hpr[1] = pitch;
hpr[2] = roll;
} else {
typedef FLOATNAME(LMatrix3) Matrix;
// Extract the axes from the matrix.
FLOATNAME(LVector3) x, y, z;
mat.get_row(x,0);
mat.get_row(y,1);
mat.get_row(z,2);
// Project X into the XZ plane.
FLOATNAME(LVector2) xz(x[0], x[2]);
xz = normalize(xz);
// Compute the rotation about the -Y (back) axis. This is roll.
FLOATTYPE roll = rad_2_deg(((FLOATTYPE)atan2(xz[1], xz[0])));
if (y[1] < 0.0f) {
if (roll < 0.0f) {
roll += 180.0;
} else {
roll -= 180.0;
}
}
// Unwind the roll from the axes, and continue.
Matrix rot_y;
rot_y = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
CS_zup_right);
x = x * rot_y;
y = y * rot_y;
z = z * rot_y;
// Project the rotated X into the XY plane.
FLOATNAME(LVector2) xy(x[0], x[1]);
xy = normalize(xy);
// Compute the rotation about the +Z (up) axis. This is yaw, or
// "heading".
FLOATTYPE heading = rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0])));
// Unwind the heading, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
CS_zup_right);
x = x * rot_z;
y = y * rot_z;
z = z * rot_z;
// Project the rotated Y into the YZ plane.
FLOATNAME(LVector2) yz(y[1], y[2]);
yz = normalize(yz);
// Compute the rotation about the +X (right) axis. This is pitch.
FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)atan2(yz[1], yz[0])));
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
CS_zup_right);
x = x * rot_x;
y = y * rot_x;
z = z * rot_x;
// Reset the matrix to reflect the unwinding.
mat.set_row(0, x);
mat.set_row(1, y);
mat.set_row(2, z);
// Return the three rotation components.
hpr[0] = heading;
hpr[1] = pitch;
hpr[2] = roll;
}
}
////////////////////////////////////////////////////////////////////
// Function: unwind_zup_rotation
// Description: Extracts the rotation about the x, y, and z axes from
// the given hpr & scale matrix, given the indicated
// roll amount as a hint. Adjusts the matrix to
// eliminate the rotation.
//
// This function assumes the matrix is stored in a
// right-handed Z-up coordinate system.
////////////////////////////////////////////////////////////////////
static void
unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
FLOATTYPE roll) {
if (temp_hpr_fix) {
unwind_zup_rotation(mat, hpr);
return;
}
unwind_zup_rotation_old_hpr(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
typedef FLOATNAME(LMatrix3) Matrix;
// Extract the axes from the matrix.
FLOATNAME(LVector3) x, y, z;
mat.get_row(x,0);
mat.get_row(y,1);
mat.get_row(z,2);
// Project X into the XZ plane.
FLOATNAME(LVector2) xz(x[0], x[2]);
xz = normalize(xz);
// Compute the rotation about the -Y (back) axis. This is roll.
FLOATTYPE roll = rad_2_deg(((FLOATTYPE)atan2(xz[1], xz[0])));
if (y[1] < 0.0f) {
if (roll < 0.0f) {
roll += 180.0;
} else {
roll -= 180.0;
}
}
// Unwind the roll from the axes, and continue.
Matrix rot_y;
rot_y = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
CS_zup_right);
x = x * rot_y;
y = y * rot_y;
z = z * rot_y;
// Project the rotated X into the XY plane.
FLOATNAME(LVector2) xy(x[0], x[1]);
xy = normalize(xy);
// Compute the rotation about the +Z (up) axis. This is yaw, or
// "heading".
FLOATTYPE heading = rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0])));
// Unwind the heading, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
CS_zup_right);
x = x * rot_z;
y = y * rot_z;
z = z * rot_z;
// Project the rotated Y into the YZ plane.
FLOATNAME(LVector2) yz(y[1], y[2]);
yz = normalize(yz);
// Compute the rotation about the +X (right) axis. This is pitch.
FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)atan2(yz[1], yz[0])));
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
CS_zup_right);
x = x * rot_x;
y = y * rot_x;
z = z * rot_x;
// Reset the matrix to reflect the unwinding.
mat.set_row(0, x);
mat.set_row(1, y);
mat.set_row(2, z);
// Return the three rotation components.
hpr[0] = heading;
hpr[1] = pitch;
@ -513,7 +204,7 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
}
////////////////////////////////////////////////////////////////////
// Function: decompose_matrix
// Function: decompose_matrix_old_hpr
// Description: Extracts out the components of a 3x3 rotation matrix.
// Returns true if successful, or false if there was an
// error. Since a 3x3 matrix always contains an affine
@ -521,11 +212,11 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
// singular transforms are not treated as an error.
////////////////////////////////////////////////////////////////////
bool
decompose_matrix(const FLOATNAME(LMatrix3) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &shear,
FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs) {
decompose_matrix_old_hpr(const FLOATNAME(LMatrix3) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &shear,
FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs) {
if (cs == CS_default) {
cs = get_default_coordinate_system();
}
@ -544,14 +235,14 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat,
switch (cs) {
case CS_zup_right:
{
unwind_zup_rotation(new_mat, hpr);
unwind_zup_rotation_old_hpr(new_mat, hpr);
is_left_handed = false;
}
break;
case CS_yup_right:
{
unwind_yup_rotation(new_mat, hpr);
unwind_yup_rotation_old_hpr(new_mat, hpr);
is_left_handed = false;
}
break;
@ -567,7 +258,7 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat,
mat(1, 0), mat(1, 1), -mat(1, 2),
-mat(2, 0), -mat(2, 1), mat(2, 2));
*/
unwind_zup_rotation(new_mat, hpr);
unwind_zup_rotation_old_hpr(new_mat, hpr);
hpr[0] = -hpr[0];
hpr[2] = -hpr[2];
is_left_handed = true;
@ -585,7 +276,7 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat,
mat(1, 0), mat(1, 1), -mat(1, 2),
-mat(2, 0), -mat(2, 1), mat(2, 2));
*/
unwind_yup_rotation(new_mat, hpr);
unwind_yup_rotation_old_hpr(new_mat, hpr);
is_left_handed = true;
}
break;
@ -625,37 +316,205 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat,
return true;
}
////////////////////////////////////////////////////////////////////
// Function: decompose_matrix
// Function: compose_matrix_new_hpr
// Description: Computes the 3x3 matrix from scale, shear, and
// rotation.
////////////////////////////////////////////////////////////////////
void
compose_matrix_new_hpr(FLOATNAME(LMatrix3) &mat,
const FLOATNAME(LVecBase3) &scale,
const FLOATNAME(LVecBase3) &shear,
const FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs) {
mat =
FLOATNAME(LMatrix3)::scale_shear_mat(scale, shear, cs) *
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[2], FLOATNAME(LVector3)::forward(cs), cs) *
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[1], FLOATNAME(LVector3)::right(cs), cs) *
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[0], FLOATNAME(LVector3)::up(cs), cs);
}
////////////////////////////////////////////////////////////////////
// Function: unwind_yup_rotation_new_hpr
// Description: Extracts the rotation about the x, y, and z axes from
// the given hpr & scale matrix. Adjusts the matrix
// to eliminate the rotation.
//
// This function assumes the matrix is stored in a
// right-handed Y-up coordinate system.
////////////////////////////////////////////////////////////////////
static void
unwind_yup_rotation_new_hpr(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
typedef FLOATNAME(LMatrix3) Matrix;
// Extract the axes from the matrix.
FLOATNAME(LVector3) x, y, z;
mat.get_row(x,0);
mat.get_row(y,1);
mat.get_row(z,2);
// Project Z into the XZ plane.
FLOATNAME(LVector2) xz(z[0], z[2]);
xz = normalize(xz);
// Compute the rotation about the +Y (up) axis. This is yaw, or
// "heading".
FLOATTYPE heading = rad_2_deg(((FLOATTYPE)atan2(xz[0], xz[1])));
// Unwind the heading, and continue.
Matrix rot_y;
rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
CS_yup_right);
x = x * rot_y;
y = y * rot_y;
z = z * rot_y;
// Project the rotated Z into the YZ plane.
FLOATNAME(LVector2) yz(z[1], z[2]);
yz = normalize(yz);
// Compute the rotation about the +X (right) axis. This is pitch.
FLOATTYPE pitch = rad_2_deg((FLOATTYPE)(-atan2(yz[0], yz[1])));
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
CS_yup_right);
x = x * rot_x;
y = y * rot_x;
z = z * rot_x;
// Project the rotated X onto the XY plane.
FLOATNAME(LVector2) xy(x[0], x[1]);
xy = normalize(xy);
// Compute the rotation about the +Z (back) axis. This is roll.
FLOATTYPE roll = -rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0])));
// Unwind the roll from the axes, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
CS_yup_right);
x = x * rot_z;
y = y * rot_z;
z = z * rot_z;
// Reset the matrix to reflect the unwinding.
mat.set_row(0, x);
mat.set_row(1, y);
mat.set_row(2, z);
// Return the three rotation components.
hpr[0] = heading;
hpr[1] = pitch;
hpr[2] = roll;
}
////////////////////////////////////////////////////////////////////
// Function: unwind_zup_rotation_new_hpr
// Description: Extracts the rotation about the x, y, and z axes from
// the given hpr & scale matrix. Adjusts the matrix
// to eliminate the rotation.
//
// This function assumes the matrix is stored in a
// right-handed Z-up coordinate system.
////////////////////////////////////////////////////////////////////
static void
unwind_zup_rotation_new_hpr(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
typedef FLOATNAME(LMatrix3) Matrix;
// Extract the axes from the matrix.
FLOATNAME(LVector3) x, y, z;
mat.get_row(x,0);
mat.get_row(y,1);
mat.get_row(z,2);
// Project Y into the XY plane.
FLOATNAME(LVector2) xy(y[0], y[1]);
xy = normalize(xy);
// Compute the rotation about the +Z (up) axis. This is yaw, or
// "heading".
FLOATTYPE heading = -rad_2_deg(((FLOATTYPE)atan2(xy[0], xy[1])));
// Unwind the heading, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
CS_zup_right);
x = x * rot_z;
y = y * rot_z;
z = z * rot_z;
// Project the rotated Y into the YZ plane.
FLOATNAME(LVector2) yz(y[1], y[2]);
yz = normalize(yz);
// Compute the rotation about the +X (right) axis. This is pitch.
FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)atan2(yz[1], yz[0])));
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
CS_zup_right);
x = x * rot_x;
y = y * rot_x;
z = z * rot_x;
// Project X into the XZ plane.
FLOATNAME(LVector2) xz(x[0], x[2]);
xz = normalize(xz);
// Compute the rotation about the -Y (back) axis. This is roll.
FLOATTYPE roll = -rad_2_deg(((FLOATTYPE)atan2(xz[1], xz[0])));
// Unwind the roll from the axes, and continue.
Matrix rot_y;
rot_y = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
CS_zup_right);
x = x * rot_y;
y = y * rot_y;
z = z * rot_y;
// Reset the matrix to reflect the unwinding.
mat.set_row(0, x);
mat.set_row(1, y);
mat.set_row(2, z);
// Return the three rotation components.
hpr[0] = heading;
hpr[1] = pitch;
hpr[2] = roll;
}
////////////////////////////////////////////////////////////////////
// Function: decompose_matrix_new_hpr
// Description: Extracts out the components of a 3x3 rotation matrix.
// Returns true if the scale and hpr completely describe
// the matrix, or false if there is also a shear
// component or if the matrix is not affine.
//
// This flavor of the function accepts an expected roll
// amount. This amount will be used as the roll
// component, rather than attempting to determine roll
// by examining the matrix; this helps alleviate roll
// instability due to roundoff errors or gimbal lock.
//
// This function is deprecated and will soon be removed,
// especially when the need for temp_hpr_fix is
// eliminated.
// Returns true if successful, or false if there was an
// error. Since a 3x3 matrix always contains an affine
// transform, this should succeed in the normal case;
// singular transforms are not treated as an error.
////////////////////////////////////////////////////////////////////
bool
decompose_matrix(const FLOATNAME(LMatrix3) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &hpr,
FLOATTYPE roll,
CoordinateSystem cs) {
decompose_matrix_new_hpr(const FLOATNAME(LMatrix3) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &shear,
FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs) {
if (cs == CS_default) {
cs = get_default_coordinate_system();
}
if (linmath_cat.is_debug()) {
linmath_cat.debug()
<< "decomposing " << mat << " via cs " << cs
<< " with roll = " << roll << "\n";
<< "decomposing " << mat << " via cs " << cs << "\n";
}
// Extract the rotation and scale, according to the coordinate
@ -667,14 +526,14 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat,
switch (cs) {
case CS_zup_right:
{
unwind_zup_rotation(new_mat, hpr, roll);
unwind_zup_rotation_new_hpr(new_mat, hpr);
is_left_handed = false;
}
break;
case CS_yup_right:
{
unwind_yup_rotation(new_mat, hpr, roll);
unwind_yup_rotation_new_hpr(new_mat, hpr);
is_left_handed = false;
}
break;
@ -690,7 +549,9 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat,
mat(1, 0), mat(1, 1), -mat(1, 2),
-mat(2, 0), -mat(2, 1), mat(2, 2));
*/
unwind_zup_rotation(new_mat, hpr, roll);
unwind_zup_rotation_new_hpr(new_mat, hpr);
hpr[0] = -hpr[0];
hpr[2] = -hpr[2];
is_left_handed = true;
}
break;
@ -706,7 +567,7 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat,
mat(1, 0), mat(1, 1), -mat(1, 2),
-mat(2, 0), -mat(2, 1), mat(2, 2));
*/
unwind_yup_rotation(new_mat, hpr, roll);
unwind_yup_rotation_new_hpr(new_mat, hpr);
is_left_handed = true;
}
break;
@ -722,21 +583,53 @@ decompose_matrix(const FLOATNAME(LMatrix3) &mat,
<< "after unwind, mat is " << new_mat << "\n";
}
scale[0] = new_mat._m.m._00;
scale[1] = new_mat._m.m._11;
scale[2] = new_mat._m.m._22;
/*
if (is_left_handed) {
scale[0] = -new_mat._m.m._00;
scale[1] = -new_mat._m.m._11;
scale.set(new_mat(0, 0), new_mat(1, 1), new_mat(2, 2));
// Normalize the scale out of the shear components, and return the
// shear.
if (scale[0] != 0.0) {
new_mat(0, 1) /= scale[0];
new_mat(0, 2) /= scale[0];
}
if (scale[1] != 0.0) {
new_mat(1, 0) /= scale[1];
new_mat(1, 2) /= scale[1];
}
if (scale[2] != 0.0) {
new_mat(2, 0) /= scale[2];
new_mat(2, 1) /= scale[2];
}
*/
bool has_no_shear =
(fabs(new_mat(0, 1)) + fabs(new_mat(0, 2)) +
fabs(new_mat(1, 0)) + fabs(new_mat(1, 2)) +
fabs(new_mat(2, 0)) + fabs(new_mat(2, 1))) < 0.0001;
return has_no_shear;
shear.set(new_mat(0, 1) + new_mat(1, 0),
new_mat(2, 0) + new_mat(0, 2),
new_mat(2, 1) + new_mat(1, 2));
return true;
}
////////////////////////////////////////////////////////////////////
// Function: old_to_new_hpr
// Description: Converts the HPR as represented in the old, broken
// way to the new, correct representation. Returns the
// new HPR.
//
// This function is provided to ease transition from old
// systems that relied on Panda's original broken HPR
// calculation.
////////////////////////////////////////////////////////////////////
FLOATNAME(LVecBase3)
old_to_new_hpr(const FLOATNAME(LVecBase3) &old_hpr) {
FLOATNAME(LMatrix3) mat;
compose_matrix_old_hpr(mat,
FLOATNAME(LVecBase3)(1.0f, 1.0f, 1.0f),
FLOATNAME(LVecBase3)::zero(),
old_hpr);
FLOATNAME(LVecBase3) new_scale;
FLOATNAME(LVecBase3) new_shear;
FLOATNAME(LVecBase3) new_hpr;
decompose_matrix_new_hpr(mat, new_scale, new_shear, new_hpr);
return new_hpr;
}

105
panda/src/linmath/compose_matrix_src.h
View File

@ -18,7 +18,7 @@
BEGIN_PUBLISH
EXPCL_PANDA void
INLINE_LINMATH void
compose_matrix(FLOATNAME(LMatrix3) &mat,
const FLOATNAME(LVecBase3) &scale,
const FLOATNAME(LVecBase3) &shear,
@ -34,10 +34,11 @@ compose_matrix(FLOATNAME(LMatrix4) &mat,
CoordinateSystem cs = CS_default);
INLINE_LINMATH void
compose_matrix(FLOATNAME(LMatrix4) &mat, const FLOATTYPE components[num_matrix_components],
compose_matrix(FLOATNAME(LMatrix4) &mat,
const FLOATTYPE components[num_matrix_components],
CoordinateSystem cs = CS_default);
EXPCL_PANDA bool
INLINE_LINMATH bool
decompose_matrix(const FLOATNAME(LMatrix3) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &shear,
@ -61,7 +62,7 @@ decompose_matrix(const FLOATNAME(LMatrix4) &mat, FLOATTYPE components[num_matrix
// The following functions are deprecated; they have been replaced
// with new versions, above, that accept a shear component as well.
INLINE EXPCL_PANDA void
INLINE_LINMATH void
compose_matrix(FLOATNAME(LMatrix3) &mat,
const FLOATNAME(LVecBase3) &scale,
const FLOATNAME(LVecBase3) &hpr,
@ -74,19 +75,12 @@ compose_matrix(FLOATNAME(LMatrix4) &mat,
const FLOATNAME(LVecBase3) &translate,
CoordinateSystem cs = CS_default);
INLINE EXPCL_PANDA bool
INLINE_LINMATH bool
decompose_matrix(const FLOATNAME(LMatrix3) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs = CS_default);
EXPCL_PANDA bool
decompose_matrix(const FLOATNAME(LMatrix3) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &hpr,
FLOATTYPE roll,
CoordinateSystem cs);
INLINE_LINMATH bool
decompose_matrix(const FLOATNAME(LMatrix4) &mat,
FLOATNAME(LVecBase3) &scale,
@ -94,6 +88,93 @@ decompose_matrix(const FLOATNAME(LMatrix4) &mat,
FLOATNAME(LVecBase3) &translate,
CoordinateSystem cs = CS_default);
// The following functions are transitional and serve only to migrate
// code from the old, incorrect hpr calculations that Panda used to
// use. New code should not call these functions directly; use the
// unqualified functions, above, instead.
EXPCL_PANDA void
compose_matrix_old_hpr(FLOATNAME(LMatrix3) &mat,
const FLOATNAME(LVecBase3) &scale,
const FLOATNAME(LVecBase3) &shear,
const FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs = CS_default);
INLINE_LINMATH void
compose_matrix_old_hpr(FLOATNAME(LMatrix4) &mat,
const FLOATNAME(LVecBase3) &scale,
const FLOATNAME(LVecBase3) &shear,
const FLOATNAME(LVecBase3) &hpr,
const FLOATNAME(LVecBase3) &translate,
CoordinateSystem cs = CS_default);
INLINE_LINMATH void
compose_matrix_old_hpr(FLOATNAME(LMatrix4) &mat, const FLOATTYPE components[num_matrix_components],
CoordinateSystem cs = CS_default);
EXPCL_PANDA bool
decompose_matrix_old_hpr(const FLOATNAME(LMatrix3) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &shear,
FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs = CS_default);
INLINE_LINMATH bool
decompose_matrix_old_hpr(const FLOATNAME(LMatrix4) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &shear,
FLOATNAME(LVecBase3) &hpr,
FLOATNAME(LVecBase3) &translate,
CoordinateSystem cs = CS_default);
INLINE_LINMATH bool
decompose_matrix_old_hpr(const FLOATNAME(LMatrix4) &mat, FLOATTYPE components[num_matrix_components],
CoordinateSystem CS = CS_default);
EXPCL_PANDA void
compose_matrix_new_hpr(FLOATNAME(LMatrix3) &mat,
const FLOATNAME(LVecBase3) &scale,
const FLOATNAME(LVecBase3) &shear,
const FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs = CS_default);
INLINE_LINMATH void
compose_matrix_new_hpr(FLOATNAME(LMatrix4) &mat,
const FLOATNAME(LVecBase3) &scale,
const FLOATNAME(LVecBase3) &shear,
const FLOATNAME(LVecBase3) &hpr,
const FLOATNAME(LVecBase3) &translate,
CoordinateSystem cs = CS_default);
INLINE_LINMATH void
compose_matrix_new_hpr(FLOATNAME(LMatrix4) &mat, const FLOATTYPE components[num_matrix_components],
CoordinateSystem cs = CS_default);
EXPCL_PANDA bool
decompose_matrix_new_hpr(const FLOATNAME(LMatrix3) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &shear,
FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs = CS_default);
INLINE_LINMATH bool
decompose_matrix_new_hpr(const FLOATNAME(LMatrix4) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &shear,
FLOATNAME(LVecBase3) &hpr,
FLOATNAME(LVecBase3) &translate,
CoordinateSystem cs = CS_default);
INLINE_LINMATH bool
decompose_matrix_new_hpr(const FLOATNAME(LMatrix4) &mat, FLOATTYPE components[num_matrix_components],
CoordinateSystem CS = CS_default);
EXPCL_PANDA FLOATNAME(LVecBase3)
old_to_new_hpr(const FLOATNAME(LVecBase3) &old_hpr);
END_PUBLISH
#include "compose_matrix_src.I"

36
panda/src/mathutil/fftCompressor.cxx
View File

@ -18,6 +18,7 @@
#include "fftCompressor.h"
#include "config_mathutil.h"
#include "config_linmath.h"
#include "datagram.h"
#include "datagramIterator.h"
@ -646,9 +647,15 @@ read_reals(DatagramIterator &di, vector_float &array) {
// onto the end of the indicated vector, which is not
// cleared first; it is the user's responsibility to
// ensure that the array is initially empty.
//
// new_hpr is a temporary, transitional parameter. If
// it is set false, the hprs are decompressed according
// to the old, broken hpr calculation; if true, the hprs
// are decompressed according to the new, correct hpr
// calculation. See temp_hpr_fix.
////////////////////////////////////////////////////////////////////
bool FFTCompressor::
read_hprs(DatagramIterator &di, vector_LVecBase3f &array) {
read_hprs(DatagramIterator &di, vector_LVecBase3f &array, bool new_hpr) {
#ifndef NDEBUG
if (_quality >= 104) {
// If quality level is at least 104, we don't even convert hpr to
@ -663,7 +670,7 @@ read_hprs(DatagramIterator &di, vector_LVecBase3f &array) {
if (okflag) {
nassertr(h.size() == p.size() && p.size() == r.size(), false);
for (int i = 0; i < (int)h.size(); i++) {
array.push_back(LVecBase3f(h[i], p[i], r[i]));
array.push_back(LVecBase3f(h[i], p[i], r[i]));
}
}
@ -694,7 +701,11 @@ read_hprs(DatagramIterator &di, vector_LVecBase3f &array) {
m10[i], m11[i], m12[i],
m20[i], m21[i], m22[i]);
LVecBase3f scale, shear, hpr;
decompose_matrix(mat, scale, shear, hpr);
if (new_hpr) {
decompose_matrix_new_hpr(mat, scale, shear, hpr);
} else {
decompose_matrix_old_hpr(mat, scale, shear, hpr);
}
array.push_back(hpr);
}
}
@ -753,7 +764,11 @@ read_hprs(DatagramIterator &di, vector_LVecBase3f &array) {
mat.transpose_in_place();
}
LVecBase3f scale, shear, hpr;
decompose_matrix(mat, scale, shear, hpr);
if (new_hpr) {
decompose_matrix_new_hpr(mat, scale, shear, hpr);
} else {
decompose_matrix_old_hpr(mat, scale, shear, hpr);
}
array.push_back(hpr);
}
}
@ -761,6 +776,19 @@ read_hprs(DatagramIterator &di, vector_LVecBase3f &array) {
return okflag;
}
////////////////////////////////////////////////////////////////////
// Function: FFTCompressor::read_hprs
// Access: Public
// Description: Reads an array of HPR angles. The result is pushed
// onto the end of the indicated vector, which is not
// cleared first; it is the user's responsibility to
// ensure that the array is initially empty.
////////////////////////////////////////////////////////////////////
bool FFTCompressor::
read_hprs(DatagramIterator &di, vector_LVecBase3f &array) {
return read_hprs(di, array, temp_hpr_fix);
}
////////////////////////////////////////////////////////////////////
// Function: FFTCompressor::free_storage

2
panda/src/mathutil/fftCompressor.h
View File

@ -66,6 +66,8 @@ public:
bool read_header(DatagramIterator &di, int bam_minor_version);
bool read_reals(DatagramIterator &di, vector_float &array);
bool read_hprs(DatagramIterator &di, vector_LVecBase3f &array,
bool new_hpr);
bool read_hprs(DatagramIterator &di, vector_LVecBase3f &array);
static void free_storage();

3
panda/src/putil/bam.h
View File

@ -34,7 +34,7 @@ static const unsigned short _bam_major_ver = 4;
// Bumped to major version 3 on 12/8/00 to change float64's to float32's.
// Bumped to major version 4 on 4/10/02 to store new scene graph.
static const unsigned short _bam_minor_ver = 13;
static const unsigned short _bam_minor_ver = 14;
// Bumped to minor version 1 on 4/10/03 to add CullFaceAttrib::reverse.
// Bumped to minor version 1 on 4/10/03 to add CullFaceAttrib::reverse.
// Bumped to minor version 2 on 4/12/03 to add num_components to texture.
@ -49,6 +49,7 @@ static const unsigned short _bam_minor_ver = 13;
// Bumped to minor version 11 on 07/26/04 to add multitexture pointers.
// Bumped to minor version 12 on 09/22/04 to add PandaNode::into_collide_mask.
// Bumped to minor version 13 on 09/24/04 to store actual LODNode switch distances instead of squares.
// Bumped to minor version 14 on 11/18/04 to differentiate old_hpr from new_hpr in compressed anim channels.
#endif