380 lines
14 KiB
C#
380 lines
14 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 System;
|
|
using System.Collections.Generic;
|
|
using UnityEngine;
|
|
using UnityEngine.Serialization;
|
|
|
|
namespace Oculus.Interaction.Grab.GrabSurfaces
|
|
{
|
|
/// <summary>
|
|
/// Defines a series of points that specify paths along with the snapping pose is valid.
|
|
/// The points can be connected to the previous and/or next one, even looping, or completely isolated.
|
|
/// When two or more consecutive points are connected, they define a continuous path. The Tangent can be
|
|
/// used to specify the curve between two points. It is also possible to connect the last point with the
|
|
/// first one to create a closed loop. The rotation at a point in the path is specified by interpolating
|
|
/// the rotation of the starting and ending point of the current segment.
|
|
/// When a point is not connected to any other point, it is considered as a single point in space.
|
|
/// </summary>
|
|
[Serializable]
|
|
public class BezierGrabSurface : MonoBehaviour, IGrabSurface
|
|
{
|
|
[SerializeField]
|
|
private List<BezierControlPoint> _controlPoints = new List<BezierControlPoint>();
|
|
|
|
[SerializeField]
|
|
[Tooltip("Transform used as a reference to measure the local data of the grab surface")]
|
|
private Transform _relativeTo;
|
|
|
|
public List<BezierControlPoint> ControlPoints => _controlPoints;
|
|
|
|
private const float MAX_PLANE_DOT = 0.95f;
|
|
|
|
#region editor events
|
|
protected virtual void Reset()
|
|
{
|
|
_relativeTo = this.GetComponentInParent<IRelativeToRef>()?.RelativeTo;
|
|
}
|
|
#endregion
|
|
|
|
protected virtual void Start()
|
|
{
|
|
this.AssertCollectionField(ControlPoints, nameof(ControlPoints));
|
|
this.AssertField(_relativeTo, nameof(_relativeTo));
|
|
}
|
|
|
|
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)
|
|
{
|
|
Pose testPose = Pose.identity;
|
|
Pose smallestRotationPose = Pose.identity;
|
|
bestPose = targetPose;
|
|
GrabPoseScore bestScore = GrabPoseScore.Max;
|
|
for (int i = 0; i < _controlPoints.Count; i++)
|
|
{
|
|
BezierControlPoint currentControlPoint = _controlPoints[i];
|
|
BezierControlPoint nextControlPoint = _controlPoints[(i + 1) % _controlPoints.Count];
|
|
|
|
if (!currentControlPoint.Disconnected
|
|
&& nextControlPoint.Disconnected)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
GrabPoseScore score;
|
|
if ((currentControlPoint.Disconnected && nextControlPoint.Disconnected)
|
|
|| _controlPoints.Count == 1)
|
|
{
|
|
Pose worldPose = currentControlPoint.GetPose(relativeTo);
|
|
testPose.CopyFrom(worldPose);
|
|
score = new GrabPoseScore(targetPose, testPose, offset, scoringModifier);
|
|
}
|
|
else
|
|
{
|
|
Pose start = currentControlPoint.GetPose(relativeTo);
|
|
Pose end = nextControlPoint.GetPose(relativeTo);
|
|
Vector3 tangent = currentControlPoint.GetTangent(relativeTo);
|
|
|
|
NearestPointInTriangle(targetPose.position, start.position, tangent, end.position, out float positionT);
|
|
float rotationT = ProgressForRotation(targetPose.rotation, start.rotation, end.rotation);
|
|
|
|
score = GrabPoseHelper.CalculateBestPoseAtSurface(targetPose, offset, out testPose, scoringModifier, relativeTo,
|
|
(in Pose target, Transform relativeTo) =>
|
|
{
|
|
Pose result;
|
|
result.position = EvaluateBezier(start.position, tangent, end.position, positionT);
|
|
result.rotation = Quaternion.Slerp(start.rotation, end.rotation, positionT);
|
|
return result;
|
|
|
|
},
|
|
(in Pose target, Transform relativeTo) =>
|
|
{
|
|
Pose result;
|
|
result.position = EvaluateBezier(start.position, tangent, end.position, rotationT);
|
|
result.rotation = Quaternion.Slerp(start.rotation, end.rotation, rotationT);
|
|
return result;
|
|
});
|
|
}
|
|
|
|
if (score.IsBetterThan(bestScore))
|
|
{
|
|
bestScore = score;
|
|
bestPose.CopyFrom(testPose);
|
|
}
|
|
}
|
|
return bestScore;
|
|
}
|
|
|
|
public bool CalculateBestPoseAtSurface(Ray targetRay, out Pose bestPose, Transform relativeTo)
|
|
{
|
|
Pose testPose = Pose.identity;
|
|
Pose targetPose = Pose.identity;
|
|
bestPose = Pose.identity;
|
|
bool poseFound = false;
|
|
GrabPoseScore bestScore = GrabPoseScore.Max;
|
|
for (int i = 0; i < _controlPoints.Count; i++)
|
|
{
|
|
BezierControlPoint currentControlPoint = _controlPoints[i];
|
|
BezierControlPoint nextControlPoint = _controlPoints[(i + 1) % _controlPoints.Count];
|
|
|
|
if (!currentControlPoint.Disconnected
|
|
&& nextControlPoint.Disconnected)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if ((currentControlPoint.Disconnected && nextControlPoint.Disconnected)
|
|
|| _controlPoints.Count == 1)
|
|
{
|
|
Pose worldPose = currentControlPoint.GetPose(relativeTo);
|
|
Plane plane = new Plane(-targetRay.direction, worldPose.position);
|
|
if (!plane.Raycast(targetRay, out float enter))
|
|
{
|
|
continue;
|
|
}
|
|
targetPose.position = targetRay.GetPoint(enter);
|
|
testPose.CopyFrom(worldPose);
|
|
}
|
|
else
|
|
{
|
|
Pose start = currentControlPoint.GetPose(relativeTo);
|
|
Pose end = nextControlPoint.GetPose(relativeTo);
|
|
Vector3 tangent = currentControlPoint.GetTangent(relativeTo);
|
|
Plane plane = GenerateRaycastPlane(start.position, tangent, end.position, -targetRay.direction);
|
|
if (!plane.Raycast(targetRay, out float enter))
|
|
{
|
|
continue;
|
|
}
|
|
targetPose.position = targetRay.GetPoint(enter);
|
|
NearestPointInTriangle(targetPose.position, start.position, tangent, end.position, out float t);
|
|
testPose.position = EvaluateBezier(start.position, tangent, end.position, t);
|
|
testPose.rotation = Quaternion.Slerp(start.rotation, end.rotation, t);
|
|
}
|
|
|
|
GrabPoseScore score = new GrabPoseScore(targetPose.position, testPose.position);
|
|
if (score.IsBetterThan(bestScore))
|
|
{
|
|
bestScore = score;
|
|
bestPose.CopyFrom(testPose);
|
|
poseFound = true;
|
|
}
|
|
}
|
|
return poseFound;
|
|
}
|
|
|
|
private Plane GenerateRaycastPlane(Vector3 p0, Vector3 p1, Vector3 p2, Vector3 fallbackDir)
|
|
{
|
|
Vector3 line0 = (p1 - p0).normalized;
|
|
Vector3 line1 = (p2 - p0).normalized;
|
|
|
|
Plane plane;
|
|
if (Mathf.Abs(Vector3.Dot(line0, line1)) > MAX_PLANE_DOT)
|
|
{
|
|
plane = new Plane(fallbackDir, (p0 + p2 + p1) / 3f);
|
|
}
|
|
else
|
|
{
|
|
plane = new Plane(p0, p1, p2);
|
|
}
|
|
return plane;
|
|
}
|
|
|
|
private float ProgressForRotation(Quaternion targetRotation, Quaternion from, Quaternion to)
|
|
{
|
|
Vector3 targetForward = targetRotation * Vector3.forward;
|
|
Vector3 fromForward = from * Vector3.forward;
|
|
Vector3 toForward = to * Vector3.forward;
|
|
Vector3 axis = Vector3.Cross(fromForward, toForward).normalized;
|
|
|
|
float angleFrom = Vector3.SignedAngle(targetForward, fromForward, axis);
|
|
float angleTo = Vector3.SignedAngle(targetForward, toForward, axis);
|
|
|
|
if (angleFrom < 0 && angleTo < 0)
|
|
{
|
|
return 1f;
|
|
}
|
|
if (angleFrom > 0 && angleTo > 0)
|
|
{
|
|
return 0f;
|
|
}
|
|
return Mathf.Abs(angleFrom) / Vector3.Angle(fromForward, toForward);
|
|
}
|
|
|
|
private Vector3 NearestPointInTriangle(Vector3 point, Vector3 p0, Vector3 p1, Vector3 p2, out float t)
|
|
{
|
|
Vector3 centroid = (p0 + p1 + p2) / 3f;
|
|
|
|
Vector3 pointInMedian0 = NearestPointToSegment(point, p0, centroid, out float t0);
|
|
Vector3 pointInMedian1 = NearestPointToSegment(point, centroid, p2, out float t1);
|
|
|
|
float median0 = Vector3.Distance(p0, centroid);
|
|
float median2 = Vector3.Distance(p2, centroid);
|
|
float alpha = median2 / (median0 + median2);
|
|
|
|
float distance0 = (pointInMedian0 - point).sqrMagnitude;
|
|
float distance1 = (pointInMedian1 - point).sqrMagnitude;
|
|
if (distance0 < distance1)
|
|
{
|
|
t = t0 * alpha;
|
|
return pointInMedian0;
|
|
}
|
|
else
|
|
{
|
|
t = alpha + t1 * (1f - alpha);
|
|
return pointInMedian1;
|
|
}
|
|
}
|
|
|
|
private Vector3 NearestPointToSegment(Vector3 point, Vector3 start, Vector3 end, out float progress)
|
|
{
|
|
Vector3 segment = end - start;
|
|
Vector3 projection = Vector3.Project(point - start, segment.normalized);
|
|
Vector3 pointInSegment;
|
|
if (Vector3.Dot(segment, projection) <= 0)
|
|
{
|
|
pointInSegment = start;
|
|
progress = 0;
|
|
}
|
|
else if (projection.sqrMagnitude >= segment.sqrMagnitude)
|
|
{
|
|
pointInSegment = end;
|
|
progress = 1;
|
|
}
|
|
else
|
|
{
|
|
pointInSegment = start + projection;
|
|
progress = projection.magnitude / segment.magnitude;
|
|
}
|
|
|
|
return pointInSegment;
|
|
}
|
|
|
|
public IGrabSurface CreateDuplicatedSurface(GameObject gameObject)
|
|
{
|
|
BezierGrabSurface surface = gameObject.AddComponent<BezierGrabSurface>();
|
|
surface._controlPoints = new List<BezierControlPoint>(_controlPoints);
|
|
return surface;
|
|
}
|
|
|
|
public IGrabSurface CreateMirroredSurface(GameObject gameObject)
|
|
{
|
|
BezierGrabSurface mirroredSurface = gameObject.AddComponent<BezierGrabSurface>();
|
|
mirroredSurface._controlPoints = new List<BezierControlPoint>();
|
|
foreach (BezierControlPoint controlPoint in _controlPoints)
|
|
{
|
|
Pose pose = controlPoint.GetPose(_relativeTo);
|
|
pose.rotation = pose.rotation * Quaternion.Euler(180f, 180f, 0f);
|
|
controlPoint.SetPose(pose, _relativeTo);
|
|
mirroredSurface._controlPoints.Add(controlPoint);
|
|
|
|
}
|
|
return mirroredSurface;
|
|
}
|
|
|
|
public Pose MirrorPose(in Pose gripPose, Transform relativeTo)
|
|
{
|
|
return gripPose;
|
|
}
|
|
|
|
public static Vector3 EvaluateBezier(Vector3 start, Vector3 middle, Vector3 end, float t)
|
|
{
|
|
t = Mathf.Clamp01(t);
|
|
float oneMinusT = 1f - t;
|
|
return (oneMinusT * oneMinusT * start)
|
|
+ (2f * oneMinusT * t * middle)
|
|
+ (t * t * end);
|
|
}
|
|
|
|
#region Inject
|
|
public void InjectAllBezierSurface(List<BezierControlPoint> controlPoints,
|
|
Transform relativeTo)
|
|
{
|
|
InjectControlPoints(controlPoints);
|
|
InjectRelativeTo(relativeTo);
|
|
}
|
|
|
|
public void InjectControlPoints(List<BezierControlPoint> controlPoints)
|
|
{
|
|
_controlPoints = controlPoints;
|
|
}
|
|
|
|
public void InjectRelativeTo(Transform relativeTo)
|
|
{
|
|
_relativeTo = relativeTo;
|
|
}
|
|
#endregion
|
|
}
|
|
|
|
[Serializable]
|
|
public struct BezierControlPoint
|
|
{
|
|
[SerializeField]
|
|
[FormerlySerializedAs("pose")]
|
|
private Pose _pose;
|
|
[SerializeField]
|
|
[FormerlySerializedAs("tangentPoint")]
|
|
private Vector3 _tangentPoint;
|
|
[SerializeField]
|
|
[FormerlySerializedAs("disconnected")]
|
|
private bool _disconnected;
|
|
|
|
public bool Disconnected
|
|
{
|
|
get
|
|
{
|
|
return _disconnected;
|
|
}
|
|
set
|
|
{
|
|
_disconnected = value;
|
|
}
|
|
}
|
|
|
|
public Pose GetPose(Transform relativeTo)
|
|
{
|
|
return PoseUtils.GlobalPoseScaled(relativeTo, _pose);
|
|
}
|
|
|
|
public void SetPose(in Pose worldSpacePose, Transform relativeTo)
|
|
{
|
|
_pose = PoseUtils.DeltaScaled(relativeTo, worldSpacePose);
|
|
}
|
|
|
|
public Vector3 GetTangent(Transform relativeTo)
|
|
{
|
|
return relativeTo.TransformPoint(_tangentPoint);
|
|
}
|
|
|
|
public void SetTangent(in Vector3 tangent, Transform relativeTo)
|
|
{
|
|
_tangentPoint = relativeTo.InverseTransformPoint(tangent);
|
|
}
|
|
|
|
}
|
|
}
|