/// <summary>
/// Samples the current velocity and adds it to a rolling average.
/// </summary>
public void OnHold(InteractionBehaviour intObj,
ReadonlyList <InteractionController> controllers)
{
_velocityQueue.Enqueue(new VelocitySample(intObj.rigidbody.position,
intObj.rigidbody.rotation,
Time.fixedTime));
while (true)
{
VelocitySample oldestVelocity = _velocityQueue.Peek();
// Dequeue conservatively if the oldest velocity is more than 4 frames later
// than the start of the window.
if (oldestVelocity.time + (Time.fixedDeltaTime * 4) < Time.fixedTime
- _windowLength
- _windowDelay)
{
_velocityQueue.Dequeue();
}
else
{
break;
}
}
}
/** Moves the object by applying forces and torque. */
public override void MoveTo(ReadonlyList <Hand> hands, PhysicsMoveInfo info, Vector3 solvedPosition, Quaternion solvedRotation)
{
if (info.shouldTeleport)
{
_obj.warper.Teleport(solvedPosition, solvedRotation);
}
else
{
Vector3 targetVelocity = PhysicsUtility.ToLinearVelocity(_obj.warper.RigidbodyPosition, solvedPosition, Time.fixedDeltaTime);
Vector3 targetAngularVelocity = PhysicsUtility.ToAngularVelocity(_obj.warper.RigidbodyRotation, solvedRotation, Time.fixedDeltaTime);
float targetSpeedSqrd = targetVelocity.sqrMagnitude;
if (targetSpeedSqrd > _maxVelocitySqrd)
{
float targetPercent = (_maxVelocity * _obj.Manager.SimulationScale) / Mathf.Sqrt(targetSpeedSqrd);
targetVelocity *= targetPercent;
targetAngularVelocity *= targetPercent;
}
float followStrength = _strengthByDistance.Evaluate(info.remainingDistanceLastFrame / _obj.Manager.SimulationScale);
Vector3 lerpedVelocity = Vector3.Lerp(_obj.rigidbody.velocity, targetVelocity, followStrength);
Vector3 lerpedAngularVelocity = Vector3.Lerp(_obj.rigidbody.angularVelocity, targetAngularVelocity, followStrength);
Vector3 centerOfMassOffset = _obj.warper.RigidbodyRotation * _obj.rigidbody.centerOfMass;
_obj.rigidbody.velocity = lerpedVelocity + Vector3.Cross(lerpedAngularVelocity, centerOfMassOffset);
_obj.rigidbody.angularVelocity = lerpedAngularVelocity;
}
}
public override void GetHoldingPose(ReadonlyList <Hand> hands, out Vector3 newPosition, out Quaternion newRotation)
{
points.Clear(); refPoints.Clear();
Vector3 bodyPosition = _obj.warper.RigidbodyPosition;
Quaternion bodyRotation = _obj.warper.RigidbodyRotation;
Matrix4x4 it = Matrix4x4.TRS(bodyPosition, bodyRotation, Vector3.one);
handCentroid = Vector3.zero; objectCentroid = Vector3.zero; boneCount = 0f;
for (int h = 0; h < hands.Count; h++)
{
Hand hand = hands[h];
var collection = _handIdToPoints[hand.Id];
for (int f = 0; f < NUM_FINGERS; f++)
{
Finger finger = hand.Fingers[f];
Finger.FingerType fingerType = finger.Type;
for (int j = 0; j < NUM_BONES; j++)
{
Bone.BoneType boneType = (Bone.BoneType)j;
Bone bone = finger.Bone(boneType);
Vector3 localPos = collection.GetLocalPosition(fingerType, boneType);
Vector3 bonePos = bone.NextJoint.ToVector3();
//Do the solve such that the objects positions are matched to the new bone positions
Vector3 point1 = (it.MultiplyPoint3x4(localPos) - bodyPosition);
Vector3 point2 = (bonePos - bodyPosition);
switch (_solveMethod)
{
case SolveMethod.SixDegreeSolve:
//Set the relevant points in each array
points.Add(point1); refPoints.Add(point2);
break;
case SolveMethod.PivotAroundOrigin:
//Calculate the Centroids of the object and hand(s) points
objectCentroid += point1;
handCentroid += point2;
boneCount += 1f;
break;
}
}
}
}
Matrix4x4 KabschTransform = performSolve(bodyPosition);
newPosition = bodyPosition + KabschTransform.GetVector3();
newRotation = KabschTransform.GetQuaternion() * bodyRotation;
}
/** Moves the object kinematically. */
public override void MoveTo(ReadonlyList <Hand> hands, PhysicsMoveInfo info, Vector3 solvedPosition, Quaternion solvedRotation)
{
if (info.shouldTeleport)
{
_obj.warper.Teleport(solvedPosition, solvedRotation);
}
else
{
_obj.rigidbody.MovePosition(solvedPosition);
_obj.rigidbody.MoveRotation(solvedRotation);
}
}
public override void GetHoldingPose(ReadonlyList <Hand> hands, out Vector3 newPosition, out Quaternion newRotation)
{
KabschC.Reset(ref _kabsch);
Vector3 bodyPosition = _obj.warper.RigidbodyPosition;
Quaternion bodyRotation = _obj.warper.RigidbodyRotation;
Matrix4x4 it = Matrix4x4.TRS(bodyPosition, bodyRotation, Vector3.one);
for (int h = 0; h < hands.Count; h++)
{
Hand hand = hands[h];
var collection = _handIdToPoints[hand.Id];
for (int f = 0; f < NUM_FINGERS; f++)
{
Finger finger = hand.Fingers[f];
Finger.FingerType fingerType = finger.Type;
for (int j = 0; j < NUM_BONES; j++)
{
Bone.BoneType boneType = (Bone.BoneType)j;
Bone bone = finger.Bone(boneType);
Vector3 localPos = collection.GetLocalPosition(fingerType, boneType);
Vector3 bonePos = bone.NextJoint.ToVector3();
//Do the solve such that the objects positions are matched to the new bone positions
LEAP_VECTOR point1 = (it.MultiplyPoint3x4(localPos) - bodyPosition).ToCVector();
LEAP_VECTOR point2 = (bonePos - bodyPosition).ToCVector();
KabschC.AddPoint(ref _kabsch, ref point1, ref point2, 1.0f);
}
}
}
performSolve();
LEAP_VECTOR leapTranslation;
LEAP_QUATERNION leapRotation;
KabschC.GetTranslation(ref _kabsch, out leapTranslation);
KabschC.GetRotation(ref _kabsch, out leapRotation);
newPosition = bodyPosition + leapTranslation.ToVector3();
newRotation = leapRotation.ToQuaternion() * bodyRotation;
}
protected override void OnHandsHoldPhysics(ReadonlyList <Hand> hands)
{
base.OnHandsHoldPhysics(hands);
PhysicsMoveInfo info = new PhysicsMoveInfo();
info.remainingDistanceLastFrame = Vector3.Distance(_warper.RigidbodyPosition, _solvedPosition);
info.shouldTeleport = _notifiedOfTeleport;
_controllers.HoldingPoseController.GetHoldingPose(hands, out _solvedPosition, out _solvedRotation);
_controllers.MoveToController.MoveTo(hands, info, _solvedPosition, _solvedRotation);
if (_controllers.ThrowingController != null)
{
_controllers.ThrowingController.OnHold(hands);
}
}
/** Samples the current velocity and adds it to the rolling average. */
public override void OnHold(ReadonlyList <Hand> hands)
{
_velocityQueue.Enqueue(new VelocitySample(_obj.warper.RigidbodyPosition, _obj.warper.RigidbodyRotation, Time.fixedTime));
while (true)
{
VelocitySample oldest = _velocityQueue.Peek();
//Dequeue conservatively if the oldest is more than 4 frames later than the start of the window
if (oldest.time + Time.fixedDeltaTime * 4 < Time.fixedTime - _windowLength - _windowDelay)
{
_velocityQueue.Dequeue();
}
else
{
break;
}
}
}
protected override void OnHandsHoldGraphics(ReadonlyList <Hand> hands)
{
base.OnHandsHoldGraphics(hands);
if (_material.WarpingEnabled)
{
Vector3 deltaPosition = Quaternion.Inverse(_solvedRotation) * (_warper.RigidbodyPosition - _solvedPosition);
Quaternion deltaRotation = Quaternion.Inverse(_solvedRotation) * _warper.RigidbodyRotation;
Vector3 newPosition;
Quaternion newRotation;
_controllers.HoldingPoseController.GetHoldingPose(hands, out newPosition, out newRotation);
Vector3 graphicalPosition = newPosition + newRotation * deltaPosition;
Quaternion graphicalRotation = newRotation * deltaRotation;
_warper.WarpPercent = _material.WarpCurve.Evaluate(deltaPosition.magnitude / _manager.SimulationScale);
_warper.SetGraphicalPosition(graphicalPosition, graphicalRotation);
}
}
/** Does nothing in this implementation. */
public override void OnHold(ReadonlyList <Hand> hands)
{
}
/// <summary>
/// Called every LateUpdate that a Hand continues to grasp this object.
/// </summary>
protected virtual void OnHandsHoldGraphics(ReadonlyList <Hand> hands)
{
_baseCallGuard.NotifyBaseCalled("OnHandsHoldGraphics");
}
public override sealed void NotifyHandsHoldGraphics(ReadonlyList <Hand> hands)
{
_baseCallGuard.Begin("OnHandsHoldGraphics");
OnHandsHoldGraphics(hands);
_baseCallGuard.AssertBaseCalled();
}
/// <summary> /// Called by InteractionManager every frame that a Hand continues to grasp this object. This callback /// is invoked both in LateUpdate. /// </summary> public abstract void NotifyHandsHoldGraphics(ReadonlyList <Hand> hands);
/** * Calculate the best holding pose given the current state of the hands and * interactable object. * @param hands the list of hands with the current tracking data. * @param position A Vector3 object to be filled with the disred object position. * @param rotation A Quaternion object to be filled with the desired rotation. * @since 4.1.4 */ public abstract void GetHoldingPose(ReadonlyList <Hand> hands, out Vector3 position, out Quaternion rotation);
/** * Called every physics frame while an interactable object is being held. * @param hands a list of the hands holding the object. Note that currently, only one * hand will ever be holding an object simultaneously. * @since 4.1.4 */ public abstract void OnHold(ReadonlyList <Hand> hands);
/** * Move the interactable object to or towards the Interaction Engine's desired position. * * If info.shouldTeleport is true, this function must move the object using a teleport-style * movement, ie.e by changing the transform.position and transform.rotation, or the * RigidBody.position and RigidBody.rotation properties directly. InteractionBehaviour.NotifyTeleport() does * not be called in MoveTo(). * * If info.shouldTeleport is false, this function must move the object using linear and angular * forces and velocities. * * @param hands the list of Leap.Hand objects involved in the interaction. * @param info hints about the move. * @param solvedPosition the target position calculated by the Interaction Engine. * @param solvedRotation the target rotation calculated by the Interaction Engine. * @since 4.1.4 */ public abstract void MoveTo(ReadonlyList <Hand> hands, PhysicsMoveInfo info, Vector3 solvedPosition, Quaternion solvedRotation);
