64inline static bool wm_isnan(
const double x) {
77const double fovX = Q_DEG_TO_RAD(43.0),
fovY = Q_DEG_TO_RAD(32.00);
86#define MAKE_IDENTITY_QUAT(dest) \
87 dest[0] = dest[1] = dest[2] = 0; dest[3] = 1
91#define MAKE_NULL_VEC(dest) \
92 dest[0] = dest[1] = dest[2] = 0
112 if (wiimote == NULL) {
114 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: "
115 "Can't start without a valid specified Wiimote device!");
156 fprintf(stderr,
"vrpn_Tracker_WiimoteHead::~vrpn_Tracker_WiimoteHead(): delete failed\n");
179 fprintf(stderr,
"vrpn_Tracker_WiimoteHead::setup_wiimote(): delete failed\n");
194 }
catch (...) {
d_ana = NULL; }
197 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: "
198 "Can't open Analog %s\n",
d_name);
205 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: "
206 "Can't setup change handler on Analog %s\n",
d_name);
210 fprintf(stderr,
"vrpn_Tracker_WiimoteHead::setup_wiimote(): delete failed\n");
247 q_xyz_quat_type newPose;
288 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: "
289 "cannot write message: tossing\n");
292 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: "
293 "No valid connection\n");
310 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: "
311 "got first report from Wiimote!\n");
317 for (i = 0; i < 4; i++) {
318 firstchan = i * 3 + 4;
321 if (info.
channel[firstchan] > 0
322 && info.
channel[firstchan + 1] > 0
323 && info.
channel[firstchan + 2] > 0) {
336 bool newgrav =
false;
339 for (i = 0; i < 3; i++) {
352 for (i = 0; i < 3; i++) {
359 wh->vrpn_Tracker::timestamp = info.
msg_time;
378 q_vec_type movingAvg = Q_NULL_VECTOR;
380 q_vec_copy(movingAvg,
d_vGrav);
383 q_vec_scale(movingAvg, 0.33333, movingAvg);
389 q_vec_type regulargravity = Q_NULL_VECTOR;
390 regulargravity[2] = 1;
411 double X0, X1, Y0, Y1;
429 const double dx = X0 - X1;
430 const double dy = Y0 - Y1;
431 const double dist = sqrt(dx * dx + dy * dy);
439 newPose.xyz[2] = headDist;
444 const double avgX = (X0 + X1) / 2.0;
445 const double avgY = (Y0 + Y1) / 2.0;
451 if (wm_isnan(avgX)) {
452 std::cerr <<
"NaN detected in avgX: X0 = " << X0 <<
", X1 = " << X1 << std::endl;
456 if (wm_isnan(avgY)) {
457 std::cerr <<
"NaN detected in avgY: Y0 = " << Y0 <<
", Y1 = " << Y1 << std::endl;
468 newPose.xyz[0] = headDist * (avgX -
xResSensor / 2) / bHoriz;
469 newPose.xyz[1] = headDist * (avgY -
yResSensor / 2) / bVert;
472 q_from_euler(newPose.quat, rz, ry, rx);
484 q_vec_type upVec = {0, 1, 0};
491 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: d_flipState = FLIP_180\n");
498 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: d_flipState = FLIP_NORMAL\n");
int register_autodeleted_handler(vrpn_int32 type, vrpn_MESSAGEHANDLER handler, void *userdata, vrpn_int32 sender=vrpn_ANY_SENDER)
Registers a handler with the connection, and remembers to delete at destruction.
vrpn_Connection * d_connection
Connection that this object talks to.
vrpn_int32 d_sender_id
Sender ID registered with the connection.
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
void _reset_points()
reset cached points, point count, and flip state,
static void VRPN_CALLBACK VRPN_CALLBACK handle_analog_update(void *userdata, const vrpn_ANALOGCB info)
Callback triggered when our data source issues an update.
void _update_gravity_moving_avg()
based on cached gravity data, use a moving average to update the tracker's stored gravity transform.
virtual ~vrpn_Tracker_WiimoteHead()
destructor
q_xyz_quat_type d_currentPose
Current pose estimate.
void report()
Pack and send tracker report.
vrpn_Analog_Remote * d_ana
Source of analog data, traditionally vrpn_WiiMote Must present analog channels in this order:
struct timeval d_prevtime
Time of last tracker report issued.
void _update_2_LED_pose(q_xyz_quat_type &newPose)
Create tracker-relative pose estimate based on sensor location of 2 tracked points.
FlipState d_flipState
Whether we need to flip the order of the tracked points before calculating a pose.
const double d_blobDistance
distance between LEDs on glasses, in meters
vrpn_Tracker_WiimoteHead(const char *name, vrpn_Connection *trackercon, const char *wiimote, float update_rate, float led_spacing=0.145)
constructor
q_xyz_quat_type d_gravityXform
Gravity correction transformation.
q_vec_type d_vGravPenultimate
q_vec_type d_vGravAntepenultimate
bool d_lock
Flag: Does the tracking algorithm report a lock?
virtual void mainloop()
VRPN mainloop function.
void _reset_gravity()
reset gravity transform and cached gravity vectors
void _update_flip_state()
If flip state is unknown, set flip state appropriately.
bool _should_report(double elapsedInterval) const
return true if we have new data or max time elapsed
static int VRPN_CALLBACK VRPN_CALLBACK handle_connection(void *, vrpn_HANDLERPARAM)
Callback triggered when a new client connects to the tracker.
bool d_updated
Flag: Have we received updated Wiimote data since last report?
bool d_gravDirty
Flag: Have we received updated gravity data since last gravity update?
virtual void reset()
reset pose, gravity transform, and cached points and gravity
bool d_contact
Flag: Have we received the first message from the Wiimote?
const char * d_name
Tracker device name.
bool _have_gravity() const
return true if our gravity values look like real data
void _reset_pose()
reset current pose, last report time, and tracker pose
const double d_update_interval
maximum time between updates, in seconds
void update_pose()
function to drive the full pose update process
void setup_wiimote()
set up connection to wiimote-like analog device
void _convert_pose_to_tracker()
Set the vrpn_Tracker position and rotation to that indicated by our d_currentPose;.
virtual int encode_to(char *buf)
vrpn_Tracker(const char *name, vrpn_Connection *c=NULL, const char *tracker_cfg_file_name=NULL)
vrpn_float64 channel[vrpn_CHANNEL_MAX]
This structure is what is passed to a vrpn_Connection message callback.
const char * vrpn_got_connection
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
Return the number of seconds between startT and endT as a floating-point value.
#define vrpn_gettimeofday
const double fovX
Field of view experimentally determined at Iowa State University March 2010.
#define MAKE_IDENTITY_QUAT(dest)
Utility function to set a quat equal to the identity rotation.
const double cvtDistToAngle
#define MAKE_NULL_VEC(dest)
Utility function to set a 3-vector equal to the zero vector.
vrpn_Tracker interface provided by processing Wii Remote data for head tracking.
class VRPN_API vrpn_Connection