private void FixedUpdate()
{
if (_controller == null)
{
return;
}
if (Anchor == null)
{
_transition *= 0.875f;
if (_transition <= 0.01f)
{
_transition = 0f;
}
}
else
{
_transition = Mathf.Lerp(_transition, 1f, 0.125f);
}
if (Anchor == null && _transition <= 0f)
{
if (MatchRotation)
{
_xform.rotation = _controller.Rotation;
}
if (MatchPosition)
{
_xform.position = _controller.Position;
}
}
else
{
var hand = _controller.Hand;
if (MatchPosition)
{
var offsetPos = _xform.position - _attachPoint.Position;
//_xform.position = Anchor.GetPosition(hand) + offsetPos;
if (Anchor != null)
{
_targetPos = Anchor.GetPosition(hand) + offsetPos;
}
_xform.position = Vector3.Lerp(_controller.Position, _targetPos, _transition);
}
if (MatchRotation)
{
//_xform.rotation = Anchor.GetRotation(hand);
if (Anchor != null)
{
_targetRot = Anchor.GetRotation(hand);
}
_xform.rotation = Quaternion.Lerp(_controller.Rotation, _targetRot, _transition);
}
}
}
private void FixedUpdate()
{
if (DisableForce)
{
return;
}
if (_grabbedBody && _handBody)
{
bool isTwoHanded = _secondTarget && (_secondAnchor != null);
// How far is the grabbed anchor from the hand?
var mass = _grabbedBody.mass * (isTwoHanded ? 0.5f : 1f);
var delta = (_target.position - _grabbedAnchor.GetPosition(_firstGrabbedBy)) * (_moveSpeed / (1f + mass));
// Find the angular delta
Quaternion angDelta;
//TODO Calculate hand velocity in HandController and use that instead
//TODO Should be a blend of both hands' velocities for two-handed
var handVelocity = _handBody.velocity;
var handAngVelocity = _handBody.angularVelocity;
// Move the object towards the hand(s)
if (_engaged < 1f)
{
_engaged = Mathf.Lerp(_engaged, 1f, 0.125f);
}
if (isTwoHanded)
{
// Two hands are manipulating this object...
var secondHand = _firstGrabbedBy == Hand.Left ? Hand.Right : Hand.Left;
var secondAnchorPos = _secondAnchor.GetPosition(secondHand);
// Blend the position deltas of the each hand compared to its anchor
var delta2 = (_secondTarget.position - secondAnchorPos) * (_moveSpeed / (1f + _grabbedBody.mass));
delta = Vector3.LerpUnclamped(delta, delta2, 0.5f * _engaged);
// TODO Calculate stretch, twist and bend
// TODO Add option to auto-release second hand if stretch goes beyond a tolerance. Priority could be specified by GrabAnchors
// Find the angle between the anchors
var fromDir = (_grabbedAnchor.GetPosition(_firstGrabbedBy) - secondAnchorPos).normalized;
var fromUp = _secondAnchor.GetUp(secondHand);
var from = Quaternion.LookRotation(fromDir, fromUp);
// Find the angle between the hands
var toDir = (_target.position - secondAnchorPos).normalized;
var toUp = Vector3.Lerp(_target.up, _secondTarget.up, 0.5f).normalized;
var to = Quaternion.LookRotation(toDir, toUp);
// Find the difference between those angles to turn both anchors towards their respective hands
angDelta = to * Quaternion.Inverse(from);
}
else
{
angDelta = _target.rotation * Quaternion.Inverse(_grabbedAnchor.GetRotation(_firstGrabbedBy));
}
//! Not sure if there's any difference between these two in terms of natural behavior
//_grabbedBody.velocity = Vector3.Lerp(_grabbedBody.velocity, delta + handVelocity, _engaged);
_grabbedBody.AddForce(Vector3.Lerp(_grabbedBody.velocity, delta + handVelocity, _engaged) - _grabbedBody.velocity, ForceMode.VelocityChange);
// Convert angular delta from Quaternion to Angle/Axis
angDelta.ToAngleAxis(out float angle, out Vector3 axis);
// Check that we're not already aligned
if (float.IsInfinity(axis.x))
{
return;
}
// Apply angular velocity to align the object to the hand controller(s)
if (angle > 180f)
{
angle -= 360f;
}
var magnitude = (_turnSpeed * Mathf.Deg2Rad * angle / Time.fixedUnscaledDeltaTime);
var newAngVel = magnitude * axis.normalized;
_grabbedBody.angularVelocity = Vector3.Lerp(_grabbedBody.angularVelocity, newAngVel, _engaged);
_grabbedBody.AddTorque(handAngVelocity, ForceMode.VelocityChange);
}
else
{
// Nothing is grabbed, so disengage
_engaged = 0f;
}
}
