64 inline static bool wm_isnan(
const double x) {
77 const 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 102 d_update_interval(update_rate ? (1.0 / update_rate) : 1.0 / 60.0),
103 d_blobDistance(led_spacing),
104 d_flipState(FLIP_UNKNOWN),
112 if (wiimote == NULL) {
114 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: " 115 "Can't start without a valid specified Wiimote device!");
185 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: " 186 "Can't open Analog %s\n",
d_name);
193 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: " 194 "Can't setup change handler on Analog %s\n",
d_name);
230 q_xyz_quat_type newPose;
271 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: " 272 "cannot write message: tossing\n");
275 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: " 276 "No valid connection\n");
293 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: " 294 "got first report from Wiimote!\n");
300 for (i = 0; i < 4; i++) {
301 firstchan = i * 3 + 4;
304 if (info.
channel[firstchan] > 0
305 && info.
channel[firstchan + 1] > 0
306 && info.
channel[firstchan + 2] > 0) {
319 bool newgrav =
false;
322 for (i = 0; i < 3; i++) {
335 for (i = 0; i < 3; i++) {
342 wh->vrpn_Tracker::timestamp = info.
msg_time;
361 q_vec_type movingAvg = Q_NULL_VECTOR;
363 q_vec_copy(movingAvg,
d_vGrav);
366 q_vec_scale(movingAvg, 0.33333, movingAvg);
372 q_vec_type regulargravity = Q_NULL_VECTOR;
373 regulargravity[2] = 1;
394 double X0, X1, Y0, Y1;
412 const double dx = X0 - X1;
413 const double dy = Y0 - Y1;
414 const double dist = sqrt(dx * dx + dy * dy);
422 newPose.xyz[2] = headDist;
427 const double avgX = (X0 + X1) / 2.0;
428 const double avgY = (Y0 + Y1) / 2.0;
434 if (wm_isnan(avgX)) {
435 std::cerr <<
"NaN detected in avgX: X0 = " << X0 <<
", X1 = " << X1 << std::endl;
439 if (wm_isnan(avgY)) {
440 std::cerr <<
"NaN detected in avgY: Y0 = " << Y0 <<
", Y1 = " << Y1 << std::endl;
451 newPose.xyz[0] = headDist * (avgX -
xResSensor / 2) / bHoriz;
452 newPose.xyz[1] = headDist * (avgY -
yResSensor / 2) / bVert;
455 q_from_euler(newPose.quat, rz, ry, rx);
467 q_vec_type upVec = {0, 1, 0};
474 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: d_flipState = FLIP_180\n");
481 fprintf(stderr,
"vrpn_Tracker_WiimoteHead: d_flipState = FLIP_NORMAL\n");
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
void _reset_gravity()
reset gravity transform and cached gravity vectors
q_vec_type d_vGravPenultimate
static int VRPN_CALLBACK VRPN_CALLBACK handle_connection(void *, vrpn_HANDLERPARAM)
Callback triggered when a new client connects to the tracker.
virtual ~vrpn_Tracker_WiimoteHead()
destructor
const double d_update_interval
maximum time between updates, in seconds
const char * d_name
Tracker device name.
void _reset_points()
reset cached points, point count, and flip state,
void setup_wiimote()
set up connection to wiimote-like analog device
void _update_2_LED_pose(q_xyz_quat_type &newPose)
Create tracker-relative pose estimate based on sensor location of 2 tracked points.
void _update_gravity_moving_avg()
based on cached gravity data, use a moving average to update the tracker's stored gravity transform...
#define MAKE_IDENTITY_QUAT(dest)
Utility function to set a quat equal to the identity rotation.
bool d_contact
Flag: Have we received the first message from the Wiimote?
vrpn_Analog_Remote * d_ana
Source of analog data, traditionally vrpn_WiiMote Must present analog channels in this order: ...
#define MAKE_NULL_VEC(dest)
Utility function to set a 3-vector equal to the zero vector.
Generic connection class not specific to the transport mechanism.
q_vec_type d_vGravAntepenultimate
const double cvtDistToAngle
q_xyz_quat_type d_currentPose
Current pose estimate.
bool _have_gravity() const
return true if our gravity values look like real data
double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
Return the number of seconds between startT and endT as a floating-point value.
bool _should_report(double elapsedInterval) const
return true if we have new data or max time elapsed
void _update_flip_state()
If flip state is unknown, set flip state appropriately.
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.
void update_pose()
function to drive the full pose update process
vrpn_Connection * d_connection
Connection that this object talks to.
Provides a tracker device given data from a Wii Remote and LED glasses.
This structure is what is passed to a vrpn_Connection message callback.
virtual int pack_message(vrpn_uint32 len, struct timeval time, vrpn_int32 type, vrpn_int32 sender, const char *buffer, vrpn_uint32 class_of_service)
Pack a message that will be sent the next time mainloop() is called. Turn off the RELIABLE flag if yo...
void _convert_pose_to_tracker()
Set the vrpn_Tracker position and rotation to that indicated by our d_currentPose;.
vrpn_float64 channel[vrpn_CHANNEL_MAX]
vrpn_Tracker interface provided by processing Wii Remote data for head tracking.
void report()
Pack and send tracker report.
const char * vrpn_got_connection
virtual int encode_to(char *buf)
virtual int register_change_handler(void *userdata, vrpn_ANALOGCHANGEHANDLER handler)
bool d_updated
Flag: Have we received updated Wiimote data since last report?
virtual int unregister_change_handler(void *userdata, vrpn_ANALOGCHANGEHANDLER handler)
static void VRPN_CALLBACK VRPN_CALLBACK handle_analog_update(void *userdata, const vrpn_ANALOGCB info)
Callback triggered when our data source issues an update.
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
virtual void mainloop()
VRPN mainloop function.
#define vrpn_gettimeofday
vrpn_int32 d_sender_id
Sender ID registered with the connection.
const double d_blobDistance
distance between LEDs on glasses, in meters
virtual void reset()
reset pose, gravity transform, and cached points and gravity
const double fovX
Field of view experimentally determined at Iowa State University March 2010.
bool d_gravDirty
Flag: Have we received updated gravity data since last gravity update?
q_xyz_quat_type d_gravityXform
Gravity correction transformation.
struct timeval d_prevtime
Time of last tracker report issued.
vrpn_Tracker_WiimoteHead(const char *name, vrpn_Connection *trackercon, const char *wiimote, float update_rate, float led_spacing=0.145)
constructor
virtual vrpn_int32 register_message_type(const char *name)
void _reset_pose()
reset current pose, last report time, and tracker pose
bool d_lock
Flag: Does the tracking algorithm report a lock?
FlipState d_flipState
Whether we need to flip the order of the tracked points before calculating a pose.