Panda3D
|
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 }