public static TransformData GetTransformAtTime(RingBuffer <TransformData> history, long desiredTime)
{
for (int i = history.Count - 1; i > 0; i--)
{
if (history.Get(i).time >= desiredTime && history.Get(i - 1).time < desiredTime)
{
return(Lerp(history.Get(i - 1), history.Get(i), desiredTime));
}
}
if (history.Count > 0)
{
return(history.GetLatest());
}
else
{
// No history data available.
return(new TransformData()
{
time = desiredTime,
position = Vector3.zero,
rotation = Quaternion.identity
});
}
}
private Pose getAverage(int start, int end)
{
if (start == end)
{
return(buffer.Get(start));
}
var sum = Vector3.zero;
for (int i = start; i < end; i++)
{
sum += buffer.Get(i).position;
}
var avgPos = sum / (end - start);
// Try a fractionally-slerped accumulation for "averaging" the rotations.
var rot = buffer.Get(start).rotation;
var div = 1 / (end - start);
for (int i = start + 1; i < end; i++)
{
rot = Quaternion.Slerp(rot, buffer.Get(i).rotation, div);
}
return(new Pose(avgPos, rot));
}
public void Receive(Pose data)
{
bool wasNotFull = false;
if (!_poseBuffer.IsFull)
{
wasNotFull = true;
}
_poseBuffer.Add(data);
if (_poseBuffer.IsFull)
{
if (wasNotFull)
{
send(_poseBuffer.Get(0), _poseBuffer.Get(0),
_poseBuffer.Get(1), _poseBuffer.Get(2));
}
send(_poseBuffer.Get(0), _poseBuffer.Get(1),
_poseBuffer.Get(2), _poseBuffer.Get(3));
}
}
public SampleType Get(int idx)
{
return(_buffer.Get(idx).value);
}
public void Receive(Pose data)
{
bool wasNotFull = false;
if (!_poseBuffer.IsFull)
{
wasNotFull = true;
}
if (_poseBuffer.Count == 0)
{
_firstPose = data;
}
if (_poseBuffer.Count == 1)
{
_secondPose = data;
}
if (_poseBuffer.Count == 2)
{
_thirdPose = data;
}
_poseBuffer.Add(data);
if (_poseBuffer.IsFull)
{
if (wasNotFull && !loop)
{
send(_poseBuffer.Get(0), _poseBuffer.Get(0),
_poseBuffer.Get(1), _poseBuffer.Get(2));
}
send(_poseBuffer.Get(0), _poseBuffer.Get(1),
_poseBuffer.Get(2), _poseBuffer.Get(3));
}
}
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);
}
}