386 lines
16 KiB
C#
386 lines
16 KiB
C#
/*
|
|
* Copyright (c) Meta Platforms, Inc. and affiliates.
|
|
* All rights reserved.
|
|
*
|
|
* Licensed under the Oculus SDK License Agreement (the "License");
|
|
* you may not use the Oculus SDK except in compliance with the License,
|
|
* which is provided at the time of installation or download, or which
|
|
* otherwise accompanies this software in either electronic or hard copy form.
|
|
*
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* https://developer.oculus.com/licenses/oculussdk/
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, the Oculus SDK
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
using Oculus.Interaction.Input;
|
|
using System;
|
|
using UnityEngine;
|
|
|
|
namespace Oculus.Interaction.Grab.GrabSurfaces
|
|
{
|
|
[Serializable]
|
|
public class BoxGrabSurfaceData : ICloneable
|
|
{
|
|
public object Clone()
|
|
{
|
|
BoxGrabSurfaceData clone = new BoxGrabSurfaceData();
|
|
clone.widthOffset = this.widthOffset;
|
|
clone.snapOffset = this.snapOffset;
|
|
clone.size = this.size;
|
|
clone.eulerAngles = this.eulerAngles;
|
|
return clone;
|
|
}
|
|
|
|
public BoxGrabSurfaceData Mirror()
|
|
{
|
|
BoxGrabSurfaceData mirror = Clone() as BoxGrabSurfaceData;
|
|
mirror.snapOffset = new Vector4(
|
|
-mirror.snapOffset.y, -mirror.snapOffset.x,
|
|
-mirror.snapOffset.w, -mirror.snapOffset.z);
|
|
|
|
return mirror;
|
|
}
|
|
|
|
[Range(0f, 1f)]
|
|
public float widthOffset = 0.5f;
|
|
public Vector4 snapOffset;
|
|
public Vector3 size = new Vector3(0.1f, 0f, 0.1f);
|
|
public Vector3 eulerAngles;
|
|
}
|
|
|
|
/// <summary>
|
|
/// This GrabSurface defines a Rectangle around which the grip point is valid.
|
|
/// Since the grip point might be offset from the fingers, a valid range for each opposite
|
|
/// side of the rectangle can be set so the grabbing fingers are constrained to the object bounds.
|
|
/// </summary>
|
|
[Serializable]
|
|
public class BoxGrabSurface : MonoBehaviour, IGrabSurface
|
|
{
|
|
[SerializeField]
|
|
protected BoxGrabSurfaceData _data = new BoxGrabSurfaceData();
|
|
|
|
[SerializeField]
|
|
[Tooltip("Transform used as a reference to measure the local data of the grab surface")]
|
|
private Transform _relativeTo;
|
|
|
|
private Pose RelativePose => PoseUtils.DeltaScaled(_relativeTo, this.transform);
|
|
|
|
/// <summary>
|
|
/// The origin pose of the surface. This is the point from which
|
|
/// the base of the box must start.
|
|
/// </summary>
|
|
/// <param name="relativeTo">The reference transform to apply the surface to</param>
|
|
/// <returns>Pose in world space</returns>
|
|
public Pose GetReferencePose(Transform relativeTo)
|
|
{
|
|
return PoseUtils.GlobalPoseScaled(relativeTo, RelativePose);
|
|
}
|
|
|
|
/// <summary>
|
|
/// The lateral displacement of the grip point in the main side.
|
|
/// </summary>
|
|
/// <param name="relativeTo">The reference transform to apply the surface to</param>
|
|
/// <returns>Lateral displacement in world space</returns>
|
|
public float GetWidthOffset(Transform relativeTo)
|
|
{
|
|
return _data.widthOffset * relativeTo.lossyScale.x;
|
|
}
|
|
public void SetWidthOffset(float widthOffset, Transform relativeTo)
|
|
{
|
|
_data.widthOffset = widthOffset / relativeTo.lossyScale.x;
|
|
}
|
|
|
|
/// <summary>
|
|
/// The range at which the sides are constrained.
|
|
/// X,Y for the back and forward sides range.
|
|
/// Z,W for the left and right sides range.
|
|
/// </summary>
|
|
/// <param name="relativeTo">The reference transform to apply the surface to</param>
|
|
/// <returns>Offsets in world space</returns>
|
|
public Vector4 GetSnapOffset(Transform relativeTo)
|
|
{
|
|
return _data.snapOffset * relativeTo.lossyScale.x;
|
|
}
|
|
public void SetSnapOffset(Vector4 snapOffset, Transform relativeTo)
|
|
{
|
|
_data.snapOffset = snapOffset / relativeTo.lossyScale.x;
|
|
}
|
|
|
|
/// <summary>
|
|
/// The size of the rectangle. Y is ignored.
|
|
/// </summary>
|
|
/// <param name="relativeTo">The reference transform to apply the surface to</param>
|
|
/// <returns>Size in world space</returns>
|
|
public Vector3 GetSize(Transform relativeTo)
|
|
{
|
|
return _data.size * relativeTo.lossyScale.x;
|
|
}
|
|
public void SetSize(Vector3 size, Transform relativeTo)
|
|
{
|
|
_data.size = size / relativeTo.lossyScale.x;
|
|
}
|
|
|
|
/// <summary>
|
|
/// The rotation of the rectangle from the Grip point
|
|
/// </summary>
|
|
/// <param name="relativeTo">The reference transform to apply the surface to</param>
|
|
/// <returns>Rotation in world space</returns>
|
|
public Quaternion GetRotation(Transform relativeTo)
|
|
{
|
|
return relativeTo.rotation * Quaternion.Euler(_data.eulerAngles);
|
|
}
|
|
public void SetRotation(Quaternion rotation, Transform relativeTo)
|
|
{
|
|
_data.eulerAngles = (Quaternion.Inverse(relativeTo.rotation) * rotation).eulerAngles;
|
|
}
|
|
|
|
/// <summary>
|
|
/// The forward direction of the rectangle (based on its rotation)
|
|
/// </summary>
|
|
/// <param name="relativeTo">The reference transform to apply the surface to</param>
|
|
/// <returns>Direction in world space</returns>
|
|
public Vector3 GetDirection(Transform relativeTo)
|
|
{
|
|
return GetRotation(relativeTo) * Vector3.forward;
|
|
}
|
|
|
|
#region editor events
|
|
protected virtual void Reset()
|
|
{
|
|
_relativeTo = this.GetComponentInParent<IRelativeToRef>()?.RelativeTo;
|
|
}
|
|
#endregion
|
|
|
|
protected virtual void Start()
|
|
{
|
|
this.AssertField(_data, nameof(_data));
|
|
this.AssertField(_relativeTo, nameof(_relativeTo));
|
|
}
|
|
|
|
public Pose MirrorPose(in Pose pose, Transform relativeTo)
|
|
{
|
|
Quaternion localRotation = Quaternion.Euler(_data.eulerAngles);
|
|
Vector3 mirrorPlane = localRotation * Vector3.right;
|
|
Quaternion reflectedRot = HandMirroring.Reflect(pose.rotation, mirrorPlane);
|
|
return new Pose(pose.position, reflectedRot);
|
|
}
|
|
|
|
public IGrabSurface CreateMirroredSurface(GameObject gameObject)
|
|
{
|
|
BoxGrabSurface surface = gameObject.AddComponent<BoxGrabSurface>();
|
|
surface._data = _data.Mirror();
|
|
return surface;
|
|
}
|
|
|
|
public IGrabSurface CreateDuplicatedSurface(GameObject gameObject)
|
|
{
|
|
BoxGrabSurface surface = gameObject.AddComponent<BoxGrabSurface>();
|
|
surface._data = _data;
|
|
return surface;
|
|
}
|
|
|
|
public GrabPoseScore CalculateBestPoseAtSurface(in Pose targetPose, out Pose bestPose,
|
|
in PoseMeasureParameters scoringModifier, Transform relativeTo)
|
|
{
|
|
return CalculateBestPoseAtSurface(targetPose, Pose.identity, out bestPose,
|
|
scoringModifier, relativeTo);
|
|
}
|
|
|
|
public GrabPoseScore CalculateBestPoseAtSurface(in Pose targetPose, in Pose offset, out Pose bestPose,
|
|
in PoseMeasureParameters scoringModifier, Transform relativeTo)
|
|
{
|
|
return GrabPoseHelper.CalculateBestPoseAtSurface(targetPose, offset, out bestPose,
|
|
scoringModifier, relativeTo,
|
|
MinimalTranslationPoseAtSurface, MinimalRotationPoseAtSurface);
|
|
}
|
|
|
|
private void CalculateCorners(out Vector3 bottomLeft, out Vector3 bottomRight, out Vector3 topLeft, out Vector3 topRight,
|
|
Transform relativeTo)
|
|
{
|
|
Pose referencePose = GetReferencePose(relativeTo);
|
|
Vector3 size = GetSize(relativeTo);
|
|
float widthOffset = GetWidthOffset(relativeTo);
|
|
Vector3 rightRot = GetRotation(relativeTo) * Vector3.right;
|
|
bottomLeft = referencePose.position - rightRot * size.x * (1f - widthOffset);
|
|
bottomRight = referencePose.position + rightRot * size.x * (widthOffset);
|
|
Vector3 forwardOffset = GetRotation(relativeTo) * Vector3.forward * size.z;
|
|
topLeft = bottomLeft + forwardOffset;
|
|
topRight = bottomRight + forwardOffset;
|
|
}
|
|
|
|
private Vector3 ProjectOnSegment(Vector3 point, (Vector3, Vector3) segment)
|
|
{
|
|
Vector3 line = segment.Item2 - segment.Item1;
|
|
Vector3 projection = Vector3.Project(point - segment.Item1, line);
|
|
if (Vector3.Dot(projection, line) < 0f)
|
|
{
|
|
projection = segment.Item1;
|
|
}
|
|
else if (projection.magnitude > line.magnitude)
|
|
{
|
|
projection = segment.Item2;
|
|
}
|
|
else
|
|
{
|
|
projection += segment.Item1;
|
|
}
|
|
return projection;
|
|
}
|
|
|
|
public bool CalculateBestPoseAtSurface(Ray targetRay, out Pose bestPose, Transform relativeTo)
|
|
{
|
|
Pose recordedPose = GetReferencePose(relativeTo);
|
|
Plane plane = new Plane(GetRotation(relativeTo) * Vector3.up, this.transform.position);
|
|
plane.Raycast(targetRay, out float rayDistance);
|
|
Vector3 proximalPoint = targetRay.origin + targetRay.direction * rayDistance;
|
|
|
|
Vector3 surfacePoint = NearestPointInSurface(proximalPoint, relativeTo);
|
|
Pose desiredPose = new Pose(surfacePoint, recordedPose.rotation);
|
|
bestPose = MinimalTranslationPoseAtSurface(desiredPose, relativeTo);
|
|
return true;
|
|
}
|
|
|
|
protected Vector3 NearestPointInSurface(Vector3 targetPosition, Transform relativeTo)
|
|
{
|
|
NearestPointAndAngleInSurface(targetPosition, out Vector3 surfacePoint, out float angle, relativeTo);
|
|
return surfacePoint;
|
|
}
|
|
|
|
private void NearestPointAndAngleInSurface(Vector3 targetPosition, out Vector3 surfacePoint, out float angle, Transform relativeTo)
|
|
{
|
|
Quaternion rotation = GetRotation(relativeTo);
|
|
Vector4 snappOffset = GetSnapOffset(relativeTo);
|
|
Vector3 rightDir = rotation * Vector3.right;
|
|
Vector3 forwardDir = rotation * Vector3.forward;
|
|
Vector3 bottomLeft, bottomRight, topLeft, topRight;
|
|
CalculateCorners(out bottomLeft, out bottomRight, out topLeft, out topRight, relativeTo);
|
|
|
|
Vector3 bottomP = ProjectOnSegment(targetPosition,
|
|
(bottomLeft + rightDir * snappOffset.y, bottomRight + rightDir * snappOffset.x));
|
|
Vector3 topP = ProjectOnSegment(targetPosition,
|
|
(topLeft - rightDir * snappOffset.x, topRight - rightDir * snappOffset.y));
|
|
Vector3 leftP = ProjectOnSegment(targetPosition,
|
|
(bottomLeft - forwardDir * snappOffset.z, topLeft - forwardDir * snappOffset.w));
|
|
Vector3 rightP = ProjectOnSegment(targetPosition,
|
|
(bottomRight + forwardDir * snappOffset.w, topRight + forwardDir * snappOffset.z));
|
|
|
|
float bottomDistance = (bottomP - targetPosition).sqrMagnitude;
|
|
float topDistance = (topP - targetPosition).sqrMagnitude;
|
|
float leftDistance = (leftP - targetPosition).sqrMagnitude;
|
|
float rightDistance = (rightP - targetPosition).sqrMagnitude;
|
|
|
|
float minDistance = Mathf.Min(bottomDistance,
|
|
Mathf.Min(topDistance,
|
|
Mathf.Min(leftDistance, rightDistance)));
|
|
if (bottomDistance == minDistance)
|
|
{
|
|
surfacePoint = bottomP;
|
|
angle = 0f;
|
|
return;
|
|
}
|
|
if (topDistance == minDistance)
|
|
{
|
|
surfacePoint = topP;
|
|
angle = 180f;
|
|
return;
|
|
}
|
|
if (leftDistance == minDistance)
|
|
{
|
|
surfacePoint = leftP;
|
|
angle = 90f;
|
|
return;
|
|
}
|
|
surfacePoint = rightP;
|
|
angle = -90f;
|
|
}
|
|
|
|
protected Pose MinimalRotationPoseAtSurface(in Pose userPose, Transform relativeTo)
|
|
{
|
|
Quaternion rotation = GetRotation(relativeTo);
|
|
Pose referencePose = GetReferencePose(relativeTo);
|
|
Vector4 snappOffset = GetSnapOffset(relativeTo);
|
|
Vector3 desiredPos = userPose.position;
|
|
Quaternion baseRot = referencePose.rotation;
|
|
Quaternion desiredRot = userPose.rotation;
|
|
Vector3 up = rotation * Vector3.up;
|
|
|
|
Quaternion bottomRot = baseRot;
|
|
Quaternion topRot = Quaternion.AngleAxis(180f, up) * baseRot;
|
|
Quaternion leftRot = Quaternion.AngleAxis(90f, up) * baseRot;
|
|
Quaternion rightRot = Quaternion.AngleAxis(-90f, up) * baseRot;
|
|
|
|
float bottomDot = RotationalScore(bottomRot, desiredRot);
|
|
float topDot = RotationalScore(topRot, desiredRot);
|
|
float leftDot = RotationalScore(leftRot, desiredRot);
|
|
float rightDot = RotationalScore(rightRot, desiredRot);
|
|
|
|
Vector3 rightDir = rotation * Vector3.right;
|
|
Vector3 forwardDir = rotation * Vector3.forward;
|
|
Vector3 bottomLeft, bottomRight, topLeft, topRight;
|
|
CalculateCorners(out bottomLeft, out bottomRight, out topLeft, out topRight, relativeTo);
|
|
|
|
float maxDot = Mathf.Max(bottomDot, Mathf.Max(topDot, Mathf.Max(leftDot, rightDot)));
|
|
if (bottomDot == maxDot)
|
|
{
|
|
Vector3 projBottom = ProjectOnSegment(desiredPos, (bottomLeft + rightDir * snappOffset.y, bottomRight + rightDir * snappOffset.x));
|
|
return new Pose(projBottom, bottomRot);
|
|
}
|
|
if (topDot == maxDot)
|
|
{
|
|
Vector3 projTop = ProjectOnSegment(desiredPos, (topLeft - rightDir * snappOffset.x, topRight - rightDir * snappOffset.y));
|
|
return new Pose(projTop, topRot);
|
|
}
|
|
if (leftDot == maxDot)
|
|
{
|
|
Vector3 projLeft = ProjectOnSegment(desiredPos, (bottomLeft - forwardDir * snappOffset.z, topLeft - forwardDir * snappOffset.w));
|
|
return new Pose(projLeft, leftRot);
|
|
}
|
|
Vector3 projRight = ProjectOnSegment(desiredPos, (bottomRight + forwardDir * snappOffset.w, topRight + forwardDir * snappOffset.z));
|
|
return new Pose(projRight, rightRot);
|
|
}
|
|
|
|
protected Pose MinimalTranslationPoseAtSurface(in Pose userPose, Transform relativeTo)
|
|
{
|
|
Pose referencePose = GetReferencePose(relativeTo);
|
|
Quaternion rotation = GetRotation(relativeTo);
|
|
Vector3 desiredPos = userPose.position;
|
|
Quaternion baseRot = referencePose.rotation;
|
|
NearestPointAndAngleInSurface(desiredPos, out Vector3 surfacePoint, out float surfaceAngle, relativeTo);
|
|
Quaternion surfaceRotation = Quaternion.AngleAxis(surfaceAngle, rotation * Vector3.up) * baseRot;
|
|
return new Pose(surfacePoint, surfaceRotation);
|
|
}
|
|
|
|
private static float RotationalScore(in Quaternion from, in Quaternion to)
|
|
{
|
|
float forwardDifference = Vector3.Dot(from * Vector3.forward, to * Vector3.forward) * 0.5f + 0.5f;
|
|
float upDifference = Vector3.Dot(from * Vector3.up, to * Vector3.up) * 0.5f + 0.5f;
|
|
return (forwardDifference * upDifference);
|
|
}
|
|
|
|
#region Inject
|
|
|
|
public void InjectAllBoxSurface(BoxGrabSurfaceData data, Transform relativeTo)
|
|
{
|
|
InjectData(data);
|
|
InjectRelativeTo(relativeTo);
|
|
}
|
|
|
|
public void InjectData(BoxGrabSurfaceData data)
|
|
{
|
|
_data = data;
|
|
}
|
|
|
|
public void InjectRelativeTo(Transform relativeTo)
|
|
{
|
|
_relativeTo = relativeTo;
|
|
}
|
|
#endregion
|
|
}
|
|
}
|