Luis Gonzalez
131 lines
3.6 KiB
C#
131 lines
3.6 KiB
C#
|
|
using System;
|
|
using Unity.Collections;
|
|
using Unity.Mathematics;
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
namespace Rukhanka.Toolbox
|
|
{
|
|
public static class MathUtils
|
|
{
|
|
public static quaternion ShortestRotation(quaternion from, quaternion to)
|
|
{
|
|
uint4 sign = math.asuint(math.dot(from, to)) & 0x80000000;
|
|
var rv = math.asfloat(sign ^ math.asuint(to.value));
|
|
return rv;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
public static quaternion FromToRotation(float3 from, float3 to)
|
|
{
|
|
var rv = quaternion.identity;
|
|
|
|
var fromNormalized = math.normalizesafe(from);
|
|
var toNormalized = math.normalizesafe(to);
|
|
float t = math.dot(fromNormalized, toNormalized);
|
|
|
|
if (t < 1 && t > -1)
|
|
{
|
|
var ac = math.acos(t);
|
|
var crossP = math.cross(from, to);
|
|
crossP = math.normalizesafe(crossP);
|
|
rv = quaternion.AxisAngle(crossP, ac);
|
|
}
|
|
else if (t <= -1)
|
|
{
|
|
var crossP = math.cross(from, math.right());
|
|
if (math.lengthsq(crossP) < math.EPSILON)
|
|
crossP = math.cross(from, math.up());
|
|
|
|
rv = quaternion.AxisAngle(crossP, math.PI * 0.5f);
|
|
}
|
|
|
|
return rv;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
public static quaternion FromToRotationForNormalizedVectors(float3 from, float3 to)
|
|
{
|
|
var w = math.cross(from, to);
|
|
var q = new quaternion(w.x, w.y, w.z, math.dot(from, to) + 1);
|
|
q = math.normalize(q);
|
|
return q;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
// Shuffle array according to given indices
|
|
public static void ShuffleArray<T>(Span<T> arr, in NativeArray<int> shuffleIndices) where T: unmanaged
|
|
{
|
|
if (arr.Length < 2) return;
|
|
if (arr.Length != shuffleIndices.Length) return;
|
|
|
|
Span<T> scatterArr = stackalloc T[arr.Length];
|
|
for (int i = 0; i < arr.Length; ++i)
|
|
{
|
|
var shuffleIndex = shuffleIndices[i];
|
|
var v = arr[shuffleIndex];
|
|
scatterArr[i] = v;
|
|
}
|
|
|
|
for (int i = 0; i < arr.Length; ++i)
|
|
{
|
|
arr[i] = scatterArr[i];
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
// Result is not normalized
|
|
public static float3 MakePerpendicularVector(float3 v)
|
|
{
|
|
var av = math.abs(v);
|
|
|
|
if (av.z < math.min(av.x, av.y))
|
|
return new float3(v.y, -v.x, 0);
|
|
|
|
if (av.y < av.x)
|
|
return new float3(-v.z, 0, v.x);
|
|
|
|
return new float3(0, v.z, -v.y);
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
public static bool PlaneLineIntersection(float3 linePoint, float3 lineVector, float4 plane, ref float3 intersectionPoint)
|
|
{
|
|
var fv = math.dot(lineVector, plane.xyz);
|
|
if (math.abs(fv) > math.EPSILON)
|
|
{
|
|
intersectionPoint = linePoint - lineVector * math.dot(plane, new float4(linePoint, 1)) * math.rcp(fv);
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
public static float4 BuildPlaneFromNormalAndPoint(float3 n, float3 p)
|
|
{
|
|
var d = -math.dot(n, p);
|
|
var pln = new float4(n, d);
|
|
return pln;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
public static float4 BuildPlaneFromThreePoints(float3 p0, float3 p1, float3 p2)
|
|
{
|
|
var dp0 = p1 - p0;
|
|
var dp1 = p2 - p0;
|
|
|
|
var n = math.cross(dp0, dp1);
|
|
n = math.normalizesafe(n);
|
|
return BuildPlaneFromNormalAndPoint(n, p0);
|
|
}
|
|
}
|
|
}
|