457 lines
18 KiB
C#
457 lines
18 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;
|
|
using UnityEngine.Serialization;
|
|
|
|
namespace Oculus.Interaction.Grab.GrabSurfaces
|
|
{
|
|
[Serializable]
|
|
public class CylinderSurfaceData : ICloneable
|
|
{
|
|
public object Clone()
|
|
{
|
|
CylinderSurfaceData clone = new CylinderSurfaceData();
|
|
clone.startPoint = this.startPoint;
|
|
clone.endPoint = this.endPoint;
|
|
clone.arcOffset = this.arcOffset;
|
|
clone.arcLength = this.arcLength;
|
|
return clone;
|
|
}
|
|
|
|
public CylinderSurfaceData Mirror()
|
|
{
|
|
CylinderSurfaceData mirror = Clone() as CylinderSurfaceData;
|
|
return mirror;
|
|
}
|
|
|
|
public Vector3 startPoint = new Vector3(0f, 0.1f, 0f);
|
|
public Vector3 endPoint = new Vector3(0f, -0.1f, 0f);
|
|
|
|
[Range(0f, 360f)]
|
|
public float arcOffset = 0f;
|
|
[Range(0f, 360f)]
|
|
[FormerlySerializedAs("angle")]
|
|
public float arcLength = 360f;
|
|
}
|
|
|
|
/// <summary>
|
|
/// This type of surface defines a cylinder in which the grip pose is valid around an object.
|
|
/// An angle and offset can be used to constrain the cylinder and not use a full circle.
|
|
/// The radius is automatically specified as the distance from the axis of the cylinder to the original grip position.
|
|
/// </summary>
|
|
[Serializable]
|
|
public class CylinderGrabSurface : MonoBehaviour, IGrabSurface
|
|
{
|
|
[SerializeField]
|
|
protected CylinderSurfaceData _data = new CylinderSurfaceData();
|
|
|
|
[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);
|
|
private const float Epsilon = 0.000001f;
|
|
/// <summary>
|
|
/// The reference pose of the surface. It defines the radius of the cylinder
|
|
/// as the point from the relative transform to the reference pose to ensure
|
|
/// that the cylinder 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>
|
|
/// Degrees from the starting radius from which the arc section starts
|
|
/// </summary>
|
|
public float ArcOffset
|
|
{
|
|
get
|
|
{
|
|
return _data.arcOffset;
|
|
}
|
|
set
|
|
{
|
|
if (value != 0 && value % 360f == 0)
|
|
{
|
|
_data.arcOffset = 360f;
|
|
}
|
|
else
|
|
{
|
|
_data.arcOffset = Mathf.Repeat(value, 360f);
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// The maximum angle for the surface of the cylinder, starting from the ArcOffset.
|
|
/// To invert the direction of the angle, swap the caps order.
|
|
/// </summary>
|
|
public float ArcLength
|
|
{
|
|
get
|
|
{
|
|
return _data.arcLength;
|
|
}
|
|
set
|
|
{
|
|
if (value != 0 && value % 360f == 0)
|
|
{
|
|
_data.arcLength = 360f;
|
|
}
|
|
else
|
|
{
|
|
_data.arcLength = Mathf.Repeat(value, 360f);
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// The direction of the main radius of the cylinder. This is the
|
|
/// radius from the center of the cylinder to the reference position.
|
|
/// </summary>
|
|
private Vector3 LocalPerpendicularDir
|
|
{
|
|
get
|
|
{
|
|
return Vector3.ProjectOnPlane(RelativePose.position - _data.startPoint, LocalDirection).normalized;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// The direction of the central axis of the cylinder in local space.
|
|
/// </summary>
|
|
private Vector3 LocalDirection
|
|
{
|
|
get
|
|
{
|
|
Vector3 dir = (_data.endPoint - _data.startPoint);
|
|
if (dir.sqrMagnitude <= Epsilon)
|
|
{
|
|
return Vector3.up;
|
|
}
|
|
return dir.normalized;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Direction from the axis of the cylinder 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 GetPerpendicularDir(Transform relativeTo)
|
|
{
|
|
return relativeTo.TransformDirection(LocalPerpendicularDir);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Direction from the axis of the cylinder to the minimum angle allowance.
|
|
/// </summary>
|
|
/// <param name="relativeTo">The reference transform to apply the surface to</param>
|
|
/// <returns>Direction in world space</returns>
|
|
public Vector3 GetStartArcDir(Transform relativeTo)
|
|
{
|
|
Vector3 localStartArcDir = Quaternion.AngleAxis(ArcOffset, LocalDirection) * LocalPerpendicularDir;
|
|
return relativeTo.TransformDirection(localStartArcDir);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Direction from the axis of the cylinder to the maximum angle allowance.
|
|
/// </summary>
|
|
/// <param name="relativeTo">The reference transform to apply the surface to</param>
|
|
/// <returns>Direction in world space</returns>
|
|
public Vector3 GetEndArcDir(Transform relativeTo)
|
|
{
|
|
Vector3 localEndArcDir = Quaternion.AngleAxis(ArcLength, LocalDirection) *
|
|
Quaternion.AngleAxis(ArcOffset, LocalDirection) * LocalPerpendicularDir;
|
|
return relativeTo.TransformDirection(localEndArcDir);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Base cap of the cylinder, in world coordinates.
|
|
/// </summary>
|
|
/// <param name="relativeTo">The reference transform to apply the surface to</param>
|
|
/// <returns>Position in world space</returns>
|
|
public Vector3 GetStartPoint(Transform relativeTo)
|
|
{
|
|
return relativeTo.TransformPoint(_data.startPoint);
|
|
}
|
|
public void SetStartPoint(Vector3 point, Transform relativeTo)
|
|
{
|
|
_data.startPoint = relativeTo.InverseTransformPoint(point);
|
|
}
|
|
|
|
/// <summary>
|
|
/// End cap of the cylinder, in world coordinates.
|
|
/// </summary>
|
|
/// <param name="relativeTo">The reference transform to apply the surface to</param>
|
|
/// <returns>Position in world space</returns>
|
|
public Vector3 GetEndPoint(Transform relativeTo)
|
|
{
|
|
return relativeTo.TransformPoint(_data.endPoint);
|
|
}
|
|
public void SetEndPoint(Vector3 point, Transform relativeTo)
|
|
{
|
|
_data.endPoint = relativeTo.InverseTransformPoint(point);
|
|
}
|
|
|
|
/// <summary>
|
|
/// The generated radius of the cylinder.
|
|
/// Represents the distance from the axis of the cylinder to the original grip position.
|
|
/// </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 start = GetStartPoint(relativeTo);
|
|
Pose referencePose = GetReferencePose(relativeTo);
|
|
Vector3 direction = GetDirection(relativeTo);
|
|
Vector3 projectedPoint = start + Vector3.Project(referencePose.position - start, direction);
|
|
return Vector3.Distance(projectedPoint, referencePose.position);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Direction of the cylinder, from the start cap to the end cap.
|
|
/// </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 relativeTo.TransformDirection(LocalDirection);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Length of the cylinder, from the start cap to the end cap.
|
|
/// </summary>
|
|
/// <param name="relativeTo">The reference transform to apply the surface to</param>
|
|
/// <returns>Distance in world space</returns>
|
|
private float GetHeight(Transform relativeTo)
|
|
{
|
|
Vector3 start = GetStartPoint(relativeTo);
|
|
Vector3 end = GetEndPoint(relativeTo);
|
|
return Vector3.Distance(start, end);
|
|
}
|
|
|
|
/// <summary>
|
|
/// The rotation of the central axis of the cylinder.
|
|
/// </summary>
|
|
/// <param name="relativeTo">The reference transform to apply the surface to</param>
|
|
/// <returns>Rotation in world space</returns>
|
|
private Quaternion GetRotation(Transform relativeTo)
|
|
{
|
|
if (_data.startPoint == _data.endPoint)
|
|
{
|
|
return relativeTo.rotation;
|
|
}
|
|
return relativeTo.rotation * Quaternion.LookRotation(LocalPerpendicularDir, LocalDirection);
|
|
}
|
|
|
|
#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(LocalPerpendicularDir, LocalDirection).normalized;
|
|
Quaternion reflectedRot = HandMirroring.Reflect(pose.rotation, mirrorPlane);
|
|
return new Pose(pose.position, reflectedRot);
|
|
}
|
|
|
|
private Vector3 PointAltitude(Vector3 point, Transform relativeTo)
|
|
{
|
|
Vector3 start = GetStartPoint(relativeTo);
|
|
Vector3 direction = GetDirection(relativeTo);
|
|
Vector3 projectedPoint = start + Vector3.Project(point - start, direction);
|
|
return projectedPoint;
|
|
}
|
|
|
|
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)
|
|
{
|
|
CylinderGrabSurface surface = gameObject.AddComponent<CylinderGrabSurface>();
|
|
surface._data = _data.Mirror();
|
|
return surface;
|
|
}
|
|
|
|
public IGrabSurface CreateDuplicatedSurface(GameObject gameObject)
|
|
{
|
|
CylinderGrabSurface surface = gameObject.AddComponent<CylinderGrabSurface>();
|
|
surface._data = _data.Clone() as CylinderSurfaceData;
|
|
return surface;
|
|
}
|
|
|
|
protected Vector3 NearestPointInSurface(Vector3 targetPosition, Transform relativeTo)
|
|
{
|
|
Vector3 start = GetStartPoint(relativeTo);
|
|
Vector3 dir = GetDirection(relativeTo);
|
|
Vector3 projectedVector = Vector3.Project(targetPosition - start, dir);
|
|
float height = GetHeight(relativeTo);
|
|
if (projectedVector.magnitude > height)
|
|
{
|
|
projectedVector = projectedVector.normalized * height;
|
|
}
|
|
if (Vector3.Dot(projectedVector, dir) < 0f)
|
|
{
|
|
projectedVector = Vector3.zero;
|
|
}
|
|
|
|
Vector3 projectedPoint = start + projectedVector;
|
|
Vector3 targetDirection = Vector3.ProjectOnPlane((targetPosition - projectedPoint), dir).normalized;
|
|
|
|
Vector3 startArcDir = GetStartArcDir(relativeTo);
|
|
float desiredAngle = Mathf.Repeat(Vector3.SignedAngle(startArcDir, targetDirection, dir), 360f);
|
|
if (desiredAngle > ArcLength)
|
|
{
|
|
if (Mathf.Abs(desiredAngle - ArcLength) >= Mathf.Abs(360f - desiredAngle))
|
|
{
|
|
targetDirection = startArcDir;
|
|
}
|
|
else
|
|
{
|
|
targetDirection = GetEndArcDir(relativeTo);
|
|
}
|
|
}
|
|
Vector3 surfacePoint = projectedPoint + targetDirection * GetRadius(relativeTo);
|
|
return surfacePoint;
|
|
}
|
|
|
|
public bool CalculateBestPoseAtSurface(Ray targetRay, out Pose bestPose,
|
|
Transform relativeTo)
|
|
{
|
|
Pose recordedPose = GetReferencePose(relativeTo);
|
|
Vector3 start = GetStartPoint(relativeTo);
|
|
Vector3 direction = GetDirection(relativeTo);
|
|
Vector3 lineToCylinder = start - targetRay.origin;
|
|
|
|
float perpendiculiarity = Vector3.Dot(targetRay.direction, direction);
|
|
float rayToLineDiff = Vector3.Dot(lineToCylinder, targetRay.direction);
|
|
float cylinderToLineDiff = Vector3.Dot(lineToCylinder, direction);
|
|
|
|
float determinant = 1f / (perpendiculiarity * perpendiculiarity - 1f);
|
|
|
|
float lineOffset = (perpendiculiarity * cylinderToLineDiff - rayToLineDiff) * determinant;
|
|
float cylinderOffset = (cylinderToLineDiff - perpendiculiarity * rayToLineDiff) * determinant;
|
|
|
|
float radius = GetRadius(relativeTo);
|
|
Vector3 pointInLine = targetRay.origin + targetRay.direction * lineOffset;
|
|
Vector3 pointInCylinder = start + direction * cylinderOffset;
|
|
float distanceToSurface = Mathf.Max(Vector3.Distance(pointInCylinder, pointInLine) - radius);
|
|
if (distanceToSurface < radius)
|
|
{
|
|
float adjustedDistance = Mathf.Sqrt(radius * radius - distanceToSurface * distanceToSurface);
|
|
pointInLine -= targetRay.direction * adjustedDistance;
|
|
}
|
|
Vector3 surfacePoint = NearestPointInSurface(pointInLine, relativeTo);
|
|
Pose desiredPose = new Pose(surfacePoint, recordedPose.rotation);
|
|
bestPose = MinimalTranslationPoseAtSurface(desiredPose, relativeTo);
|
|
|
|
return true;
|
|
}
|
|
|
|
protected Pose MinimalRotationPoseAtSurface(in Pose userPose, Transform relativeTo)
|
|
{
|
|
Pose referencePose = GetReferencePose(relativeTo);
|
|
Vector3 direction = GetDirection(relativeTo);
|
|
Quaternion rotation = GetRotation(relativeTo);
|
|
float radius = GetRadius(relativeTo);
|
|
Vector3 desiredPos = userPose.position;
|
|
Quaternion desiredRot = userPose.rotation;
|
|
Quaternion baseRot = referencePose.rotation;
|
|
Quaternion rotDif = (desiredRot) * Quaternion.Inverse(baseRot);
|
|
Vector3 desiredDirection = (rotDif * rotation) * Vector3.forward;
|
|
Vector3 projectedDirection = Vector3.ProjectOnPlane(desiredDirection, direction).normalized;
|
|
Vector3 altitudePoint = PointAltitude(desiredPos, relativeTo);
|
|
Vector3 surfacePoint = NearestPointInSurface(altitudePoint + projectedDirection * radius, relativeTo);
|
|
Quaternion surfaceRotation = CalculateRotationOffset(surfacePoint, relativeTo) * baseRot;
|
|
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 = CalculateRotationOffset(surfacePoint, relativeTo) * baseRot;
|
|
|
|
return new Pose(surfacePoint, surfaceRotation);
|
|
}
|
|
|
|
protected Quaternion CalculateRotationOffset(Vector3 surfacePoint, Transform relativeTo)
|
|
{
|
|
Vector3 start = GetStartPoint(relativeTo);
|
|
Vector3 direction = GetDirection(relativeTo);
|
|
Vector3 referenceDir = GetPerpendicularDir(relativeTo);
|
|
Vector3 recordedDirection = Vector3.ProjectOnPlane(referenceDir, direction);
|
|
Vector3 desiredDirection = Vector3.ProjectOnPlane(surfacePoint - start, direction);
|
|
return Quaternion.FromToRotation(recordedDirection, desiredDirection);
|
|
}
|
|
|
|
#region Inject
|
|
|
|
public void InjectAllCylinderSurface(CylinderSurfaceData data,
|
|
Transform relativeTo)
|
|
{
|
|
InjectData(data);
|
|
InjectRelativeTo(relativeTo);
|
|
}
|
|
|
|
public void InjectData(CylinderSurfaceData data)
|
|
{
|
|
_data = data;
|
|
}
|
|
|
|
public void InjectRelativeTo(Transform relativeTo)
|
|
{
|
|
_relativeTo = relativeTo;
|
|
}
|
|
#endregion
|
|
}
|
|
}
|