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