fix problem with non-normalized quat

panda/src/pgraph/transformState.I

@@ -504,8 +504,15 @@ get_hpr() const {
 //     Function: TransformState::get_quat
 //       Access: Published
 //  Description: Returns the rotation component of the transform as a
-//               quaternion.  It is an error to call this if
-//               has_components() returned false.
+//               quaternion.  The return value will be normalized if a
+//               normalized quaternion was given to the constructor
+//               (or if the quaternion was computed implicitly); it
+//               will be non-normalized if a non-normalized quaternion
+//               was given to the constructor.  See also
+//               get_norm_quat().
+//
+//               It is an error to call this if has_components()
+//               returned false.
 ////////////////////////////////////////////////////////////////////
 INLINE const LQuaternionf &TransformState::
 get_quat() const {
@@ -514,6 +521,22 @@ get_quat() const {
   return _quat;
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: TransformState::get_norm_quat
+//       Access: Published
+//  Description: Returns the rotation component of the transform as a
+//               quaternion.  Unlike the result of get_quat(), the
+//               return value of this method is guaranteed to be
+//               normalized.  It is an error to call this if
+//               has_components() returned false.
+////////////////////////////////////////////////////////////////////
+INLINE const LQuaternionf &TransformState::
+get_norm_quat() const {
+  check_norm_quat();
+  nassertr(!is_invalid(), _norm_quat);
+  return _norm_quat;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: TransformState::get_scale
 //       Access: Published
@@ -835,6 +858,21 @@ check_quat() const {
   }
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: TransformState::check_norm_quat
+//       Access: Private
+//  Description: Ensures that we know the normalized quat of the transform
+//               (or that we know they cannot be derived).
+////////////////////////////////////////////////////////////////////
+INLINE void TransformState::
+check_norm_quat() const {
+  // This pretends to be a const function, even though it's not,
+  // because it only updates a transparent cache value.
+  if ((_flags & F_norm_quat_known) == 0) {
+    ((TransformState *)this)->calc_norm_quat();
+  }
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: TransformState::check_mat
 //       Access: Private

panda/src/pgraph/transformState.cxx

@@ -1327,7 +1327,7 @@ do_compose(const TransformState *other) const {
       // Do a 2-d compose.
       LVecBase2f pos = get_pos2d();
       float rotate = get_rotate2d();
-      LQuaternionf quat = get_quat();
+      LQuaternionf quat = get_norm_quat();
       float scale = get_uniform_scale();
 
       LPoint3f op = quat.xform(other->get_pos());
@@ -1341,12 +1341,11 @@ do_compose(const TransformState *other) const {
     } else {
       // A normal 3-d compose.
       LVecBase3f pos = get_pos();
-      LQuaternionf quat = get_quat();
+      LQuaternionf quat = get_norm_quat();
       float scale = get_uniform_scale();
       
       pos += quat.xform(other->get_pos()) * scale;
-      quat = other->get_quat() * quat;
-      quat.normalize();
+      quat = other->get_norm_quat() * quat;
       LVecBase3f new_scale = other->get_scale() * scale;
       
       result = make_pos_quat_scale(pos, quat, new_scale);
@@ -1406,7 +1405,7 @@ do_invert_compose(const TransformState *other) const {
       // Do a 2-d invert compose.
       LVecBase2f pos = get_pos2d();
       float rotate = get_rotate2d();
-      LQuaternionf quat = get_quat();
+      LQuaternionf quat = get_norm_quat();
       float scale = get_uniform_scale();
       
       // First, invert our own transform.
@@ -1435,7 +1434,7 @@ do_invert_compose(const TransformState *other) const {
     } else {
       // Do a normal, 3-d invert compose.
       LVecBase3f pos = get_pos();
-      LQuaternionf quat = get_quat();
+      LQuaternionf quat = get_norm_quat();
       float scale = get_uniform_scale();
       
       // First, invert our own transform.
@@ -1451,8 +1450,7 @@ do_invert_compose(const TransformState *other) const {
       // Now compose the inverted transform with the other transform.
       if (!other->is_identity()) {
         pos += quat.xform(other->get_pos()) * scale;
-        quat = other->get_quat() * quat;
-        quat.normalize();
+        quat = other->get_norm_quat() * quat;
         new_scale = other->get_scale() * scale;
       }
 
@@ -1873,6 +1871,20 @@ calc_quat() {
   }
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: TransformState::calc_norm_quat
+//       Access: Private
+//  Description: Derives the normalized quat from the quat.
+////////////////////////////////////////////////////////////////////
+void TransformState::
+calc_norm_quat() {
+  LQuaternionf quat = get_quat();
+  MutexHolder holder(_lock);
+  _norm_quat = quat;
+  _norm_quat.normalize();
+  _flags |= F_norm_quat_known;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: TransformState::do_calc_mat
 //       Access: Private

panda/src/pgraph/transformState.h

@@ -142,6 +142,7 @@ PUBLISHED:
   INLINE const LPoint3f &get_pos() const;
   INLINE const LVecBase3f &get_hpr() const;
   INLINE const LQuaternionf &get_quat() const;
+  INLINE const LQuaternionf &get_norm_quat() const;
   INLINE const LVecBase3f &get_scale() const;
   INLINE float get_uniform_scale() const;
   INLINE const LVecBase3f &get_shear() const;
@@ -271,6 +272,7 @@ private:
   INLINE void check_components() const;
   INLINE void check_hpr() const;
   INLINE void check_quat() const;
+  INLINE void check_norm_quat() const;
   INLINE void check_mat() const;
   INLINE void calc_hash();
   void do_calc_hash();
@@ -280,6 +282,7 @@ private:
   INLINE void calc_hpr();
   void do_calc_hpr();
   void calc_quat();
+  void calc_norm_quat();
   INLINE void calc_mat();
   void do_calc_mat();
 
@@ -311,10 +314,11 @@ private:
     F_is_destructing     = 0x00008000,
     F_is_2d              = 0x00010000,
     F_hash_known         = 0x00020000,
+    F_norm_quat_known    = 0x00040000,
   };
   LPoint3f _pos;
   LVecBase3f _hpr, _scale, _shear;
-  LQuaternionf _quat;
+  LQuaternionf _quat, _norm_quat;
   LMatrix4f _mat;
   LMatrix4f *_inv_mat;
   size_t _hash;