Exemple #1
0
 public void Set(int idx, SampleType sample, float sampleTime)
 {
     _buffer.Set(idx, new ValueTimePair {
         value = sample, time = sampleTime
     });
 }
Exemple #2
0
        public void Receive(Pose data)
        {
            _buffer.Add(data);

            if (_buffer.Count == 2)
            {
                Pose a = _buffer.Get(0), b = _buffer.Get(1);
                var  ab = b.position - a.position;

                var handDorsal = a.rotation * Vector3.up;

                Quaternion initRot;

                var initAngle = Vector3.Angle(handDorsal, ab);
                if (initAngle < 10f)
                {
                    var handDistal  = a.rotation * Vector3.forward;
                    var ribbonRight = Vector3.Cross(ab, handDistal).normalized;
                    var ribbonUp    = Vector3.Cross(ribbonRight, ab).normalized;

                    initRot = Quaternion.LookRotation(ab, ribbonUp);
                }
                else
                {
                    var ribbonRight = Vector3.Cross(ab, handDorsal).normalized;
                    var ribbonUp    = Vector3.Cross(ribbonRight, ab).normalized;

                    initRot = Quaternion.LookRotation(ab, ribbonUp);
                }

                var initPose = new Pose(a.position, initRot);
                _buffer.Set(0, initPose);
                OnSend(initPose);
            }

            if (_buffer.IsFull)
            {
                Pose a = _buffer.Get(0), b = _buffer.Get(1), c = _buffer.Get(2);

                var rolllessRot = getRolllessRotation(a, b, c);

                var midPoseRot = Quaternion.Slerp(a.rotation, rolllessRot, 0.5f);
                var midPose    = new Pose(b.position, midPoseRot);

                // Canvas alignment.
                if (doCanvasAlignment)
                {
                    var ribbonNormal  = midPoseRot * Vector3.up;
                    var canvasNormal  = b.rotation * Vector3.up;
                    var ribbonForward = midPoseRot * Vector3.forward;

                    if (Vector3.Dot(ribbonNormal, canvasNormal) < 0f)
                    {
                        canvasNormal *= -1f;
                    }

                    var ribbonCanvasAngle = Vector3.SignedAngle(ribbonNormal, canvasNormal,
                                                                ribbonForward);

                    var alignmentStrengthParam = Mathf.Abs(ribbonCanvasAngle)
                                                 .Map(maximumAlignmentAngleFromY,
                                                      minimumAlignmentAngleFromY,
                                                      0f, 1f);
                    var alignmentStrength = alignmentStrengthCurve.Evaluate(alignmentStrengthParam);
                    var aligningAngle     = alignmentStrength * ribbonCanvasAngle;

                    // Limit canvas alignment based on how much the pitch is currently changing.
                    var right              = a.rotation * Vector3.right;
                    var forward            = a.rotation * Vector3.forward;
                    var bc                 = (c.position - b.position);
                    var bc_pitchProjection = Vector3.ProjectOnPlane(bc, right);
                    var pitchAngle         = Vector3.Angle(forward, bc_pitchProjection);
                    alignmentStrength *= pitchAngle.Map(0f, maxSegmentAngleToAlign, 1f, 0f);

                    var abDistInCM         = (b.position - a.position).magnitude * 100f;
                    var maxAngleCorrection = abDistInCM * maxAlignmentAnglePerCentimeter
                                             * alignmentStrength;
                    aligningAngle = Mathf.Clamp(aligningAngle,
                                                -1f * maxAngleCorrection, maxAngleCorrection);

                    var aligningRot = Quaternion.AngleAxis(aligningAngle, ribbonForward);
                    var alignedRot  = aligningRot * midPoseRot;

                    midPose = new Pose(midPose.position, alignedRot);
                }

                _buffer.Set(1, midPose);
                OnSend(midPose);
            }
        }