*** empty log message ***

panda/src/collide/collisionHandlerFloor.cxx

@@ -86,14 +86,23 @@ handle_entries() {
       }
 
       // Now set our height accordingly.
-      if (collide_cat.is_debug()) {
-	collide_cat.debug()
-	  << "Resetting height to " << max_height << " + " << _offset << "\n";
-      }
       LMatrix4f mat;
       def.get_mat(mat);
-      mat(3, 2) = max_height + _offset;
+      if (!IS_THRESHOLD_EQUAL(mat(3, 2), max_height + _offset, 0.001)) {
-      def.set_mat(mat);
+	if (collide_cat.is_debug()) {
+	  collide_cat.debug()
+	    << "Resetting height to " << max_height << " + " 
+	    << _offset << "\n";
+	}
+	mat(3, 2) = max_height + _offset;
+	def.set_mat(mat);
+      } else {
+	if (collide_cat.is_spam()) {
+	  collide_cat.spam()
+	    << "Height is already at " << max_height << " + " 
+	    << _offset << "\n";
+	}
+      }
     }
   }
 }

panda/src/linmath/compose_matrix.cxx

@@ -125,6 +125,81 @@ unwind_yup_rotation(LMatrix3<NumType> &mat, LVecBase3<NumType> &hpr) {
   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.
+////////////////////////////////////////////////////////////////////
+template<class NumType>
+static void
+unwind_yup_rotation(LMatrix3<NumType> &mat, LVecBase3<NumType> &hpr,
+		    NumType roll) {
+  typedef LMatrix3<NumType> Matrix;
+
+  // Extract the axes from the matrix.
+  LVector3<NumType> x, y, z;
+  x = mat.get_row(0);
+  y = mat.get_row(1);
+  z = mat.get_row(2);
+
+  // Unwind the roll from the axes, and continue.
+  Matrix rot_z;
+  rot_z = Matrix::rotate_mat(-roll, LVector3<NumType>(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.
+  LVector2<NumType> xz(x[0], x[2]);
+  xz = normalize(xz);
+
+  // Compute the rotation about the +Y (up) axis.  This is yaw, or
+  // "heading".
+  NumType heading = rad_2_deg(-atan2(xz[1], xz[0]));
+
+  // Unwind the heading, and continue.
+  Matrix rot_y;
+  rot_y = Matrix::rotate_mat(-heading, LVector3<NumType>(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.
+  LVector2<NumType> yz(z[1], z[2]);
+  yz = normalize(yz);
+
+  // Compute the rotation about the +X (right) axis.  This is pitch.
+  NumType pitch = rad_2_deg(-atan2(yz[0], yz[1]));
+ 
+  // Unwind the pitch.
+  Matrix rot_x;
+  rot_x = Matrix::rotate_mat(-pitch, LVector3<NumType>(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;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: unwind_zup_rotation
 //  Description: Extracts the rotation about the x, y, and z axes from
@@ -170,6 +245,80 @@ unwind_zup_rotation(LMatrix3<NumType> &mat, LVecBase3<NumType> &hpr) {
   y = y * rot_y;
   z = z * rot_y;
   
+  // Project the rotated X into the XY plane.
+  LVector2<NumType> xy(x[0], x[1]);
+  xy = normalize(xy);
+
+  // Compute the rotation about the +Z (up) axis.  This is yaw, or
+  // "heading".
+  NumType heading = rad_2_deg(atan2(xy[1], xy[0]));
+
+  // Unwind the heading, and continue.
+  Matrix rot_z;
+  rot_z = Matrix::rotate_mat(-heading, LVector3<NumType>(0.0, 0.0, 1.0),
+			     CS_zup_right);
+
+  x = x * rot_z;
+  y = y * rot_z;
+  z = z * rot_z;
+
+  // Project the rotated Y into the YZ plane.
+  LVector2<NumType> yz(y[1], y[2]);
+  yz = normalize(yz);
+
+  // Compute the rotation about the +X (right) axis.  This is pitch.
+  NumType pitch = rad_2_deg(atan2(yz[1], yz[0]));
+
+  // Unwind the pitch.
+  Matrix rot_x;
+  rot_x = Matrix::rotate_mat(-pitch, LVector3<NumType>(1.0, 0.0, 0.0), 
+			     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.
+////////////////////////////////////////////////////////////////////
+template<class NumType>
+static void
+unwind_zup_rotation(LMatrix3<NumType> &mat, LVecBase3<NumType> &hpr,
+		    NumType roll) {
+  typedef LMatrix3<NumType> Matrix;
+
+  // Extract the axes from the matrix.
+  LVector3<NumType> x, y, z;
+  x = mat.get_row(0);
+  y = mat.get_row(1);
+  z = mat.get_row(2);
+  
+  // Unwind the roll from the axes, and continue.
+  Matrix rot_y;
+  rot_y = Matrix::rotate_mat(roll, LVector3<NumType>(0.0, 1.0, 0.0),
+			     CS_zup_right);
+  
+  x = x * rot_y;
+  y = y * rot_y;
+  z = z * rot_y;
   
   // Project the rotated X into the XY plane.
   LVector2<NumType> xy(x[0], x[1]);
@@ -188,7 +337,6 @@ unwind_zup_rotation(LMatrix3<NumType> &mat, LVecBase3<NumType> &hpr) {
   y = y * rot_z;
   z = z * rot_z;
 
-
   // Project the rotated Y into the YZ plane.
   LVector2<NumType> yz(y[1], y[2]);
   yz = normalize(yz);
@@ -205,7 +353,6 @@ unwind_zup_rotation(LMatrix3<NumType> &mat, LVecBase3<NumType> &hpr) {
   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);
@@ -308,6 +455,104 @@ _decompose_matrix(const LMatrix3<NumType> &mat,
   return !shear;
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: decompose_matrix
+//  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.
+////////////////////////////////////////////////////////////////////
+template<class NumType>
+static bool
+_decompose_matrix(const LMatrix3<NumType> &mat,
+		  LVecBase3<NumType> &scale,
+		  LVecBase3<NumType> &hpr,
+		  NumType roll,
+		  CoordinateSystem cs) {
+  if (cs == CS_default) {
+    cs = default_coordinate_system;
+  }
+
+  // Extract the rotation and scale, according to the coordinate
+  // system of choice.
+  bool shear;
+
+  switch (cs) {
+  case CS_zup_right:
+    {
+      LMatrix3<NumType> rm(mat);
+      unwind_zup_rotation(rm, hpr, roll);
+      scale[0] = rm(0, 0);
+      scale[1] = rm(1, 1);
+      scale[2] = rm(2, 2);
+      shear = 
+	(fabs(rm(0, 1)) + fabs(rm(0, 2)) +
+	 fabs(rm(1, 0)) + fabs(rm(1, 2)) +
+	 fabs(rm(2, 0)) + fabs(rm(2, 1))) >= 0.0001;
+    }
+    break;
+
+  case CS_yup_right:
+    {
+      LMatrix3<NumType> rm(mat);
+      unwind_yup_rotation(rm, hpr, roll);
+      scale[0] = rm(0, 0);
+      scale[1] = rm(1, 1);
+      scale[2] = rm(2, 2);
+      shear = 
+	(fabs(rm(0, 1)) + fabs(rm(0, 2)) +
+	 fabs(rm(1, 0)) + fabs(rm(1, 2)) +
+	 fabs(rm(2, 0)) + fabs(rm(2, 1))) >= 0.0001;
+    }
+    break;
+
+  case CS_zup_left:
+    {
+      LMatrix3<NumType> lm(mat(0, 0), mat(0, 1), -mat(0, 2),
+			   mat(1, 0), mat(1, 1), -mat(1, 2),
+			   -mat(2, 0), -mat(2, 1), mat(2, 2));
+      unwind_zup_rotation(lm, hpr, roll);
+      scale[0] = -lm(0, 0);
+      scale[1] = -lm(1, 1);
+      scale[2] = lm(2, 2);
+      shear = 
+	(fabs(lm(0, 1)) + fabs(lm(0, 2)) +
+	 fabs(lm(1, 0)) + fabs(lm(1, 2)) +
+	 fabs(lm(2, 0)) + fabs(lm(2, 1))) >= 0.0001;
+    }
+    break;
+
+  case CS_yup_left:
+    {
+      LMatrix3<NumType> lm(mat(0, 0), mat(0, 1), -mat(0, 2),
+			   mat(1, 0), mat(1, 1), -mat(1, 2),
+			   -mat(2, 0), -mat(2, 1), mat(2, 2));
+      unwind_yup_rotation(lm, hpr, roll);
+      scale[0] = -lm(0, 0);
+      scale[1] = -lm(1, 1);
+      scale[2] = lm(2, 2);
+      shear = 
+	(fabs(lm(0, 1)) + fabs(lm(0, 2)) +
+	 fabs(lm(1, 0)) + fabs(lm(1, 2)) +
+	 fabs(lm(2, 0)) + fabs(lm(2, 1))) >= 0.0001;
+    }
+    break;
+
+  default:
+    linmath_cat.error()
+      << "Unexpected coordinate system!\n";
+    return false;
+  }
+
+  return !shear;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: decompose_matrix
 //  Description: Extracts out the components of an affine matrix.
@@ -328,6 +573,33 @@ _decompose_matrix(const LMatrix4<NumType> &mat,
   return _decompose_matrix(mat.get_upper_3(), scale, hpr, 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.
+//
+//               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.
+////////////////////////////////////////////////////////////////////
+template<class NumType>
+INLINE bool
+_decompose_matrix(const LMatrix4<NumType> &mat,
+		  LVecBase3<NumType> &scale,
+		  LVecBase3<NumType> &hpr,
+		  LVecBase3<NumType> &translate,
+		  NumType roll,
+		  CoordinateSystem cs) {
+  // Get the translation first.
+  translate = mat.get_row3(3);
+  return _decompose_matrix(mat.get_upper_3(), scale, hpr, roll, cs);
+}
+
 void
 compose_matrix(LMatrix3f &mat,
 	       const LVecBase3f &scale,
@@ -344,6 +616,15 @@ decompose_matrix(const LMatrix3f &mat,
   return _decompose_matrix(mat, scale, hpr, cs);
 }
 
+bool
+decompose_matrix(const LMatrix3f &mat,
+		 LVecBase3f &scale,
+		 LVecBase3f &hpr,
+		 float roll,
+		 CoordinateSystem cs) {
+  return _decompose_matrix(mat, scale, hpr, roll, cs);
+}
+
 void
 compose_matrix(LMatrix3d &mat,
 	       const LVecBase3d &scale,
@@ -360,6 +641,15 @@ decompose_matrix(const LMatrix3d &mat,
   return _decompose_matrix(mat, scale, hpr, cs);
 }
 
+bool
+decompose_matrix(const LMatrix3d &mat,
+		 LVecBase3d &scale,
+		 LVecBase3d &hpr,
+		 double roll,
+		 CoordinateSystem cs) {
+  return _decompose_matrix(mat, scale, hpr, roll, cs);
+}
+
 void
 compose_matrix(LMatrix4f &mat,
 	       const LVecBase3f &scale,
@@ -378,6 +668,16 @@ decompose_matrix(const LMatrix4f &mat,
   return _decompose_matrix(mat, scale, hpr, translate, cs);
 }
 
+bool
+decompose_matrix(const LMatrix4f &mat,
+		 LVecBase3f &scale,
+		 LVecBase3f &hpr,
+		 LVecBase3f &translate,
+		 float roll,
+		 CoordinateSystem cs) {
+  return _decompose_matrix(mat, scale, hpr, translate, roll, cs);
+}
+
 void
 compose_matrix(LMatrix4d &mat,
 	       const LVecBase3d &scale,
@@ -396,3 +696,13 @@ decompose_matrix(const LMatrix4d &mat,
   return _decompose_matrix(mat, scale, hpr, translate, cs);
 }
 
+bool
+decompose_matrix(const LMatrix4d &mat,
+		 LVecBase3d &scale,
+		 LVecBase3d &hpr,
+		 LVecBase3d &translate,
+		 double roll,
+		 CoordinateSystem cs) {
+  return _decompose_matrix(mat, scale, hpr, translate, roll, cs);
+}
+

panda/src/linmath/compose_matrix.h

@@ -42,6 +42,12 @@ decompose_matrix(const LMatrix3f &mat,
 		 LVecBase3f &scale,
 		 LVecBase3f &hpr,
 		 CoordinateSystem cs = CS_default);
+EXPCL_PANDA bool
+decompose_matrix(const LMatrix3f &mat,
+		 LVecBase3f &scale,
+		 LVecBase3f &hpr,
+		 float roll,
+		 CoordinateSystem cs = CS_default);
 
 EXPCL_PANDA void
 compose_matrix(LMatrix3d &mat,
@@ -53,6 +59,12 @@ decompose_matrix(const LMatrix3d &mat,
 		 LVecBase3d &scale,
 		 LVecBase3d &hpr,
 		 CoordinateSystem cs = CS_default);
+EXPCL_PANDA bool
+decompose_matrix(const LMatrix3d &mat,
+		 LVecBase3d &scale,
+		 LVecBase3d &hpr,
+		 double roll,
+		 CoordinateSystem cs = CS_default);
 
 EXPCL_PANDA void
 compose_matrix(LMatrix4f &mat,
@@ -69,6 +81,13 @@ decompose_matrix(const LMatrix4f &mat,
 		 LVecBase3f &hpr,
 		 LVecBase3f &translate,
 		 CoordinateSystem cs = CS_default);
+EXPCL_PANDA bool
+decompose_matrix(const LMatrix4f &mat,
+		 LVecBase3f &scale,
+		 LVecBase3f &hpr,
+		 LVecBase3f &translate,
+		 float roll,
+		 CoordinateSystem cs = CS_default);
 INLINE bool decompose_matrix(const LMatrix4f &mat, float components[9],
 			     CoordinateSystem CS = CS_default);
 
@@ -87,6 +106,13 @@ decompose_matrix(const LMatrix4d &mat,
 		 LVecBase3d &hpr,
 		 LVecBase3d &translate,
 		 CoordinateSystem cs = CS_default);
+bool EXPCL_PANDA
+decompose_matrix(const LMatrix4d &mat,
+		 LVecBase3d &scale,
+		 LVecBase3d &hpr,
+		 LVecBase3d &translate,
+		 double roll,
+		 CoordinateSystem cs = CS_default);
 INLINE bool decompose_matrix(const LMatrix4d &mat, double components[9],
 			     CoordinateSystem cs = CS_default);
 

panda/src/net/connectionReader.cxx

@@ -451,26 +451,17 @@ process_incoming_udp_data(SocketInfo *sinfo) {
   }
 
   DatagramUDPHeader header(buffer);
-  PRInt32 size = header.get_datagram_size();
 
   PRInt8 *dp = buffer + datagram_udp_header_size;
   bytes_read -= datagram_udp_header_size;
 
-  PRInt32 datagram_bytes = min(bytes_read, size);
+  NetDatagram datagram(dp, bytes_read);
-  NetDatagram datagram(dp, datagram_bytes);
 
   if (_shutdown) {
     finish_socket(sinfo);
     return;
   }
 
-  if (bytes_read > datagram_bytes) {
-    // There were some extra bytes at the end of the datagram.  Huh.
-    net_cat.error()
-      << "Discarding " << bytes_read - datagram_bytes
-      << " bytes following UDP datagram.\n";
-  }
-
   // Now that we've read all the data, it's time to finish the socket
   // so another thread can read the next datagram.
   finish_socket(sinfo);

panda/src/net/datagramUDPHeader.cxx

@@ -19,16 +19,12 @@
 DatagramUDPHeader::
 DatagramUDPHeader(const NetDatagram &datagram) {
   const string &str = datagram.get_message();
-  PRUint16 size = str.length();
-  nassertv(size == str.length());
-
   PRUint16 checksum = 0;
   for (size_t p = 0; p < str.size(); p++) {
     checksum += (PRUint16)(PRUint8)str[p];
   }
 
   // Now pack the header.
-  _header.add_uint16(size);
   _header.add_uint16(checksum);
   nassertv((int)_header.get_length() == datagram_udp_header_size);
 }
@@ -44,18 +40,6 @@ DatagramUDPHeader::
 DatagramUDPHeader(const void *data) : _header(data, datagram_udp_header_size) {
 }
 
-////////////////////////////////////////////////////////////////////
-//     Function: DatagramUDPHeader::get_datagram_size
-//       Access: Public
-//  Description: Returns the number of bytes in the associated
-//               datagram.
-////////////////////////////////////////////////////////////////////
-int DatagramUDPHeader::
-get_datagram_size() const {
-  DatagramIterator di(_header);
-  return di.get_uint16();
-}
-
 ////////////////////////////////////////////////////////////////////
 //     Function: DatagramUDPHeader::get_datagram_checksum
 //       Access: Public
@@ -90,26 +74,19 @@ get_header() const {
 bool DatagramUDPHeader::
 verify_datagram(const NetDatagram &datagram) const {
   const string &str = datagram.get_message();
-  PRUint16 size = str.length();
-  nassertr(size == str.length(), false);
 
   PRUint16 checksum = 0;
   for (size_t p = 0; p < str.size(); p++) {
     checksum += (PRUint16)(PRUint8)str[p];
   }
 
-  if (size == get_datagram_size() && checksum == get_datagram_checksum()) {
+  if (checksum == get_datagram_checksum()) {
     return true;
   }
 
   if (net_cat.is_debug()) {
     net_cat.debug()
       << "Invalid datagram!\n";
-    if (size != get_datagram_size()) {
-      net_cat.debug()
-	<< "  size is " << size << " bytes, header reports " 
-	<< get_datagram_size() << "\n";
-    }
     if (checksum != get_datagram_checksum()) {
       net_cat.debug()
 	<< "  checksum is " << checksum << ", header reports " 

panda/src/net/datagramUDPHeader.h

@@ -12,8 +12,7 @@
 
 #include <prtypes.h>
 
-static const int datagram_udp_header_size =
+static const int datagram_udp_header_size = sizeof(PRUint16);
-  sizeof(PRUint16) + sizeof(PRUint16);
 
 class NetDatagram;
 
@@ -30,7 +29,6 @@ public:
   DatagramUDPHeader(const NetDatagram &datagram);
   DatagramUDPHeader(const void *data);
 
-  int get_datagram_size() const;
   int get_datagram_checksum() const;
 
   const string &get_header() const;

panda/src/tform/driveInterface.cxx

@@ -121,6 +121,8 @@ DriveInterface(const string &name) : DataNode(name) {
   _xyz.set(0.0, 0.0, 0.0);
   _hpr.set(0.0, 0.0, 0.0);
   _mat = LMatrix4f::ident_mat();
+  _force_roll = 0.0;
+  _is_force_roll = true;
 
   _cs = default_coordinate_system;
 
@@ -398,6 +400,7 @@ void DriveInterface::
 reset() {
   _xyz.set(0.0, 0.0, 0.0);
   _hpr.set(0.0, 0.0, 0.0);
+  _force_roll = 0.0;
   _mat = LMatrix4f::ident_mat();
   _up_arrow.clear();
   _down_arrow.clear();
@@ -509,6 +512,10 @@ void DriveInterface::
 set_hpr(float h, float p, float r) {
   _hpr.set(h, p, r);
   recompute();
+
+  if (_is_force_roll && r != _force_roll) {
+    reextract();
+  }
 }
 
 void DriveInterface::
@@ -527,8 +534,58 @@ void DriveInterface::
 set_r(float r) {
   _hpr[2] = r;
   recompute();
+
+  if (_is_force_roll && r != _force_roll) {
+    reextract();
+  }
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: DriveInterface::set_force_roll
+//       Access: Public, Scheme
+//  Description: Sets the force_roll state.  In this state, roll is
+//               always fixed to a particular value (typically zero),
+//               regardless of what it is explicitly set to via
+//               set_hpr().
+////////////////////////////////////////////////////////////////////
+void DriveInterface::
+set_force_roll(float force_roll) {
+  if (_is_force_roll) {
+    // If we already had force_roll() in effect, we have to
+    // recompensate for it.
+    if (_force_roll != force_roll) {
+      _force_roll = force_roll;
+      reextract();
+    }
+
+  } else {
+    _force_roll = force_roll;
+    _is_force_roll = true;
+  }
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: DriveInterface::is_force_roll
+//       Access: Public, Scheme
+//  Description: Returns true if the force_roll state is in effect,
+//               e.g. because of a previous call to set_force_roll().
+//               In this state, the roll cannot be set to any value
+//               other than what the force_roll value indicates.
+////////////////////////////////////////////////////////////////////
+bool DriveInterface::
+is_force_roll() const {
+  return _is_force_roll;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: DriveInterface::clear_force_roll
+//       Access: Public, Scheme
+//  Description: Disables the force_roll state.  See set_force_roll().
+////////////////////////////////////////////////////////////////////
+void DriveInterface::
+clear_force_roll() {
+  _is_force_roll = false;
+}
 
 
 ////////////////////////////////////////////////////////////////////
@@ -761,7 +818,19 @@ apply(double x, double y, bool any_button) {
 void DriveInterface::
 reextract() {
   LVecBase3f scale;
-  decompose_matrix(_mat, scale, _hpr, _xyz);
+
+  if (_is_force_roll) {
+    // We strongly discourage a roll other than _force_roll.
+    decompose_matrix(_mat, scale, _hpr, _xyz, _force_roll);
+  } else {
+    decompose_matrix(_mat, scale, _hpr, _xyz);
+  }
+
+  if (tform_cat.is_debug()) {
+    tform_cat.debug() 
+      << "Reextract " << _hpr << ", " << _xyz << " from:\n";
+    _mat.write(tform_cat.debug(false), 2);
+  }
 }
 
 ////////////////////////////////////////////////////////////////////

panda/src/tform/driveInterface.h

@@ -79,6 +79,10 @@ PUBLISHED:
   void set_p(float p);
   void set_r(float r);
 
+  void set_force_roll(float force_roll);
+  bool is_force_roll() const;
+  void clear_force_roll();
+
   void set_coordinate_system(CoordinateSystem cs);
   CoordinateSystem get_coordinate_system() const;
 
@@ -114,6 +118,8 @@ private:
   LPoint3f _xyz;
   LVecBase3f _hpr;
   LMatrix4f _mat;
+  float _force_roll;
+  bool _is_force_roll;
   CoordinateSystem _cs;
 
   // Remember which arrow keys are being held down and which aren't,