Пример #1
0
        /// <summary>
        /// Inserts in the circulare buffer to keep it ordered
        /// by timestamp.
        /// </summary>
        /// <returns>the index where the data was inserted timestamp.</returns>
        private int InsertByTimestamp(TrackingData data)
        {
            System.Type entryType = data.GetType();
            System.Type exitType;
            trackingDataBuffer.PushFront(data);
            int index = 0;

            for (int i = 1; i < trackingDataBuffer.Size; i++)
            {
                index = i - 1;
                if (trackingDataBuffer[i - 1].timestamp < trackingDataBuffer[i].timestamp)
                {
                    // Invert
                    TrackingData tempData = trackingDataBuffer[i];
                    trackingDataBuffer[i]     = trackingDataBuffer[i - 1];
                    trackingDataBuffer[i - 1] = tempData;
                }
                else
                {
                    exitType = trackingDataBuffer[i - 1].GetType();
                    if (entryType != exitType)
                    {
                        Debug.LogWarning("A. Entrype: " + entryType.ToString() + "  ExitType: " + exitType.ToString());
                    }
                    return(index);
                }
            }
            exitType = trackingDataBuffer[index].GetType();
            if (entryType != exitType)
            {
                Debug.LogWarning("B. Entrype: " + entryType.ToString() + "  ExitType: " + exitType.ToString() + "  Index: " + index.ToString());
            }
            return(index);
        }
Пример #2
0
        public virtual Vector3 GetPosition(double timestamp)
        {
            if (dataInQueue)
            {
                lock (dataQueue){
                    for (int i = 0; i < dataQueue.Size; i++)
                    {
                        TrackingData data = dataQueue.Back();
                        if (data.GetType() == typeof(TrackingDataIMU))
                        {
                            AddAccelerationMeasurement((TrackingDataIMU)data);
                        }
                        else if (data.GetType() == typeof(TrackingDataPosition))
                        {
                            AddPositionMeasurement((TrackingDataPosition)data);
                        }

                        dataQueue.PopBack();
                    }
                    dataInQueue = false;
                }
            }



            int lastPositionIndex = GetLastPositionIndex();

            if (lastPositionIndex == -1)
            {
                //  Debug.LogWarning("Couldn't find any last position");

                return(lastCalculatedPosition);
            }



            float delaySinceLastGetPosition         = (float)(timestamp - lastCalculatedPositionTimestamp);
            float delaySinceLastPositionMeasurement = lastPositionIndex == -1 ? maxSpeedViabilityDelay : (float)(timestamp - trackingDataBuffer[lastPositionIndex].timestamp);
            float delaySinceLastMeasurement         = (float)(timestamp - (trackingDataBuffer.Size > 0 ? trackingDataBuffer[0].timestamp : 0.0d));

            if (delaySinceLastMeasurement > maxPredictionDelaySinceLastMeasurement || trackingDataBuffer.Size < 1)
            {
                return(lastCalculatedPosition);
            }


            int     lastAccelerationIndex = GetLastAccelerationIndex();
            Vector3 lastAcceleration      = lastAccelerationIndex == -1 ? Vector3.zero : ((TrackingDataIMU)trackingDataBuffer[lastAccelerationIndex]).acceleration;
            float   delaySinceLastAccelerationMeasurement = lastAccelerationIndex == -1 ? maxAccelerationViabilityDelay : (float)(timestamp - trackingDataBuffer[lastAccelerationIndex].timestamp);

            List <PositionOffset> deleteList = new List <PositionOffset>();
            Vector3 currentOffset            = Vector3.zero;

            foreach (PositionOffset off in offsets)
            {
                currentOffset += off.GetOffset(timestamp);
                if (off.CorrectionOver(timestamp))
                {
                    deleteList.Add(off);
                }
            }

            foreach (PositionOffset off in deleteList)
            {
                offsets.Remove(off);
            }

            if (currentOffset.magnitude > lerpDiscardDistance || (trackingDataBuffer[lastPositionIndex].position - lastCalculatedPosition).magnitude > lerpDiscardDistance)
            {
                //  if (blink)
                //  {
                offsets.Clear();
                if (Blink != null)
                {
                    Blink();
                }
                //  }
            }

            // ACC
            if (trackingDataBuffer[0].GetType() == typeof(TrackingDataIMU))
            {
                // Position calculation with Slerp in case we have been predicting with ACC for too long
                Vector3 newOffset = Vector3.Slerp(trackingDataBuffer[0].speed, Vector3.zero, delaySinceLastPositionMeasurement / maxSpeedViabilityDelay) * delaySinceLastGetPosition + 0.5f * Vector3.Slerp(lastAcceleration, Vector3.zero, delaySinceLastAccelerationMeasurement / maxAccelerationViabilityDelay) * delaySinceLastGetPosition * delaySinceLastGetPosition;
                lastCalculatedPosition          = Vector3.Slerp(lastCalculatedPosition + newOffset, lastCalculatedPosition, delaySinceLastPositionMeasurement / accelerationOnlyTrackingDelay);
                lastCalculatedPositionTimestamp = timestamp;
            }

            // POS
            else
            {
                // Position calculation with Slerp in case we have been predicting with ACC for too long
                Vector3 newOffset = Vector3.Slerp(trackingDataBuffer[0].speed, Vector3.zero, delaySinceLastPositionMeasurement / maxSpeedViabilityDelay) * delaySinceLastGetPosition + 0.5f * Vector3.Slerp(lastAcceleration, Vector3.zero, delaySinceLastAccelerationMeasurement / maxAccelerationViabilityDelay) * delaySinceLastGetPosition * delaySinceLastGetPosition;
                lastCalculatedPosition          = Vector3.Slerp(lastCalculatedPosition + newOffset, lastCalculatedPosition, delaySinceLastPositionMeasurement / accelerationOnlyTrackingDelay);
                lastCalculatedPositionTimestamp = timestamp;
            }

            /* TRY TO FIX LONG OFFSET JUMPS, if line 110 isn't enought
             * if ((trackingDataBuffer[lastPositionIndex].position - lastCalculatedPosition).magnitude > discardDistance)
             * {
             *  ResetFilter();
             *  if (blink)
             *  {
             *      if (Blink != null)
             *          Blink();
             *  }
             * }*/

            // return lastCalculatedPosition + currentOffset;
            return(oneEuro.Filter(lastCalculatedPosition + currentOffset, (float)timestamp));
        }