From 866cf836dd99a4036396f303c3b26406485a15e6 Mon Sep 17 00:00:00 2001 From: Stephen Hodgson Date: Tue, 10 Sep 2019 12:24:20 -0400 Subject: [PATCH] Ported extension changes from pointer offset --- .../BoundarySystem/InscribedRectangle.cs | 64 +--- .../Extensions/BoundsExtensions.cs | 74 +++-- .../Extensions/BoxColliderExtensions.cs | 41 +++ .../Extensions/BoxColliderExtensions.cs.meta | 11 + .../Extensions/QuaternionExtensions.cs | 13 + .../Extensions/TransformExtensions.cs | 84 ++++-- .../Extensions/VectorExtensions.cs | 279 +++++++++++++++--- .../com.xrtk.core/Utilities/Solvers/Solver.cs | 50 +--- .../Utilities/Solvers/SurfaceMagnetism.cs | 116 ++++---- 9 files changed, 495 insertions(+), 237 deletions(-) create mode 100644 XRTK-Core/Packages/com.xrtk.core/Extensions/BoxColliderExtensions.cs create mode 100644 XRTK-Core/Packages/com.xrtk.core/Extensions/BoxColliderExtensions.cs.meta diff --git a/XRTK-Core/Packages/com.xrtk.core/Definitions/BoundarySystem/InscribedRectangle.cs b/XRTK-Core/Packages/com.xrtk.core/Definitions/BoundarySystem/InscribedRectangle.cs index 64028d66f..5eea05a9d 100644 --- a/XRTK-Core/Packages/com.xrtk.core/Definitions/BoundarySystem/InscribedRectangle.cs +++ b/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); } - /// - /// Rotate a two dimensional point about another point by the specified angle. - /// - /// The point to be rotated. - /// The point about which the rotation is to occur. - /// The angle for the rotation, in radians - /// - /// The coordinates of the rotated point. - /// - 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; - } - /// /// 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); } } -} +} \ No newline at end of file diff --git a/XRTK-Core/Packages/com.xrtk.core/Extensions/BoundsExtensions.cs b/XRTK-Core/Packages/com.xrtk.core/Extensions/BoundsExtensions.cs index ee7cb0c85..0aa34ab88 100644 --- a/XRTK-Core/Packages/com.xrtk.core/Extensions/BoundsExtensions.cs +++ b/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 { /// - /// Extension methods for Unity's Bounds struct + /// Extension methods for Unity's /// 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) } /// - /// Gets all the corner points of the bounds in world space + /// Gets all the corner points of the bounds in world space. /// /// /// /// - /// - /// Use BoxColliderExtensions.{Left|Right}{Bottom|Top}{Front|Back} constants to index into the output - /// corners array. - /// - 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 } /// - /// Gets all the corner points from Renderer's Bounds + /// Gets all the corner points of the bounds in local space. + /// + /// + /// + 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]); + } + } + + /// + /// Gets all the corner points of the bounds. /// /// /// - public static void GetCornerPositionsFromRendererBounds(this Bounds bounds, ref Vector3[] positions) + /// + /// is world space bounding volume. + /// is local space bounding volume. + /// is the same as but in world space coords. + /// + 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 /// /// /// - 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 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 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 + /// Extension methods for Unity's + /// + public static class BoxColliderExtensions + { + /// + /// Gets all the corner points of the collider's bounds in world space. + /// + /// + /// + /// + 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); + } + } + } +} \ No newline at end of file diff --git a/XRTK-Core/Packages/com.xrtk.core/Extensions/BoxColliderExtensions.cs.meta b/XRTK-Core/Packages/com.xrtk.core/Extensions/BoxColliderExtensions.cs.meta new file mode 100644 index 000000000..73e33be40 --- /dev/null +++ b/XRTK-Core/Packages/com.xrtk.core/Extensions/BoxColliderExtensions.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8e95f9265cae48a4d8e535c7e8d924ff +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {fileID: 2800000, guid: 8ac5213854cf4dbabd140decf8df1946, type: 3} + userData: + assetBundleName: + assetBundleVariant: diff --git a/XRTK-Core/Packages/com.xrtk.core/Extensions/QuaternionExtensions.cs b/XRTK-Core/Packages/com.xrtk.core/Extensions/QuaternionExtensions.cs index 392e9bf85..a50bde920 100644 --- a/XRTK-Core/Packages/com.xrtk.core/Extensions/QuaternionExtensions.cs +++ b/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); } + + /// + /// Slerps Quaternion source to goal, handles lerpTime of 0 + /// + /// + /// + /// + /// + /// + 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); + } } } \ No newline at end of file diff --git a/XRTK-Core/Packages/com.xrtk.core/Extensions/TransformExtensions.cs b/XRTK-Core/Packages/com.xrtk.core/Extensions/TransformExtensions.cs index 824897f76..8e6fb5bcd 100644 --- a/XRTK-Core/Packages/com.xrtk.core/Extensions/TransformExtensions.cs +++ b/XRTK-Core/Packages/com.xrtk.core/Extensions/TransformExtensions.cs @@ -137,6 +137,51 @@ public static Bounds GetColliderBounds(this Transform transform, bool syncTransf return bounds; } + /// + /// Calculates the bounds of all the renderers attached to this GameObject and all it's children + /// + /// + /// Transform of root GameObject the renderers are attached to. + /// + /// + /// True, by default, this will sync the rotation to calculate the axis aligned orientation. + /// + /// The total bounds of all renderers attached to this GameObject. + /// If no renderers attached, returns a bounds of center and extents 0 + public static Bounds GetRenderBounds(this Transform transform, bool syncTransform = true) + { + // 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; + + if (syncTransform) + { + transform.rotation = Quaternion.identity; + Physics.SyncTransforms(); // Update collider bounds + } + + var renderers = transform.GetComponentsInChildren(); + + if (renderers.Length == 0) { return default; } + + var bounds = renderers[0].bounds; + + for (int i = 1; i < renderers.Length; i++) + { + bounds.Encapsulate(renderers[i].bounds); + } + + if (syncTransform) + { + // After bounds are computed, restore rotation... + // ReSharper disable once Unity.InefficientPropertyAccess + transform.rotation = currentRotation; + Physics.SyncTransforms(); + } + + return bounds; + } + /// /// Checks if the provided transforms are child/parent related. /// @@ -284,17 +329,21 @@ public static bool TryGetDepth(Transform target, Transform parent, ref int depth /// /// /// + /// /// - public static Vector3 GetPointOnBoundsEdge(this Transform transform, Vector3 direction) + public static Vector3 GetPointOnBoundsEdge(this Transform transform, Vector3 direction, Bounds bounds = default) { if (direction != Vector3.zero) { direction /= Mathf.Max(Mathf.Max(Mathf.Abs(direction.x), Mathf.Abs(direction.y), Mathf.Abs(direction.y))); } - var bounds = transform.GetColliderBounds(); - direction = bounds.center + Vector3.Scale(bounds.size, direction * 0.5f); - return direction; + if (bounds == default) + { + bounds = transform.GetColliderBounds(); + } + + return bounds.center + Vector3.Scale(bounds.size, direction * 0.5f); } /// @@ -360,18 +409,12 @@ public static void SetCollidersActive(this Transform transform, bool isActive) /// public static void SetLayerRecursively(this Transform transform, int layer) { - transform.gameObject.layer = layer; - - for (int i = 0; i < transform.childCount; i++) - { - var child = transform.GetChild(i); - child.gameObject.layer = layer; - } + transform.gameObject.SetLayerRecursively(layer); } /// /// Scales the target by the provided position using the - /// provided . + /// provided . /// /// Similar to how works. /// @@ -380,18 +423,17 @@ public static void SetLayerRecursively(this Transform transform, int layer) /// /// /// - /// - public static void ScaleAround(this Transform target, Vector3 pivot, Vector3 scale) + /// + public static void ScaleAround(this Transform target, Vector3 pivot, Vector3 newScale) { - var A = target.localPosition; - var B = pivot; - var C = A - B; // diff from object pivot to desired pivot/origin - var RS = scale.x / target.localScale.x; // relative scale factor - var FP = B + C * RS; // calc final position post-scale + var point = target.localPosition; + var distance = point - pivot; // diff from object pivot to desired pivot/origin + var relativeScale = newScale.x / target.localScale.x; // relative scale factor + var finalPosition = pivot + distance * relativeScale; // calc final position post-scale // finally, actually perform the scale / translation - target.localScale = scale; - target.localPosition = FP; + target.localScale = newScale; + target.localPosition = finalPosition; } } } \ No newline at end of file diff --git a/XRTK-Core/Packages/com.xrtk.core/Extensions/VectorExtensions.cs b/XRTK-Core/Packages/com.xrtk.core/Extensions/VectorExtensions.cs index ef2e5a157..a4329d4d3 100644 --- a/XRTK-Core/Packages/com.xrtk.core/Extensions/VectorExtensions.cs +++ b/XRTK-Core/Packages/com.xrtk.core/Extensions/VectorExtensions.cs @@ -33,31 +33,186 @@ public static Vector3 Div(this Vector3 value, Vector3 scale) return new Vector3(value.x / scale.x, value.y / scale.y, value.z / scale.z); } - public static Vector3 RotateAround(this Vector3 point, Vector3 pivot, Quaternion rotation) + /// + /// Rotate a two dimensional point by the specified angle. + /// + /// + /// + /// + /// The coordinates of the rotated point. + /// + public static Vector2 RotatePoint(this Vector2 point, float angleRadians) + { + return point.RotateAroundPoint(Vector2.zero, angleRadians); + } + + /// + /// Rotate a two dimensional point about another point by the specified angle. + /// + /// The point to be rotated. + /// The point about which the rotation is to occur. + /// The angle for the rotation, in radians + /// + /// The coordinates of the rotated point. + /// + public static Vector2 RotateAroundPoint(this Vector2 point, Vector2 pivot, float angleRadians) + { + if (angleRadians.Equals(0f)) + { + return point; + } + + var rotated = point; + + // Translate to origin of rotation + rotated.x -= pivot.x; + rotated.y -= pivot.y; + + // Rotate the point + var sin = Mathf.Sin(angleRadians); + var cos = Mathf.Cos(angleRadians); + var x = rotated.x * cos - rotated.y * sin; + var y = rotated.x * sin + rotated.y * cos; + + // Translate back and return + rotated.x = x + pivot.x; + rotated.y = y + pivot.y; + + return rotated; + } + + /// + /// Rotate a three dimensional point about another point by the specified rotation. + /// + /// + /// + /// + /// + /// + /// The coordinates of the rotated point. + /// + public static Vector3 RotateAroundPoint(this Vector3 point, Vector3 pivot, float angle, Vector3 axis) + { + return point.RotateAroundPoint(pivot, Quaternion.AngleAxis(angle, axis)); + } + + /// + /// Rotate a three dimensional point about another point by the specified rotation. + /// + /// The point to be rotated. + /// The point about which the rotation is to occur. + /// The specified rotation. + /// + /// The coordinates of the rotated point. + /// + public static Vector3 RotateAroundPoint(this Vector3 point, Vector3 pivot, Quaternion rotation) { return rotation * (point - pivot) + pivot; } - public static Vector3 RotateAround(this Vector3 point, Vector3 pivot, Vector3 eulerAngles) + /// + /// Rotate a three dimensional point about another point by the specified rotation. + /// + /// The point to be rotated. + /// The point about which the rotation is to occur. + /// The specified rotation. + /// + /// The coordinates of the rotated point. + /// + public static Vector3 RotateAroundPoint(this Vector3 point, Vector3 pivot, Vector3 eulerAngles) { - return RotateAround(point, pivot, Quaternion.Euler(eulerAngles)); + return RotateAroundPoint(point, pivot, Quaternion.Euler(eulerAngles)); } - public static Vector3 TransformPoint(this Vector3 point, Vector3 translation, Quaternion rotation, Vector3 lossyScale) + /// + /// Gets the angle of rotation between two directions around the specified axis. + /// + /// The first direction. + /// The second direction. + /// The axis of rotation. + /// + /// The rotation angle. + /// + public static float AngleAroundAxis(this Vector3 directionA, Vector3 directionB, Vector3 axis) { - return rotation * Vector3.Scale(lossyScale, point) + translation; + directionA -= Vector3.Project(directionA, axis); + directionB -= Vector3.Project(directionB, axis); + return Vector3.Angle(directionA, directionB) * (Vector3.Dot(axis, Vector3.Cross(directionA, directionB)) < 0 ? -1 : 1); } - public static Vector3 InverseTransformPoint(this Vector3 point, Vector3 translation, Quaternion rotation, Vector3 lossyScale) + /// + /// Transforms a three dimensional point by the specified transform. + /// + /// + /// + /// + /// The coordinates of the updated point. + /// + /// + /// This IS NOT the same as which translates a point from local space to world space. + /// + public static Vector3 TransformPoint(this Vector3 point, Transform transform) { - var scaleInv = new Vector3(1 / lossyScale.x, 1 / lossyScale.y, 1 / lossyScale.z); - return Vector3.Scale(scaleInv, (Quaternion.Inverse(rotation) * (point - translation))); + return point.TransformPoint(transform.position, transform.rotation, transform.localScale); + } + + /// + /// Transforms a three dimensional point by the specified position, rotation, and scale. + /// + /// + /// + /// + /// + /// + /// The coordinates of the updated point. + /// + /// + /// This IS NOT the same as which translates a point from local space to world space. + /// + public static Vector3 TransformPoint(this Vector3 point, Vector3 position, Quaternion rotation, Vector3 localScale) + { + return rotation * Vector3.Scale(localScale, point) + position; + } + + /// + /// Inversely Transforms a three dimensional point by the specified transform. + /// + /// + /// + /// + /// The coordinates of the updated point. + /// + /// + /// This IS NOT the same as which translates a point from local space to world space. + /// + public static Vector3 InverseTransformPoint(this Vector3 point, Transform transform) + { + return point.InverseTransformPoint(transform.position, transform.rotation, transform.localScale); + } + + /// + /// Transforms a three dimensional point by the specified position, rotation, and scale. + /// + /// + /// + /// + /// + /// + /// The coordinates of the updated point. + /// + /// + /// This IS NOT the same as which translates a point from local space to world space. + /// + public static Vector3 InverseTransformPoint(this Vector3 point, Vector3 position, Quaternion rotation, Vector3 localScale) + { + var scaleInv = new Vector3(1 / localScale.x, 1 / localScale.y, 1 / localScale.z); + return Vector3.Scale(scaleInv, (Quaternion.Inverse(rotation) * (point - position))); } public static Vector2 Average(this IEnumerable vectors) { - float x = 0f; - float y = 0f; + var x = 0f; + var y = 0f; int count = 0; foreach (var pos in vectors) @@ -72,9 +227,9 @@ public static Vector2 Average(this IEnumerable vectors) public static Vector3 Average(this IEnumerable vectors) { - float x = 0f; - float y = 0f; - float z = 0f; + var x = 0f; + var y = 0f; + var z = 0f; int count = 0; foreach (var pos in vectors) @@ -91,13 +246,14 @@ public static Vector3 Average(this IEnumerable vectors) public static Vector2 Average(this ICollection vectors) { int count = vectors.Count; + if (count == 0) { return Vector2.zero; } - float x = 0f; - float y = 0f; + var x = 0f; + var y = 0f; foreach (var pos in vectors) { @@ -117,9 +273,9 @@ public static Vector3 Average(this ICollection vectors) return Vector3.zero; } - float x = 0f; - float y = 0f; - float z = 0f; + var x = 0f; + var y = 0f; + var z = 0f; foreach (var pos in vectors) { @@ -157,6 +313,12 @@ public static Vector3 Median(this ICollection vectors) return count == 0 ? Vector3.zero : vectors.OrderBy(v => v.sqrMagnitude).ElementAt(count / 2); } + /// + /// Validates the vector data to ensure that none of the values are , + /// , or . + /// + /// The vector data to verify. + /// True, if the vector values are valid. public static bool IsValidVector(this Vector3 vector) { return !float.IsNaN(vector.x) && !float.IsNaN(vector.y) && !float.IsNaN(vector.z) && @@ -171,17 +333,13 @@ public static bool IsValidVector(this Vector3 vector) /// public static Vector3 SphericalMapping(Vector3 source, float radius) { - float circ = 2f * Mathf.PI * radius; - - float xAngle = (source.x / circ) * 360f; - float yAngle = -(source.y / circ) * 360f; + var circumference = 2f * Mathf.PI * radius; + var xAngle = (source.x / circumference) * 360f; + var yAngle = -(source.y / circumference) * 360f; + var rotation = Quaternion.Euler(yAngle, xAngle, 0.0f); source.Set(0.0f, 0.0f, radius); - - Quaternion rot = Quaternion.Euler(yAngle, xAngle, 0.0f); - source = rot * source; - - return source; + return rotation * source; } /// @@ -192,16 +350,13 @@ public static Vector3 SphericalMapping(Vector3 source, float radius) /// public static Vector3 CylindricalMapping(Vector3 source, float radius) { - float circ = 2f * Mathf.PI * radius; - - float xAngle = (source.x / circ) * 360f; + var circumference = 2f * Mathf.PI * radius; + var xAngle = (source.x / circumference) * 360f; + var rotation = Quaternion.Euler(0.0f, xAngle, 0.0f); source.Set(0.0f, source.y, radius); - Quaternion rot = Quaternion.Euler(0.0f, xAngle, 0.0f); - source = rot * source; - - return source; + return rotation * source; } /// @@ -224,21 +379,20 @@ public static Vector3 RadialMapping(Vector3 source, float radialRange, float rad source.y = 0f; source.z = (radius / totalRows) * row; var yAngle = radialCellAngle * (column - (totalColumns * offset)) + (radialCellAngle * offset); - var rot = Quaternion.Euler(0.0f, yAngle, 0.0f); - - return rot * source; + var rotation = Quaternion.Euler(0.0f, yAngle, 0.0f); + return rotation * source; } /// - /// Randomized mapping based on a source Vec3 and a radius for randomization distance. + /// Randomized mapping based on a source position and a radius for randomization distance. /// /// The source to be mapped to cylinder /// This is a for the radius of the cylinder /// public static Vector3 ScatterMapping(this Vector3 source, float radius) { - source.x = UnityEngine.Random.Range(-radius, radius); - source.y = UnityEngine.Random.Range(-radius, radius); + source.x = Random.Range(-radius, radius); + source.y = Random.Range(-radius, radius); return source; } @@ -262,5 +416,48 @@ public static MoveDirection DetermineMoveDirection(this Vector2 direction, float return direction.y > 0 ? MoveDirection.Up : MoveDirection.Down; } + + /// + /// Checks if a normal is nearly vertical + /// + /// + /// Returns true, if normal is vertical. + public static bool IsNormalVertical(this Vector3 normal) => 1f - Mathf.Abs(normal.y) < 0.01f; + + /// + /// Lerps Vector3 source to goal. + /// + /// + /// Handles lerpTime of 0. + /// + /// + /// + /// + /// + /// + public static Vector3 SmoothTo(this Vector3 source, Vector3 goal, float deltaTime, float lerpTime) + { + return Vector3.Lerp(source, goal, lerpTime.Equals(0.0f) ? 1f : deltaTime / lerpTime); + } + + /// + /// Returns the midpoint between two vectors. + /// + /// + /// + public static Vector2 MidPoint(this Vector2 source, Vector2 point) + { + return (source + point) * 0.5f; + } + + /// + /// Returns the midpoint between two vectors. + /// + /// + /// + public static Vector3 MidPoint(this Vector3 source, Vector3 point) + { + return (source + point) * 0.5f; + } } -} +} \ No newline at end of file diff --git a/XRTK-Core/Packages/com.xrtk.core/Utilities/Solvers/Solver.cs b/XRTK-Core/Packages/com.xrtk.core/Utilities/Solvers/Solver.cs index 07214ea40..01560aba3 100644 --- a/XRTK-Core/Packages/com.xrtk.core/Utilities/Solvers/Solver.cs +++ b/XRTK-Core/Packages/com.xrtk.core/Utilities/Solvers/Solver.cs @@ -2,6 +2,7 @@ // Licensed under the MIT License. See LICENSE in the project root for license information. using UnityEngine; +using XRTK.Extensions; namespace XRTK.SDK.Utilities.Solvers { @@ -217,35 +218,6 @@ public virtual void AddOffset(Vector3 offset) GoalPosition += offset; } - /// - /// Lerps Vector3 source to goal. - /// - /// - /// Handles lerpTime of 0. - /// - /// - /// - /// - /// - /// - public static Vector3 SmoothTo(Vector3 source, Vector3 goal, float deltaTime, float lerpTime) - { - return Vector3.Lerp(source, goal, lerpTime.Equals(0.0f) ? 1f : deltaTime / lerpTime); - } - - /// - /// Slerps Quaternion source to goal, handles lerpTime of 0 - /// - /// - /// - /// - /// - /// - public static Quaternion SmoothTo(Quaternion source, Quaternion goal, float deltaTime, float lerpTime) - { - return Quaternion.Slerp(source, goal, lerpTime.Equals(0.0f) ? 1f : deltaTime / lerpTime); - } - /// /// Updates all object orientations to the goal orientation for this solver, with smoothing accounted for (smoothing may be off) /// @@ -259,9 +231,9 @@ protected void UpdateTransformToGoal() var rot = cachedTransform.rotation; var scale = cachedTransform.localScale; - pos = SmoothTo(pos, GoalPosition, SolverHandler.DeltaTime, moveLerpTime); - rot = SmoothTo(rot, GoalRotation, SolverHandler.DeltaTime, rotateLerpTime); - scale = SmoothTo(scale, GoalScale, SolverHandler.DeltaTime, scaleLerpTime); + pos = pos.SmoothTo(GoalPosition, SolverHandler.DeltaTime, moveLerpTime); + rot = rot.SmoothTo(GoalRotation, SolverHandler.DeltaTime, rotateLerpTime); + scale = scale.SmoothTo(GoalScale, SolverHandler.DeltaTime, scaleLerpTime); cachedTransform.position = pos; cachedTransform.rotation = rot; @@ -282,9 +254,9 @@ public void UpdateWorkingToGoal() { if (smoothing) { - WorkingPosition = SmoothTo(WorkingPosition, GoalPosition, SolverHandler.DeltaTime, moveLerpTime); - WorkingRotation = SmoothTo(WorkingRotation, GoalRotation, SolverHandler.DeltaTime, rotateLerpTime); - WorkingScale = SmoothTo(WorkingScale, GoalScale, SolverHandler.DeltaTime, scaleLerpTime); + WorkingPosition = WorkingPosition.SmoothTo(GoalPosition, SolverHandler.DeltaTime, moveLerpTime); + WorkingRotation = WorkingRotation.SmoothTo(GoalRotation, SolverHandler.DeltaTime, rotateLerpTime); + WorkingScale = WorkingScale.SmoothTo(GoalScale, SolverHandler.DeltaTime, scaleLerpTime); } else { @@ -299,7 +271,7 @@ public void UpdateWorkingToGoal() /// public void UpdateWorkingPositionToGoal() { - WorkingPosition = smoothing ? SmoothTo(WorkingPosition, GoalPosition, SolverHandler.DeltaTime, moveLerpTime) : GoalPosition; + WorkingPosition = smoothing ? WorkingPosition.SmoothTo(GoalPosition, SolverHandler.DeltaTime, moveLerpTime) : GoalPosition; } /// @@ -307,7 +279,7 @@ public void UpdateWorkingPositionToGoal() /// public void UpdateWorkingRotationToGoal() { - WorkingRotation = smoothing ? SmoothTo(WorkingRotation, GoalRotation, SolverHandler.DeltaTime, rotateLerpTime) : GoalRotation; + WorkingRotation = smoothing ? WorkingRotation.SmoothTo(GoalRotation, SolverHandler.DeltaTime, rotateLerpTime) : GoalRotation; } /// @@ -315,7 +287,7 @@ public void UpdateWorkingRotationToGoal() /// public void UpdateWorkingScaleToGoal() { - WorkingScale = smoothing ? SmoothTo(WorkingScale, GoalScale, SolverHandler.DeltaTime, scaleLerpTime) : GoalScale; + WorkingScale = smoothing ? WorkingScale.SmoothTo(GoalScale, SolverHandler.DeltaTime, scaleLerpTime) : GoalScale; } } -} +} \ No newline at end of file diff --git a/XRTK-Core/Packages/com.xrtk.core/Utilities/Solvers/SurfaceMagnetism.cs b/XRTK-Core/Packages/com.xrtk.core/Utilities/Solvers/SurfaceMagnetism.cs index 4ec948aa9..691fd1a14 100644 --- a/XRTK-Core/Packages/com.xrtk.core/Utilities/Solvers/SurfaceMagnetism.cs +++ b/XRTK-Core/Packages/com.xrtk.core/Utilities/Solvers/SurfaceMagnetism.cs @@ -1,9 +1,10 @@ // Copyright (c) Microsoft Corporation. All rights reserved. // Licensed under the MIT License. See LICENSE in the project root for license information. +using UnityEngine; using XRTK.Definitions.Physics; +using XRTK.Extensions; using XRTK.Utilities.Physics; -using UnityEngine; namespace XRTK.SDK.Utilities.Solvers { @@ -16,6 +17,7 @@ public class SurfaceMagnetism : Solver private enum RaycastDirectionEnum { + None = 0, CameraFacing, ToObject, ToLinkedPosition @@ -23,7 +25,7 @@ private enum RaycastDirectionEnum private enum OrientModeEnum { - None, + None = 0, Vertical, Full, Blended @@ -31,7 +33,7 @@ private enum OrientModeEnum [SerializeField] [Tooltip("LayerMask to apply Surface Magnetism to")] - private LayerMask[] magneticSurfaces = { UnityEngine.Physics.DefaultRaycastLayers }; + private LayerMask[] magneticSurfaces = { Physics.DefaultRaycastLayers }; [SerializeField] [Tooltip("Max distance to check for surfaces")] @@ -111,8 +113,7 @@ private Vector3 RaycastEndPoint { get { - Vector3 origin = RaycastOrigin; - Vector3 endPoint = Vector3.forward; + var endPoint = Vector3.forward; switch (raycastDirection) { @@ -140,7 +141,7 @@ private Vector3 RaycastDirection { get { - Vector3 direction = Vector3.forward; + var direction = Vector3.forward; if (raycastDirection == RaycastDirectionEnum.CameraFacing) { @@ -203,42 +204,31 @@ private void Start() private Quaternion CalculateMagnetismOrientation(Vector3 direction, Vector3 surfaceNormal) { // Calculate the surface rotation - Vector3 newDirection = -surfaceNormal; + var newDirection = -surfaceNormal; - if (IsNormalVertical(newDirection)) + if (newDirection.IsNormalVertical()) { newDirection = direction; } newDirection.y = 0; - var surfaceRot = Quaternion.LookRotation(newDirection, Vector3.up); + var surfaceOrientation = Quaternion.LookRotation(newDirection, Vector3.up); switch (orientationMode) { - case OrientModeEnum.None: - return SolverHandler.GoalRotation; - case OrientModeEnum.Vertical: - return surfaceRot; - + return surfaceOrientation; case OrientModeEnum.Full: return Quaternion.LookRotation(-surfaceNormal, Vector3.up); - case OrientModeEnum.Blended: - return Quaternion.Slerp(SolverHandler.GoalRotation, surfaceRot, orientationBlend); + return Quaternion.Slerp(SolverHandler.GoalRotation, surfaceOrientation, orientationBlend); default: - return Quaternion.identity; + case OrientModeEnum.None: + return SolverHandler.GoalRotation; } } - /// - /// Checks if a normal is nearly vertical - /// - /// - /// Returns true, if normal is vertical. - private static bool IsNormalVertical(Vector3 normal) => 1f - Mathf.Abs(normal.y) < 0.01f; - public override void SolverUpdate() { // Pass-through by default @@ -277,13 +267,13 @@ private void SimpleRaycastStepUpdate(RayStep rayStep) bool isHit; // Do the cast! - isHit = MixedRealityRaycaster.RaycastSimplePhysicsStep(rayStep, maxDistance, magneticSurfaces, out RaycastHit result); + isHit = MixedRealityRaycaster.RaycastSimplePhysicsStep(rayStep, maxDistance, magneticSurfaces, out var result); OnSurface = isHit; // Enforce CloseDistance - Vector3 hitDelta = result.point - rayStep.Origin; - float length = hitDelta.magnitude; + var hitDelta = result.point - rayStep.Origin; + var length = hitDelta.magnitude; if (length < closeDistance) { @@ -301,17 +291,17 @@ private void SimpleRaycastStepUpdate(RayStep rayStep) private void SphereRaycastStepUpdate(RayStep rayStep) { bool isHit; - float scaleOverride = ScaleOverride; + var scaleOverride = ScaleOverride; // Do the cast! - float size = scaleOverride > 0 ? scaleOverride : transform.lossyScale.x * sphereSize; - isHit = MixedRealityRaycaster.RaycastSpherePhysicsStep(rayStep, size, maxDistance, magneticSurfaces, out RaycastHit result); + var size = scaleOverride > 0 ? scaleOverride : transform.lossyScale.x * sphereSize; + isHit = MixedRealityRaycaster.RaycastSpherePhysicsStep(rayStep, size, maxDistance, magneticSurfaces, out var result); OnSurface = isHit; // Enforce CloseDistance - Vector3 hitDelta = result.point - rayStep.Origin; - float length = hitDelta.magnitude; + var hitDelta = result.point - rayStep.Origin; + var length = hitDelta.magnitude; if (length < closeDistance) { @@ -328,19 +318,19 @@ private void SphereRaycastStepUpdate(RayStep rayStep) private void BoxRaycastStepUpdate(RayStep rayStep) { - Vector3 scale = transform.lossyScale; - float scaleOverride = ScaleOverride; + var scale = transform.lossyScale; + var scaleOverride = ScaleOverride; if (scaleOverride > 0) { scale = scale.normalized * scaleOverride; } - Quaternion orientation = orientationMode == OrientModeEnum.None ? - Quaternion.LookRotation(rayStep.Direction, Vector3.up) : - CalculateMagnetismOrientation(rayStep.Direction, Vector3.up); + var orientation = orientationMode == OrientModeEnum.None + ? Quaternion.LookRotation(rayStep.Direction, Vector3.up) + : CalculateMagnetismOrientation(rayStep.Direction, Vector3.up); - Matrix4x4 targetMatrix = Matrix4x4.TRS(Vector3.zero, orientation, scale); + var targetMatrix = Matrix4x4.TRS(Vector3.zero, orientation, scale); if (boxCollider == null) { @@ -349,23 +339,24 @@ private void BoxRaycastStepUpdate(RayStep rayStep) Debug.Assert(boxCollider != null, $"Missing a box collider for Surface Magnetism on {gameObject}"); - Vector3 extents = boxCollider.size; + var extents = boxCollider.size; - if (MixedRealityRaycaster.RaycastBoxPhysicsStep(rayStep, extents, transform.position, targetMatrix, maxDistance, magneticSurfaces, boxRaysPerEdge, orthographicBoxCast, out Vector3[] positions, out Vector3[] normals, out bool[] hits)) + if (MixedRealityRaycaster.RaycastBoxPhysicsStep(rayStep, extents, transform.position, targetMatrix, maxDistance, magneticSurfaces, boxRaysPerEdge, orthographicBoxCast, out var positions, out var normals, out var hits)) { - // Place an unconstrained plane down the ray. Don't use vertical constrain. - FindPlacementPlane(rayStep.Origin, rayStep.Direction, positions, normals, hits, boxCollider.size.x, maximumNormalVariance, false, orientationMode == OrientModeEnum.None, out Plane plane, out float distance); + // Place an unconstrained plane down the ray. Don't use vertical constraint. + FindPlacementPlane(rayStep.Origin, rayStep.Direction, positions, normals, hits, boxCollider.size.x, maximumNormalVariance, false, orientationMode == OrientModeEnum.None, out var plane, out var distance); // If placing on a horizontal surface, need to adjust the calculated distance by half the app height - float verticalCorrectionOffset = 0; - if (IsNormalVertical(plane.normal) && !Mathf.Approximately(rayStep.Direction.y, 0)) + var verticalCorrectionOffset = 0f; + + if (plane.normal.IsNormalVertical() && !Mathf.Approximately(rayStep.Direction.y, 0)) { - float boxSurfaceVerticalOffset = targetMatrix.MultiplyVector(new Vector3(0, extents.y * 0.5f, 0)).magnitude; - Vector3 correctionVector = boxSurfaceVerticalOffset * (rayStep.Direction / rayStep.Direction.y); + var boxSurfaceVerticalOffset = targetMatrix.MultiplyVector(new Vector3(0, extents.y * 0.5f, 0)).magnitude; + var correctionVector = boxSurfaceVerticalOffset * (rayStep.Direction / rayStep.Direction.y); verticalCorrectionOffset = -correctionVector.magnitude; } - float boxSurfaceOffset = targetMatrix.MultiplyVector(new Vector3(0, 0, extents.z * 0.5f)).magnitude; + var boxSurfaceOffset = targetMatrix.MultiplyVector(new Vector3(0, 0, extents.z * 0.5f)).magnitude; // Apply boxSurfaceOffset to ray direction and not surface normal direction to reduce sliding GoalPosition = rayStep.Origin + rayStep.Direction * Mathf.Max(closeDistance, distance + surfaceRayOffset + boxSurfaceOffset + verticalCorrectionOffset) + plane.normal * (0 * boxSurfaceOffset + surfaceNormalOffset); @@ -394,9 +385,8 @@ private void BoxRaycastStepUpdate(RayStep rayStep) /// private void FindPlacementPlane(Vector3 origin, Vector3 direction, Vector3[] positions, Vector3[] normals, bool[] hits, float assetWidth, float maxNormalVariance, bool constrainVertical, bool useClosestDistance, out Plane plane, out float closestDistance) { - int rayCount = positions.Length; - - Vector3 originalDirection = direction; + var rayCount = positions.Length; + var originalDirection = direction; if (constrainVertical) { @@ -407,16 +397,16 @@ private void FindPlacementPlane(Vector3 origin, Vector3 direction, Vector3[] pos // Go through all the points and find the closest distance closestDistance = float.PositiveInfinity; - int numHits = 0; - int closestPoint = -1; - float farthestDistance = 0f; + var numHits = 0; + var closestPoint = -1; + var farthestDistance = 0f; var averageNormal = Vector3.zero; for (int hitIndex = 0; hitIndex < rayCount; hitIndex++) { if (hits[hitIndex]) { - float distance = Vector3.Dot(direction, positions[hitIndex] - origin); + var distance = Vector3.Dot(direction, positions[hitIndex] - origin); if (distance < closestDistance) { @@ -460,8 +450,8 @@ private void FindPlacementPlane(Vector3 origin, Vector3 direction, Vector3[] pos // go through all the points and find the most orthogonal plane var lowAngle = float.PositiveInfinity; var highAngle = float.NegativeInfinity; - int lowIndex = -1; - int highIndex = -1; + var lowIndex = -1; + var highIndex = -1; for (int hitIndex = 0; hitIndex < rayCount; hitIndex++) { @@ -470,7 +460,7 @@ private void FindPlacementPlane(Vector3 origin, Vector3 direction, Vector3[] pos continue; } - Vector3 difference = positions[hitIndex] - positions[closestPoint]; + var difference = positions[hitIndex] - positions[closestPoint]; if (constrainVertical) { @@ -485,7 +475,7 @@ private void FindPlacementPlane(Vector3 origin, Vector3 direction, Vector3[] pos difference.Normalize(); - float angle = Vector3.Dot(direction, difference); + var angle = Vector3.Dot(direction, difference); if (angle < lowAngle) { @@ -503,14 +493,14 @@ private void FindPlacementPlane(Vector3 origin, Vector3 direction, Vector3[] pos continue; } - float dot = Mathf.Abs(Vector3.Dot((positions[hitIndex] - positions[closestPoint]).normalized, (positions[lowIndex] - positions[closestPoint]).normalized)); + var dot = Mathf.Abs(Vector3.Dot((positions[hitIndex] - positions[closestPoint]).normalized, (positions[lowIndex] - positions[closestPoint]).normalized)); if (dot > MaxDot) { continue; } - float nextAngle = Mathf.Abs(Vector3.Dot(direction, Vector3.Cross(positions[lowIndex] - positions[closestPoint], positions[hitIndex] - positions[closestPoint]).normalized)); + var nextAngle = Mathf.Abs(Vector3.Dot(direction, Vector3.Cross(positions[lowIndex] - positions[closestPoint], positions[hitIndex] - positions[closestPoint]).normalized)); if (nextAngle > highAngle) { @@ -540,7 +530,7 @@ private void FindPlacementPlane(Vector3 origin, Vector3 direction, Vector3[] pos } else { - Vector3 planeUp = Vector3.Cross(positions[lowIndex] - positions[closestPoint], direction); + var planeUp = Vector3.Cross(positions[lowIndex] - positions[closestPoint], direction); placementNormal = Vector3.Cross(positions[lowIndex] - positions[closestPoint], constrainVertical ? Vector3.up : planeUp).normalized; } @@ -569,7 +559,7 @@ private void FindPlacementPlane(Vector3 origin, Vector3 direction, Vector3[] pos // Figure out how far the plane should be. if (!useClosestDistance && closestPoint >= 0) { - if (plane.Raycast(new Ray(origin, originalDirection), out float centerPlaneDistance) || !centerPlaneDistance.Equals(0.0f)) + if (plane.Raycast(new Ray(origin, originalDirection), out var centerPlaneDistance) || !centerPlaneDistance.Equals(0.0f)) { // When the plane is nearly parallel to the user, we need to clamp the distance to where the raycasts hit. closestDistance = Mathf.Clamp(centerPlaneDistance, closestDistance, farthestDistance + assetWidth * 0.5f); @@ -581,4 +571,4 @@ private void FindPlacementPlane(Vector3 origin, Vector3 direction, Vector3[] pos } } } -} +} \ No newline at end of file