248 lines
10 KiB
C#
248 lines
10 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 SphereGrabSurfaceData : ICloneable
|
|
{
|
|
public object Clone()
|
|
{
|
|
SphereGrabSurfaceData clone = new SphereGrabSurfaceData();
|
|
clone.centre = this.centre;
|
|
return clone;
|
|
}
|
|
|
|
public SphereGrabSurfaceData Mirror()
|
|
{
|
|
SphereGrabSurfaceData mirror = Clone() as SphereGrabSurfaceData;
|
|
return mirror;
|
|
}
|
|
|
|
public Vector3 centre = Vector3.zero;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Specifies an entire sphere around an object in which the grip point is valid.
|
|
/// One of the main advantages of spheres is that the rotation of the hand pose does
|
|
/// not really matters, as it will always fit the surface correctly.
|
|
/// </summary>
|
|
[Serializable]
|
|
public class SphereGrabSurface : MonoBehaviour, IGrabSurface
|
|
{
|
|
[SerializeField]
|
|
protected SphereGrabSurfaceData _data = new SphereGrabSurfaceData();
|
|
|
|
[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 reference pose of the surface. It defines the radius of the sphere
|
|
/// as the point from the relative transform to the reference pose to ensure
|
|
/// that the sphere covers this pose.
|
|
/// </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 center of the sphere in world coordinates.
|
|
/// </summary>
|
|
/// <param name="relativeTo">The reference transform to apply the surface to</param>
|
|
/// <returns>Position in world space</returns>
|
|
public Vector3 GetCentre(Transform relativeTo)
|
|
{
|
|
return relativeTo.TransformPoint(_data.centre);
|
|
}
|
|
public void SetCentre(Vector3 point, Transform relativeTo)
|
|
{
|
|
_data.centre = relativeTo.InverseTransformPoint(point);
|
|
}
|
|
|
|
/// <summary>
|
|
/// The radius of the sphere, this is automatically calculated as the distance between
|
|
/// the center and the original grip pose.
|
|
/// </summary>
|
|
/// <param name="relativeTo">The reference transform to apply the surface to</param>
|
|
/// <returns>Distance in world space</returns>
|
|
public float GetRadius(Transform relativeTo)
|
|
{
|
|
Vector3 centre = GetCentre(relativeTo);
|
|
Pose referencePose = GetReferencePose(relativeTo);
|
|
return Vector3.Distance(centre, referencePose.position);
|
|
}
|
|
|
|
/// <summary>
|
|
/// The direction of the sphere, measured from the center to the original grip position.
|
|
/// </summary>
|
|
/// <param name="relativeTo">The reference transform to apply the surface to</param>
|
|
/// <returns>Direction in world space</returns>
|
|
public Vector3 GetDirection(Transform relativeTo)
|
|
{
|
|
Vector3 centre = GetCentre(relativeTo);
|
|
Pose referencePose = GetReferencePose(relativeTo);
|
|
return (referencePose.position - centre).normalized;
|
|
}
|
|
|
|
#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)
|
|
{
|
|
Vector3 mirrorPlane = Vector3.Cross(pose.position, Vector3.up).normalized;
|
|
Quaternion reflectedRot = HandMirroring.Reflect(pose.rotation, mirrorPlane);
|
|
return new Pose(pose.position, reflectedRot);
|
|
}
|
|
|
|
public bool CalculateBestPoseAtSurface(Ray targetRay, out Pose bestPose, Transform relativeTo)
|
|
{
|
|
Vector3 centre = GetCentre(relativeTo);
|
|
Vector3 projection = Vector3.Project(centre - targetRay.origin, targetRay.direction);
|
|
Vector3 nearestCentre = targetRay.origin + projection;
|
|
float radius = GetRadius(relativeTo);
|
|
float distanceToSurface = Mathf.Max(Vector3.Distance(centre, nearestCentre) - radius);
|
|
if (distanceToSurface < radius)
|
|
{
|
|
float adjustedDistance = Mathf.Sqrt(radius * radius - distanceToSurface * distanceToSurface);
|
|
nearestCentre -= targetRay.direction * adjustedDistance;
|
|
}
|
|
|
|
Pose recordedPose = GetReferencePose(relativeTo);
|
|
Vector3 surfacePoint = NearestPointInSurface(nearestCentre, relativeTo);
|
|
Pose desiredPose = new Pose(surfacePoint, recordedPose.rotation);
|
|
bestPose = MinimalTranslationPoseAtSurface(desiredPose, relativeTo);
|
|
return true;
|
|
}
|
|
|
|
|
|
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);
|
|
}
|
|
|
|
public IGrabSurface CreateMirroredSurface(GameObject gameObject)
|
|
{
|
|
SphereGrabSurface surface = gameObject.AddComponent<SphereGrabSurface>();
|
|
surface._data = _data.Mirror();
|
|
return surface;
|
|
}
|
|
|
|
public IGrabSurface CreateDuplicatedSurface(GameObject gameObject)
|
|
{
|
|
SphereGrabSurface surface = gameObject.AddComponent<SphereGrabSurface>();
|
|
surface._data = _data;
|
|
return surface;
|
|
}
|
|
|
|
protected Vector3 NearestPointInSurface(Vector3 targetPosition, Transform relativeTo)
|
|
{
|
|
Vector3 centre = GetCentre(relativeTo);
|
|
Vector3 direction = (targetPosition - centre).normalized;
|
|
float radius = GetRadius(relativeTo);
|
|
return centre + direction * radius;
|
|
}
|
|
|
|
protected Pose MinimalRotationPoseAtSurface(in Pose userPose, Transform relativeTo)
|
|
{
|
|
Vector3 centre = GetCentre(relativeTo);
|
|
Pose referencePose = GetReferencePose(relativeTo);
|
|
float radius = GetRadius(relativeTo);
|
|
Quaternion rotCorrection = userPose.rotation * Quaternion.Inverse(referencePose.rotation);
|
|
Vector3 correctedDir = rotCorrection * GetDirection(relativeTo);
|
|
Vector3 surfacePoint = NearestPointInSurface(centre + correctedDir * radius, relativeTo);
|
|
Quaternion surfaceRotation = RotationAtPoint(surfacePoint, referencePose.rotation, userPose.rotation, relativeTo);
|
|
|
|
return new Pose(surfacePoint, surfaceRotation);
|
|
}
|
|
|
|
protected Pose MinimalTranslationPoseAtSurface(in Pose userPose, Transform relativeTo)
|
|
{
|
|
Pose referencePose = GetReferencePose(relativeTo);
|
|
Vector3 desiredPos = userPose.position;
|
|
Quaternion baseRot = referencePose.rotation;
|
|
Vector3 surfacePoint = NearestPointInSurface(desiredPos, relativeTo);
|
|
Quaternion surfaceRotation = RotationAtPoint(surfacePoint, baseRot, userPose.rotation, relativeTo);
|
|
return new Pose(surfacePoint, surfaceRotation);
|
|
}
|
|
|
|
protected Quaternion RotationAtPoint(Vector3 surfacePoint,
|
|
Quaternion baseRot, Quaternion desiredRotation,
|
|
Transform relativeTo)
|
|
{
|
|
Vector3 desiredDirection = (surfacePoint - GetCentre(relativeTo)).normalized;
|
|
Quaternion targetRotation = Quaternion.FromToRotation(GetDirection(relativeTo), desiredDirection) * baseRot;
|
|
Vector3 targetProjected = Vector3.ProjectOnPlane(targetRotation * Vector3.forward, desiredDirection).normalized;
|
|
Vector3 desiredProjected = Vector3.ProjectOnPlane(desiredRotation * Vector3.forward, desiredDirection).normalized;
|
|
Quaternion rotCorrection = Quaternion.FromToRotation(targetProjected, desiredProjected);
|
|
return rotCorrection * targetRotation;
|
|
}
|
|
|
|
#region Inject
|
|
|
|
public void InjectAllSphereSurface(SphereGrabSurfaceData data, Transform relativeTo)
|
|
{
|
|
InjectData(data);
|
|
InjectRelativeTo(relativeTo);
|
|
}
|
|
|
|
public void InjectData(SphereGrabSurfaceData data)
|
|
{
|
|
_data = data;
|
|
}
|
|
|
|
public void InjectRelativeTo(Transform relativeTo)
|
|
{
|
|
_relativeTo = relativeTo;
|
|
}
|
|
#endregion
|
|
}
|
|
}
|