public void Receive(Pose data)
{
if (buffer.Capacity == 0)
{
OnSend(data);
}
bool bufferWasNotFull = false;
if (!buffer.IsFull)
{
bufferWasNotFull = true;
}
buffer.Add(data);
if (buffer.IsFull)
{
if (bufferWasNotFull)
{
for (int i = 1; i < buffer.Count; i += 2)
{
OnSend(getAverage(0, i));
}
}
OnSend(getAverage(0, buffer.Count));
}
}
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 Add(SampleType sample, float sampleTime)
{
if (!IsEmpty && sampleTime == GetLatestTime())
{
SetLatest(sample, sampleTime);
return;
}
_buffer.Add(new ValueTimePair {
value = sample, time = sampleTime
});
}
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));
}
}
private void Update()
{
// Calculate information about the current angle of the hinge element relative to
// the hinge resting direction.
_elementAngle = Vector3.SignedAngle(hingeRestAngleDir,
this.transform.rotation * _elementLocalExtensionAxis,
hingeAxis);
// Apply resting forces to the elementAngle.
float?restingAngleCorrection = null;
int restwardPolarity = -_elementAngle > 0 ? 1 : -1;
if (_elementAngle != 0)
{
// Rotate the element towards its resting angle.
var restForce = _elementAngle * -1f * restSpringForce;
_restCorrectionVelocity += restForce * Time.deltaTime;
var friction
= -_restCorrectionVelocity * restSpringFriction;
_restCorrectionVelocity += friction * Time.deltaTime;
var dragSign = _restCorrectionVelocity > 0 ? -1 : 1;
var drag = dragSign * _restCorrectionVelocity.Squared() * restSpringDrag;
_restCorrectionVelocity += drag * Time.deltaTime;
if (_restCorrectionVelocity < 0.2f)
{
restwardPolarity = 0;
}
restingAngleCorrection = _restCorrectionVelocity * Time.deltaTime;
_elementAngle += restingAngleCorrection.Value;
}
else
{
restwardPolarity = 0;
}
// Update interactors.
LocalSegment3?interactorMotion = null;
{
if (_interactorPos.HasValue && _lastInteractorPos.HasValue)
{
_lastInteractorPos = Vector3.Lerp(_lastInteractorPos.Value,
_interactorPos.Value,
20f * Time.deltaTime);
}
else
{
_lastInteractorPos = _interactorPos;
}
if (restingAngleCorrection.HasValue && _interactorPos.HasValue)
{
// Shift the last interactor position by the translation of it produced by
// the resting angle correct. This is akin to computing interactor sweeps in
// hinge-element-local space, where the correction of the hinge element to rest
// accounts for a sweep of the interactor position across its motion.
var rotatedPos = _lastInteractorPos.Value
.RotatedAround(hingeTransform.position,
((float)restingAngleCorrection.Value) * 0.05f,
hingeAxis);
var toRotated = rotatedPos - _lastInteractorPos.Value;
toRotated = toRotated.RotatedBy(Quaternion.AngleAxis(-30f, hingeAxis));
_lastInteractorPos = _lastInteractorPos + toRotated * 20f;
}
_interactorPos = null;
if (intObj.isPrimaryHovered)
{
_interactorPos = intObj.primaryHoveringControllerPoint;
}
if (_lastInteractorPos.HasValue && _interactorPos.HasValue)
{
var lastToCurrentDir
= (_interactorPos.Value - _lastInteractorPos.Value).normalized;
interactorMotion
= new LocalSegment3(
_lastInteractorPos.Value - lastToCurrentDir * _interactorRadius * 0.01f,
_interactorPos.Value);
}
// Update debug data.
_interactorMotionsBuffer.Add(interactorMotion);
}
// Update interactor collision.
var isSweptInteractorColliding = false;
var isCurrInteractorColliding = false;
var currInteractor = (Sphere?)null;
{
if (interactorMotion.HasValue)
{
var segment = interactorMotion.Value;
var motionSqrDist = segment.Intersect(buttonSurface);
if (motionSqrDist < _interactorRadius * _interactorRadius)
{
isSweptInteractorColliding = true;
}
}
if (_interactorPos.HasValue)
{
currInteractor = new Sphere(_interactorPos.Value, _interactorRadius);
isCurrInteractorColliding = currInteractor.Value.Overlaps(buttonSurface);
}
_wasMotionIntersectingBuffer.Add(isSweptInteractorColliding);
}
// Apply depenetration forces to the elementAngle.
if (currInteractor.HasValue)
{
var interactor = new Sphere(_interactorPos.Value, _interactorRadius);
// Determine whether or not we should attempt to hinge the button to depenetrate
// from the interacting sphere geometry.
bool shouldDepenetrate = false;
if (isCurrInteractorColliding || isSweptInteractorColliding)
{
shouldDepenetrate = true;
}
// Determine which way the element should depenetrate itself from the interacting
// sphere -- here, we simply depenetrate to the closest side.
var latestPos_button = this.transform.worldToLocalMatrix
.MultiplyPoint3x4(_interactorPos.Value);
var depenetrationPolarity
= Vector3.Dot(latestPos_button, _elementLocalActivationAxis) > 0 ? 1 : -1;
// However, if the button velocity is large enough, we actually should use the
// sign of the curl from its velocity vector about the hinge axis.
if (_lastInteractorPos.HasValue)
{
var angleToLastPos = Vector3.SignedAngle(
_lastInteractorPos.Value - _hingeTransform.position,
_hingeTransform.rotation
* Quaternion.AngleAxis((float)_elementAngle, _hingeLocalAxis)
* _elementLocalExtensionAxis,
hingeAxis);
depenetrationPolarity = angleToLastPos > 0 ? 1 : -1;
var velocity = (_lastInteractorPos.Value - _interactorPos.Value).magnitude
/ Time.deltaTime;
if (depenetrationPolarity == restwardPolarity && velocity < 1f)
{
shouldDepenetrate = false;
}
}
// TODO: Uncomment this and test with an applicable hinge element (with the
// element extending over the hinge, notice how there is another sign problem,
// where the facing of the element also flips).
//// If the hinging element surface extends onto the other side of the hinge,
//// the correct depenetration direction gets flipped when it is being pushed from
//// that side.
//if (Vector3.Dot(latestPos_button, _elementLocalExtensionAxis) < 0) {
// _depenetrationPolarity = -_depenetrationPolarity;
//}
// If we should depenetrate, we were "interacting" with the button. This is
// publicly-exposed, read-only state.
_isInteracting = shouldDepenetrate;
// Depenetrate the button by moving it on its hinge.
if (shouldDepenetrate)
{
var depenetratingAngle
= performTangencyCalculation(interactor, depenetrationPolarity);
_elementAngle += depenetratingAngle;
_restCorrectionVelocity = 0f;
}
}
else
{
_isInteracting = false;
}
// Update the transform to reflect the latest angle.
{
// Get the rotation about the hinge axis from the updated hinging element angle.
var elementRotation
= Quaternion.AngleAxis(_elementAngle, _hingeLocalAxis);
// Start with the current hinge pose, rotate it with the element rotation, then
// use the rotated hinge pose to recalculate where the hinging element should be
// relative to it, using the rigid delta-pose that is calculated on Start.
var hingeRotation = _hingeTransform.rotation * elementRotation;
var rotatedHingePose = _hingeTransform.ToPose().WithRotation(hingeRotation);
this.transform.SetPose(rotatedHingePose * _elementRestPose_hinge);
}
}
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);
}
}