Exemple #1
0
 public bool InitializeRigidBody()
 {
     _nRigidBodies = 0;
     for (int i = 0; i < 200; i++)
     {
         _rigidBodies[i] = new OptiTrackRigidBody();
     }
     return(true);
 }
        // Unpack RigidBody data
        private void ReadRigidBody(Byte[] b, ref int offset, OptiTrackRigidBody rb)
        {
            try
            {
                rb.used = true;

                int[]   iData = new int[100];
                float[] fData = new float[100];

                // RB ID
                Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
                //int iSkelID = iData[0] >> 16;           // hi 16 bits = ID of bone's parent skeleton
                //int iBoneID = iData[0] & 0xffff;       // lo 16 bits = ID of bone
                rb.ID = iData[0] - 1;               // already have it from data descriptions

                // RB pos
                float[] pos = new float[3];
                Buffer.BlockCopy(b, offset, pos, 0, 4 * 3); offset += 4 * 3;
                rb.position.x = pos[0]; rb.position.y = pos[1]; rb.position.z = pos[2];

                // RB ori
                float[] ori = new float[4];
                Buffer.BlockCopy(b, offset, ori, 0, 4 * 4); offset += 4 * 4;
                rb.orientation.x = ori[0]; rb.orientation.y = ori[1]; rb.orientation.z = ori[2]; rb.orientation.w = ori[3];

                Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
                int nMarkers = iData[0];

                Buffer.BlockCopy(b, offset, fData, 0, 4 * 3 * nMarkers); offset += 4 * 3 * nMarkers;

                /*
                 * for (int i = 0; i < nMarkers;i++)
                 * {
                 *  Buffer.BlockCopy(b, offset, fData, 0, 4 * 3); offset += 4 * 3;
                 *  _dataStream.marker[_dataStream.markerIndex].pos = new Vector3(fData[0], fData[1], fData[2]);
                 *  _dataStream.markerIndex++;
                 * }
                 */

                Buffer.BlockCopy(b, offset, iData, 0, 4 * nMarkers); offset += 4 * nMarkers;

                Buffer.BlockCopy(b, offset, fData, 0, 4 * nMarkers); offset += 4 * nMarkers;

                Buffer.BlockCopy(b, offset, fData, 0, 4); offset += 6;
            } catch (Exception e)
            {
                Debug.LogError(e.ToString());
            }
        }