From 4e8ff2a2e3189b80e679cfead897cbda7ac4ecc2 Mon Sep 17 00:00:00 2001 From: David Rose Date: Tue, 17 Apr 2001 18:41:49 +0000 Subject: [PATCH] *** empty log message *** --- panda/src/egg/config_egg.cxx | 4 + panda/src/egg/config_egg.h | 1 + panda/src/egg/eggXfmAnimData.I | 19 +- panda/src/egg/eggXfmAnimData.cxx | 9 +- panda/src/egg/eggXfmAnimData.h | 3 +- panda/src/egg/eggXfmSAnim.I | 19 +- panda/src/egg/eggXfmSAnim.cxx | 297 +++++++++++++++++------ panda/src/egg/eggXfmSAnim.h | 16 ++ panda/src/egg2sg/animBundleMaker.cxx | 4 + panda/src/linmath/compose_matrix.cxx | 1 + panda/src/linmath/compose_matrix_src.cxx | 208 +++++++++++----- 11 files changed, 437 insertions(+), 144 deletions(-) diff --git a/panda/src/egg/config_egg.cxx b/panda/src/egg/config_egg.cxx index 9302d4c0f0..7d61c34965 100644 --- a/panda/src/egg/config_egg.cxx +++ b/panda/src/egg/config_egg.cxx @@ -53,6 +53,10 @@ get_egg_path() { return get_config_path("egg-path", egg_path); } +// Set this true to support loading of old character animation files, which +// had the convention that the order "phr" implied a reversed roll. +bool egg_support_old_anims = config_egg.GetBool("egg-support-old-anims", true); + //////////////////////////////////////////////////////////////////// // Function: init_libegg // Description: Initializes the library. This must be called at diff --git a/panda/src/egg/config_egg.h b/panda/src/egg/config_egg.h index afd12afba0..3e4cccd104 100644 --- a/panda/src/egg/config_egg.h +++ b/panda/src/egg/config_egg.h @@ -14,6 +14,7 @@ class DSearchPath; NotifyCategoryDecl(egg, EXPCL_PANDAEGG, EXPTP_PANDAEGG); const DSearchPath &get_egg_path(); +extern bool egg_support_old_anims; extern EXPCL_PANDAEGG void init_libegg(); diff --git a/panda/src/egg/eggXfmAnimData.I b/panda/src/egg/eggXfmAnimData.I index 42138381ea..04abe5d0db 100644 --- a/panda/src/egg/eggXfmAnimData.I +++ b/panda/src/egg/eggXfmAnimData.I @@ -82,7 +82,24 @@ has_order() const { //////////////////////////////////////////////////////////////////// INLINE const string &EggXfmAnimData:: get_order() const { - return _order; + if (has_order()) { + return _order; + } else { + return get_standard_order(); + } +} + +//////////////////////////////////////////////////////////////////// +// Function: EggXfmAnimData::get_standard_order +// Access: Public, Static +// Description: Returns the standard order of matrix component +// composition. This is what the order string must be +// set to in order to use set_value() or add_data() +// successfully. +//////////////////////////////////////////////////////////////////// +INLINE const string &EggXfmAnimData:: +get_standard_order() { + return EggXfmSAnim::get_standard_order(); } diff --git a/panda/src/egg/eggXfmAnimData.cxx b/panda/src/egg/eggXfmAnimData.cxx index 796b64cbee..bcbbead631 100644 --- a/panda/src/egg/eggXfmAnimData.cxx +++ b/panda/src/egg/eggXfmAnimData.cxx @@ -131,7 +131,8 @@ get_value(int row, LMatrix4d &mat) const { } // So now we've got the nine components; build a matrix. - compose_matrix(mat, scale, hpr, translate, _coordsys); + EggXfmSAnim::compose_with_order(mat, scale, hpr, translate, get_order(), + _coordsys); } //////////////////////////////////////////////////////////////////// @@ -193,9 +194,9 @@ r_transform(const LMatrix4d &mat, const LMatrix4d &inv, if (has_fps()) { new_table.set_fps(get_fps()); } - if (has_order()) { - new_table.set_order(get_order()); - } + + // We insist on the standard order now. + new_table.set_order(get_standard_order()); // Now build up the data into the new table. LMatrix4d orig_mat; diff --git a/panda/src/egg/eggXfmAnimData.h b/panda/src/egg/eggXfmAnimData.h index bc70ec6798..ef186d1747 100644 --- a/panda/src/egg/eggXfmAnimData.h +++ b/panda/src/egg/eggXfmAnimData.h @@ -9,8 +9,8 @@ #include #include "eggAnimData.h" +#include "eggXfmSAnim.h" -class EggXfmSAnim; //////////////////////////////////////////////////////////////////// // Class : EggXfmAnimData @@ -33,6 +33,7 @@ public: INLINE void clear_order(); INLINE bool has_order() const; INLINE const string &get_order() const; + INLINE static const string &get_standard_order(); INLINE void set_contents(const string &contents); INLINE void clear_contents(); diff --git a/panda/src/egg/eggXfmSAnim.I b/panda/src/egg/eggXfmSAnim.I index 02656fe6cd..1211e68779 100644 --- a/panda/src/egg/eggXfmSAnim.I +++ b/panda/src/egg/eggXfmSAnim.I @@ -128,7 +128,24 @@ has_order() const { //////////////////////////////////////////////////////////////////// INLINE const string &EggXfmSAnim:: get_order() const { - return _order; + if (has_order()) { + return _order; + } else { + return get_standard_order(); + } +} + +//////////////////////////////////////////////////////////////////// +// Function: EggXfmSAnim::get_standard_order +// Access: Public, Static +// Description: Returns the standard order of matrix component +// composition. This is what the order string must be +// set to in order to use set_value() or add_data() +// successfully. +//////////////////////////////////////////////////////////////////// +INLINE const string &EggXfmSAnim:: +get_standard_order() { + return _standard_order; } //////////////////////////////////////////////////////////////////// diff --git a/panda/src/egg/eggXfmSAnim.cxx b/panda/src/egg/eggXfmSAnim.cxx index 4957fbde93..4c6113a4ed 100644 --- a/panda/src/egg/eggXfmSAnim.cxx +++ b/panda/src/egg/eggXfmSAnim.cxx @@ -7,6 +7,7 @@ #include "eggSAnimData.h" #include "eggXfmAnimData.h" #include "eggParameters.h" +#include "config_egg.h" #include #include @@ -15,6 +16,14 @@ TypeHandle EggXfmSAnim::_type_handle; +//string EggXfmSAnim::_standard_order = "srpht"; + +// For now, the standard order is sphrt. This matches the old, +// incorrect behavior of decompose_matrix(). When we have a new +// egg-optchar, we can safely remove the old decompose_matrix() and +// restore the correct standard order (above). +string EggXfmSAnim::_standard_order = "sphrt"; + //////////////////////////////////////////////////////////////////// // Function: EggXfmSAnim::Conversion constructor @@ -86,6 +95,23 @@ optimize() { } } +//////////////////////////////////////////////////////////////////// +// Function: EggXfmSAnim::optimize_to_standard_order +// Access: Public +// Description: Optimizes the table by collapsing redundant +// sub-tables, and simultaneously ensures that the order +// string is the standard order (which is the same as +// that supported by compose_matrix() and +// decompose_matrix()). +//////////////////////////////////////////////////////////////////// +void EggXfmSAnim:: +optimize_to_standard_order() { + if (get_order() != get_standard_order()) { + normalize_by_rebuilding(); + } + optimize(); +} + //////////////////////////////////////////////////////////////////// // Function: EggXfmSAnim::normalize // Access: Public @@ -97,71 +123,17 @@ optimize() { //////////////////////////////////////////////////////////////////// void EggXfmSAnim:: normalize() { - iterator ci; + if (get_order() != get_standard_order()) { + // If our order string is wrong, we must fix it now. This will + // incidentally also normalize the table, because we are totally + // rebuilding it. + normalize_by_rebuilding(); - // First, determine which tables we already have, and how long they - // are. - int num_tables = 0; - int table_length = 1; - string remaining_tables = "ijkhprxyz"; - - for (ci = begin(); ci != end(); ++ci) { - if ((*ci)->is_of_type(EggSAnimData::get_class_type())) { - EggSAnimData *sanim = DCAST(EggSAnimData, *ci); - - nassertv(sanim->get_name().length() == 1); - char name = sanim->get_name()[0]; - size_t p = remaining_tables.find(name); - nassertv(p != string::npos); - remaining_tables[p] = ' '; - - num_tables++; - if (sanim->get_num_rows() > 1) { - if (table_length == 1) { - table_length = sanim->get_num_rows(); - } else { - nassertv(sanim->get_num_rows() == table_length); - } - } - } - } - - if (num_tables < 9) { - // Create new, default, children for each table we lack. - for (size_t p = 0; p < remaining_tables.length(); p++) { - if (remaining_tables[p] != ' ') { - double default_value; - switch (remaining_tables[p]) { - case 'i': - case 'j': - case 'k': - default_value = 1.0; - break; - - default: - default_value = 0.0; - } - - string name(1, remaining_tables[p]); - EggSAnimData *sanim = new EggSAnimData(name); - add_child(sanim); - sanim->add_data(default_value); - } - } - } - - // Now expand any one-row tables as needed. - for (ci = begin(); ci != end(); ++ci) { - if ((*ci)->is_of_type(EggSAnimData::get_class_type())) { - EggSAnimData *sanim = DCAST(EggSAnimData, *ci); - if (sanim->get_num_rows() == 1) { - double value = sanim->get_value(0); - for (int i = 1; i < table_length; i++) { - sanim->add_data(value); - } - } - nassertv(sanim->get_num_rows() == table_length); - } + } else { + // Otherwise, if the order string is already the standard order + // string, we can do this the easy way (from a computational + // standpoint), which is just to lengthen the tables directly. + normalize_by_expanding(); } } @@ -190,6 +162,67 @@ write(ostream &out, int indent_level) const { indent(out, indent_level) << "}\n"; } + +//////////////////////////////////////////////////////////////////// +// Function: EggXfmSAnim::compose_with_order +// Access: Public, Static +// Description: Composes a matrix out of the nine individual +// components, respecting the order string. The +// components will be applied in the order indicated by +// the string. +//////////////////////////////////////////////////////////////////// +void EggXfmSAnim:: +compose_with_order(LMatrix4d &mat, + const LVecBase3d &scale, + const LVecBase3d &hpr, + const LVecBase3d &trans, + const string &order, + CoordinateSystem cs) { + + mat = LMatrix4d::ident_mat(); + + bool reverse_roll = false; + + if (order == "sphrt" && egg_support_old_anims) { + // As a special case, if the order string is exactly "sphrt" + // (which is what all our legacy anim files used), we interpret + // roll in the opposite direction (as our legacy anim files did). + reverse_roll = true; + } + + string::const_iterator pi; + for (pi = order.begin(); pi != order.end(); ++pi) { + switch (*pi) { + case 's': + mat = mat * LMatrix4d::scale_mat(scale); + break; + + case 'h': + mat = mat * LMatrix4d::rotate_mat(hpr[0], LVector3d::up(cs), cs); + break; + + case 'p': + mat = mat * LMatrix4d::rotate_mat(hpr[1], LVector3d::right(cs), cs); + break; + + case 'r': + if (reverse_roll) { + mat = mat * LMatrix4d::rotate_mat(-hpr[2], LVector3d::forward(cs), cs); + } else { + mat = mat * LMatrix4d::rotate_mat(hpr[2], LVector3d::forward(cs), cs); + } + break; + + case 't': + mat = mat * LMatrix4d::translate_mat(trans); + break; + + default: + egg_cat.warning() + << "Invalid letter in order string: " << *pi << "\n"; + } + } +} //////////////////////////////////////////////////////////////////// // Function: EggXfmSAnim::get_num_rows @@ -232,7 +265,7 @@ get_num_rows() const { // table of matrices. It is an error to call this if // any SAnimData children of this node have an improper // name (e.g. not a single letter, or not one of -// "ijkphrxyz"). +// "ijkhprxyz"). //////////////////////////////////////////////////////////////////// void EggXfmSAnim:: get_value(int row, LMatrix4d &mat) const { @@ -307,7 +340,7 @@ get_value(int row, LMatrix4d &mat) const { } // So now we've got the nine components; build a matrix. - compose_matrix(mat, scale, hpr, translate, _coordsys); + compose_with_order(mat, scale, hpr, translate, get_order(), _coordsys); } //////////////////////////////////////////////////////////////////// @@ -326,6 +359,8 @@ get_value(int row, LMatrix4d &mat) const { //////////////////////////////////////////////////////////////////// bool EggXfmSAnim:: set_value(int row, const LMatrix4d &mat) { + nassertr(get_order() == get_standard_order(), false); + LVector3d scale, hpr, translate; bool result = decompose_matrix(mat, scale, hpr, translate, _coordsys); if (!result) { @@ -416,11 +451,16 @@ set_value(int row, const LMatrix4d &mat) { // table of matrices. It is an error to call this if // any SAnimData children of this node have an improper // name (e.g. not a single letter, or not one of -// "ijkphrxyz"). +// "ijkhprxyz"). // // This function has the further requirement that all // nine of the subtables must exist and be of the same -// length. Thus, you probably cannot take an existing +// length. Furthermore, the order string must be the +// standard order string, "ijkrphxyz", which matches the +// system compose_matrix() and decompose_matrix() +// functions. +// +// Thus, you probably cannot take an existing // EggXfmSAnim object and start adding matrices to the // end; you must clear out the original data first. (As // a special exception, if no tables exist, they will be @@ -442,13 +482,18 @@ add_data(const LMatrix4d &mat) { if (empty()) { // If we have no children, create all nine tables now. - const char *table_ids = "ijkphrxyz"; + const char *table_ids = "ijkhprxyz"; for (const char *p = table_ids; *p; p++) { EggSAnimData *sanim = new EggSAnimData(string(1, *p)); add_child(sanim); } + + // Also insist on the correct ordering right off the bat. + set_order(get_standard_order()); } + nassertr(get_order() == get_standard_order(), false); + #ifndef NDEBUG int table_length = -1; int num_tables = 0; @@ -526,7 +571,8 @@ add_data(const LMatrix4d &mat) { // Access: Protected, Virtual // Description: Applies the indicated transform to all the rows of // the table. This actually forces the generation of a -// totally new set of rows. +// totally new set of rows, and will quietly change the +// order to the standard order (if it is different). //////////////////////////////////////////////////////////////////// void EggXfmSAnim:: r_transform(const LMatrix4d &mat, const LMatrix4d &inv, @@ -575,3 +621,114 @@ void EggXfmSAnim:: r_mark_coordsys(CoordinateSystem cs) { _coordsys = cs; } + +//////////////////////////////////////////////////////////////////// +// Function: EggXfmSAnim::normalize_by_rebuilding +// Access: Private +// Description: One implementation of normalize() that rebuilds the +// entire table by composing and decomposing the rows. +// This has the advantage that it will also reset the +// order string to the standard order string, but it is +// more computationally intensive and is subject to +// roundoff error. +//////////////////////////////////////////////////////////////////// +void EggXfmSAnim:: +normalize_by_rebuilding() { + // Save a temporary copy of the original data. + EggXfmSAnim original; + original.steal_children(*this); + original = (*this); + + // Now we have no children, so our data is clear. Rebuild it. + int num_rows = original.get_num_rows(); + LMatrix4d orig_mat; + for (int r = 0; r < num_rows; r++) { + original.get_value(r, orig_mat); + bool result = add_data(orig_mat); + + // If this assertion fails, we somehow got a matrix out of the + // original table that we could not represent in the new table. + // That shouldn't be possible; there's probably something wrong + // in decompose_matrix(). + nassertv(result); + } +} + +//////////////////////////////////////////////////////////////////// +// Function: EggXfmSAnim::normalize_by_expanding +// Access: Private +// Description: Another implementation of normalize() that simply +// expands any one-row tables and creates default-valued +// tables where none were before. This will not change +// the order string, but is much faster and does not +// introduce roundoff error. +//////////////////////////////////////////////////////////////////// +void EggXfmSAnim:: +normalize_by_expanding() { + iterator ci; + + // First, determine which tables we already have, and how long they + // are. + int num_tables = 0; + int table_length = 1; + string remaining_tables = "ijkhprxyz"; + + for (ci = begin(); ci != end(); ++ci) { + if ((*ci)->is_of_type(EggSAnimData::get_class_type())) { + EggSAnimData *sanim = DCAST(EggSAnimData, *ci); + + nassertv(sanim->get_name().length() == 1); + char name = sanim->get_name()[0]; + size_t p = remaining_tables.find(name); + nassertv(p != string::npos); + remaining_tables[p] = ' '; + + num_tables++; + if (sanim->get_num_rows() > 1) { + if (table_length == 1) { + table_length = sanim->get_num_rows(); + } else { + nassertv(sanim->get_num_rows() == table_length); + } + } + } + } + + if (num_tables < 9) { + // Create new, default, children for each table we lack. + for (size_t p = 0; p < remaining_tables.length(); p++) { + if (remaining_tables[p] != ' ') { + double default_value; + switch (remaining_tables[p]) { + case 'i': + case 'j': + case 'k': + default_value = 1.0; + break; + + default: + default_value = 0.0; + } + + string name(1, remaining_tables[p]); + EggSAnimData *sanim = new EggSAnimData(name); + add_child(sanim); + sanim->add_data(default_value); + } + } + } + + // Now expand any one-row tables as needed. + for (ci = begin(); ci != end(); ++ci) { + if ((*ci)->is_of_type(EggSAnimData::get_class_type())) { + EggSAnimData *sanim = DCAST(EggSAnimData, *ci); + if (sanim->get_num_rows() == 1) { + double value = sanim->get_value(0); + for (int i = 1; i < table_length; i++) { + sanim->add_data(value); + } + } + nassertv(sanim->get_num_rows() == table_length); + } + } +} diff --git a/panda/src/egg/eggXfmSAnim.h b/panda/src/egg/eggXfmSAnim.h index d4fd6133ac..a5fa1cc29c 100644 --- a/panda/src/egg/eggXfmSAnim.h +++ b/panda/src/egg/eggXfmSAnim.h @@ -39,10 +39,12 @@ public: INLINE void clear_order(); INLINE bool has_order() const; INLINE const string &get_order() const; + INLINE static const string &get_standard_order(); INLINE CoordinateSystem get_coordinate_system() const; void optimize(); + void optimize_to_standard_order(); void normalize(); int get_num_rows() const; @@ -54,16 +56,30 @@ public: virtual void write(ostream &out, int indent_level) const; + static void compose_with_order(LMatrix4d &mat, + const LVecBase3d &scale, + const LVecBase3d &hpr, + const LVecBase3d &trans, + const string &order, + CoordinateSystem cs); + protected: virtual void r_transform(const LMatrix4d &mat, const LMatrix4d &inv, CoordinateSystem to_cs); virtual void r_mark_coordsys(CoordinateSystem cs); +private: + void normalize_by_rebuilding(); + void normalize_by_expanding(); + + private: double _fps; bool _has_fps; string _order; CoordinateSystem _coordsys; + + static string _standard_order; public: diff --git a/panda/src/egg2sg/animBundleMaker.cxx b/panda/src/egg2sg/animBundleMaker.cxx index 85f175e8ab..81690edb7d 100644 --- a/panda/src/egg2sg/animBundleMaker.cxx +++ b/panda/src/egg2sg/animBundleMaker.cxx @@ -274,6 +274,10 @@ create_xfm_channel(EggNode *egg_node, const string &name, AnimChannelMatrixXfmTable *AnimBundleMaker:: create_xfm_channel(EggXfmSAnim *egg_anim, const string &name, AnimGroup *parent) { + // Ensure that the anim table is optimal and that it is standard + // order. + egg_anim->optimize_to_standard_order(); + AnimChannelMatrixXfmTable *table = new AnimChannelMatrixXfmTable(parent, name); diff --git a/panda/src/linmath/compose_matrix.cxx b/panda/src/linmath/compose_matrix.cxx index 07ac175034..994b22f9f6 100644 --- a/panda/src/linmath/compose_matrix.cxx +++ b/panda/src/linmath/compose_matrix.cxx @@ -11,6 +11,7 @@ Configure(config_linmath_2); ConfigureFn(config_linmath_2) { } + static const bool temp_hpr_fix = config_linmath_2.GetBool("temp-hpr-fix", false); #include "fltnames.h" diff --git a/panda/src/linmath/compose_matrix_src.cxx b/panda/src/linmath/compose_matrix_src.cxx index 9375aa772c..5d20fa6a68 100644 --- a/panda/src/linmath/compose_matrix_src.cxx +++ b/panda/src/linmath/compose_matrix_src.cxx @@ -16,7 +16,7 @@ compose_matrix(FLOATNAME(LMatrix3) &mat, if (temp_hpr_fix) { mat = FLOATNAME(LMatrix3)::scale_mat(scale) * - FLOATNAME(LMatrix3)::rotate_mat(hpr[2], FLOATNAME(LVector3)::back(cs), cs) * + FLOATNAME(LMatrix3)::rotate_mat(hpr[2], FLOATNAME(LVector3)::forward(cs), cs) * FLOATNAME(LMatrix3)::rotate_mat(hpr[1], FLOATNAME(LVector3)::right(cs), cs) * FLOATNAME(LMatrix3)::rotate_mat(hpr[0], FLOATNAME(LVector3)::up(cs), cs); } else { @@ -39,72 +39,141 @@ compose_matrix(FLOATNAME(LMatrix3) &mat, //////////////////////////////////////////////////////////////////// static void unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { - typedef FLOATNAME(LMatrix3) Matrix; + if (temp_hpr_fix) { + typedef FLOATNAME(LMatrix3) Matrix; - // Extract the axes from the matrix. - FLOATNAME(LVector3) x, y, z; - x = mat.get_row(0); - y = mat.get_row(1); - z = mat.get_row(2); + // Extract the axes from the matrix. + FLOATNAME(LVector3) x, y, z; + x = mat.get_row(0); + y = mat.get_row(1); + z = mat.get_row(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(atan2(xz[0], xz[1])); + + // Unwind the heading, and continue. + Matrix rot_y; + rot_y = Matrix::rotate_mat(-heading, FLOATNAME(LVector3)(0.0, 1.0, 0.0), + 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(-atan2(yz[0], yz[1])); + + // Unwind the pitch. + Matrix rot_x; + rot_x = Matrix::rotate_mat(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0), + 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(atan2(xy[1], xy[0])); - // 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(atan2(xy[1], xy[0])); - - // Unwind the roll from the axes, and continue. - Matrix rot_z; - rot_z = Matrix::rotate_mat(-roll, FLOATNAME(LVector3)(0.0, 0.0, 1.0), - 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(-atan2(xz[1], xz[0])); - - // Unwind the heading, and continue. - Matrix rot_y; - rot_y = Matrix::rotate_mat(-heading, FLOATNAME(LVector3)(0.0, 1.0, 0.0), - 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(-atan2(yz[0], yz[1])); - - // Unwind the pitch. - Matrix rot_x; - rot_x = Matrix::rotate_mat(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0), - 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; + // Unwind the roll from the axes, and continue. + Matrix rot_z; + rot_z = Matrix::rotate_mat(-roll, FLOATNAME(LVector3)(0.0, 0.0, 1.0), + 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 { + typedef FLOATNAME(LMatrix3) Matrix; + + // Extract the axes from the matrix. + FLOATNAME(LVector3) x, y, z; + x = mat.get_row(0); + y = mat.get_row(1); + z = mat.get_row(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(atan2(xy[1], xy[0])); + + // Unwind the roll from the axes, and continue. + Matrix rot_z; + rot_z = Matrix::rotate_mat(-roll, FLOATNAME(LVector3)(0.0, 0.0, 1.0), + 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(-atan2(xz[1], xz[0])); + + // Unwind the heading, and continue. + Matrix rot_y; + rot_y = Matrix::rotate_mat(-heading, FLOATNAME(LVector3)(0.0, 1.0, 0.0), + 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(-atan2(yz[0], yz[1])); + + // Unwind the pitch. + Matrix rot_x; + rot_x = Matrix::rotate_mat(-pitch, FLOATNAME(LVector3)(1.0, 0.0, 0.0), + 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; + } } //////////////////////////////////////////////////////////////////// @@ -120,6 +189,11 @@ unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { static void unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr, FLOATTYPE roll) { + if (temp_hpr_fix) { + unwind_yup_rotation(mat, hpr); + return; + } + typedef FLOATNAME(LMatrix3) Matrix; // Extract the axes from the matrix. @@ -239,11 +313,11 @@ unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) { xz = normalize(xz); // Compute the rotation about the -Y (back) axis. This is roll. - FLOATTYPE roll = rad_2_deg(atan2(xz[1], xz[0])); + FLOATTYPE roll = -rad_2_deg(atan2(xz[1], xz[0])); // Unwind the roll from the axes, and continue. Matrix rot_y; - rot_y = Matrix::rotate_mat(roll, FLOATNAME(LVector3)(0.0, 1.0, 0.0), + rot_y = Matrix::rotate_mat(-roll, FLOATNAME(LVector3)(0.0, 1.0, 0.0), CS_zup_right); x = x * rot_y;