private void UpdateMovementVectors(Dictionary <JointType, bool> jointDirChange, Dictionary <JointType, float> distanceScale)
        {
            foreach (var change in jointDirChange)
            {
                if (_workingMode == 1)
                {
                    if (change.Value == true) // if a dir is changed
                    {
                        var joint = change.Key;
                        //int currentNumberOfVectors = jointMovementVectors[joint].Count();
                        var lastCoordinates = jointMovementVectors[joint]._endPoint;

                        lastCoordinates = SmoothingLine(smoothingBuffer, lastCoordinates, joint);

                        var newVector = new MovementVector(new Point3D(lastCoordinates.X, lastCoordinates.Y, lastCoordinates.Z), lastPositions[0][joint]);

                        bool CheckIfNewVectorIsNeeded = checkNewVector(newVector, joint);
                        if (CheckIfNewVectorIsNeeded == true)
                        {
                            newVector._distance         = newVector._distance * distanceScale[joint];
                            jointMovementVectors[joint] = newVector;
                        }
                        else
                        {
                            var lastPoint = jointMovementVectors[joint];
                            //Batev: var lastPoint = new Point3D();

                            jointMovementVectors[joint]._endPoint = lastPositions[0][joint];
                            var angle = Math3DHelper.AngleBetweenTwoPointsInDegrees(lastPositions[0][joint], lastPoint._startPoint);
                            //Batev: var angle = Math3DHelper.AngleBetweenTwoPointsInDegrees(lastPoint, lastPositions[0][joint]);

                            if (angle < 0)
                            {
                                angle = 360 + angle;
                            }
                            jointMovementVectors[joint]._angle    = angle;
                            jointMovementVectors[joint]._distance = Math3DHelper.DistanceBetwenTwoPoints(lastPoint._startPoint, lastPositions[0][joint]);
                            //Batev: jointMovementVectors[joint]._distance = Math3DHelper.DistanceBetwenTwoPoints(lastPoint, lastPositions[0][joint]);
                        }
                    }
                }
                // for working mode 2 - we replace the vector every time. We have to skip several frames in order to have better angle calculation
                else if (_workingMode == 2)
                {
                    if (_calculatedFramesSinceStart % _skippedFramesForMode2 == 0)
                    {
                        var joint           = change.Key;
                        var lastCoordinates = jointMovementVectors[joint]._endPoint;
                        var newVector       = new MovementVector(new Point3D(lastCoordinates.X, lastCoordinates.Y, lastCoordinates.Z), lastPositions[0][joint]);
                        jointMovementVectors[joint] = newVector;
                    }
                }
            }
        }
        private void UpdateMovementVectors(Dictionary <JointType, bool> jointDirChange)
        {
            foreach (var change in jointDirChange)
            {
                if (change.Value == true) // if a dir is changed
                {
                    var joint = change.Key;
                    int currentNumberOfVectors = jointMovementVectors[joint].Count;
                    var lastCoordinates        = jointMovementVectors[joint][currentNumberOfVectors - 1]._endPoint;


                    lastCoordinates = SmoothingLine(smoothingBuffer, lastCoordinates, joint);

                    var newVector = new MovementVector(new Point3D(lastCoordinates.X, lastCoordinates.Y, lastCoordinates.Z), lastPositions[0][joint]);


                    bool CheckIfNewVectorIsNeeded = checkNewVector(newVector, joint);
                    if (CheckIfNewVectorIsNeeded == true)
                    {
                        jointMovementVectors[joint].Add(newVector);
                    }
                    else
                    {
                        var lastPoint = jointMovementVectors[joint][currentNumberOfVectors - 1];
                        //Batev: var lastPoint = new Point3D();
                        jointMovementVectors[joint][currentNumberOfVectors - 1]._endPoint = lastPositions[0][joint];
                        var angle = Math3DHelper.AngleBetweenTwoPointsInDegrees(lastPoint._startPoint, lastPositions[0][joint]);
                        //Batev: var angle = Math3DHelper.AngleBetweenTwoPointsInDegrees(lastPoint, lastPositions[0][joint]);
                        jointMovementVectors[joint][currentNumberOfVectors - 1]._angle    = angle;
                        jointMovementVectors[joint][currentNumberOfVectors - 1]._distance = Math3DHelper.DistanceBetwenTwoPoints(lastPoint._startPoint, lastPositions[0][joint]);
                        //Batev:  jointMovementVectors[joint][currentNumberOfVectors - 1]._distance = Math3DHelper.DistanceBetwenTwoPoints(lastPoint, lastPositions[0][joint]);
                    }
                }
            }
        }