// -------------------------------------------------------------------------------------------------------------------- // // Copyright (c) VRMADA, All rights reserved. // // -------------------------------------------------------------------------------------------------------------------- using System.Collections.Generic; using System.Globalization; using System.Linq; using System.Threading; using System.Threading.Tasks; using UltimateXR.Core; using UltimateXR.Extensions.System; using UltimateXR.Extensions.System.Math; using UnityEngine; namespace UltimateXR.Extensions.Unity.Math { /// /// extensions. /// public static class Vector3Ext { #region Public Types & Data /// /// Represents the NaN vector, an invalid value. /// public static ref readonly Vector3 NaN => ref s_nan; /// /// Represents the Vector3 with minimum float values per component. /// public static ref readonly Vector3 MinValue => ref s_minValue; /// /// Represents the Vector3 with maximum float values per component. /// public static ref readonly Vector3 MaxValue => ref s_maxValue; #endregion #region Public Methods /// /// Compares two Unity Vector3 objects for equality with a specified precision threshold. /// /// The first Vector3 to compare /// The second Vector3 to compare /// /// The precision threshold for float comparisons. Defaults to /// . /// /// /// true if the Vector3 objects are equal; otherwise, false. /// /// /// This method performs a component-wise comparison between two Vector3 objects. /// Each component is compared using the specified precision threshold for float comparisons. /// public static bool EqualsUsingPrecision(this Vector3 a, Vector3 b, float precisionThreshold = UxrConstants.Math.DefaultPrecisionThreshold) { return Mathf.Abs(a.x - b.x) <= precisionThreshold && Mathf.Abs(a.y - b.y) <= precisionThreshold && Mathf.Abs(a.z - b.z) <= precisionThreshold; } /// /// Checks whether the given vector has any NaN component. /// /// Source vector /// Whether any of the vector components has a NaN value public static bool IsNaN(this in Vector3 self) { return float.IsNaN(self.x) || float.IsNaN(self.y) || float.IsNaN(self.z); } /// /// Checks whether the given vector has any infinity component. /// /// Source vector /// Whether any of the vector components has an infinity value public static bool IsInfinity(this in Vector3 self) { return float.IsInfinity(self.x) || float.IsInfinity(self.y) || float.IsInfinity(self.z); } /// /// Checks whether the given vector contains valid data. /// /// Source vector /// Whether the vector contains all valid values public static bool IsValid(this in Vector3 self) { return !self.IsNaN() && !self.IsInfinity(); } /// /// Replaces NaN component values with valid values. /// /// Vector whose NaN values to replace /// Vector with valid values /// Result vector public static Vector3 FillNanWith(this in Vector3 self, in Vector3 other) { float[] result = new float[VectorLength]; for (int i = 0; i < VectorLength; ++i) { result[i] = float.IsNaN(self[i]) ? other[i] : self[i]; } return result.ToVector3(); } /// /// Computes the absolute value of each component in a vector. /// /// Source vector /// Vector whose components are the absolute values public static Vector3 Abs(this in Vector3 self) { return new Vector3(Mathf.Abs(self.x), Mathf.Abs(self.y), Mathf.Abs(self.z)); } /// /// Clamps values component by component. /// /// Vector whose components to clamp /// Minimum values /// Maximum values /// Clamped vector public static Vector3 Clamp(this in Vector3 self, in Vector3 min, in Vector3 max) { float[] result = new float[VectorLength]; for (int i = 0; i < VectorLength; ++i) { result[i] = Mathf.Clamp(self[i], min[i], max[i]); } return result.ToVector3(); } /// /// Fixes Euler angles so that they are always in the -180, 180 degrees range. /// /// Euler angles to fix /// Euler angles in the -180, 180 degrees range public static Vector3 ToEuler180(this in Vector3 self) { float[] result = new float[VectorLength]; for (int i = 0; i < VectorLength; ++i) { result[i] = self[i].ToEuler180(); } return result.ToVector3(); } /// /// Computes the average of a set of vectors. /// /// Input vectors /// Vector with components averaged public static Vector3 Average(params Vector3[] vectors) { return new Vector3(vectors.Average(v => v.x), vectors.Average(v => v.y), vectors.Average(v => v.z)); } /// /// Computes the average of a set of vectors. /// /// Input vectors /// The default value to return if the list is empty /// Vector with components averaged public static Vector3 Average(IEnumerable vectors, Vector3 defaultIfEmpty = default) { if (vectors == null || !vectors.Any()) { return defaultIfEmpty; } return new Vector3(vectors.Average(v => v.x), vectors.Average(v => v.y), vectors.Average(v => v.z)); } /// /// Computes the maximum values of a set of vectors. /// /// Input vectors /// Vector with maximum component values public static Vector3 Max(params Vector3[] vectors) { return new Vector3(vectors.Max(v => v.x), vectors.Max(v => v.y), vectors.Max(v => v.z)); } /// /// Computes the maximum values of a set of vectors. /// /// Input vectors /// The default value to return if the list is empty /// Vector with maximum component values public static Vector3 Max(IEnumerable vectors, Vector3 defaultIfEmpty = default) { if (vectors == null || !vectors.Any()) { return defaultIfEmpty; } return new Vector3(vectors.Max(v => v.x), vectors.Max(v => v.y), vectors.Max(v => v.z)); } /// /// Computes the minimum values of a set of vectors. /// /// Input vectors /// Vector with minimum component values public static Vector3 Min(params Vector3[] vectors) { return new Vector3(vectors.Min(v => v.x), vectors.Min(v => v.y), vectors.Min(v => v.z)); } /// /// Computes the minimum values of a set of vectors. /// /// Input vectors /// The default value to return if the list is empty /// Vector with minimum component values public static Vector3 Min(IEnumerable vectors, Vector3 defaultIfEmpty = default) { if (vectors == null || !vectors.Any()) { return defaultIfEmpty; } return new Vector3(vectors.Min(v => v.x), vectors.Min(v => v.y), vectors.Min(v => v.z)); } /// /// returns a vector with all components containing 1/component, checking for divisions by 0. Divisions by 0 have a /// result of 0. /// /// Source vector /// Result vector public static Vector3 Inverse(this in Vector3 self) { return new Vector3(Mathf.Approximately(self.x, 0f) ? 0f : 1f / self.x, Mathf.Approximately(self.y, 0f) ? 0f : 1f / self.y, Mathf.Approximately(self.z, 0f) ? 0f : 1f / self.z); } /// /// Gets the number of components that are different between two vectors. /// /// First vector /// Second vector /// The number of components [0, 3] that are different public static int DifferentComponentCount(Vector3 a, Vector3 b) { int count = 0; for (int axisIndex = 0; axisIndex < 3; ++axisIndex) { if (!Mathf.Approximately(a[axisIndex], b[axisIndex])) { count++; } } return count; } /// /// Multiplies two component by component. /// /// Operand A /// Operand B /// Result of multiplying both vectors component by component public static Vector3 Multiply(this in Vector3 self, in Vector3 other) { return new Vector3(self.x * other.x, self.y * other.y, self.z * other.z); } /// /// Divides a by another, checking for divisions by 0. Divisions by 0 have a result of 0. /// /// Dividend /// Divisor /// Result vector public static Vector3 Divide(this in Vector3 self, in Vector3 divisor) { return self.Multiply(divisor.Inverse()); } /// /// Transforms an array of floats to a component by component. If there are not enough values to /// read, the remaining values are set to NaN. /// /// Source data /// Result vector public static Vector3 ToVector3(this float[] data) { return data.Length switch { 0 => NaN, 1 => new Vector3(data[0], float.NaN, float.NaN), 2 => new Vector3(data[0], data[1], float.NaN), _ => new Vector3(data[0], data[1], data[2]) }; } /// /// Tries to parse a from a string. /// /// Source string /// Parsed vector or if there was an error /// Whether the vector was parsed successfully public static bool TryParse(string s, out Vector3 result) { try { result = Parse(s); return true; } catch { result = NaN; return false; } } /// /// Parses a from a string. /// /// Source string /// Parsed vector public static Vector3 Parse(string s) { s.ThrowIfNullOrWhitespace(nameof(s)); // Remove the parentheses s = s.TrimStart(' ', '(', '['); s = s.TrimEnd(' ', ')', ']'); // split the items string[] sArray = s.Split(s_cardinalSeparator, VectorLength); // store as an array float[] result = new float[VectorLength]; for (int i = 0; i < sArray.Length; ++i) { result[i] = float.TryParse(sArray[i], NumberStyles.Float, CultureInfo.InvariantCulture.NumberFormat, out float f) ? f : float.NaN; } return result.ToVector3(); } /// /// Tries to parse a from a string, asynchronously. /// /// Source string /// Optional cancellation token, to cancel the operation /// Awaitable task returning the parsed vector or null if there was an error public static Task ParseAsync(string s, CancellationToken ct = default) { return Task.Run(() => TryParse(s, out Vector3 result) ? result : (Vector3?)null, ct); } /// /// Gets the vector which is the dominant negative or positive axis it is mostly pointing towards. /// /// Vector to process /// /// Can return , , , - /// , - or -. /// public static Vector3 GetClosestAxis(this Vector3 vector) { float absX = Mathf.Abs(vector.x); float absY = Mathf.Abs(vector.y); float absZ = Mathf.Abs(vector.z); if (absX > absY) { return absX > absZ ? Mathf.Sign(vector.x) * Vector3.right : Mathf.Sign(vector.z) * Vector3.forward; } return absY > absZ ? Mathf.Sign(vector.y) * Vector3.up : Mathf.Sign(vector.z) * Vector3.forward; } /// /// Computes a perpendicular vector. /// /// Vector to compute another perpendicular to /// Perpendicular vector in 3D space public static Vector3 GetPerpendicularVector(this Vector3 vector) { if (Mathf.Approximately(vector.x, 0.0f) == false) { return new Vector3(-vector.y, vector.x, 0.0f); } if (Mathf.Approximately(vector.y, 0.0f) == false) { return new Vector3(0.0f, -vector.z, vector.y); } return new Vector3(vector.z, 0.0f, -vector.y); } /// /// Computes the signed distance from a point to a plane. /// /// The point to compute the distance from /// Point in a plane /// Plane normal /// Signed distance from a point to a plane public static float DistanceToPlane(this Vector3 point, Vector3 planePoint, Vector3 planeNormal) { return new Plane(planeNormal, planePoint).GetDistanceToPoint(point); } /// /// Computes the distance from a point to a line. /// /// The point to compute the distance from /// Point A in the line /// Point B in the line /// Distance from point to the line public static float DistanceToLine(this Vector3 point, Vector3 lineA, Vector3 lineB) { return Vector3.Cross(lineB - lineA, point - lineA).magnitude; } /// /// Computes the distance from a point to a segment. /// /// The point to compute the distance from /// Segment start point /// Segment end point /// Distance from point to the segment public static float DistanceToSegment(this Vector3 point, Vector3 segmentA, Vector3 segmentB) { Vector3 ab = segmentB - segmentA; Vector3 av = point - segmentA; if (Vector3.Dot(av, ab) <= 0.0f) { return av.magnitude; } Vector3 bv = point - segmentB; if (Vector3.Dot(bv, ab) >= 0.0) { return bv.magnitude; } return Vector3.Cross(ab, av).magnitude / ab.magnitude; } /// /// Computes the closest point in a segment to another point. /// /// The point to project /// Segment start point /// Segment end point /// Closest point in the segment public static Vector3 ProjectOnSegment(this Vector3 point, Vector3 segmentA, Vector3 segmentB) { Vector3 ab = segmentB - segmentA; Vector3 av = point - segmentA; if (Vector3.Dot(av, ab) <= 0.0f) { return segmentA; } Vector3 bv = point - segmentB; if (Vector3.Dot(bv, ab) >= 0.0) { return segmentB; } return segmentA + Vector3.Project(av, ab.normalized); } /// /// Computes the closest point in a line to another point. /// /// The point to project /// Point in the line /// Line direction /// Point projected on the line public static Vector3 ProjectOnLine(this Vector3 point, Vector3 pointInLine, Vector3 lineDirection) { return pointInLine + Vector3.Project(point - pointInLine, lineDirection); } /// /// Checks if a point is inside a sphere. Supports spheres without uniform scaling. /// /// Point to check /// Sphere collider to test against /// Boolean telling whether the point is inside public static bool IsInsideSphere(this Vector3 point, SphereCollider sphere) { Vector3 localPos = sphere.transform.InverseTransformPoint(point); return localPos.magnitude <= sphere.radius; } /// /// Checks if a point is inside of a BoxCollider. /// /// Point in world coordinates /// Box collider to test against /// Optional margin to be added to the each of the box sides /// Whether the margin is specified in world coordinates or local /// Whether point is inside public static bool IsInsideBox(this Vector3 point, BoxCollider box, Vector3 margin = default, bool marginIsWorld = true) { if (box == null) { return false; } Vector3 localPos = box.transform.InverseTransformPoint(point); if (marginIsWorld && box.transform.lossyScale != Vector3.one) { Vector3 pointPlusX = box.transform.InverseTransformPoint(point + box.transform.right); Vector3 pointPlusY = box.transform.InverseTransformPoint(point + box.transform.up); Vector3 pointPlusZ = box.transform.InverseTransformPoint(point + box.transform.forward); margin.x *= Vector3.Distance(localPos, pointPlusX); margin.y *= Vector3.Distance(localPos, pointPlusY); margin.z *= Vector3.Distance(localPos, pointPlusZ); } if (localPos.x - box.center.x >= -box.size.x * 0.5f - margin.x && localPos.x - box.center.x <= box.size.x * 0.5f + margin.x) { if (localPos.y - box.center.y >= -box.size.y * 0.5f - margin.y && localPos.y - box.center.y <= box.size.y * 0.5f + margin.y) { if (localPos.z - box.center.z >= -box.size.z * 0.5f - margin.z && localPos.z - box.center.z <= box.size.z * 0.5f + margin.z) { return true; } } } return false; } /// /// Checks if a point is inside of a box. /// /// Point in world coordinates /// The box position in world space /// The box rotation in world space /// The box scale /// The box center in local box coordinates /// The box size in local box coordinates /// Optional margin to be added to the each of the box sides /// True if it is inside, false if not public static bool IsInsideBox(this Vector3 point, Vector3 boxPosition, Quaternion boxRotation, Vector3 boxScale, Vector3 boxCenter, Vector3 boxSize, Vector3 margin = default) { Matrix4x4 boxMatrix = Matrix4x4.TRS(boxPosition, boxRotation, boxScale); Matrix4x4 inverseBoxMatrix = boxMatrix.inverse; Vector3 localPos = inverseBoxMatrix.MultiplyPoint(point); if (localPos.x - boxCenter.x >= -boxSize.x * 0.5f - margin.x && localPos.x - boxCenter.x <= boxSize.x * 0.5f + margin.x) { if (localPos.y - boxCenter.y >= -boxSize.y * 0.5f - margin.y && localPos.y - boxCenter.y <= boxSize.y * 0.5f + margin.y) { if (localPos.z - boxCenter.z >= -boxSize.z * 0.5f - margin.z && localPos.z - boxCenter.z <= boxSize.z * 0.5f + margin.z) { return true; } } } return false; } /// /// Checks if a point is inside of a BoxCollider. If it is outside, it is clamped to remain inside. /// /// Point in world coordinates /// Box collider to test against /// Point clamped inside given box volume public static Vector3 ClampToBox(this Vector3 point, BoxCollider box) { if (box == null) { return point; } Vector3 pos = box.transform.InverseTransformPoint(point); Vector3 center = box.center; Vector3 halfBoxSize = box.size * 0.5f; if (pos.x < center.x - halfBoxSize.x) { pos.x = center.x - halfBoxSize.x; } if (pos.x > center.x + halfBoxSize.x) { pos.x = center.x + halfBoxSize.x; } if (pos.y < center.y - halfBoxSize.y) { pos.y = center.y - halfBoxSize.y; } if (pos.y > center.y + halfBoxSize.y) { pos.y = center.y + halfBoxSize.y; } if (pos.z < center.z - halfBoxSize.z) { pos.z = center.z - halfBoxSize.z; } if (pos.z > center.z + halfBoxSize.z) { pos.z = center.z + halfBoxSize.z; } return box.transform.TransformPoint(pos); } /// /// Checks if a point is inside of a SphereCollider. If it is outside, it is clamped to remain inside. /// /// Point in world coordinates /// Sphere collider to test against /// Point restricted to the given sphere volume public static Vector3 ClampToSphere(this Vector3 point, SphereCollider sphere) { if (sphere == null) { return point; } Vector3 pos = sphere.transform.InverseTransformPoint(point); Vector3 center = sphere.center; float distance = Vector3.Distance(center, pos); if (distance > sphere.radius) { pos = center + (pos - center).normalized * sphere.radius; return sphere.transform.TransformPoint(pos); } return point; } /// /// Computes the rotation of a direction around an axis. /// /// Direction to rotate /// The rotation axis to use for the rotation /// Rotation angle /// Rotated direction public static Vector3 GetRotationAround(this Vector3 direction, Vector3 axis, float degrees) { return Quaternion.AngleAxis(degrees, axis) * direction; } /// /// Computes the rotation of a point around a pivot and an axis. /// /// Point to rotate /// Pivot to rotate it around to /// The rotation axis to use for the rotation /// Rotation angle /// Rotated point public static Vector3 GetRotationAround(this Vector3 point, Vector3 pivot, Vector3 axis, float degrees) { Vector3 dir = point - pivot; dir = Quaternion.AngleAxis(degrees, axis) * dir; return dir + pivot; } #endregion #region Private Types & Data private const int VectorLength = 3; private const string CardinalSeparator = ","; private static readonly char[] s_cardinalSeparator = CardinalSeparator.ToCharArray(); private static readonly Vector3 s_nan = float.NaN * Vector3.one; private static readonly Vector3 s_minValue = new Vector3(float.MinValue, float.MinValue, float.MinValue); private static readonly Vector3 s_maxValue = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue); #endregion } }