コード例 #1
0
    /// <summary>
    /// Obtain smoothed position (x, y, z) and orientation (p, r, y) data
    /// </summary>
    private void ExtractData()
    {
        const char REMOVE_F = 'f';

        // Get next OSC Packet for this GameObject (Pozyx Tag)
        _PozyxData = OSC.GetOSCPacket(TagID);

        // Position Data
        _XCoord = _AVG_X.SmoothPositionData(Convert.ToInt32(_PozyxData[2]));
        // z and y axis reversed for unity reference frame
        _YCoord = _AVG_Y.SmoothPositionData(Convert.ToInt32(_PozyxData[4]));
        _ZCoord = _AVG_Z.SmoothPositionData(Convert.ToInt32(_PozyxData[3]));

        // Orientation Data
        _Pitch = _AVG_Pitch.SmoothOrientationData(float.Parse(_PozyxData[5].TrimEnd(REMOVE_F)));
        _Roll  = _AVG_Roll.SmoothOrientationData(float.Parse(_PozyxData[6].TrimEnd(REMOVE_F)));
        _Yaw   = _AVG_Yaw.SmoothOrientationData(float.Parse(_PozyxData[7].TrimEnd(REMOVE_F))) - 90;  // Rotate 90 deg to compensate for Unity Reference Frame / Pozyx difference

        Debug.Log(string.Format("TagID: [{6}] Position: [x:{0} y:{1} z:{2}], Rotation: [p:{3:f2} r:{4:f2} y:{5:f2}]", _XCoord, _YCoord, _ZCoord, _Pitch, _Roll, _Yaw, TagID));
        //Debug.Log(string.Format("Position: [x:{0} y:{1} z:{2}]", _XCoord, _YCoord, _ZCoord));

        // Convert Euler (Rotation) Angles to Quaternions
        _QT = Quaternion.Euler(_Roll, _Yaw, -_Pitch);
    }