public void Set(int idx, SampleType sample, float sampleTime)
{
_buffer.Set(idx, new ValueTimePair {
value = sample, time = sampleTime
});
}
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);
}
}
