コード例 #1
0
ファイル: vrpn_Tracker_Filter.C プロジェクト: ASPePeX/vrpn
void VRPN_CALLBACK vrpn_Tracker_FilterOneEuro::handle_tracker_update(void *userdata, const vrpn_TRACKERCB info)
{
  // Get pointer to the object we're dealing with.
  vrpn_Tracker_FilterOneEuro *me = static_cast<vrpn_Tracker_FilterOneEuro *>(userdata);

  // See if this sensor is within our range.  If not, we ignore it.
  if (info.sensor >= me->d_channels) {
    return;
  }

  // Filter the position and orientation and then report the filtered value
  // for this channel.  Keep track of the delta-time, and update our current
  // time so we get the right one next time.
  double dt = vrpn_TimevalDurationSeconds(info.msg_time, me->d_last_report_times[info.sensor]);
  if (dt <= 0) { dt = 1; }  // Avoid divide-by-zero in case of fluke.
  vrpn_float64 pos[3];
  vrpn_float64 quat[4];
  memcpy(pos, info.pos, sizeof(pos));
  memcpy(quat, info.quat, sizeof(quat));
  const vrpn_float64 *filtered = me->d_filters[info.sensor].filter(dt, pos);
  q_vec_copy(me->pos, filtered);
  const double *q_filtered = me->d_qfilters[info.sensor].filter(dt, quat);
  q_normalize(me->d_quat, q_filtered);
  me->timestamp = info.msg_time;
  me->d_sensor = info.sensor;
  me->d_last_report_times[info.sensor] = info.msg_time;

  // Send the filtered report.
  char msgbuf[512];
  int len = me->encode_to(msgbuf);
  if (me->d_connection->pack_message(len, me->timestamp, me->position_m_id,
          me->d_sender_id, msgbuf,vrpn_CONNECTION_LOW_LATENCY)) {
    fprintf(stderr, "vrpn_Tracker_FilterOneEuro: cannot write message: tossing\n");
  }
}
コード例 #2
0
ファイル: xyzquat.c プロジェクト: dclanmaster/A4Project
void
q_xyz_quat_compose(q_xyz_quat_type *C_from_A_ptr, 
                   q_xyz_quat_type *C_from_B_ptr, 
                   q_xyz_quat_type *B_from_A_ptr)
{
    q_vec_type rotated_BA_vec;

    /* rotate local xlate into global   */
    q_xform(rotated_BA_vec, C_from_B_ptr->quat, B_from_A_ptr->xyz);

    /* now add the xformed local vec to the unchanged global vec    */
    q_vec_add(C_from_A_ptr->xyz, C_from_B_ptr->xyz, rotated_BA_vec);

    /* compose the rotations    */
    /* CA_rotate = CB_rotate . BA_rotate */
    q_mult(C_from_A_ptr->quat, C_from_B_ptr->quat, B_from_A_ptr->quat);

    q_normalize(C_from_A_ptr->quat, C_from_A_ptr->quat);

} 	/* qp_xyz_quat_compose */
コード例 #3
0
ファイル: vrpn_Freespace.C プロジェクト: BlueBrain/vrpn
void vrpn_Freespace::handleUserFrame(const struct freespace_UserFrame& user) {
			vrpn_gettimeofday(&(vrpn_Tracker::timestamp), NULL);

    // Translate the Freespace linear position into VRPN linear position.
	vrpn_Tracker::pos[0] = user.linearPosY;
	vrpn_Tracker::pos[1] = user.linearPosZ;
	vrpn_Tracker::pos[2] = user.linearPosX;
		
    // Transfer the Freespace coordinate system angular position quanternion
    // into the VRPN coordinate system.  
	vrpn_Tracker::d_quat[Q_W] = user.angularPosA;
	vrpn_Tracker::d_quat[Q_X] = user.angularPosC;
	vrpn_Tracker::d_quat[Q_Y] = -user.angularPosD;
	vrpn_Tracker::d_quat[Q_Z] = user.angularPosB;
    q_normalize(vrpn_Tracker::d_quat, vrpn_Tracker::d_quat);

    vrpn_Tracker_Server::report_pose(0, vrpn_Tracker::timestamp, vrpn_Tracker::pos, vrpn_Tracker::d_quat);

	vrpn_Button::buttons[0] = user.button1;
	vrpn_Button::buttons[1] = user.button2;
	vrpn_Button::buttons[2] = user.button3;
	vrpn_Button::buttons[3] = user.button4;
	vrpn_Button::buttons[4] = user.button5;
	
	vrpn_Button::timestamp = vrpn_Tracker::timestamp;
	vrpn_Button::report_changes();

	// this is totally arbitrary
	// I'm not sure how exactly the 'deltaWheel' should be interpreted, but it seems
	// like it would be the number of 'clicks' while turning the scroll wheel...
	// vrpn was this as a 'fraction of a revolution' so I would assume that a return of 1 means
	// the dial was spun 360 degrees.  I'm going to pretend it takes 16 clicks to make a full
	// revolution....'
	// this values should probably set in a configuration file.
	vrpn_Dial::dials[0] = user.deltaWheel / (vrpn_float64) 16;
	vrpn_Dial::timestamp = vrpn_Tracker::timestamp;
	vrpn_Dial::report_changes();
}
コード例 #4
0
void vrpn_Tracker_OculusRift::_generate_report(int sensorNum)
{
    if (!d_connection)
    {
        return;
    }

    d_sensor = sensorNum;

    // Keep position set to 0, position may be added later by Oculus.
    pos[0] = 0;
    pos[1] = 0;
    pos[2] = 0;

    // Set d_quat
    //fprintf(stderr, "Got here\n");
    Quatf tmpOrientation = pRiftSFusion.GetOrientation();
    //fprintf(stderr, "Got here too\n");

    d_quat[Q_W] = tmpOrientation.w;
    d_quat[Q_X] = tmpOrientation.x;
    d_quat[Q_Y] = tmpOrientation.y;
    d_quat[Q_Z] = tmpOrientation.z;
    q_normalize(d_quat, d_quat);

    // Replace old positon with current position
    //q_vec_copy(_old_position[sensorNum], pos);

    char msgbuf[512];
    int len = vrpn_Tracker::encode_to(msgbuf);
    if (d_connection->pack_message(len, _timestamp, position_m_id, d_sender_id, msgbuf,
                                   vrpn_CONNECTION_LOW_LATENCY))
    {
        fprintf(stderr, "vrpn_Tracker_OculusRift: cannot write message: tossing\n");
    }
}
コード例 #5
0
void vrpn_Tracker_OSVRHackerDevKit::on_data_received(std::size_t bytes,
                                                     vrpn_uint8 *buffer)
{
    if (bytes != 32 && bytes != 16) {
        send_text_message(vrpn_TEXT_WARNING)
            << "Received a report " << bytes
            << " in length, but expected it to be 32 or 16 bytes. Discarding. "
               "(May indicate issues with HID!)";
        return;
    }

    vrpn_uint8 firstByte = vrpn_unbuffer_from_little_endian<vrpn_uint8>(buffer);

    vrpn_uint8 version = vrpn_uint8(0x0f) & firstByte;
    _reportVersion = version;

    switch (version) {
    case 1:
        if (bytes != 32 && bytes != 16) {
            send_text_message(vrpn_TEXT_WARNING)
                << "Received a v1 report " << bytes
                << " in length, but expected it to be 32 or 16 bytes. "
                   "Discarding. "
                   "(May indicate issues with HID!)";
            return;
        }
        break;
    case 2:
        if (bytes != 16) {
            send_text_message(vrpn_TEXT_WARNING)
                << "Received a v2 report " << bytes
                << " in length, but expected it to be 16 bytes. Discarding. "
                   "(May indicate issues with HID!)";
            return;
        }
        break;

    case 3:
        /// @todo once this report format is finalized, tighten up the
        /// requirements.
        if (bytes < 16) {
            send_text_message(vrpn_TEXT_WARNING)
                << "Received a v3 report " << bytes
                << " in length, but expected it to be at least 16 bytes. "
                   "Discarding. "
                   "(May indicate issues with HID!)";
            return;
        }
        break;
    default:
        /// Highlight that we don't know this report version well...
        _knownVersion = false;
        /// Do a minimal check of it.
        if (bytes < 16) {
            send_text_message(vrpn_TEXT_WARNING)
                << "Received a report claiming to be version " << int(version)
                << " that was " << bytes << " in length, but expected it to be "
                                            "at least 16 bytes. Discarding. "
                                            "(May indicate issues with HID!)";
            return;
        }
        break;
    }

    // Report version as analog channel 0.
    channel[0] = version;

    vrpn_uint8 msg_seq = vrpn_unbuffer_from_little_endian<vrpn_uint8>(buffer);

    // Signed, 16-bit, fixed-point numbers in Q1.14 format.
    typedef vrpn::FixedPoint<1, 14> FixedPointValue;
    d_quat[Q_X] =
        FixedPointValue(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
            .get<vrpn_float64>();
    d_quat[Q_Y] =
        FixedPointValue(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
            .get<vrpn_float64>();
    d_quat[Q_Z] =
        FixedPointValue(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
            .get<vrpn_float64>();
    d_quat[Q_W] =
        FixedPointValue(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
            .get<vrpn_float64>();

    vrpn_Tracker::timestamp = _timestamp;
    {
        char msgbuf[512];
        int len = vrpn_Tracker::encode_to(msgbuf);
        if (d_connection->pack_message(len, _timestamp, position_m_id,
                                       d_sender_id, msgbuf,
                                       vrpn_CONNECTION_LOW_LATENCY)) {
            fprintf(stderr, "vrpn_Tracker_OSVRHackerDevKit: cannot write "
                            "message: tossing\n");
        }
    }
    if (version >= 2) {
        // We've got angular velocity in this message too
        // Given XYZ radians per second velocity.
        // Signed Q6.9
        typedef vrpn::FixedPoint<6, 9> VelFixedPoint;
        q_vec_type angVel;
        angVel[0] =
            VelFixedPoint(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
                .get<vrpn_float64>();
        angVel[1] =
            VelFixedPoint(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
                .get<vrpn_float64>();
        angVel[2] =
            VelFixedPoint(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
                .get<vrpn_float64>();

        //==================================================================
        // Determine the rotational velocity, which is
        // measured in the rotated coordinate system.  We need to rotate the
        // difference Euler angles back to the canonical orientation, apply
        // the change, and then rotate back to change our coordinates.
        // Be sure to scale by the time value vrpn_HDK_DT.
        q_type forward, inverse;
        q_copy(forward, d_quat);
        q_invert(inverse, forward);
        // Remember that Euler angles in Quatlib have rotation around Z in
        // the first term.  Compute the time-scaled delta transform in
        // canonical space.
        q_type delta;
        {
            delta[Q_W] = 0;
            delta[Q_X] = angVel[Q_X] * vrpn_HDK_DT * 0.5;
            delta[Q_Y] = angVel[Q_Y] * vrpn_HDK_DT * 0.5;
            delta[Q_Z] = angVel[Q_Z] * vrpn_HDK_DT * 0.5;
            q_exp(delta, delta);
            q_normalize(delta, delta);
        }
        // Bring the delta back into canonical space
        q_type canonical;
        q_mult(canonical, delta, inverse);
        q_mult(vel_quat, forward, canonical);

        // Send the rotational velocity information.
        // The dt value was set once, in the constructor.
        char msgbuf[512];
        int len = vrpn_Tracker::encode_vel_to(msgbuf);
        if (d_connection->pack_message(len, _timestamp, velocity_m_id,
                                       d_sender_id, msgbuf,
                                       vrpn_CONNECTION_LOW_LATENCY)) {
            fprintf(stderr, "vrpn_Tracker_OSVRHackerDevKit: cannot write "
                            "message: tossing\n");
        }
    }
    if (version < 3) {
        // No status info hidden in the first byte.
        channel[1] = STATUS_UNKNOWN;
    }
    else {
        // v3+: We've got status info in the upper nibble of the first byte.
        bool gotVideo = (firstByte & (0x01 << 4)) != 0;    // got video?
        bool gotPortrait = (firstByte & (0x01 << 5)) != 0; // portrait mode?
        if (!gotVideo) {
            channel[1] = STATUS_NO_VIDEO_INPUT;
        }
        else {
            if (gotPortrait) {
                channel[1] = STATUS_PORTRAIT_VIDEO_INPUT;
            }
            else {
                channel[1] = STATUS_LANDSCAPE_VIDEO_INPUT;
            }
        }
    }

    if (_messageCount == 0) {
        // When _messageCount overflows, send a report whether or not there was
        // a change.
        vrpn_Analog::report();
    }
    else {
        // otherwise just report if we have a change.
        vrpn_Analog::report_changes();
    };
    _messageCount = (_messageCount + 1) % vrpn_HDK_STATUS_STRIDE;
}