robustify quats

panda/src/linmath/compose_matrix.cxx

@@ -20,13 +20,6 @@
 #include "config_linmath.h"
 #include "compose_matrix.h"
 
-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"
 #include "compose_matrix_src.cxx"
 

panda/src/linmath/config_linmath.cxx

@@ -36,6 +36,15 @@ ConfigureFn(config_linmath) {
 // The default is true for now.
 const bool paranoid_hpr_quat = config_linmath.GetBool("paranoid-hpr-quat", true);
 
+// Set this true to compute hpr's correctly.  Presently, we apply
+// these in the wrong order, and roll is backwards relative to the
+// other two.  But we can't globally fix this because some of our old
+// tools, most notably egg-optchar, depend on the old broken behavior.
+// Until we are able to rewrite these tools into the new system, we
+// must keep the old behavior; setting this switch lets you use the
+// new, correct behavior but you don't get animated characters.
+const bool temp_hpr_fix = config_linmath.GetBool("temp-hpr-fix", false);
+
 ////////////////////////////////////////////////////////////////////
 //     Function: init_liblinmath
 //  Description: Initializes the library.  This must be called at

panda/src/linmath/lquaternion_src.I

@@ -31,8 +31,8 @@ FLOATNAME(LQuaternion)() {
 //  Description:
 ////////////////////////////////////////////////////////////////////
 INLINE_LINMATH FLOATNAME(LQuaternion)::
-FLOATNAME(LQuaternion)(const FLOATNAME(LQuaternion) &c) :
-  FLOATNAME(LVecBase4)(c)
+FLOATNAME(LQuaternion)(const FLOATNAME(LVecBase4) &copy) :
+  FLOATNAME(LVecBase4)(copy)
 {
 }
 
@@ -46,9 +46,25 @@ FLOATNAME(LQuaternion)(FLOATTYPE r, FLOATTYPE i, FLOATTYPE j, FLOATTYPE k) {
   set(r, i, j, k);
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: LQuaternion::xform
+//       Access: Published
+//  Description: Transforms a 3-d vector by the indicated rotation
+////////////////////////////////////////////////////////////////////
+INLINE_LINMATH FLOATNAME(LVecBase3) FLOATNAME(LQuaternion)::
+xform(const FLOATNAME(LVecBase3) &v) const {
+  FLOATNAME(LQuaternion) v_quat(0.0f, v[0], v[1], v[2]);
+
+  FLOATNAME(LQuaternion) inv;
+  inv.invert_from(*this);
+  v_quat = (*this) * v_quat * inv;
+
+  return FLOATNAME(LVecBase3)(v_quat[1], v_quat[2], v_quat[3]);
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: LQuaternion::multiply
-//       Access: protected
+//       Access: Published
 //  Description: actual multiply call (non virtual)
 ////////////////////////////////////////////////////////////////////
 INLINE_LINMATH FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::
@@ -61,13 +77,23 @@ multiply(const FLOATNAME(LQuaternion)& rhs) const {
   return FLOATNAME(LQuaternion)(r, i , j, k);
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: LQuaternion::unary -
+//       Access: Public
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE_LINMATH FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::
+operator - () const {
+  return FLOATNAME(LVecBase4)::operator - ();
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: LQuaternion::Multiply Operator
 //       Access: public
 //  Description:
 ////////////////////////////////////////////////////////////////////
 INLINE_LINMATH FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::
-operator *(const FLOATNAME(LQuaternion)& c) {
+operator *(const FLOATNAME(LQuaternion)& c) const {
   return multiply(c);
 }
 
@@ -278,12 +304,7 @@ normalize() {
 ////////////////////////////////////////////////////////////////////
 INLINE_LINMATH bool FLOATNAME(LQuaternion)::
 invert_from(const FLOATNAME(LQuaternion) &other) {
-  FLOATTYPE norm = 1.0f / (other.dot(other));
-  set(other[0] * norm,
-      -other[1] * norm,
-      -other[2] * norm,
-      -other[3] * norm);
-
+  set(other._v.v._0, -other._v.v._1, -other._v.v._2, -other._v.v._3);
   return true;
 }
 
@@ -296,12 +317,9 @@ invert_from(const FLOATNAME(LQuaternion) &other) {
 ////////////////////////////////////////////////////////////////////
 INLINE_LINMATH bool FLOATNAME(LQuaternion)::
 invert_in_place() {
-  FLOATTYPE norm = 1.0f / ((*this).dot(*this));
-  set((*this)[0] * norm,
-      -(*this)[1] * norm,
-      -(*this)[2] * norm,
-      -(*this)[3] * norm);
-
+  _v.v._1 = -_v.v._1;
+  _v.v._2 = -_v.v._2;
+  _v.v._3 = -_v.v._3;
   return true;
 }
 

panda/src/linmath/lquaternion_src.cxx

@@ -108,13 +108,18 @@ set_hpr(const FLOATNAME(LVecBase3) &hpr) {
 
   (*this) = quat_h * quat_p * quat_r;
 
+  if (!temp_hpr_fix) {
+    // Compute the old, broken hpr.
+    (*this) = invert(quat_r) * quat_h * quat_p;
+  }
+
 #ifndef NDEBUG
   if (paranoid_hpr_quat) {
     FLOATNAME(LMatrix3) mat;
     compose_matrix(mat, FLOATNAME(LVecBase3)(1.0f, 1.0f, 1.0f), hpr);
     FLOATNAME(LQuaternion) compare;
     compare.set_from_matrix(mat);
-    if (!compare.almost_equal(*this)) {
+    if (!compare.almost_equal(*this) && !compare.almost_equal(-(*this))) {
       linmath_cat.warning()
         << "hpr-to-quat of " << hpr << " computed " << *this
         << " instead of " << compare << "\n";
@@ -132,6 +137,19 @@ set_hpr(const FLOATNAME(LVecBase3) &hpr) {
 ////////////////////////////////////////////////////////////////////
 FLOATNAME(LVecBase3) FLOATNAME(LQuaternion)::
 get_hpr() const {
+  if (!temp_hpr_fix) {
+    // With the old, broken hpr code in place, just go through the
+    // existing matrix decomposition code.  Not particularly speedy,
+    // but I don't want to bother with working out how to do it
+    // directly for code that hopefully won't need to last much
+    // longer.
+    FLOATNAME(LMatrix3) mat;
+    extract_to_matrix(mat);
+    FLOATNAME(LVecBase3) scale, hpr;
+    decompose_matrix(mat, scale, hpr);
+    return hpr;
+  }
+
   FLOATNAME(LVecBase3) hpr;
   FLOATTYPE N = (_v.data[0] * _v.data[0]) + (_v.data[1] * _v.data[1]) + (_v.data[2] * _v.data[2]) + (_v.data[3] * _v.data[3]);
   FLOATTYPE s = (N == 0.0f) ? 0.0f : (2.0f / N);
@@ -147,7 +165,7 @@ get_hpr() const {
   c3 = 1.0f - (yy + zz);
   c4 = xy + wz;
 
-  if (c1 == 0.0f) {  // (roll = 0 or 180) or (pitch = +/- 90
+  if (c1 == 0.0f) {  // (roll = 0 or 180) or (pitch = +/- 90)
     if (c2 >= 0.0f) {
       hpr[2] = 0.0f;
       ch = c3;

panda/src/linmath/lquaternion_src.h

@@ -21,18 +21,22 @@
 // Description : This is the base quaternion class
 ////////////////////////////////////////////////////////////////////
 class EXPCL_PANDA FLOATNAME(LQuaternion) : public FLOATNAME(LVecBase4) {
-protected:
-  INLINE_LINMATH FLOATNAME(LQuaternion)
-    multiply(const FLOATNAME(LQuaternion)&) const;
-
 PUBLISHED:
   INLINE_LINMATH FLOATNAME(LQuaternion)();
-  INLINE_LINMATH FLOATNAME(LQuaternion)(const FLOATNAME(LQuaternion) &);
+  INLINE_LINMATH FLOATNAME(LQuaternion)(const FLOATNAME(LVecBase4) &copy);
   INLINE_LINMATH FLOATNAME(LQuaternion)(FLOATTYPE, FLOATTYPE, FLOATTYPE, FLOATTYPE);
 
   static FLOATNAME(LQuaternion) pure_imaginary(const FLOATNAME(LVector3) &);
 
-  INLINE_LINMATH FLOATNAME(LQuaternion) operator *(const FLOATNAME(LQuaternion) &);
+  INLINE_LINMATH FLOATNAME(LVecBase3)
+    xform(const FLOATNAME(LVecBase3) &v) const;
+
+  INLINE_LINMATH FLOATNAME(LQuaternion)
+    multiply(const FLOATNAME(LQuaternion) &rhs) const;
+
+  INLINE_LINMATH FLOATNAME(LQuaternion) operator - () const;
+
+  INLINE_LINMATH FLOATNAME(LQuaternion) operator *(const FLOATNAME(LQuaternion) &) const;
   INLINE_LINMATH FLOATNAME(LQuaternion)& operator *=(const FLOATNAME(LQuaternion) &);
 
   INLINE_LINMATH FLOATNAME(LMatrix3) operator *(const FLOATNAME(LMatrix3) &);