text: /*=========================================================================
  
    Program: ParaView
    Module:    vtkVRUIConnection.cxx
  
    Copyright (c) 2005,2006 Sandia Corporation, Kitware Inc.
    All rights reserved.
  
    ParaView is a free software; you can redistribute it and/or modify it
    under the terms of the ParaView license version 1.2.
  
    See License_v1.2.txt for the full ParaView license.
    A copy of this license can be obtained by contacting
    Kitware Inc.
    28 Corporate Drive
    Clifton Park, NY 12065
    USA
  
    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
    A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR
    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  
    ========================================================================*/
  #include "pqVRUIConnection.h"
  
  #include "pqActiveObjects.h"
  #include "pqView.h"
  #include "vtkMath.h"
  #include "vtkMatrix4x4.h"
  #include "vtkObjectFactory.h"
  #include "vtkPVXMLElement.h"
  #include "vtkSMDoubleVectorProperty.h"
  #include "vtkSMPropertyHelper.h"
  #include "vtkSMRenderViewProxy.h"
  #include "vtkSMRepresentationProxy.h"
  #include "vtkTransform.h"
  #include "vtkVRUIPipe.h"
  #include "vtkVRUIServerState.h"
  #include "vtkVRUITrackerState.h"
  #include <QDateTime>
  #include <QDebug>
  #include <QMutex>
  #include <algorithm>
  #include <iostream>
  #include <pqDataRepresentation.h>
  #include <sstream>
  #include <vector>
  #include <vtkCamera.h>
  #include <vtkRenderWindow.h>
  #include <vtkRenderer.h>
  #ifdef QTSOCK
  #include <QTcpSocket>
  #else
  #include <fcntl.h>
  #include <netdb.h>
  #include <netinet/in.h>
  #include <sys/socket.h>
  #include <unistd.h>
  #endif
  
  // ----------------------------------------------------------------------------
  class pqVRUIConnection::pqInternals
  {
  public:
    // --------------------------------------------------------------------------
    pqInternals()
    {
  #ifdef QTSOCK
      this->Socket = false;
  #else
      this->Socket = -1;
  #endif
      this->Active = false;
      this->Pipe = 0;
      this->State = 0;
      this->StateMutex = 0;
      this->Streaming = false; // streaming
      //  this->PacketSignalCond=0;        // for streaming
      //   this->PacketSignalCondMutex=0; // for streaming
    }
  
    // --------------------------------------------------------------------------
    ~pqInternals()
    {
      if (this->Pipe != 0)
      {
        delete this->Pipe;
      }
      if (this->State != 0)
      {
        delete this->State;
      }
    }
  
  #ifdef QTSOCK
    QTcpSocket* Socket;
  #else
    int Socket;
  #endif
    bool Active;
    vtkVRUIPipe* Pipe;
    vtkVRUIServerState* State;
    QMutex* StateMutex;
    bool Streaming; // streaming
    // QWaitCondition *PacketSignalCond; // for streaming
    // QMutex *PacketSignalCondMutex;    // for streaming
  
    // --------------------------------------------------------------------------
    void initSocket(std::string address, std::string port)
    {
  #ifdef QTSOCK
      this->Socket = new QTcpSocket;
  #ifdef VRUI_ENABLE_DEBUG
      qDebug() << QString(address.c_str()) << "::" << QString(port.c_str()).toInt();
  #endif
  
      this->Socket->connectToHost(QString(address.c_str()),
        QString(port.c_str()).toInt()); // ReadWrite?
  #else
      struct sockaddr_in client_addr;
      struct hostent* hp;
  
      this->Socket = socket(AF_INET, SOCK_STREAM, 0);
      if (this->Socket < 0)
      {
  #ifdef VRUI_ENABLE_DEBUG
        qDebug() << "Error opening stream socket";
        abort();
  #endif
      }
  
      /* Name socket using file system name */
      hp = gethostbyname(address.c_str());
      if (hp == 0)
      {
        fprintf(stderr, "%s: unknown host\n", address.c_str());
        return;
      }
      bcopy(hp->h_addr, &client_addr.sin_addr, hp->h_length);
  
      client_addr.sin_family = AF_INET;
      client_addr.sin_port = htons((uint16_t)atoi(port.c_str()));
  
      if (::connect(this->Socket, (struct sockaddr*)&client_addr, sizeof(struct sockaddr_in)) < 0)
      { /* TODO: why is this "sockaddr", when the type is "sockaddr_in" ?? */
        close(this->Socket);
        perror("connecting stream socket");
        return;
      }
  #endif
    }
  
    // --------------------------------------------------------------------------
    void initPipe() { this->Pipe = new vtkVRUIPipe(this->Socket); }
  
    // --------------------------------------------------------------------------
    bool connect()
    {
  #ifdef VRUI_ENABLE_DEBUG
      std::cout << "Trying to connect" << std::endl;
  #endif
      this->Pipe->Send(vtkVRUIPipe::CONNECT_REQUEST);
      if (!this->Pipe->WaitForServerReply(30000)) // 30s
      {
        cerr << "Timeout while waiting for CONNECT_REPLY" << endl;
        delete this->Pipe;
        this->Pipe = 0;
        return false;
      }
      if (this->Pipe->Receive() != vtkVRUIPipe::CONNECT_REPLY)
      {
        cerr << "Mismatching message while waiting for CONNECT_REPLY" << endl;
        delete this->Pipe;
        this->Pipe = 0;
        return false;
      }
      this->State = new vtkVRUIServerState;
      this->StateMutex = new QMutex;
      this->Pipe->ReadLayout(this->State);
      return true;
    }
  
    // --------------------------------------------------------------------------
    void activate()
    {
      if (!this->Active)
      {
        this->Pipe->Send(vtkVRUIPipe::ACTIVATE_REQUEST);
        this->Active = true;
      }
    }
  
    // --------------------------------------------------------------------------
    void deactivate()
    {
      if (this->Active)
      {
        this->Active = false;
        this->Pipe->Send(vtkVRUIPipe::DEACTIVATE_REQUEST);
      }
    }
  
    // --------------------------------------------------------------------------
    void startStream()
    {
      if (this->Active)
      {
  #ifdef VRUI_ENABLE_DEBUG
        std::cout << "start streaming" << std::endl;
  #endif
        this->Streaming = true;
        this->Pipe->Send(vtkVRUIPipe::STARTSTREAM_REQUEST);
      }
    }
  
    // --------------------------------------------------------------------------
    void stopStream()
    {
      if (this->Streaming)
      {
        this->Streaming = false;
        this->Pipe->Send(vtkVRUIPipe::STOPSTREAM_REQUEST);
      }
    }
  
    // --------------------------------------------------------------------------
    // Streaming routine called when streaming is used
    void readStream()
    {
      this->StateMutex->lock();
      this->Pipe->ReadState(this->State);
      this->StateMutex->unlock();
      bool done = false;
      vtkVRUIPipe::MessageTag m = this->Pipe->Receive();
      switch (m)
      {
        case vtkVRUIPipe::PACKET_REPLY:
  #ifdef VRUI_ENABLE_DEBUG
          cout << "thread:PACKET_REPLY ok : tag=" << m << endl;
  #endif
          this->StateMutex->lock();
          this->Pipe->ReadState(this->State);
          this->StateMutex->unlock();
  
          break;
        case vtkVRUIPipe::STOPSTREAM_REPLY:
          cout << "thread:STOPSTREAM_REPLY ok : tag=" << m << endl;
          done = true;
          break;
        default:
          cerr << "thread: Mismatching message while waiting for PACKET_REPLY: tag="
               << this->Pipe->GetString(m) << "::" << m << endl;
          done = true;
          break;
      }
    }
  };
  
  // ----------------------------------------------------------------------------
  pqVRUIConnection::pqVRUIConnection(QObject* parentObject)
    : Superclass(parentObject)
  {
    this->Internals = new pqInternals();
    this->Initialized = false;
    this->_Stop = false;
    this->Address = "";
    this->Port = "8555";
    this->Name = "";
    this->Type = "VRUI";
    this->TrackerPresent = false;
    this->AnalogPresent = false;
    this->ButtonPresent = false;
    this->TrackerTransformPresent = false;
    this->Transformation = vtkMatrix4x4::New();
    this->Matrix = vtkMatrix4x4::New();
    this->ZUpToYUpMatrix = vtkMatrix4x4::New();
  }
  
  // ----------------------------------------------------------------------------
  pqVRUIConnection::~pqVRUIConnection()
  {
    this->ZUpToYUpMatrix->Delete();
    this->Matrix->Delete();
    this->Transformation->Delete();
    this->Internals->stopStream();
    this->Internals->deactivate();
    delete this->Internals;
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::addButton(std::string id, std::string name)
  {
    std::stringstream returnStr;
    returnStr << "button." << id;
    this->ButtonMapping[returnStr.str()] = name;
    this->ButtonPresent = true;
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::addAnalog(std::string id, std::string name)
  {
    std::stringstream returnStr;
    returnStr << "analog." << id;
    this->AnalogMapping[returnStr.str()] = name;
    this->AnalogPresent = true;
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::addTracking(std::string id, std::string name)
  {
    std::stringstream returnStr;
    returnStr << "tracker." << id;
    this->TrackerMapping[returnStr.str()] = name;
    this->TrackerPresent = true;
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::setName(std::string name)
  {
    this->Name = name;
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::setAddress(std::string address)
  {
    this->Address = address;
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::setQueue(vtkVRQueue* queue)
  {
    this->EventQueue = queue;
  }
  
  // ----------------------------------------------------------------------------
  bool pqVRUIConnection::init()
  {
    // Initialize the socket connection;
    this->Internals->initSocket(this->Address, this->Port);
    this->Internals->initPipe();
    if (!this->Internals->connect())
      return false;
    this->Internals->activate();
    // this->Internals->startStream();
    this->Initialized = true;
    return true;
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::run()
  {
    while (!this->_Stop)
    {
      if (this->Initialized)
      {
        if (this->Internals->Streaming)
        {
          this->Internals->readStream();
        }
        else
        {
          this->callback();
        }
      }
    }
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::stop()
  {
    this->_Stop = true;
    this->Internals->stopStream();
    this->Internals->deactivate();
    this->Initialized = false;
    QThread::wait();
  }
  
  // ----------------------------------------------------------------------------
  std::string pqVRUIConnection::name(int eventType, int id)
  {
    std::stringstream returnStr, connection, e;
    if (this->Name.size())
      returnStr << this->Name << ".";
    else
      returnStr << this->Address << ".";
    switch (eventType)
    {
      case ANALOG_EVENT:
        e << "analog." << id;
        if (this->AnalogMapping.find(e.str()) != this->AnalogMapping.end())
          returnStr << this->AnalogMapping[e.str()];
        else
          returnStr << e.str();
        break;
      case BUTTON_EVENT:
        e << "button." << id;
        if (this->ButtonMapping.find(e.str()) != this->ButtonMapping.end())
          returnStr << this->ButtonMapping[e.str()];
        else
          returnStr << e.str();
        break;
      case TRACKER_EVENT:
        e << "tracker." << id;
        if (this->TrackerMapping.find(e.str()) != this->TrackerMapping.end())
          returnStr << this->TrackerMapping[e.str()];
        else
          returnStr << e.str();
        break;
    }
    return returnStr.str();
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::verifyConfig(const char* id, const char* name)
  {
    if (!id)
    {
      qWarning() << "\"id\" should be specified";
    }
    if (!name)
    {
      qWarning() << "\"name\" should be specified";
    }
  }
  
  // ----------------------------------------------------------------------------
  bool pqVRUIConnection::configure(vtkPVXMLElement* child, vtkSMProxyLocator*)
  {
    bool returnVal = false;
    if (child->GetName() && strcmp(child->GetName(), "VRUIConnection") == 0)
    {
      for (unsigned cc = 0; cc < child->GetNumberOfNestedElements(); ++cc)
      {
        vtkPVXMLElement* e = child->GetNestedElement(cc);
        if (e && e->GetName())
        {
          const char* id = e->GetAttributeOrEmpty("id");
          const char* name = e->GetAttributeOrEmpty("name");
          this->verifyConfig(id, name);
  
          if (strcmp(e->GetName(), "Button") == 0)
          {
            this->addButton(id, name);
          }
          else if (strcmp(e->GetName(), "Analog") == 0)
          {
            this->addAnalog(id, name);
          }
          else if (strcmp(e->GetName(), "Tracker") == 0)
          {
            this->addTracking(id, name);
          }
          else if (strcmp(e->GetName(), "TrackerTransform") == 0)
          {
            this->configureTransform(e);
          }
  
          else
          {
            qWarning() << "Unknown Device type: \"" << e->GetName() << "\"";
          }
          returnVal = true;
        }
      }
    }
    return returnVal;
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::configureTransform(vtkPVXMLElement* child)
  {
    if (child->GetName() && strcmp(child->GetName(), "TrackerTransform") == 0)
    {
      child->GetVectorAttribute("value", 16, (double*)this->Transformation->Element);
      this->TrackerTransformPresent = true;
    }
  }
  
  // ----------------------------------------------------------------------------
  vtkPVXMLElement* pqVRUIConnection::saveConfiguration() const
  {
    vtkPVXMLElement* child = vtkPVXMLElement::New();
    child->SetName("VRUIConnection");
    child->AddAttribute("name", this->Name.c_str());
    child->AddAttribute("address", this->Address.c_str());
    child->AddAttribute("port", this->Port.c_str());
    saveButtonEventConfig(child);
    saveAnalogEventConfig(child);
    saveTrackerEventConfig(child);
    saveTrackerTransformationConfig(child);
    return child;
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::saveButtonEventConfig(vtkPVXMLElement* child) const
  {
    if (!this->ButtonPresent)
      return;
    for (std::map<std::string, std::string>::const_iterator it = this->ButtonMapping.begin();
         it != this->ButtonMapping.end(); ++it)
    {
      std::string key = it->first;
      std::string value = it->second;
      std::replace(key.begin(), key.end(), '.', ' ');
      std::istringstream stm(key);
      std::vector<std::string> token;
      for (;;)
      {
        std::string word;
        if (!(stm >> word))
          break;
        token.push_back(word);
      }
      vtkPVXMLElement* e = vtkPVXMLElement::New();
      if (strcmp(token[0].c_str(), "button") == 0)
      {
        e->SetName("Button");
        e->AddAttribute("id", token[1].c_str());
        e->AddAttribute("name", value.c_str());
      }
      child->AddNestedElement(e);
      e->FastDelete();
    }
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::saveAnalogEventConfig(vtkPVXMLElement* child) const
  {
    if (!this->AnalogPresent)
      return;
    for (std::map<std::string, std::string>::const_iterator it = this->AnalogMapping.begin();
         it != this->AnalogMapping.end(); ++it)
    {
      std::string key = it->first;
      std::string value = it->second;
      std::replace(key.begin(), key.end(), '.', ' ');
      std::istringstream stm(key);
      std::vector<std::string> token;
      for (;;)
      {
        std::string word;
        if (!(stm >> word))
          break;
        token.push_back(word);
      }
      vtkPVXMLElement* e = vtkPVXMLElement::New();
      if (strcmp(token[0].c_str(), "analog") == 0)
      {
        e->SetName("Analog");
        e->AddAttribute("id", token[1].c_str());
        e->AddAttribute("name", value.c_str());
      }
      child->AddNestedElement(e);
      e->FastDelete();
    }
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::saveTrackerEventConfig(vtkPVXMLElement* child) const
  {
    if (!this->TrackerPresent)
      return;
    for (std::map<std::string, std::string>::const_iterator it = this->TrackerMapping.begin();
         it != this->TrackerMapping.end(); ++it)
    {
      std::string key = it->first;
      std::string value = it->second;
      std::replace(key.begin(), key.end(), '.', ' ');
      std::istringstream stm(key);
      std::vector<std::string> token;
      for (;;)
      {
        std::string word;
        if (!(stm >> word))
          break;
        token.push_back(word);
      }
      vtkPVXMLElement* e = vtkPVXMLElement::New();
      if (strcmp(token[0].c_str(), "tracker") == 0)
      {
        e->SetName("Tracker");
        e->AddAttribute("id", token[1].c_str());
        e->AddAttribute("name", value.c_str());
      }
      child->AddNestedElement(e);
      e->FastDelete();
    }
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::saveTrackerTransformationConfig(vtkPVXMLElement* child) const
  {
    if (!this->TrackerTransformPresent)
      return;
    vtkPVXMLElement* transformationMatrix = vtkPVXMLElement::New();
    transformationMatrix->SetName("TrackerTransform");
    std::stringstream matrix;
    for (int i = 0; i < 16; ++i)
    {
      matrix << double(*((double*)this->Transformation->Element + i)) << " ";
    }
    transformationMatrix->AddAttribute("value", matrix.str().c_str());
    child->AddNestedElement(transformationMatrix);
    transformationMatrix->FastDelete();
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::setTransformation(vtkMatrix4x4* matrix)
  {
    for (int i = 0; i < 4; ++i)
    {
      for (int j = 0; j < 4; ++j)
      {
        this->Transformation->SetElement(i, j, matrix->GetElement(i, j));
      }
    }
    this->TrackerTransformPresent = true;
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::callback()
  {
    if (this->Initialized)
    {
  
  #ifdef VRUI_ENABLE_DEBUG
      std::cout << "callback()" << std::endl;
  #endif
      this->Internals->StateMutex->lock();
      this->getAndEnqueueButtonData();
      this->getAndEnqueueAnalogData();
      this->getAndEnqueueTrackerData();
      this->Internals->StateMutex->unlock();
  
      this->getNextPacket(); // for the next step
    }
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::getNextPacket()
  {
    if (this->Internals->Active)
    {
      if (this->Internals->Streaming)
      {
        // With a thread
        // this->Internals->PacketSignalCondMutex->lock();
        // this->Internals->PacketSignalCond->wait(this->Internals->PacketSignalCondMutex);
        // this->Internals->PacketSignalCondMutex->unlock();
      }
      else
      {
        // With a loop
        this->Internals->Pipe->Send(vtkVRUIPipe::PACKET_REQUEST);
        if (this->Internals->Pipe->WaitForServerReply(30000))
        {
          if (this->Internals->Pipe->Receive() != vtkVRUIPipe::PACKET_REPLY)
          {
            cout << "VRUI Mismatching message while waiting for PACKET_REPLY" << std::endl;
            abort();
          }
          else
          {
            this->Internals->StateMutex->lock();
            this->Internals->Pipe->ReadState(this->Internals->State);
            this->Internals->StateMutex->unlock();
  
            //          this->PacketNotificationMutex->lock();
            //          this->PacketNotificationMutex->unlock();
          }
        }
        else
        {
          cout << "timeout for PACKET_REPLY" << endl;
        }
      }
    }
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::newAnalogValue(std::vector<float>* data)
  {
    vtkVREvent temp;
    temp.connId = this->Address;
    temp.name = name(ANALOG_EVENT);
    temp.eventType = ANALOG_EVENT;
    temp.timeStamp = QDateTime::currentDateTime().toTime_t();
    temp.data.analog.num_channels = (int)(*data).size();
    for (unsigned int i = 0; i < (*data).size(); ++i)
    {
      temp.data.analog.channel[i] = (*data)[i];
    }
    this->EventQueue->Enqueue(temp);
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::newButtonValue(int state, int button)
  {
    vtkVREvent temp;
    temp.connId = this->Address;
    temp.name = this->name(BUTTON_EVENT, button);
    temp.eventType = BUTTON_EVENT;
    temp.timeStamp = QDateTime::currentDateTime().toTime_t();
    temp.data.button.button = button;
    temp.data.button.state = state;
    this->EventQueue->Enqueue(temp);
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::newTrackerValue(vtkSmartPointer<vtkVRUITrackerState> data, int sensor)
  {
    vtkVREvent temp;
    temp.connId = this->Address;
    temp.name = name(TRACKER_EVENT, sensor);
    temp.eventType = TRACKER_EVENT;
    temp.timeStamp = QDateTime::currentDateTime().toTime_t();
    temp.data.tracker.sensor = sensor;
    float rotMatrix[3][3];
    float pos[3];
    float q[4];
    data->GetPosition(pos);
    data->GetUnitQuaternion(q);
  
  #if defined(VRUI_ENABLE_DEBUG) || 0
    cout << "pos=(" << pos[0] << "," << pos[1] << "," << pos[2] << ")" << endl;
    cout << "q=(" << q[0] << "," << q[1] << "," << q[2] << "," << q[3] << ")" << endl;
  #endif
  
    // VTK expects quaternion in the format of (real, i, j, k), where as
    // FreeVR VRUI Daemon is giving us (i,j,k, real)
    float vtkQuat[4] = { q[3], q[0], q[1], q[2] };
  
    vtkMath::QuaternionToMatrix3x3(&vtkQuat[0], rotMatrix);
  
    this->Matrix->Element[0][0] = rotMatrix[0][0];
    this->Matrix->Element[1][0] = rotMatrix[1][0];
    this->Matrix->Element[2][0] = rotMatrix[2][0];
    this->Matrix->Element[3][0] = 0.0;
  
    this->Matrix->Element[0][1] = rotMatrix[0][1];
    this->Matrix->Element[1][1] = rotMatrix[1][1];
    this->Matrix->Element[2][1] = rotMatrix[2][1];
    // this->Matrix->Element[1][3] = pos[2]*1/12;
    this->Matrix->Element[3][1] = 0.0;
  
    this->Matrix->Element[0][2] = rotMatrix[0][2];
    this->Matrix->Element[1][2] = rotMatrix[1][2];
    this->Matrix->Element[2][2] = rotMatrix[2][2];
    // this->Matrix->Element[2][3] = pos[1]*-1/12;
    this->Matrix->Element[3][2] = 0.0;
  
    this->Matrix->Element[0][3] = pos[0] / 12.0;
    this->Matrix->Element[1][3] = pos[1] / 12.0;
    this->Matrix->Element[2][3] = pos[2] / 12.0;
    this->Matrix->Element[3][3] = 1.0f;
  
  #if defined(VRUI_ENABLE_DEBUG) || 0
    if (sensor)
    {
      std::cout << "Pre multiplication matrix: " << std::endl;
      this->Matrix->PrintSelf(cout, vtkIndent(0));
    }
  #endif
  
    this->ZUpToYUpMatrix->Element[0][0] = 1.0;
    this->ZUpToYUpMatrix->Element[1][0] = 0.0;
    this->ZUpToYUpMatrix->Element[2][0] = 0.0;
    this->ZUpToYUpMatrix->Element[3][0] = 0.0;
  
    this->ZUpToYUpMatrix->Element[0][1] = 0.0;
    this->ZUpToYUpMatrix->Element[1][1] = 0.0;
    this->ZUpToYUpMatrix->Element[2][1] = -1.0;
    this->ZUpToYUpMatrix->Element[3][1] = 0.0;
  
    this->ZUpToYUpMatrix->Element[0][2] = 0.0;
    this->ZUpToYUpMatrix->Element[1][2] = 1.0;
    this->ZUpToYUpMatrix->Element[2][2] = 0.0;
    this->ZUpToYUpMatrix->Element[3][2] = 0.0;
  
    this->ZUpToYUpMatrix->Element[0][3] = 0.0;
    this->ZUpToYUpMatrix->Element[1][3] = 0.0;
    this->ZUpToYUpMatrix->Element[2][3] = 0.0;
    this->ZUpToYUpMatrix->Element[3][3] = 1.0;
  
    vtkMatrix4x4::Multiply4x4(this->ZUpToYUpMatrix, this->Matrix, this->Matrix);
  
  #if defined(VRUI_ENABLE_DEBUG) || 0
    if (sensor)
    {
      std::cout << "Post multiplication matrix: " << std::endl;
      this->Matrix->PrintSelf(cout, vtkIndent(0));
    }
  #endif
  
  #if defined(VRUI_ENABLE_DEBUG) || 0
    if (sensor)
      cout << "post pos=(" << this->Matrix->Element[0][3] << "," << this->Matrix->Element[1][3] << ","
           << this->Matrix->Element[2][3] << ")" << endl;
  #endif
  
    temp.data.tracker.matrix[0] = this->Matrix->Element[0][0];
    temp.data.tracker.matrix[1] = this->Matrix->Element[0][1];
    temp.data.tracker.matrix[2] = this->Matrix->Element[0][2];
    temp.data.tracker.matrix[3] = this->Matrix->Element[0][3];
  
    temp.data.tracker.matrix[4] = this->Matrix->Element[1][0];
    temp.data.tracker.matrix[5] = this->Matrix->Element[1][1];
    temp.data.tracker.matrix[6] = this->Matrix->Element[1][2];
    temp.data.tracker.matrix[7] = this->Matrix->Element[1][3];
  
    temp.data.tracker.matrix[8] = this->Matrix->Element[2][0];
    temp.data.tracker.matrix[9] = this->Matrix->Element[2][1];
    temp.data.tracker.matrix[10] = this->Matrix->Element[2][2];
    temp.data.tracker.matrix[11] = this->Matrix->Element[2][3];
  
    temp.data.tracker.matrix[12] = this->Matrix->Element[3][0];
    temp.data.tracker.matrix[13] = this->Matrix->Element[3][1];
    temp.data.tracker.matrix[14] = this->Matrix->Element[3][2];
    temp.data.tracker.matrix[15] = this->Matrix->Element[3][3];
  
    this->EventQueue->Enqueue(temp);
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::getAndEnqueueButtonData()
  {
    std::vector<bool>* buttons = this->Internals->State->GetButtonStates();
    for (unsigned int i = 0; i < (*buttons).size(); ++i)
    {
      newButtonValue((*buttons)[i], i);
    }
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::getAndEnqueueAnalogData()
  {
    std::vector<float>* analog = this->Internals->State->GetValuatorStates();
    newAnalogValue(analog);
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::getAndEnqueueTrackerData()
  {
    std::vector<vtkSmartPointer<vtkVRUITrackerState>>* trackers =
      this->Internals->State->GetTrackerStates();
  
    for (unsigned int i = 0; i < (*trackers).size(); ++i)
    {
      newTrackerValue((*trackers)[i], i);
    }
  }
  
  // ----------------------------------------------------------------------------
  void pqVRUIConnection::setPort(std::string port)
  {
    this->Port = port;
  }
  