Panda3D
 All Classes Functions Variables Enumerations
vrpnTracker.cxx
00001 // Filename: vrpnTracker.cxx
00002 // Created by:  drose (25Jan01)
00003 //
00004 ////////////////////////////////////////////////////////////////////
00005 //
00006 // PANDA 3D SOFTWARE
00007 // Copyright (c) Carnegie Mellon University.  All rights reserved.
00008 //
00009 // All use of this software is subject to the terms of the revised BSD
00010 // license.  You should have received a copy of this license along
00011 // with this source code in a file named "LICENSE."
00012 //
00013 ////////////////////////////////////////////////////////////////////
00014 
00015 #include "vrpnTracker.h"
00016 #include "vrpnTrackerDevice.h"
00017 #include "vrpnClient.h"
00018 #include "config_vrpn.h"
00019 
00020 #include "indent.h"
00021 
00022 #include <algorithm>
00023 
00024 ////////////////////////////////////////////////////////////////////
00025 //     Function: VrpnTracker::Constructor
00026 //       Access: Public
00027 //  Description:
00028 ////////////////////////////////////////////////////////////////////
00029 VrpnTracker::
00030 VrpnTracker(const string &tracker_name, vrpn_Connection *connection) :
00031   _tracker_name(tracker_name)
00032 {
00033   _tracker = new vrpn_Tracker_Remote(_tracker_name.c_str(), connection);
00034 
00035   _tracker->register_change_handler((void*)this, &vrpn_position_callback);
00036   _tracker->register_change_handler((void*)this, &vrpn_velocity_callback);
00037   _tracker->register_change_handler((void*)this, &vrpn_acceleration_callback);
00038 }
00039 
00040 ////////////////////////////////////////////////////////////////////
00041 //     Function: VrpnTracker::Destructor
00042 //       Access: Public
00043 //  Description:
00044 ////////////////////////////////////////////////////////////////////
00045 VrpnTracker::
00046 ~VrpnTracker() {
00047   delete _tracker;
00048 }
00049 
00050 ////////////////////////////////////////////////////////////////////
00051 //     Function: VrpnTracker::mark
00052 //       Access: Public
00053 //  Description: Adds the indicated VrpnTrackerDevice to the list of
00054 //               devices that are sharing this VrpnTracker.
00055 ////////////////////////////////////////////////////////////////////
00056 void VrpnTracker::
00057 mark(VrpnTrackerDevice *device) {
00058   if (vrpn_cat.is_debug()) {
00059     vrpn_cat.debug() << *this << " marking " << *device << "\n";
00060   }
00061   _devices.push_back(device);
00062 }
00063 
00064 ////////////////////////////////////////////////////////////////////
00065 //     Function: VrpnTracker::unmark
00066 //       Access: Public
00067 //  Description: Removes the indicated VrpnTrackerDevice from the list
00068 //               of devices that are sharing this VrpnTracker.
00069 ////////////////////////////////////////////////////////////////////
00070 void VrpnTracker::
00071 unmark(VrpnTrackerDevice *device) {
00072   if (vrpn_cat.is_debug()) {
00073     vrpn_cat.debug() << *this << " unmarking " << *device << "\n";
00074   }
00075 
00076   Devices::iterator di =
00077     find(_devices.begin(), _devices.end(), device);
00078 
00079   if (di != _devices.end()) {
00080     _devices.erase(di);
00081   }
00082 }
00083 
00084 ////////////////////////////////////////////////////////////////////
00085 //     Function: VrpnTracker::output
00086 //       Access: Public
00087 //  Description:
00088 ////////////////////////////////////////////////////////////////////
00089 void VrpnTracker::
00090 output(ostream &out) const {
00091   out << _tracker_name;
00092 }
00093 
00094 ////////////////////////////////////////////////////////////////////
00095 //     Function: VrpnTracker::write
00096 //       Access: Public
00097 //  Description:
00098 ////////////////////////////////////////////////////////////////////
00099 void VrpnTracker::
00100 write(ostream &out, int indent_level) const {
00101   indent(out, indent_level)
00102     << get_tracker_name() << " ("
00103     << _devices.size() << " devices)\n";
00104 }
00105 
00106 ////////////////////////////////////////////////////////////////////
00107 //     Function: VrpnTracker::vrpn_position_callback
00108 //       Access: Private, Static
00109 //  Description: Receives the tracker positional data from the VRPN
00110 //               code and sends it to any interested
00111 //               VrpnTrackerDevices.
00112 ////////////////////////////////////////////////////////////////////
00113 void VRPN_CALLBACK VrpnTracker::
00114 vrpn_position_callback(void *userdata, const vrpn_TRACKERCB info) {
00115   VrpnTracker *self = (VrpnTracker *)userdata;
00116   if (vrpn_cat.is_spam()) {
00117     vrpn_cat.spam()
00118       << *self << " position_callback\n";
00119   }
00120 
00121   Devices::iterator di;
00122   for (di = self->_devices.begin(); di != self->_devices.end(); ++di) {
00123     VrpnTrackerDevice *device = (*di);
00124     if (device->get_sensor() == info.sensor &&
00125         device->get_data_type() == VrpnTrackerDevice::DT_position) {
00126       device->acquire();
00127       device->_data.set_time(VrpnClient::convert_to_secs(info.msg_time));
00128       device->_data.set_pos(LPoint3(info.pos[0], info.pos[1], info.pos[2]));
00129       device->_data.set_orient(LOrientation(info.quat[3], info.quat[0], info.quat[1], info.quat[2]));
00130       device->unlock();
00131     }
00132   }
00133 }
00134 
00135 ////////////////////////////////////////////////////////////////////
00136 //     Function: VrpnTracker::vrpn_velocity_callback
00137 //       Access: Private, Static
00138 //  Description: Receives the tracker velocity data from the VRPN
00139 //               code and sends it to any interested
00140 //               VrpnTrackerDevices.
00141 ////////////////////////////////////////////////////////////////////
00142 void VRPN_CALLBACK VrpnTracker::
00143 vrpn_velocity_callback(void *userdata, const vrpn_TRACKERVELCB info) {
00144   VrpnTracker *self = (VrpnTracker *)userdata;
00145   if (vrpn_cat.is_spam()) {
00146     vrpn_cat.spam()
00147       << *self << " velocity_callback\n";
00148   }
00149 
00150   Devices::iterator di;
00151   for (di = self->_devices.begin(); di != self->_devices.end(); ++di) {
00152     VrpnTrackerDevice *device = (*di);
00153     if (device->get_sensor() == info.sensor &&
00154         device->get_data_type() == VrpnTrackerDevice::DT_velocity) {
00155       device->acquire();
00156       device->_data.set_time(VrpnClient::convert_to_secs(info.msg_time));
00157       device->_data.set_pos(LPoint3(info.vel[0], info.vel[1], info.vel[2]));
00158       device->_data.set_orient(LOrientation(info.vel_quat[3], info.vel_quat[0],
00159                                              info.vel_quat[1], info.vel_quat[2]));
00160       device->_data.set_dt(info.vel_quat_dt);
00161       device->unlock();
00162     }
00163   }
00164 }
00165 
00166 ////////////////////////////////////////////////////////////////////
00167 //     Function: VrpnTracker::vrpn_acceleration_callback
00168 //       Access: Private, Static
00169 //  Description: Receives the tracker acceleration data from the VRPN
00170 //               code and sends it to any interested
00171 //               VrpnTrackerDevices.
00172 ////////////////////////////////////////////////////////////////////
00173 void VRPN_CALLBACK VrpnTracker::
00174 vrpn_acceleration_callback(void *userdata, const vrpn_TRACKERACCCB info) {
00175   VrpnTracker *self = (VrpnTracker *)userdata;
00176   if (vrpn_cat.is_spam()) {
00177     vrpn_cat.spam()
00178       << *self << " acceleration_callback\n";
00179   }
00180 
00181   Devices::iterator di;
00182   for (di = self->_devices.begin(); di != self->_devices.end(); ++di) {
00183     VrpnTrackerDevice *device = (*di);
00184     if (device->get_sensor() == info.sensor &&
00185         device->get_data_type() == VrpnTrackerDevice::DT_acceleration) {
00186       device->acquire();
00187       device->_data.set_time(VrpnClient::convert_to_secs(info.msg_time));
00188       device->_data.set_pos(LPoint3(info.acc[0], info.acc[1], info.acc[2]));
00189       device->_data.set_orient(LOrientation(info.acc_quat[3], info.acc_quat[0],
00190                                              info.acc_quat[1], info.acc_quat[2]));
00191       device->_data.set_dt(info.acc_quat_dt);
00192       device->unlock();
00193     }
00194   }
00195 }
 All Classes Functions Variables Enumerations