// Copyright (c) Microsoft Corporation. All rights reserved. // Licensed under the MIT License. See LICENSE in the project root for license information. using System.Collections.Generic; using System.Linq; using UnityEngine; namespace HoloToolkit.Unity { /// <summary> /// Math Utilities class. /// </summary> public static class MathUtils { /// <summary> /// Get the horizontal FOV from the stereo camera /// </summary> /// <returns></returns> [System.Obsolete("Use CameraExtensions.GetHorizontalFieldOfViewRadians(Camera camera) instead.")] public static float GetHorizontalFieldOfViewRadians() { return CameraCache.Main.GetHorizontalFieldOfViewRadians(); } /// <summary> /// Returns if a point will be rendered on the screen in either eye /// </summary> /// <param name="position"></param> /// <returns></returns> [System.Obsolete("Use CameraExtensions.IsInFOV(Camera camera, Vector3 position) instead.")] public static bool IsInFOV(Vector3 position) { return CameraCache.Main.IsInFOV(position); } /// <summary> /// Takes a point in the coordinate space specified by the "from" transform and transforms it to be the correct point in the coordinate space specified by the "to" transform /// applies rotation, scale and translation /// </summary> /// <param name="from"></param> /// <param name="to"></param> /// <param name="ptInFrom"></param> /// <returns></returns> public static Vector3 TransformPointFromTo(Transform from, Transform to, Vector3 ptInFrom) { Vector3 ptInWorld = (from == null) ? ptInFrom : from.TransformPoint(ptInFrom); Vector3 ptInTo = (to == null) ? ptInWorld : to.InverseTransformPoint(ptInWorld); return ptInTo; } /// <summary> /// Takes a direction in the coordinate space specified by the "from" transform and transforms it to be the correct direction in the coordinate space specified by the "to" transform /// applies rotation only, no translation or scale /// </summary> /// <param name="from"></param> /// <param name="to"></param> /// <param name="dirInFrom"></param> /// <returns></returns> public static Vector3 TransformDirectionFromTo(Transform from, Transform to, Vector3 dirInFrom) { Vector3 dirInWorld = (from == null) ? dirInFrom : from.TransformDirection(dirInFrom); Vector3 dirInTo = (to == null) ? dirInWorld : to.InverseTransformDirection(dirInWorld); return dirInTo; } /// <summary> /// Takes a vectpr in the coordinate space specified by the "from" transform and transforms it to be the correct direction in the coordinate space specified by the "to" transform /// applies rotation and scale, no translation /// </summary> /// <param name="from"></param> /// <param name="to"></param> /// <param name="vecInFrom"></param> /// <returns></returns> public static Vector3 TransformVectorFromTo(Transform from, Transform to, Vector3 vecInFrom) { Vector3 vecInWorld = (from == null) ? vecInFrom : from.TransformVector(vecInFrom); Vector3 vecInTo = (to == null) ? vecInWorld : to.InverseTransformVector(vecInWorld); return vecInTo; } /// <summary> /// Takes a ray in the coordinate space specified by the "from" transform and transforms it to be the correct ray in the coordinate space specified by the "to" transform /// </summary> public static Ray TransformRayFromTo(Transform from, Transform to, Ray rayToConvert) { Ray outputRay = new Ray { origin = TransformPointFromTo(from, to, rayToConvert.origin), direction = TransformDirectionFromTo(from, to, rayToConvert.direction) }; return outputRay; } /// <summary> /// Creates a quaternion containing the rotation from the input matrix. /// </summary> /// <param name="m">Input matrix to convert to quaternion</param> /// <returns></returns> public static Quaternion QuaternionFromMatrix(Matrix4x4 m) { // TODO: test and replace with this simpler, more unity-friendly code // Quaternion q = Quaternion.LookRotation(m.GetColumn(2),m.GetColumn(1)); Quaternion q = new Quaternion(); q.w = Mathf.Sqrt(Mathf.Max(0, 1 + m[0, 0] + m[1, 1] + m[2, 2])) / 2; q.x = Mathf.Sqrt(Mathf.Max(0, 1 + m[0, 0] - m[1, 1] - m[2, 2])) / 2; q.y = Mathf.Sqrt(Mathf.Max(0, 1 - m[0, 0] + m[1, 1] - m[2, 2])) / 2; q.z = Mathf.Sqrt(Mathf.Max(0, 1 - m[0, 0] - m[1, 1] + m[2, 2])) / 2; q.x *= Mathf.Sign(q.x * (m[2, 1] - m[1, 2])); q.y *= Mathf.Sign(q.y * (m[0, 2] - m[2, 0])); q.z *= Mathf.Sign(q.z * (m[1, 0] - m[0, 1])); return q; } /// <summary> /// Extract the translation and rotation components of a Unity matrix /// </summary> public static void ToTranslationRotation(Matrix4x4 unityMtx, out Vector3 translation, out Quaternion rotation) { Vector3 upwards = new Vector3(unityMtx.m01, unityMtx.m11, unityMtx.m21); Vector3 forward = new Vector3(unityMtx.m02, unityMtx.m12, unityMtx.m22); translation = new Vector3(unityMtx.m03, unityMtx.m13, unityMtx.m23); rotation = Quaternion.LookRotation(forward, upwards); } /// <summary> /// Project vector onto XZ plane /// </summary> /// <param name="v"></param> /// <returns>result of projecting v onto XZ plane</returns> public static Vector3 XZProject(Vector3 v) { return new Vector3(v.x, 0.0f, v.z); } /// <summary> /// Project vector onto YZ plane /// </summary> /// <param name="v"></param> /// <returns>result of projecting v onto YZ plane</returns> public static Vector3 YZProject(Vector3 v) { return new Vector3(0.0f, v.y, v.z); } /// <summary> /// Project vector onto XY plane /// </summary> /// <param name="v"></param> /// <returns>result of projecting v onto XY plane</returns> public static Vector3 XYProject(Vector3 v) { return new Vector3(v.x, v.y, 0.0f); } /// <summary> /// Returns the distance between a point and an infinite line defined by two points; linePointA and linePointB /// </summary> /// <param name="point"></param> /// <param name="linePointA"></param> /// <param name="linePointB"></param> /// <returns></returns> public static float DistanceOfPointToLine(Vector3 point, Vector3 linePointA, Vector3 linePointB) { Vector3 closestPoint = ClosestPointOnLineToPoint(point, linePointA, linePointB); return (point - closestPoint).magnitude; } public static Vector3 ClosestPointOnLineToPoint(Vector3 point, Vector3 linePointA, Vector3 linePointB) { Vector3 v = linePointB - linePointA; Vector3 w = point - linePointA; float c1 = Vector3.Dot(w, v); float c2 = Vector3.Dot(v, v); float b = c1 / c2; Vector3 pointB = linePointA + (v * b); return pointB; } public static float DistanceOfPointToLineSegment(Vector3 point, Vector3 lineStart, Vector3 lineEnd) { Vector3 closestPoint = ClosestPointOnLineSegmentToPoint(point, lineStart, lineEnd); return (point - closestPoint).magnitude; } public static Vector3 ClosestPointOnLineSegmentToPoint(Vector3 point, Vector3 lineStart, Vector3 lineEnd) { Vector3 v = lineEnd - lineStart; Vector3 w = point - lineStart; float c1 = Vector3.Dot(w, v); if (c1 <= 0) { return lineStart; } float c2 = Vector3.Dot(v, v); if (c2 <= c1) { return lineEnd; } float b = c1 / c2; Vector3 pointB = lineStart + (v * b); return pointB; } public static bool TestPlanesAABB(Plane[] planes, int planeMask, Bounds bounds, out bool entirelyInside) { int planeIndex = 0; int entirelyInsideCount = 0; Vector3 boundsCenter = bounds.center; // center of bounds Vector3 boundsExtent = bounds.extents; // half diagonal // do intersection test for each active frame int mask = 1; // while active frames while (mask <= planeMask) { // if active if ((uint)(planeMask & mask) != 0) { Plane p = planes[planeIndex]; Vector3 n = p.normal; n.x = Mathf.Abs(n.x); n.y = Mathf.Abs(n.y); n.z = Mathf.Abs(n.z); float distance = p.GetDistanceToPoint(boundsCenter); float radius = Vector3.Dot(boundsExtent, n); if (distance + radius < 0) { // behind clip plane entirelyInside = false; return false; } if (distance > radius) { entirelyInsideCount++; } } mask += mask; planeIndex++; } entirelyInside = entirelyInsideCount == planes.Length; return true; } /// <summary> /// Tests component-wise if a Vector2 is in a given range /// </summary> /// <param name="vec">The vector to test</param> /// <param name="lower">The lower bounds</param> /// <param name="upper">The upper bounds</param> /// <returns>true if in range, otherwise false</returns> public static bool InRange(Vector2 vec, Vector2 lower, Vector2 upper) { return vec.x >= lower.x && vec.x <= upper.x && vec.y >= lower.y && vec.y <= upper.y; } /// <summary> /// Tests component-wise if a Vector3 is in a given range /// </summary> /// <param name="vec">The vector to test</param> /// <param name="lower">The lower bounds</param> /// <param name="upper">The upper bounds</param> /// <returns>true if in range, otherwise false</returns> public static bool InRange(Vector3 vec, Vector3 lower, Vector3 upper) { return vec.x >= lower.x && vec.x <= upper.x && vec.y >= lower.y && vec.y <= upper.y && vec.z >= lower.z && vec.z <= upper.z; } /// <summary> /// Element-wise addition of two Matrix4x4s - extension method /// </summary> /// <param name="a">matrix</param> /// <param name="b">matrix</param> /// <returns>element-wise (a+b)</returns> public static Matrix4x4 Add(Matrix4x4 a, Matrix4x4 b) { Matrix4x4 result = new Matrix4x4(); result.SetColumn(0, a.GetColumn(0) + b.GetColumn(0)); result.SetColumn(1, a.GetColumn(1) + b.GetColumn(1)); result.SetColumn(2, a.GetColumn(2) + b.GetColumn(2)); result.SetColumn(3, a.GetColumn(3) + b.GetColumn(3)); return result; } /// <summary> /// Element-wise subtraction of two Matrix4x4s - extension method /// </summary> /// <param name="a">matrix</param> /// <param name="b">matrix</param> /// <returns>element-wise (a-b)</returns> public static Matrix4x4 Subtract(Matrix4x4 a, Matrix4x4 b) { Matrix4x4 result = new Matrix4x4(); result.SetColumn(0, a.GetColumn(0) - b.GetColumn(0)); result.SetColumn(1, a.GetColumn(1) - b.GetColumn(1)); result.SetColumn(2, a.GetColumn(2) - b.GetColumn(2)); result.SetColumn(3, a.GetColumn(3) - b.GetColumn(3)); return result; } /// <summary> /// find unsigned distance of 3D point to an infinite line /// </summary> /// <param name="ray">ray that specifies an infinite line</param> /// <param name="point">3D point</param> /// <returns>unsigned perpendicular distance from point to line</returns> public static float DistanceOfPointToLine(Ray ray, Vector3 point) { return Vector3.Cross(ray.direction, point - ray.origin).magnitude; } /// <summary> /// Find 3D point that minimizes distance to 2 lines, midpoint of the shortest perpendicular line segment between them /// </summary> /// <param name="p">ray that specifies a line</param> /// <param name="q">ray that specifies a line</param> /// <returns>point nearest to the lines</returns> public static Vector3 NearestPointToLines(Ray p, Ray q) { float a = Vector3.Dot(p.direction, p.direction); float b = Vector3.Dot(p.direction, q.direction); float c = Vector3.Dot(q.direction, q.direction); Vector3 w0 = p.origin - q.origin; float den = a * c - b * b; float epsilon = 0.00001f; if (den < epsilon) { // parallel, so just average origins return 0.5f * (p.origin + q.origin); } float d = Vector3.Dot(p.direction, w0); float e = Vector3.Dot(q.direction, w0); float sc = (b * e - c * d) / den; float tc = (a * e - b * d) / den; Vector3 point = 0.5f * (p.origin + sc * p.direction + q.origin + tc * q.direction); return point; } /// <summary> /// Find 3D point that minimizes distance to a set of 2 or more lines, ignoring outliers /// </summary> /// <param name="rays">list of rays, each specifying a line, must have at least 1</param> /// <param name="ransac_iterations">number of iterations: log(1-p)/log(1-(1-E)^s) /// where p is probability of at least one sample containing s points is all inliers /// E is proportion of outliers (1-ransac_ratio) /// e.g. p=0.999, ransac_ratio=0.54, s=2 ==> log(0.001)/(log(1-0.54^2) = 20 /// </param> /// <param name="ransac_threshold">minimum distance from point to line for a line to be considered an inlier</param> /// <param name="numActualInliers">return number of inliers: lines that are within ransac_threshold of nearest point</param> /// <returns>point nearest to the set of lines, ignoring outliers</returns> public static Vector3 NearestPointToLinesRANSAC(List<Ray> rays, int ransac_iterations, float ransac_threshold, out int numActualInliers) { // start with something, just in case no inliers - this works for case of 1 or 2 rays Vector3 nearestPoint = NearestPointToLines(rays[0], rays[rays.Count - 1]); numActualInliers = 0; if (rays.Count > 2) { for (int it = 0; it < ransac_iterations; it++) { Vector3 testPoint = NearestPointToLines(rays[Random.Range(0, rays.Count)], rays[Random.Range(0, rays.Count)]); // count inliers int numInliersForIteration = 0; for (int ind = 0; ind < rays.Count; ++ind) { if (DistanceOfPointToLine(rays[ind], testPoint) < ransac_threshold) ++numInliersForIteration; } // remember best if (numInliersForIteration > numActualInliers) { numActualInliers = numInliersForIteration; nearestPoint = testPoint; } } } // now find and count actual inliers and do least-squares to find best fit var inlierList = rays.Where(r => DistanceOfPointToLine(r, nearestPoint) < ransac_threshold); numActualInliers = inlierList.Count(); if (numActualInliers >= 2) { nearestPoint = NearestPointToLinesLeastSquares(inlierList); } return nearestPoint; } /// <summary> /// Find 3D point that minimizes distance to a set of 2 or more lines /// </summary> /// <param name="rays">each ray specifies an infinite line</param> /// <returns>point nearest to the set of lines</returns> public static Vector3 NearestPointToLinesLeastSquares(IEnumerable<Ray> rays) { // finding the point nearest to the set of lines specified by rays // Use the following formula, where u_i are normalized direction // vectors along each ray and p_i is a point along each ray. // -1 // / ===== \ ===== // | \ / T\ | \ / T\ // | > |I - u u | | > |I - u u | p // | / \ i i/ | / \ i i/ i // | ===== | ===== // \ i / i Matrix4x4 sumOfProduct = Matrix4x4.zero; Vector4 sumOfProductTimesDirection = Vector4.zero; foreach (Ray r in rays) { Vector4 point = r.origin; Matrix4x4 directionColumnMatrix = new Matrix4x4(); Vector3 rNormal = r.direction.normalized; directionColumnMatrix.SetColumn(0, rNormal); Matrix4x4 directionRowMatrix = directionColumnMatrix.transpose; Matrix4x4 product = directionColumnMatrix * directionRowMatrix; Matrix4x4 identityMinusDirectionProduct = Subtract(Matrix4x4.identity, product); sumOfProduct = Add(sumOfProduct, identityMinusDirectionProduct); Vector4 vectorProduct = identityMinusDirectionProduct * point; sumOfProductTimesDirection += vectorProduct; } Matrix4x4 sumOfProductInverse = sumOfProduct.inverse; Vector3 nearestPoint = sumOfProductInverse * sumOfProductTimesDirection; return nearestPoint; } } }