vrpn trackers use y-up

panda/src/device/clientBase.I

@@ -60,3 +60,27 @@ INLINE double ClientBase::
 get_last_poll_time() const {
   return _last_poll_time;
 }
+
+////////////////////////////////////////////////////////////////////
+//     Function: ClientBase::set_coordinate_system
+//       Access: Published
+//  Description: Specifies the coordinate system that all devices
+//               associated with this client will operate in.
+//               Normally, this is CS_default.
+////////////////////////////////////////////////////////////////////
+void ClientBase::
+set_coordinate_system(CoordinateSystem cs) {
+  _cs = cs;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: ClientBase::get_coordinate_system
+//       Access: Published
+//  Description: Returns the coordinate system that all devices
+//               associated with this client will operate in.
+//               Normally, this is CS_default.
+////////////////////////////////////////////////////////////////////
+INLINE CoordinateSystem ClientBase::
+get_coordinate_system() const {
+  return _cs;
+}

panda/src/device/clientBase.cxx

@@ -32,6 +32,7 @@ ClientBase() {
   _forked = false;
   _last_poll_time = 0.0f;
   _last_poll_frame = 0;
+  _cs = CS_default;
 
 #ifdef HAVE_IPC
   _client_thread = (thread *)NULL;

panda/src/device/clientBase.h

@@ -19,26 +19,27 @@
 #ifndef CLIENTBASE_H
 #define CLIENTBASE_H
 
-#include <pandabase.h>
+#include "pandabase.h"
 
 #include "clientDevice.h"
 
-#include <typedReferenceCount.h>
-#include <luse.h>
-#include <vector_string.h>
-#include <vector_int.h>
-#include <clockObject.h>
-#include <pointerTo.h>
+#include "typedReferenceCount.h"
+#include "luse.h"
+#include "vector_string.h"
+#include "vector_int.h"
+#include "clockObject.h"
+#include "pointerTo.h"
+#include "coordinateSystem.h"
 
 #ifdef HAVE_IPC
-#include <ipc_thread.h>
+#include "ipc_thread.h"
 #endif
 
 #include "pmap.h"
 
 ////////////////////////////////////////////////////////////////////
 //       Class : ClientBase
-// Description : An abstract base class for a family of of client
+// Description : An abstract base class for a family of client
 //               device interfaces--including trackers, buttons,
 //               dials, and other analog inputs.
 //
@@ -59,6 +60,9 @@ PUBLISHED:
   INLINE bool poll();
   INLINE double get_last_poll_time() const;
 
+  INLINE void set_coordinate_system(CoordinateSystem cs);
+  INLINE CoordinateSystem get_coordinate_system() const;
+
 public:
   PT(ClientDevice) get_device(TypeHandle device_type,
                               const string &device_name);
@@ -81,6 +85,7 @@ private:
   bool _forked;
   double _last_poll_time;
   int _last_poll_frame;
+  CoordinateSystem _cs;
 
 #ifdef HAVE_IPC
   int _sleep_time;

panda/src/device/trackerNode.I

@@ -62,3 +62,58 @@ get_transform() const {
   return _transform;
 }
 
+
+////////////////////////////////////////////////////////////////////
+//     Function: TrackerNode::set_tracker_coordinate_system
+//       Access: Published
+//  Description: Specifies the coordinate system that the tracker
+//               associated with this node will operate in.  Normally,
+//               this is set from the ClientBase that's used to create
+//               the TrackerNode, so it should not need to be set on
+//               an individual tracker basis.
+////////////////////////////////////////////////////////////////////
+void TrackerNode::
+set_tracker_coordinate_system(CoordinateSystem cs) {
+  _tracker_cs = cs;
+  if (_tracker_cs == CS_default) {
+    _tracker_cs = default_coordinate_system;
+  }
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: TrackerNode::get_tracker_coordinate_system
+//       Access: Published
+//  Description: Returns the coordinate system that the tracker
+//               associated with this node will operate in.
+////////////////////////////////////////////////////////////////////
+INLINE CoordinateSystem TrackerNode::
+get_tracker_coordinate_system() const {
+  return _tracker_cs;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: TrackerNode::set_graph_coordinate_system
+//       Access: Published
+//  Description: Specifies the coordinate system that the TrackerNode
+//               will convert its transform into for passing down the
+//               data graph.  Normally, this is CS_default.
+////////////////////////////////////////////////////////////////////
+void TrackerNode::
+set_graph_coordinate_system(CoordinateSystem cs) {
+  _graph_cs = cs;
+  if (_graph_cs == CS_default) {
+    _graph_cs = default_coordinate_system;
+  }
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: TrackerNode::get_graph_coordinate_system
+//       Access: Published
+//  Description: Returns the coordinate system that the TrackerNode
+//               will convert its transform into for passing down the
+//               data graph.  Normally, this is CS_default.
+////////////////////////////////////////////////////////////////////
+INLINE CoordinateSystem TrackerNode::
+get_graph_coordinate_system() const {
+  return _graph_cs;
+}

panda/src/device/trackerNode.cxx

@@ -37,6 +37,9 @@ TrackerNode(ClientBase *client, const string &device_name) :
   DataNode(device_name)
 {
   nassertv(client != (ClientBase *)NULL);
+  set_tracker_coordinate_system(client->get_coordinate_system());
+  set_graph_coordinate_system(CS_default);
+
   PT(ClientDevice) device =
     client->get_device(ClientTrackerDevice::get_class_type(), device_name);
 
@@ -87,6 +90,10 @@ transmit_data(AllTransitionsWrapper &data) {
     _tracker->unlock();
 
     _data.get_orient().extract_to_matrix(_transform);
+    if (_tracker_cs != _graph_cs) {
+      // Convert the rotation for passing down the data graph.
+      _transform = _transform * LMatrix4f::convert_mat(_tracker_cs, _graph_cs);
+    }
     _transform.set_row(3, _data.get_pos());
 
     _transform_attrib->set_value(_transform);

panda/src/device/trackerNode.h

@@ -52,6 +52,11 @@ PUBLISHED:
   INLINE const LOrientationf &get_orient() const;
   INLINE const LMatrix4f &get_transform() const;
 
+  INLINE void set_tracker_coordinate_system(CoordinateSystem cs);
+  INLINE CoordinateSystem get_tracker_coordinate_system() const;
+
+  INLINE void set_graph_coordinate_system(CoordinateSystem cs);
+  INLINE CoordinateSystem get_graph_coordinate_system() const;
 
 ////////////////////////////////////////////////////////////////////
 // From parent class DataNode
@@ -70,6 +75,7 @@ private:
   PT(ClientTrackerDevice) _tracker;
   TrackerData _data;
   LMatrix4f _transform;
+  CoordinateSystem _tracker_cs, _graph_cs;
 
 public:
   virtual TypeHandle get_type() const {

panda/src/linmath/lmatrix3_src.cxx

@@ -18,11 +18,101 @@
 
 TypeHandle FLOATNAME(LMatrix3)::_type_handle;
 
-FLOATNAME(LMatrix3) FLOATNAME(LMatrix3)::_ident_mat =
+const FLOATNAME(LMatrix3) FLOATNAME(LMatrix3)::_ident_mat =
   FLOATNAME(LMatrix3)(1.0f, 0.0f, 0.0f,
                       0.0f, 1.0f, 0.0f,
                       0.0f, 0.0f, 1.0f);
 
+const FLOATNAME(LMatrix3) FLOATNAME(LMatrix3)::_y_to_z_up_mat =
+  FLOATNAME(LMatrix3)(1.0f, 0.0f, 0.0f,
+                      0.0f, 0.0f, 1.0f,
+                      0.0f,-1.0f, 0.0f);
+
+const FLOATNAME(LMatrix3) FLOATNAME(LMatrix3)::_z_to_y_up_mat =
+  FLOATNAME(LMatrix3)(1.0f, 0.0f, 0.0f,
+                      0.0f, 0.0f,-1.0f,
+                      0.0f, 1.0f, 0.0f);
+
+const FLOATNAME(LMatrix3) FLOATNAME(LMatrix3)::_flip_y_mat =
+  FLOATNAME(LMatrix3)(1.0f, 0.0f, 0.0f,
+                      0.0f,-1.0f, 0.0f,
+                      0.0f, 0.0f, 1.0f);
+
+const FLOATNAME(LMatrix3) FLOATNAME(LMatrix3)::_flip_z_mat =
+  FLOATNAME(LMatrix3)(1.0f, 0.0f, 0.0f,
+                      0.0f, 1.0f, 0.0f,
+                      0.0f, 0.0f,-1.0f);
+
+const FLOATNAME(LMatrix3) FLOATNAME(LMatrix3)::_lz_to_ry_mat =
+  FLOATNAME(LMatrix3)::_flip_y_mat * FLOATNAME(LMatrix3)::_z_to_y_up_mat;
+
+const FLOATNAME(LMatrix3) FLOATNAME(LMatrix3)::_ly_to_rz_mat =
+  FLOATNAME(LMatrix3)::_flip_z_mat * FLOATNAME(LMatrix3)::_y_to_z_up_mat;
+
+////////////////////////////////////////////////////////////////////
+//     Function: LMatrix::convert_mat
+//       Access: Public, Static
+//  Description: Returns a matrix that transforms from the indicated
+//               coordinate system to the indicated coordinate system.
+////////////////////////////////////////////////////////////////////
+const FLOATNAME(LMatrix3) &FLOATNAME(LMatrix3)::
+convert_mat(CoordinateSystem from, CoordinateSystem to) {
+  if (from == CS_default) {
+    from = default_coordinate_system;
+  }
+  if (to == CS_default) {
+    to = default_coordinate_system;
+  }
+  switch (from) {
+  case CS_zup_left:
+    switch (to) {
+    case CS_zup_left: return _ident_mat;
+    case CS_yup_left: return _z_to_y_up_mat;
+    case CS_zup_right: return _flip_y_mat;
+    case CS_yup_right: return _lz_to_ry_mat;
+    default: break;
+    }
+    break;
+
+  case CS_yup_left:
+    switch (to) {
+    case CS_zup_left: return _y_to_z_up_mat;
+    case CS_yup_left: return _ident_mat;
+    case CS_zup_right: return _ly_to_rz_mat;
+    case CS_yup_right: return _flip_z_mat;
+    default: break;
+    }
+    break;
+
+  case CS_zup_right:
+    switch (to) {
+    case CS_zup_left: return _flip_y_mat;
+    case CS_yup_left: return _lz_to_ry_mat;
+    case CS_zup_right: return _ident_mat;
+    case CS_yup_right: return _z_to_y_up_mat;
+    default: break;
+    }
+    break;
+
+  case CS_yup_right:
+    switch (to) {
+    case CS_zup_left: return _ly_to_rz_mat;
+    case CS_yup_left: return _flip_z_mat;
+    case CS_zup_right: return _y_to_z_up_mat;
+    case CS_yup_right: return _ident_mat;
+    default: break;
+    }
+    break;
+
+  default:
+    break;
+  }
+
+  linmath_cat.error()
+    << "Invalid coordinate system value!\n";
+  return _ident_mat;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: LMatrix3::fill
 //       Access: Public

panda/src/linmath/lmatrix3_src.h

@@ -157,6 +157,9 @@ PUBLISHED:
   static INLINE_LINMATH FLOATNAME(LMatrix3)
     scale_mat(FLOATTYPE sx, FLOATTYPE sy, FLOATTYPE sz);
 
+  static const FLOATNAME(LMatrix3) &convert_mat(CoordinateSystem from,
+                                                CoordinateSystem to);
+
   // We don't have a scale_mat() that takes a single uniform scale
   // parameter, because it would be ambiguous whether we mean a 2-d or
   // a 3-d scale.
@@ -175,20 +178,26 @@ public:
 
 public:
   union {
-        struct {
-           FLOATTYPE  _00, _01, _02;
-           FLOATTYPE  _10, _11, _12;
-           FLOATTYPE  _20, _21, _22;
-        } m;
-
-                FLOATTYPE data[3 * 3];
+    struct {
+      FLOATTYPE  _00, _01, _02;
+      FLOATTYPE  _10, _11, _12;
+      FLOATTYPE  _20, _21, _22;
+    } m;
+    
+    FLOATTYPE data[3 * 3];
   } _m;
 
 private:
   INLINE_LINMATH FLOATTYPE mult_cel(const FLOATNAME(LMatrix3) &other, int x, int y) const;
   INLINE_LINMATH FLOATTYPE det2(FLOATTYPE e00, FLOATTYPE e01, FLOATTYPE e10, FLOATTYPE e11) const;
 
-  static FLOATNAME(LMatrix3) _ident_mat;
+  static const FLOATNAME(LMatrix3) _ident_mat;
+  static const FLOATNAME(LMatrix3) _y_to_z_up_mat;
+  static const FLOATNAME(LMatrix3) _z_to_y_up_mat;
+  static const FLOATNAME(LMatrix3) _flip_y_mat;
+  static const FLOATNAME(LMatrix3) _flip_z_mat;
+  static const FLOATNAME(LMatrix3) _lz_to_ry_mat;
+  static const FLOATNAME(LMatrix3) _ly_to_rz_mat;
 
   //Functionality for reading and writing from/to a binary source
 public:

panda/src/linmath/lmatrix4_src.cxx

@@ -36,13 +36,31 @@ const FLOATNAME(LMatrix4) FLOATNAME(LMatrix4)::_z_to_y_up_mat =
                       0.0f, 1.0f, 0.0f, 0.0f,
                       0.0f, 0.0f, 0.0f, 1.0f);
 
+const FLOATNAME(LMatrix4) FLOATNAME(LMatrix4)::_flip_y_mat =
+  FLOATNAME(LMatrix4)(1.0f, 0.0f, 0.0f, 0.0f,
+                      0.0f,-1.0f, 0.0f, 0.0f,
+                      0.0f, 0.0f, 1.0f, 0.0f,
+                      0.0f, 0.0f, 0.0f, 1.0f);
+
+const FLOATNAME(LMatrix4) FLOATNAME(LMatrix4)::_flip_z_mat =
+  FLOATNAME(LMatrix4)(1.0f, 0.0f, 0.0f, 0.0f,
+                      0.0f, 1.0f, 0.0f, 0.0f,
+                      0.0f, 0.0f,-1.0f, 0.0f,
+                      0.0f, 0.0f, 0.0f, 1.0f);
+
+const FLOATNAME(LMatrix4) FLOATNAME(LMatrix4)::_lz_to_ry_mat =
+  FLOATNAME(LMatrix4)::_flip_y_mat * FLOATNAME(LMatrix4)::_z_to_y_up_mat;
+
+const FLOATNAME(LMatrix4) FLOATNAME(LMatrix4)::_ly_to_rz_mat =
+  FLOATNAME(LMatrix4)::_flip_z_mat * FLOATNAME(LMatrix4)::_y_to_z_up_mat;
+
 ////////////////////////////////////////////////////////////////////
 //     Function: LMatrix::convert_mat
 //       Access: Public, Static
 //  Description: Returns a matrix that transforms from the indicated
 //               coordinate system to the indicated coordinate system.
 ////////////////////////////////////////////////////////////////////
-FLOATNAME(LMatrix4) FLOATNAME(LMatrix4)::
+const FLOATNAME(LMatrix4) &FLOATNAME(LMatrix4)::
 convert_mat(CoordinateSystem from, CoordinateSystem to) {
   if (from == CS_default) {
     from = default_coordinate_system;
@@ -53,40 +71,40 @@ convert_mat(CoordinateSystem from, CoordinateSystem to) {
   switch (from) {
   case CS_zup_left:
     switch (to) {
-    case CS_zup_left: return ident_mat();
-    case CS_yup_left: return z_to_y_up_mat();
-    case CS_zup_right: return scale_mat(1.0f, -1.0f, 1.0f);
-    case CS_yup_right: return scale_mat(1.0f, -1.0f, 1.0f) * z_to_y_up_mat();
+    case CS_zup_left: return _ident_mat;
+    case CS_yup_left: return _z_to_y_up_mat;
+    case CS_zup_right: return _flip_y_mat;
+    case CS_yup_right: return _lz_to_ry_mat;
     default: break;
     }
     break;
 
   case CS_yup_left:
     switch (to) {
-    case CS_zup_left: return y_to_z_up_mat();
-    case CS_yup_left: return ident_mat();
-    case CS_zup_right: return scale_mat(1.0f, 1.0f, -1.0f) * y_to_z_up_mat();
-    case CS_yup_right: return scale_mat(1.0f, 1.0f, -1.0f);
+    case CS_zup_left: return _y_to_z_up_mat;
+    case CS_yup_left: return _ident_mat;
+    case CS_zup_right: return _ly_to_rz_mat;
+    case CS_yup_right: return _flip_z_mat;
     default: break;
     }
     break;
 
   case CS_zup_right:
     switch (to) {
-    case CS_zup_left: return scale_mat(1.0f, -1.0f, 1.0f);
-    case CS_yup_left: return scale_mat(1.0f, -1.0f, 1.0f) * z_to_y_up_mat();
-    case CS_zup_right: return ident_mat();
-    case CS_yup_right: return z_to_y_up_mat();
+    case CS_zup_left: return _flip_y_mat;
+    case CS_yup_left: return _lz_to_ry_mat;
+    case CS_zup_right: return _ident_mat;
+    case CS_yup_right: return _z_to_y_up_mat;
     default: break;
     }
     break;
 
   case CS_yup_right:
     switch (to) {
-    case CS_zup_left: return scale_mat(1.0f, 1.0f, -1.0f) * y_to_z_up_mat();
-    case CS_yup_left: return scale_mat(1.0f, 1.0f, -1.0f);
-    case CS_zup_right: return y_to_z_up_mat();
-    case CS_yup_right: return ident_mat();
+    case CS_zup_left: return _ly_to_rz_mat;
+    case CS_yup_left: return _flip_z_mat;
+    case CS_zup_right: return _y_to_z_up_mat;
+    case CS_yup_right: return _ident_mat;
     default: break;
     }
     break;
@@ -97,7 +115,7 @@ convert_mat(CoordinateSystem from, CoordinateSystem to) {
 
   linmath_cat.error()
     << "Invalid coordinate system value!\n";
-  return ident_mat();
+  return _ident_mat;
 }
 
 ////////////////////////////////////////////////////////////////////

panda/src/linmath/lmatrix4_src.h

@@ -155,8 +155,8 @@ PUBLISHED:
   INLINE_LINMATH static const FLOATNAME(LMatrix4) &y_to_z_up_mat();
   INLINE_LINMATH static const FLOATNAME(LMatrix4) &z_to_y_up_mat();
 
-  static FLOATNAME(LMatrix4) convert_mat(CoordinateSystem from,
-                                         CoordinateSystem to);
+  static const FLOATNAME(LMatrix4) &convert_mat(CoordinateSystem from,
+                                                CoordinateSystem to);
 
   bool almost_equal(const FLOATNAME(LMatrix4) &other,
                     FLOATTYPE threshold) const;
@@ -171,14 +171,14 @@ public:
 
 public:
   union {
-        struct {
-                   FLOATTYPE  _00, _01, _02, _03;
-           FLOATTYPE  _10, _11, _12, _13;
-           FLOATTYPE  _20, _21, _22, _23;
-           FLOATTYPE  _30, _31, _32, _33;
-        } m;
-
-        FLOATTYPE data[4 * 4];
+    struct {
+      FLOATTYPE  _00, _01, _02, _03;
+      FLOATTYPE  _10, _11, _12, _13;
+      FLOATTYPE  _20, _21, _22, _23;
+      FLOATTYPE  _30, _31, _32, _33;
+    } m;
+    
+    FLOATTYPE data[4 * 4];
   } _m;
 
 private:
@@ -189,6 +189,10 @@ private:
   static const FLOATNAME(LMatrix4) _ident_mat;
   static const FLOATNAME(LMatrix4) _y_to_z_up_mat;
   static const FLOATNAME(LMatrix4) _z_to_y_up_mat;
+  static const FLOATNAME(LMatrix4) _flip_y_mat;
+  static const FLOATNAME(LMatrix4) _flip_z_mat;
+  static const FLOATNAME(LMatrix4) _lz_to_ry_mat;
+  static const FLOATNAME(LMatrix4) _ly_to_rz_mat;
 
   //Functionality for reading and writing from/to a binary source
 public:

panda/src/linmath/lquaternion_src.cxx

@@ -47,9 +47,9 @@ extract_to_matrix(FLOATNAME(LMatrix3) &m) const {
   xx = _v.data[1] * xs;  xy = _v.data[1] * ys;  xz = _v.data[1] * zs;
   yy = _v.data[2] * ys;  yz = _v.data[2] * zs;  zz = _v.data[3] * zs;
 
-  m = FLOATNAME(LMatrix3)((1. - (yy + zz)), (xy - wz), (xz + wy),
-                          (xy + wz), (1. - (xx + zz)), (yz - wx),
-                          (xz - wy), (yz + wx), (1. - (xx + yy)));
+  m.set((1. - (yy + zz)), (xy - wz), (xz + wy),
+        (xy + wz), (1. - (xx + zz)), (yz - wx),
+        (xz - wy), (yz + wx), (1. - (xx + yy)));
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -68,10 +68,10 @@ extract_to_matrix(FLOATNAME(LMatrix4) &m) const {
   xx = _v.data[1] * xs;  xy = _v.data[1] * ys;  xz = _v.data[1] * zs;
   yy = _v.data[2] * ys;  yz = _v.data[2] * zs;  zz = _v.data[3] * zs;
 
-  m = FLOATNAME(LMatrix4)((1. - (yy + zz)), (xy - wz), (xz + wy), 0.,
-                          (xy + wz), (1. - (xx + zz)), (yz - wx), 0.,
-                          (xz - wy), (yz + wx), (1. - (xx + yy)), 0.,
-                          0., 0., 0., 1.);
+  m.set((1. - (yy + zz)), (xy - wz), (xz + wy), 0.,
+        (xy + wz), (1. - (xx + zz)), (yz - wx), 0.,
+        (xz - wy), (yz + wx), (1. - (xx + yy)), 0.,
+        0., 0., 0., 1.);
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -165,15 +165,17 @@ get_hpr() const {
 ////////////////////////////////////////////////////////////////////
 void FLOATNAME(LQuaternion)::
 set_from_matrix(const FLOATNAME(LMatrix3) &m) {
-  FLOATTYPE m00 = m(0, 0);
-  FLOATTYPE m01 = m(0, 1);
-  FLOATTYPE m02 = m(0, 2);
-  FLOATTYPE m10 = m(1, 0);
-  FLOATTYPE m11 = m(1, 1);
-  FLOATTYPE m12 = m(1, 2);
-  FLOATTYPE m20 = m(2, 0);
-  FLOATTYPE m21 = m(2, 1);
-  FLOATTYPE m22 = m(2, 2);
+  FLOATTYPE m00, m01, m02, m10, m11, m12, m20, m21, m22;
+
+  m00 = m(0, 0);
+  m01 = m(0, 1);
+  m02 = m(0, 2);
+  m10 = m(1, 0);
+  m11 = m(1, 1);
+  m12 = m(1, 2);
+  m20 = m(2, 0);
+  m21 = m(2, 1);
+  m22 = m(2, 2);
 
   FLOATTYPE T = m00 + m11 + m22;
 

panda/src/vrpn/vrpnClient.cxx

@@ -42,6 +42,9 @@ VrpnClient::
 VrpnClient(const string &server_name) :
   _server_name(server_name)
 {
+  // By convention, VRPN always uses a Y-up coordinate system.
+  set_coordinate_system(CS_yup_right);
+
   if (vrpn_cat.is_debug()) {
     vrpn_cat.debug()
       << "Attempting to connect to VRPN server " << _server_name