public static Vector3 QuatToAngularVeloc(Quaternion inputQuat)
{
float angle;
Vector3 axis = Vector3.zero;
(angle, axis) =
VelocityCalculatorUtilMethods.QuatToAngleAxis(inputQuat);
return(axis * angle);
}
private (Vector3, Vector3) GetLatestLinearAndAngularVelocities(Pose referencePose,
float delta)
{
// Don't compute any values if they would result in NaN.
if (!_previousReferencePosition.HasValue || delta < Mathf.Epsilon)
{
return(Vector3.zero, Vector3.zero);
}
Vector3 newLinearVelocity = (referencePose.position -
_previousReferencePosition.Value) / delta;
var newAngularVelocity = VelocityCalculatorUtilMethods.ToAngularVelocity(
_previousReferenceRotation.Value,
referencePose.rotation, delta);
return(newLinearVelocity, newAngularVelocity);
}
private void IncludeEstimatedReleaseVelocities(ref Vector3 linearVelocity,
ref Vector3 angularVelocity)
{
linearVelocity = _linearVelocity;
angularVelocity = _angularVelocity;
if (_stepBackTime < Mathf.Epsilon)
{
return;
}
int beforeIndex, afterIndex;
float lookupTime = Time.time - _stepBackTime;
(beforeIndex, afterIndex) = FindPoseIndicesAdjacentToTime(lookupTime);
if (beforeIndex < 0 || afterIndex < 0)
{
return;
}
var previousPoseData = _bufferedPoses[beforeIndex];
var nextPoseData = _bufferedPoses[afterIndex];
float previousTime = previousPoseData.Time;
float nextTime = nextPoseData.Time;
float t = (lookupTime - previousTime) / (nextTime - previousTime);
Vector3 lerpedVelocity = Vector3.Lerp(previousPoseData.LinearVelocity,
nextPoseData.LinearVelocity, t);
Quaternion previousAngularVelocityQuat =
VelocityCalculatorUtilMethods.AngularVelocityToQuat(previousPoseData.AngularVelocity);
Quaternion nextAngularVelocityQuat =
VelocityCalculatorUtilMethods.AngularVelocityToQuat(nextPoseData.AngularVelocity);
Quaternion lerpedAngularVelocQuat = Quaternion.Slerp(previousAngularVelocityQuat,
nextAngularVelocityQuat, t);
Vector3 lerpedAngularVelocity = VelocityCalculatorUtilMethods.QuatToAngularVeloc(
lerpedAngularVelocQuat);
linearVelocity = lerpedVelocity;
angularVelocity = lerpedAngularVelocity;
}
