private void ProcessHand(GameObject handTarget, Transform physicalHand, Transform autoSnapPoint, Vector3 palmToWristOffset, Vector3 handRotateOffset, Vector3[] visualCueLinePoints)
{
// Base position
var position = physicalHand.position;
var snapOffset = autoSnapPoint.localPosition;
// Find the anchor over and under the controller
ControllerAnchorPoint lower = null;
ControllerAnchorPoint upper = null;
foreach (var anchorPoint in _anchorPoints)
{
if (!anchorPoint.Active)
{
continue;
}
var anchorPointPos = anchorPoint.GetWorldPosition();
if (position.y > anchorPointPos.y && (lower == null || anchorPointPos.y > lower.GetWorldPosition().y))
{
lower = anchorPoint;
}
else if (position.y < anchorPointPos.y && (upper == null || anchorPointPos.y < upper.GetWorldPosition().y))
{
upper = anchorPoint;
}
}
if (lower == null)
{
lower = upper;
}
else if (upper == null)
{
upper = lower;
}
// Find the weight of both anchors (closest = strongest effect)
var upperPosition = upper.GetWorldPosition();
var lowerPosition = lower.GetWorldPosition();
var yUpperDelta = upperPosition.y - position.y;
var yLowerDelta = position.y - lowerPosition.y;
var totalDelta = yLowerDelta + yUpperDelta;
var upperWeight = totalDelta == 0 ? 1f : yLowerDelta / totalDelta;
var lowerWeight = 1f - upperWeight;
var lowerRotation = lower.RigidBody.transform.rotation;
var upperRotation = upper.RigidBody.transform.rotation;
var anchorPosition = Vector3.Lerp(upperPosition, lowerPosition, lowerWeight);
var anchorRotation = Quaternion.Lerp(upperRotation, lowerRotation, lowerWeight);
// TODO: Is this useful?
anchorPosition.y = position.y;
visualCueLinePoints[VisualCueLineIndices.Anchor] = anchorPosition;
// Determine the falloff (closer = stronger, fades out with distance)
// TODO: Even better to use closest point on ellipse, but not necessary.
var distance = Mathf.Abs(Vector3.Distance(anchorPosition, position));
var physicalCueSize = Vector3.Lerp(Vector3.Scale(upper.VirtualScale, upper.PhysicalScale), Vector3.Scale(lower.VirtualScale, lower.PhysicalScale), lowerWeight) * (_baseCueSize / 2f);
var physicalCueDistanceFromCenter = Mathf.Max(physicalCueSize.x, physicalCueSize.z);
// TODO: Check both x and z to determine a falloff relative to both distances
var falloff = _falloffJSON.val > 0 ? 1f - (Mathf.Clamp(distance - physicalCueDistanceFromCenter, 0, _falloffJSON.val) / _falloffJSON.val) : 1f;
// Calculate the controller offset based on the physical scale/offset of anchors
var physicalScale = Vector3.Lerp(upper.PhysicalScale, lower.PhysicalScale, lowerWeight);
var physicalOffset = Vector3.Lerp(upper.PhysicalOffset, lower.PhysicalOffset, lowerWeight);
var baseOffset = position - anchorPosition;
var resultOffset = Quaternion.Inverse(anchorRotation) * baseOffset;
resultOffset = new Vector3(resultOffset.x / physicalScale.x, resultOffset.y / physicalScale.y, resultOffset.z / physicalScale.z) - physicalOffset;
resultOffset = anchorRotation * resultOffset;
var resultPosition = anchorPosition + Vector3.Lerp(baseOffset, resultOffset, falloff);
visualCueLinePoints[VisualCueLineIndices.Hand] = resultPosition;
// Apply the hands adjustments
var resultRotation = autoSnapPoint.rotation * Quaternion.Euler(handRotateOffset);
resultPosition += resultRotation * (snapOffset + palmToWristOffset);
// Do the displacement
var rb = handTarget.GetComponent <Rigidbody>();
rb.MovePosition(resultPosition);
rb.MoveRotation(resultRotation);
visualCueLinePoints[VisualCueLineIndices.Controller] = physicalHand.transform.position;
// SuperController.singleton.ClearMessages();
// SuperController.LogMessage($"y {position.y:0.00} btwn {lower.RigidBody.name} y {yLowerDelta:0.00} w {lowerWeight:0.00} and {upper.RigidBody.name} y {yUpperDelta:0.00} w {upperWeight: 0.00}");
// SuperController.LogMessage($"dist {distance:0.00}/{physicalCueDistanceFromCenter:0.00} falloff {falloff:0.00}");
// SuperController.LogMessage($"rot {upperRotation.eulerAngles} psca {physicalScale} poff {physicalOffset} base {baseOffset} res {resultOffset}");
}
