// 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;
}
}
}
