private static void AutoSetup(Component rb, ControllerAnchorPoint anchor, IEnumerable <Collider> colliders)
{
const float raycastDistance = 100f;
var rbTransform = rb.transform;
var rbUp = rbTransform.up;
var rbOffsetPosition = rbTransform.position + rbUp * anchor.inGameOffset.y;
var rbForward = rbTransform.forward;
var rays = new List <Ray>();
for (var i = 0; i < 360; i += 5)
{
var rotation = Quaternion.AngleAxis(i, rbUp);
var origin = rbOffsetPosition + rotation * (rbForward * raycastDistance);
rays.Add(new Ray(origin, rbOffsetPosition - origin));
}
var min = Vector3.positiveInfinity;
var max = Vector3.negativeInfinity;
var isHit = false;
foreach (var collider in colliders)
{
foreach (var ray in rays)
{
RaycastHit hit;
if (!collider.Raycast(ray, out hit, raycastDistance))
{
continue;
}
isHit = true;
var localPoint = rbTransform.InverseTransformPoint(hit.point);
min = Vector3.Min(min, localPoint);
max = Vector3.Max(max, localPoint);
// var hitCue = VisualCuesHelper.CreatePrimitive(null, PrimitiveType.Cube, new Color(0f, 1f, 0f, 0.2f));
// hitCue.transform.localScale = Vector3.one * 0.002f;
// hitCue.transform.position = hit.point;
}
}
if (!isHit)
{
return;
}
var offset = min + (max - min) / 2f;
var size = max - min;
// var cue1 = VisualCuesHelper.Cross(Color.red);
// cue1.transform.localScale = Vector3.one * 0.5f;
// cue1.transform.position = min;
var padding = new Vector3(0.02f, 0f, 0.02f);
anchor.inGameSize = size + padding;
anchor.inGameOffset = offset;
anchor.Update();
}
public MeasureAnchorWidthStep(WizardContext context, string part, ControllerAnchorPoint anchor)
{
_context = context;
_part = part;
_anchor = anchor;
_leftHandControl = context.containingAtom.freeControllers.First(fc => fc.name == "lHandControl");
_rightHandControl = context.containingAtom.freeControllers.First(fc => fc.name == "rHandControl");
}
public MeasureAnchorWidthStep(EmbodyContext context, ControllerAnchorPoint anchor, float handRotate, bool ignoreDepth = false)
: base(context)
{
_anchor = anchor;
_handRotate = handRotate;
_ignoreDepth = ignoreDepth;
_leftHandControl = context.containingAtom.freeControllers.First(fc => fc.name == "lHandControl");
_rightHandControl = context.containingAtom.freeControllers.First(fc => fc.name == "rHandControl");
}
public MeasureArmsAtRestStep(EmbodyContext context, ControllerAnchorPoint anchor)
: base(context)
{
_anchor = anchor;
_leftHandControl = context.containingAtom.freeControllers.First(fc => fc.name == "lHandControl");
_rightHandControl = context.containingAtom.freeControllers.First(fc => fc.name == "rHandControl");
_leftHandMotion = context.trackers.motionControls.First(mc => mc.name == MotionControlNames.LeftHand);
_rightHandMotion = context.trackers.motionControls.First(mc => mc.name == MotionControlNames.RightHand);
}
public MeasureAnchorDepthAndOffsetStep(EmbodyContext context, ControllerAnchorPoint anchor, float handRotate)
: base(context)
{
_anchor = anchor;
_handRotate = handRotate;
_leftHandControl = context.containingAtom.freeControllers.First(fc => fc.name == "lHandControl");
_rightHandControl = context.containingAtom.freeControllers.First(fc => fc.name == "rHandControl");
_headControl = context.containingAtom.freeControllers.First(fc => fc.name == "headControl");
_chestControl = context.containingAtom.freeControllers.First(fc => fc.name == "chestControl");
}
private float ComputeEffectWeight(ControllerAnchorPoint upper, ControllerAnchorPoint lower, float lowerWeight, Vector3 motionControlPosition)
{
if (falloffDistanceJSON.val == 0)
{
return(1f);
}
var realLifeSize = Vector3.Lerp(upper.realLifeSize, lower.realLifeSize, lowerWeight);
var anchorPosition = Vector3.Lerp(upper.GetAdjustedWorldPosition(), lower.GetAdjustedWorldPosition(), lowerWeight);
var anchorRotation = Quaternion.Slerp(upper.bone.transform.rotation, lower.bone.transform.rotation, lowerWeight);
var angle = Mathf.Deg2Rad * Vector3.SignedAngle(anchorRotation * Vector3.forward, motionControlPosition - anchorPosition, anchorRotation * Vector3.up);
var anchorHook = anchorPosition + new Vector3(Mathf.Sin(angle) * realLifeSize.x / 2f, 0f, Mathf.Cos(angle) * realLifeSize.z / 2f);
var hookDistanceFromAnchor = Vector3.Distance(anchorPosition, anchorHook);
var distanceFromAnchorHook = Mathf.Clamp(Vector3.Distance(anchorPosition, motionControlPosition) - hookDistanceFromAnchor, 0f, falloffDistanceJSON.val);
var effectWeight = 1f - (distanceFromAnchorHook / falloffDistanceJSON.val);
var exponentialWeight = ExponentialScale(effectWeight, falloffMidPointJSON.val);
return(exponentialWeight);
}
private void ProcessHand(SnugHand hand)
{
var motionControl = hand.motionControl;
var motionControlPosition = motionControl.currentMotionControl.position;
var visualCueLinePoints = hand.visualCueLinePoints;
// 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.GetAdjustedWorldPosition();
if (motionControlPosition.y > anchorPointPos.y && (lower == null || anchorPointPos.y > lower.GetAdjustedWorldPosition().y))
{
lower = anchorPoint;
}
else if (motionControlPosition.y < anchorPointPos.y && (upper == null || anchorPointPos.y < upper.GetAdjustedWorldPosition().y))
{
upper = anchorPoint;
}
}
if (lower == null)
{
lower = upper;
}
else if (upper == null)
{
upper = lower;
}
// TODO: If an anchor is higher than one that should be higher, ignore it
// Find the weight of both anchors (closest = strongest effect)
// ReSharper disable once PossibleNullReferenceException
var upperPosition = upper.GetAdjustedWorldPosition();
// ReSharper disable once PossibleNullReferenceException
var lowerPosition = lower.GetAdjustedWorldPosition();
var yUpperDelta = upperPosition.y - motionControlPosition.y;
var yLowerDelta = motionControlPosition.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;
// TODO: We can use a bezier curve or similar to make a curve following the angle of the upper/lower controls
var anchorPosition = Vector3.Lerp(upperPosition, lowerPosition, lowerWeight);
var anchorRotation = Quaternion.Lerp(upperRotation, lowerRotation, lowerWeight);
var angle = Mathf.Deg2Rad * Vector3.SignedAngle(anchorRotation * Vector3.forward, motionControlPosition - anchorPosition, anchorRotation * Vector3.up);
var realLifeSize = Vector3.Lerp(upper.realLifeSize, lower.realLifeSize, lowerWeight);
var anchorHook = anchorPosition + new Vector3(Mathf.Sin(angle) * realLifeSize.x / 2f, 0f, Mathf.Cos(angle) * realLifeSize.z / 2f);
var hookDistanceFromAnchor = Vector3.Distance(anchorPosition, anchorHook);
var motionControlDistanceFromAnchor = Vector3.Distance(anchorPosition, motionControlPosition);
float effectWeight;
if (motionControlDistanceFromAnchor < hookDistanceFromAnchor)
{
effectWeight = 1f;
}
else if (falloffJSON.val == 0)
{
effectWeight = 0f;
}
else
{
var distanceFromAnchorHook = Vector3.Distance(anchorHook, motionControlPosition);
if (distanceFromAnchorHook > falloffJSON.val)
{
effectWeight = 0f;
}
else
{
effectWeight = 1f - (Mathf.Clamp(distanceFromAnchorHook, 0, falloffJSON.val) / falloffJSON.val);
}
}
var realOffset = Vector3.Lerp(upper.realLifeOffset, lower.realLifeOffset, lowerWeight);
var inGameSize = Vector3.Lerp(upper.inGameSize, lower.inGameSize, lowerWeight);
var scale = new Vector3(inGameSize.x / realLifeSize.x, 1f, inGameSize.z / realLifeSize.z);
var actualRelativePosition = motionControlPosition - anchorPosition;
var scaled = Vector3.Scale(actualRelativePosition, scale);
var finalPosition = Vector3.Lerp(motionControlPosition, anchorPosition + scaled - realOffset, effectWeight);
visualCueLinePoints[0] = finalPosition;
visualCueLinePoints[1] = motionControlPosition;
// TODO: Compute for both anchors and average them using smooth lerp
var finalPositionToControlPoint = finalPosition + (hand.motionControl.possessPointTransform.position - motionControlPosition);
if (previewSnugOffsetJSON.val)
{
visualCueLinePoints[0] = motionControl.currentMotionControl.position;
visualCueLinePoints[1] = finalPosition;
}
hand.controllerRigidbody.MovePosition(finalPositionToControlPoint);
hand.controllerRigidbody.MoveRotation(motionControl.possessPointTransform.rotation);
}
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}");
}
private static Vector3 ComputeHandPositionFromAnchor(Vector3 anchorPosition, Vector3 motionControlPosition, ControllerAnchorPoint anchorPoint)
{
var realLifeSize = anchorPoint.realLifeSize;
var realOffset = anchorPoint.realLifeOffset;
var inGameSize = anchorPoint.inGameSize;
var scale = new Vector3(inGameSize.x / realLifeSize.x, 1f, inGameSize.z / realLifeSize.z);
var actualRelativePosition = motionControlPosition - anchorPosition;
var scaled = Vector3.Scale(actualRelativePosition, scale);
var finalPosition = anchorPosition + scaled - (realOffset * SuperController.singleton.worldScale);
return(finalPosition);
}
private void ProcessHand(SnugHand hand)
{
if (!hand.active)
{
return;
}
var motionControl = hand.motionControl;
var motionControlPosition = motionControl.currentMotionControl.position;
var visualCueLinePoints = hand.visualCueLinePoints;
if (!hand.motionControl.currentMotionControl.gameObject.activeInHierarchy)
{
return;
}
// Find the anchor over and under the controller
ControllerAnchorPoint lower = null;
ControllerAnchorPoint upper = null;
for (var i = anchorPoints.Count - 1; i >= 0; i--)
{
var anchorPoint = anchorPoints[i];
if (!anchorPoint.active)
{
continue;
}
var anchorPointPos = anchorPoint.GetAdjustedWorldPosition();
if (motionControlPosition.y > anchorPointPos.y && (lower == null || anchorPointPos.y > lower.GetAdjustedWorldPosition().y))
{
lower = anchorPoint;
}
else if (motionControlPosition.y < anchorPointPos.y && (upper == null || anchorPointPos.y < upper.GetAdjustedWorldPosition().y))
{
upper = anchorPoint;
}
}
if (lower == null)
{
lower = upper;
}
else if (upper == null)
{
upper = lower;
}
// Find the weight of both anchors (closest = strongest effect)
// ReSharper disable once PossibleNullReferenceException
var upperPosition = upper.GetAdjustedWorldPosition();
// ReSharper disable once PossibleNullReferenceException
var lowerPosition = lower.GetAdjustedWorldPosition();
if (lower.floor)
{
lowerPosition = new Vector3(motionControlPosition.x, Mathf.Min(_lToe.transform.position.y, _rToe.transform.position.y), motionControlPosition.z);
}
var yUpperDelta = upperPosition.y - motionControlPosition.y;
var yLowerDelta = motionControlPosition.y - lowerPosition.y;
var totalDelta = yLowerDelta + yUpperDelta;
var upperWeight = totalDelta == 0 ? 1f : yLowerDelta / totalDelta;
var lowerWeight = 1f - upperWeight;
var finalPositionUpper = ComputeHandPositionFromAnchor(upperPosition, motionControlPosition, upper);
var finalPositionLower = ComputeHandPositionFromAnchor(lowerPosition, motionControlPosition, lower);
var finalPosition = new Vector3(
Mathf.SmoothStep(finalPositionUpper.x, finalPositionLower.x, lowerWeight),
Mathf.SmoothStep(finalPositionUpper.y, finalPositionLower.y, lowerWeight),
Mathf.SmoothStep(finalPositionUpper.z, finalPositionLower.z, lowerWeight)
);
var effectWeight = ComputeEffectWeight(upper, lower, lowerWeight, motionControlPosition);
var possessPointPosition = hand.motionControl.controllerPointTransform.position;
var finalPositionToControlPoint = finalPosition + (possessPointPosition - motionControlPosition);
if (previewSnugOffsetJSON.val)
{
visualCueLinePoints[0] = motionControlPosition;
visualCueLinePoints[1] = Vector3.Lerp(motionControlPosition, finalPosition, effectWeight);
}
hand.controllerRigidbody.MovePosition(Vector3.Lerp(possessPointPosition, finalPositionToControlPoint, effectWeight));
hand.controllerRigidbody.MoveRotation(motionControl.controllerPointTransform.rotation);
}
public MeasureAnchorDepthAndOffsetStep(WizardContext context, string part, ControllerAnchorPoint anchor)
{
_context = context;
_part = part;
_anchor = anchor;
}
