296 lines
13 KiB
C#
296 lines
13 KiB
C#
|
/************************************************************************************
|
||
|
|
Copyright : Copyright (c) Facebook Technologies, LLC and its affiliates. All rights reserved.
|
||
|
|
|
||
|
|
Your use of this SDK or tool is subject to the Oculus SDK License Agreement, available at
|
||
|
|
https://developer.oculus.com/licenses/oculussdk/
|
||
|
|
|
||
|
|
Unless required by applicable law or agreed to in writing, the Utilities 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 UnityEngine;
|
||
|
|
|
||
|
|
namespace Oculus.Interaction
|
||
|
|
{
|
||
|
|
/// <summary>
|
||
|
|
/// Tools for working with Unity Poses
|
||
|
|
/// </summary>
|
||
|
|
public static class PoseUtils
|
||
|
|
{
|
||
|
|
/// <summary>
|
||
|
|
/// Assigns a Pose to a given transform.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="transform"> The transform to which apply the pose.</param>
|
||
|
|
/// <param name="pose">The desired pose.</param>
|
||
|
|
/// <param name="space">If the pose should be applied to the local position/rotation or world position/rotation.</param>
|
||
|
|
public static void SetPose(this Transform transform, in Pose pose, Space space = Space.World)
|
||
|
|
{
|
||
|
|
if (space == Space.World)
|
||
|
|
{
|
||
|
|
transform.SetPositionAndRotation(pose.position, pose.rotation);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
transform.localRotation = pose.rotation;
|
||
|
|
transform.localPosition = pose.position;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Extract the position/rotation of a given transform.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="transform">The transform from which to extract the pose.</param>
|
||
|
|
/// <param name="space">If the desired position/rotation is the world or local one.</param>
|
||
|
|
/// <returns>A Pose containing the position/rotation of the transform.</returns>
|
||
|
|
public static Pose GetPose(this Transform transform, Space space = Space.World)
|
||
|
|
{
|
||
|
|
if (space == Space.World)
|
||
|
|
{
|
||
|
|
return new Pose(transform.position, transform.rotation);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
return new Pose(transform.localPosition, transform.localRotation);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Compose two poses, applying the first to the second one.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="a">First pose to compose.</param>
|
||
|
|
/// <param name="b">Pose to compose over the first one.</param>
|
||
|
|
/// <param name="result">A Pose with the two operands applied.</param>
|
||
|
|
public static void Multiply(in Pose a, in Pose b, ref Pose result)
|
||
|
|
{
|
||
|
|
result.position = a.position + a.rotation * b.position;
|
||
|
|
result.rotation = a.rotation * b.rotation;
|
||
|
|
}
|
||
|
|
|
||
|
|
public static Pose Multiply(in Pose a, in Pose b)
|
||
|
|
{
|
||
|
|
Pose result = new Pose();
|
||
|
|
Multiply(a, b, ref result);
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Compose two poses, applying the provided one on top of the caller.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="a">Pose to compose upon.</param>
|
||
|
|
/// <param name="b">Pose to compose over the first one.</param>
|
||
|
|
public static void Premultiply(this ref Pose a, in Pose b)
|
||
|
|
{
|
||
|
|
Multiply(a, b, ref a);
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Compose two poses, applying the caller on top of the provided pose.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="a">Pose to compose upon.</param>
|
||
|
|
/// <param name="b">Pose to compose over the first one.</param>
|
||
|
|
public static void Postmultiply(this ref Pose a, in Pose b)
|
||
|
|
{
|
||
|
|
Multiply(b, a, ref a);
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Moves the calling pose towards a target one using interpolation
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="from">Original pose to interpolate from</param>
|
||
|
|
/// <param name="to">Target pose for the interpolation.</param>
|
||
|
|
/// <param name="t">Interpolation factor, normalized but will not be clamped.</param>
|
||
|
|
public static void Lerp(this ref Pose from, in Pose to, float t)
|
||
|
|
{
|
||
|
|
Lerp(from, to, t, ref from);
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Interpolation between two poses.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="from">From pose.</param>
|
||
|
|
/// <param name="to">To pose.</param>
|
||
|
|
/// <param name="t">Interpolation factor, normalized but will not be clamped.</param>
|
||
|
|
/// <param name="result">A Pose between a and b</param>
|
||
|
|
public static void Lerp(in Pose from, in Pose to, float t, ref Pose result)
|
||
|
|
{
|
||
|
|
result.position = Vector3.LerpUnclamped(from.position, to.position, t);
|
||
|
|
result.rotation = Quaternion.SlerpUnclamped(from.rotation, to.rotation, t);
|
||
|
|
}
|
||
|
|
|
||
|
|
public static void Inverse(in Pose a, ref Pose result)
|
||
|
|
{
|
||
|
|
result.rotation = Quaternion.Inverse(a.rotation);
|
||
|
|
result.position = result.rotation * -a.position;
|
||
|
|
}
|
||
|
|
|
||
|
|
public static void Invert(this ref Pose a)
|
||
|
|
{
|
||
|
|
Inverse(a, ref a);
|
||
|
|
}
|
||
|
|
|
||
|
|
public static void CopyFrom(this ref Pose to, in Pose from)
|
||
|
|
{
|
||
|
|
to.position = from.position;
|
||
|
|
to.rotation = from.rotation;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Get the position/rotation difference between two transforms.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="to">The base transform.</param>
|
||
|
|
/// <param name="from">The target transform.</param>
|
||
|
|
/// <returns>A Pose indicating the position/rotation change</returns>
|
||
|
|
public static Pose RelativeOffset(this Transform to, Transform from)
|
||
|
|
{
|
||
|
|
return RelativeOffset(from.position, from.rotation, to.position, to.rotation);
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Get the position/rotation difference between a transform and a pose.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="to">The base transform.</param>
|
||
|
|
/// <param name="from">The target pose.</param>
|
||
|
|
/// <returns>A Pose indicating the offset.</returns>
|
||
|
|
public static Pose RelativeOffset(this Transform to, in Pose from)
|
||
|
|
{
|
||
|
|
return RelativeOffset(from.position, from.rotation, to.position, to.rotation);
|
||
|
|
}
|
||
|
|
|
||
|
|
public static void RelativeOffset(this Transform to, in Pose from, ref Pose result)
|
||
|
|
{
|
||
|
|
RelativeOffset(from.position, from.rotation, to.position, to.rotation, ref result);
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Get the position/rotation difference between two poses.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="from">The base pose.</param>
|
||
|
|
/// <param name="to">The target pose.</param>
|
||
|
|
/// <returns>A Pose indicating the offset.</returns>
|
||
|
|
public static Pose RelativeOffset(in Pose from, in Pose to)
|
||
|
|
{
|
||
|
|
return RelativeOffset(from.position, from.rotation, to.position, to.rotation);
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Get the position/rotation difference between two poses.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="from">The base pose.</param>
|
||
|
|
/// <param name="to">The target pose.</param>
|
||
|
|
/// <param name="result">>A Pose indicating the offset.</param>
|
||
|
|
public static void RelativeOffset(in Pose from, in Pose to, ref Pose result)
|
||
|
|
{
|
||
|
|
RelativeOffset(from.position, from.rotation, to.position, to.rotation, ref result);
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Get the position/rotation difference between two poses, indicated with separated positions and rotations.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="fromPosition">The base position.</param>
|
||
|
|
/// <param name="fromRotation">The base rotation.</param>
|
||
|
|
/// <param name="toPosition">The target position.</param>
|
||
|
|
/// <param name="toRotation">The target rotation.</param>
|
||
|
|
/// <returns>A Pose indicating the offset.</returns>
|
||
|
|
public static Pose RelativeOffset(Vector3 fromPosition, Quaternion fromRotation, Vector3 toPosition, Quaternion toRotation)
|
||
|
|
{
|
||
|
|
Pose result = new Pose();
|
||
|
|
RelativeOffset(fromPosition, fromRotation, toPosition, toRotation, ref result);
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
public static void RelativeOffset(Vector3 fromPosition, Quaternion fromRotation, Vector3 toPosition, Quaternion toRotation, ref Pose result)
|
||
|
|
{
|
||
|
|
Quaternion inverseTo = Quaternion.Inverse(toRotation);
|
||
|
|
result.position = inverseTo * (fromPosition - toPosition);
|
||
|
|
result.rotation = inverseTo * fromRotation;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Get the world position/rotation of a relative position.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="reference">The transform in which the offset is local.</param>
|
||
|
|
/// <param name="offset">The offset from the reference.</param>
|
||
|
|
/// <returns>A Pose in world units.</returns>
|
||
|
|
public static Pose GlobalPose(this Transform reference, in Pose offset)
|
||
|
|
{
|
||
|
|
return new Pose(
|
||
|
|
reference.position + reference.rotation * offset.position,
|
||
|
|
reference.rotation * offset.rotation);
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Indicates how similar two poses are.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="from">First pose to compare.</param>
|
||
|
|
/// <param name="to">Second pose to compare.</param>
|
||
|
|
/// <param name="maxDistance">The max distance in which the poses can be similar.</param>
|
||
|
|
/// <returns>0 indicates no similitude, 1 for equal poses</returns>
|
||
|
|
public static float Similarity(in Pose from, in Pose to, HandPosing.PoseMeasureParameters scoringModifier)
|
||
|
|
{
|
||
|
|
float rotationDifference = RotationalSimilarity(from.rotation, to.rotation);
|
||
|
|
float positionDifference = PositionalSimilarity(from.position, to.position, scoringModifier.MaxDistance);
|
||
|
|
return positionDifference * (1f - scoringModifier.PositionRotationWeight)
|
||
|
|
+ rotationDifference * (scoringModifier.PositionRotationWeight);
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Get how similar two positions are.
|
||
|
|
/// It uses a maximum value to normalize the output
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="from">The first position.</param>
|
||
|
|
/// <param name="to">The second position.</param>
|
||
|
|
/// <param name="maxDistance">The Maximum distance used to normalise the output</param>
|
||
|
|
/// <returns>0 when the input positions are further than maxDistance, 1 for equal positions.</returns>
|
||
|
|
public static float PositionalSimilarity(in Vector3 from, in Vector3 to, float maxDistance)
|
||
|
|
{
|
||
|
|
float distance = Vector3.Distance(from, to);
|
||
|
|
if (distance == 0)
|
||
|
|
{
|
||
|
|
return 1f;
|
||
|
|
}
|
||
|
|
return 1f - Mathf.Clamp01(distance / maxDistance);
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Get how similar two rotations are.
|
||
|
|
/// Since the Quaternion.Dot is bugged in unity. We compare the
|
||
|
|
/// dot products of the forward and up vectors of the rotations.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="from">The first rotation.</param>
|
||
|
|
/// <param name="to">The second rotation.</param>
|
||
|
|
/// <returns>0 for opposite rotations, 1 for equal rotations.</returns>
|
||
|
|
public static float RotationalSimilarity(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;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Rotate a pose around an axis.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="pose">The pose to mirror.</param>
|
||
|
|
/// <param name="normal">The direction of the mirror.</param>
|
||
|
|
/// <param name="tangent">The tangent of the mirror.</param>
|
||
|
|
/// <returns>A mirrored pose.</returns>
|
||
|
|
public static Pose MirrorPoseRotation(this in Pose pose, Vector3 normal, Vector3 tangent)
|
||
|
|
{
|
||
|
|
Pose mirrorPose = pose;
|
||
|
|
Vector3 forward = pose.rotation * Vector3.forward;
|
||
|
|
Vector3 projectedForward = Vector3.ProjectOnPlane(forward, normal);
|
||
|
|
float angleForward = Vector3.SignedAngle(projectedForward, tangent, normal);
|
||
|
|
Vector3 mirrorForward = Quaternion.AngleAxis(2 * angleForward, normal) * forward;
|
||
|
|
|
||
|
|
Vector3 up = pose.rotation * Vector3.up;
|
||
|
|
Vector3 projectedUp = Vector3.ProjectOnPlane(up, normal);
|
||
|
|
float angleUp = Vector3.SignedAngle(projectedUp, tangent, normal);
|
||
|
|
Vector3 mirrorUp = Quaternion.AngleAxis(2 * angleUp, normal) * up;
|
||
|
|
|
||
|
|
mirrorPose.rotation = Quaternion.LookRotation(mirrorForward, mirrorUp);
|
||
|
|
return mirrorPose;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|