Ported extension changes from pointer offset

XRTK-Core/Packages/com.xrtk.core/Definitions/BoundarySystem/InscribedRectangle.cs

@@ -3,6 +3,7 @@
 
 using System;
 using UnityEngine;
+using XRTK.Extensions;
 using XRTK.Utilities;
 using Random = System.Random;
 
@@ -265,15 +266,15 @@ private bool FindSurroundingCollisionPoints(
             // Find the top and bottom collision points by creating a large line segment that goes through the point to MAX and MIN values on Y
             var topEndpoint = new Vector2(point.x, largeValue);
             var bottomEndpoint = new Vector2(point.x, smallValue);
-            topEndpoint = RotatePoint(topEndpoint, point, angleRadians);
-            bottomEndpoint = RotatePoint(bottomEndpoint, point, angleRadians);
+            topEndpoint = topEndpoint.RotateAroundPoint(point, angleRadians);
+            bottomEndpoint = bottomEndpoint.RotateAroundPoint(point, angleRadians);
             var verticalLine = new Edge(topEndpoint, bottomEndpoint);
 
             // Find the left and right collision points by creating a large line segment that goes through the point to MAX and Min values on X
             var rightEndpoint = new Vector2(largeValue, point.y);
             var leftEndpoint = new Vector2(smallValue, point.y);
-            rightEndpoint = RotatePoint(rightEndpoint, point, angleRadians);
-            leftEndpoint = RotatePoint(leftEndpoint, point, angleRadians);
+            rightEndpoint = rightEndpoint.RotateAroundPoint(point, angleRadians);
+            leftEndpoint = leftEndpoint.RotateAroundPoint(point, angleRadians);
             var horizontalLine = new Edge(rightEndpoint, leftEndpoint);
 
             for (int i = 0; i < geometryEdges.Length; i++)
@@ -284,7 +285,7 @@ private bool FindSurroundingCollisionPoints(
                 if (EdgeUtilities.IsValidPoint(verticalIntersectionPoint))
                 {
                     // Is the intersection above or below the point?
-                    if (RotatePoint(verticalIntersectionPoint, point, -angleRadians).y > point.y)
+                    if (verticalIntersectionPoint.RotateAroundPoint(point, -angleRadians).y > point.y)
                     {
                         // Update the top collision point
                         if (!EdgeUtilities.IsValidPoint(topCollisionPoint) ||
@@ -310,7 +311,7 @@ private bool FindSurroundingCollisionPoints(
                 if (!EdgeUtilities.IsValidPoint(horizontalIntersection)) { continue; }
 
                 // Is this intersection to the left or the right of the point?
-                if (RotatePoint(horizontalIntersection, point, -angleRadians).x < point.x)
+                if (horizontalIntersection.RotateAroundPoint(point, -angleRadians).x < point.x)
                 {
                     // Update the left collision point
                     if (!EdgeUtilities.IsValidPoint(leftCollisionPoint) ||
@@ -353,49 +354,14 @@ public bool IsInsideBoundary(Vector2 point)
             }
 
             point -= Center;
-            point = RotatePoint(point, Vector2.zero, MathUtilities.DegreesToRadians(-Angle));
+            point = point.RotatePoint(MathUtilities.DegreesToRadians(-Angle));
 
-            bool inWidth = Mathf.Abs(point.x) <= (Width * 0.5f);
-            bool inHeight = Mathf.Abs(point.y) <= (Height * 0.5f);
+            var inWidth = Mathf.Abs(point.x) <= (Width * 0.5f);
+            var inHeight = Mathf.Abs(point.y) <= (Height * 0.5f);
 
             return (inWidth && inHeight);
         }
 
-        /// <summary>
-        /// Rotate a two dimensional point about another point by the specified angle.
-        /// </summary>
-        /// <param name="point">The point to be rotated.</param>
-        /// <param name="origin">The point about which the rotation is to occur.</param>
-        /// <param name="angleRadians">The angle for the rotation, in radians</param>
-        /// <returns>
-        /// The coordinates of the rotated point.
-        /// </returns>
-        private Vector2 RotatePoint(Vector2 point, Vector2 origin, float angleRadians)
-        {
-            if (angleRadians.Equals(0f))
-            {
-                return point;
-            }
-
-            Vector2 rotated = point;
-
-            // Translate to origin of rotation
-            rotated.x -= origin.x;
-            rotated.y -= origin.y;
-
-            // Rotate the point
-            float sin = Mathf.Sin(angleRadians);
-            float cos = Mathf.Cos(angleRadians);
-            float x = rotated.x * cos - rotated.y * sin;
-            float y = rotated.x * sin + rotated.y * cos;
-
-            // Translate back and return
-            rotated.x = x + origin.x;
-            rotated.y = y + origin.y;
-
-            return rotated;
-        }
-
         /// <summary>
         /// Check to see if a rectangle centered at the specified point and oriented at 
         /// the specified angle will fit within the geometry.
@@ -418,10 +384,10 @@ private bool CheckRectangleFit(Edge[] geometryEdges, Vector2 centerPoint, float
             var bottomRight = new Vector2(centerPoint.x + halfWidth, centerPoint.y - halfHeight);
 
             // Rotate the rectangle.
-            topLeft = RotatePoint(topLeft, centerPoint, angleRadians);
-            topRight = RotatePoint(topRight, centerPoint, angleRadians);
-            bottomLeft = RotatePoint(bottomLeft, centerPoint, angleRadians);
-            bottomRight = RotatePoint(bottomRight, centerPoint, angleRadians);
+            topLeft = topLeft.RotateAroundPoint(centerPoint, angleRadians);
+            topRight = topRight.RotateAroundPoint(centerPoint, angleRadians);
+            bottomLeft = bottomLeft.RotateAroundPoint(centerPoint, angleRadians);
+            bottomRight = bottomRight.RotateAroundPoint(centerPoint, angleRadians);
 
             // Get the rectangle edges.
             var topEdge = new Edge(topLeft, topRight);
@@ -551,4 +517,4 @@ private bool TryFixMaximumRectangle(
             return (aspectRatio > 0.0f);
         }
     }
-}
+}

XRTK-Core/Packages/com.xrtk.core/Extensions/BoundsExtensions.cs

@@ -3,11 +3,13 @@
 
 using System.Collections.Generic;
 using UnityEngine;
+using XRTK.Definitions.Utilities;
+using XRTK.Utilities;
 
 namespace XRTK.Extensions
 {
     /// <summary>
-    /// Extension methods for Unity's Bounds struct
+    /// Extension methods for Unity's <see cref="Bounds"/>
     /// </summary>
     public static class BoundsExtensions
     {
@@ -59,13 +61,6 @@ public static class BoundsExtensions
         public const int FWD = 4;
         public const int BCK = 5;
 
-        public enum Axis
-        {
-            X,
-            Y,
-            Z
-        }
-
         private static Vector3[] corners = null;
 
         private static readonly Vector3[] rectTransformCorners = new Vector3[4];
@@ -91,16 +86,12 @@ public static bool IsValid(this Bounds bounds)
         }
 
         /// <summary>
-        /// Gets all the corner points of the bounds in world space
+        /// Gets all the corner points of the bounds in world space.
         /// </summary>
         /// <param name="transform"></param>
         /// <param name="positions"></param>
         /// <param name="bounds"></param>
-        /// <remarks>
-        /// Use BoxColliderExtensions.{Left|Right}{Bottom|Top}{Front|Back} constants to index into the output
-        /// corners array.
-        /// </remarks>
-        public static void GetCornerPositions(this Bounds bounds, Transform transform, ref Vector3[] positions)
+        public static void GetCornerPositionsWorldSpace(this Bounds bounds, Transform transform, ref Vector3[] positions)
         {
             // Calculate the local points to transform.
             var center = bounds.center;
@@ -132,11 +123,44 @@ public static void GetCornerPositions(this Bounds bounds, Transform transform, r
         }
 
         /// <summary>
-        /// Gets all the corner points from Renderer's Bounds
+        /// Gets all the corner points of the bounds in local space.
+        /// </summary>
+        /// <param name="positions"></param>
+        /// <param name="bounds"></param>
+        public static void GetCornerPositionsLocalSpace(this Bounds bounds, ref Vector3[] positions)
+        {
+            // Allocate the array if needed.
+            const int numCorners = 8;
+
+            if (positions == null || positions.Length != numCorners)
+            {
+                positions = new Vector3[numCorners];
+            }
+
+            // Permutate all axes using minCorner and maxCorner.
+            var minCorner = bounds.center - bounds.extents;
+            var maxCorner = bounds.center + bounds.extents;
+
+            for (int cornerIndex = 0; cornerIndex < numCorners; cornerIndex++)
+            {
+                positions[cornerIndex] = new Vector3(
+                    (cornerIndex & (1 << 0)) == 0 ? minCorner[0] : maxCorner[0],
+                    (cornerIndex & (1 << 1)) == 0 ? minCorner[1] : maxCorner[1],
+                    (cornerIndex & (1 << 2)) == 0 ? minCorner[2] : maxCorner[2]);
+            }
+        }
+
+        /// <summary>
+        /// Gets all the corner points of the bounds.
         /// </summary>
         /// <param name="bounds"></param>
         /// <param name="positions"></param>
-        public static void GetCornerPositionsFromRendererBounds(this Bounds bounds, ref Vector3[] positions)
+        /// <remarks>
+        /// <see cref="Collider.bounds"/> is world space bounding volume.
+        /// <see cref="Mesh.bounds"/> is local space bounding volume.
+        /// <see cref="Renderer.bounds"/>  is the same as <see cref="Mesh.bounds"/> but in world space coords.
+        /// </remarks>
+        public static void GetCornerPositions(this Bounds bounds, ref Vector3[] positions)
         {
             var center = bounds.center;
             var extents = bounds.extents;
@@ -242,7 +266,7 @@ public static void GetCornerAndMidPointPositions(this Bounds bounds, Transform t
         /// <param name="transform"></param>
         /// <param name="positions"></param>
         /// <param name="flattenAxis"></param>
-        public static void GetCornerAndMidPointPositions2D(this Bounds bounds, Transform transform, ref Vector3[] positions, Axis flattenAxis)
+        public static void GetCornerAndMidPointPositions2D(this Bounds bounds, Transform transform, ref Vector3[] positions, CardinalAxis flattenAxis)
         {
             // Calculate the local points to transform.
             var center = bounds.center;
@@ -262,8 +286,8 @@ public static void GetCornerAndMidPointPositions2D(this Bounds bounds, Transform
 
             switch (flattenAxis)
             {
-                // case Axis.X:
                 default:
+                case CardinalAxis.X:
                     leftEdge = center.z - extents.z;
                     rightEdge = center.z + extents.z;
                     bottomEdge = center.y - extents.y;
@@ -275,7 +299,7 @@ public static void GetCornerAndMidPointPositions2D(this Bounds bounds, Transform
                     positions[RB] = transform.TransformPoint(0, bottomEdge, rightEdge);
                     break;
 
-                case Axis.Y:
+                case CardinalAxis.Y:
                     leftEdge = center.z - extents.z;
                     rightEdge = center.z + extents.z;
                     bottomEdge = center.x - extents.x;
@@ -287,7 +311,7 @@ public static void GetCornerAndMidPointPositions2D(this Bounds bounds, Transform
                     positions[RB] = transform.TransformPoint(bottomEdge, 0, rightEdge);
                     break;
 
-                case Axis.Z:
+                case CardinalAxis.Z:
                     leftEdge = center.x - extents.x;
                     rightEdge = center.x + extents.x;
                     bottomEdge = center.y - extents.y;
@@ -337,13 +361,13 @@ public static void GetColliderBoundsPoints(GameObject target, ref List<Vector3>
 
                     case BoxCollider boxCollider:
                         var boxBounds = new Bounds(boxCollider.center, boxCollider.size);
-                        boxBounds.GetCornerPositions(boxCollider.transform, ref corners);
+                        boxBounds.GetCornerPositionsWorldSpace(boxCollider.transform, ref corners);
                         boundsPoints.AddRange(corners);
                         break;
 
                     case MeshCollider meshCollider:
                         var meshBounds = meshCollider.sharedMesh.bounds;
-                        meshBounds.GetCornerPositions(meshCollider.transform, ref corners);
+                        meshBounds.GetCornerPositionsWorldSpace(meshCollider.transform, ref corners);
                         boundsPoints.AddRange(corners);
                         break;
 
@@ -396,7 +420,9 @@ public static void GetRenderBoundsPoints(GameObject target, ref List<Vector3> bo
                     continue;
                 }
 
-                rendererObj.bounds.GetCornerPositionsFromRendererBounds(ref corners);
+                var bounds = rendererObj.transform.GetRenderBounds();
+
+                bounds.GetCornerPositions(ref corners);
                 boundsPoints.AddRange(corners);
             }
         }
@@ -421,7 +447,7 @@ public static void GetMeshFilterBoundsPoints(GameObject target, ref List<Vector3
                 }
 
                 var meshBounds = meshFilterObj.sharedMesh.bounds;
-                meshBounds.GetCornerPositions(meshFilterObj.transform, ref corners);
+                meshBounds.GetCornerPositionsWorldSpace(meshFilterObj.transform, ref corners);
                 boundsPoints.AddRange(corners);
             }
 

XRTK-Core/Packages/com.xrtk.core/Extensions/BoxColliderExtensions.cs

@@ -0,0 +1,41 @@
+// Copyright (c) XRTK. All rights reserved.
+// Licensed under the MIT License. See LICENSE in the project root for license information.
+using UnityEngine;
+
+namespace XRTK.Extensions.XRTK.Extensions
+{
+    /// <summary>
+    /// Extension methods for Unity's <see cref="BoxCollider"/>
+    /// </summary>
+    public static class BoxColliderExtensions
+    {
+        /// <summary>
+        /// Gets all the corner points of the collider's bounds in world space.
+        /// </summary>
+        /// <param name="collider"></param>
+        /// <param name="transform"></param>
+        /// <param name="positions"></param>
+        public static void GetCornerPositionsWorldSpace(this BoxCollider collider, Transform transform, ref Vector3[] positions)
+        {
+            // Store current rotation then zero out the rotation so that the bounds
+            // are computed when the object is in its 'axis aligned orientation'.
+            var currentRotation = transform.rotation;
+            transform.rotation = Quaternion.identity;
+            Physics.SyncTransforms(); // Update collider bounds
+
+            var bounds = collider.bounds;
+            bounds.GetCornerPositions(ref positions);
+
+            // After bounds are computed, restore rotation...
+            // ReSharper disable once Unity.InefficientPropertyAccess
+            transform.rotation = currentRotation;
+            Physics.SyncTransforms();
+
+            // Rotate our points in case the object is also rotated.
+            for (int i = 0; i < positions.Length; i++)
+            {
+                positions[i] = positions[i].RotateAroundPoint(bounds.center, transform.rotation);
+            }
+        }
+    }
+}

XRTK-Core/Packages/com.xrtk.core/Extensions/QuaternionExtensions.cs

@@ -15,5 +15,18 @@ public static bool IsValidRotation(this Quaternion rotation)
             return !float.IsNaN(rotation.x) && !float.IsNaN(rotation.y) && !float.IsNaN(rotation.z) && !float.IsNaN(rotation.w) &&
                    !float.IsInfinity(rotation.x) && !float.IsInfinity(rotation.y) && !float.IsInfinity(rotation.z) && !float.IsInfinity(rotation.w);
         }
+
+        /// <summary>
+        /// Slerps Quaternion source to goal, handles lerpTime of 0
+        /// </summary>
+        /// <param name="source"></param>
+        /// <param name="goal"></param>
+        /// <param name="deltaTime"></param>
+        /// <param name="lerpTime"></param>
+        /// <returns></returns>
+        public static Quaternion SmoothTo(this Quaternion source, Quaternion goal, float deltaTime, float lerpTime)
+        {
+            return Quaternion.Slerp(source, goal, lerpTime.Equals(0.0f) ? 1f : deltaTime / lerpTime);
+        }
     }
 }