public static Unity.Mathematics.float4 rotate(Unity.Mathematics.float4 v, Unity.Mathematics.float4 basis0,
Unity.Mathematics.float4 basis1, float theta)
{
Unity.Mathematics.math.normalize(basis0);
Unity.Mathematics.math.normalize(basis1);
Unity.Mathematics.float4 remainder = v - (project(v, basis0) + project(v, basis1));
theta *= UnityEngine.Mathf.Deg2Rad;
Unity.Mathematics.float4 v2 = v;
Unity.Mathematics.math.normalize(v2);
if (Unity.Mathematics.math.dot(basis0, basis1) != 0f)
{
UnityEngine.Debug.LogWarning("Basis is not orthagonal : " + Unity.Mathematics.math.dot(basis0, basis1));
}
else if (Unity.Mathematics.math.dot(v2, basis0) != 1f || UnityEngine.Vector4.Dot(v, basis1) != 0f)
{
UnityEngine.Debug.LogWarning(
"Original Vector does not lie in the same plane as the first basis vector.");
}
return(UnityEngine.Vector4.Dot(v, basis0) *
(Unity.Mathematics.math.cos(theta) * basis0 + basis1 * Unity.Mathematics.math.sin(theta)) +
Unity.Mathematics.math.dot(v, basis1) *
(Unity.Mathematics.math.cos(theta) * basis1 + Unity.Mathematics.math.sin(theta) * basis0) +
remainder);
}
public static Unity.Mathematics.float4 rotate(Unity.Mathematics.float4 from, Unity.Mathematics.float4 to,
float theta)
{
Unity.Mathematics.float4 basis0 = Unity.Mathematics.math.normalize(from);
Unity.Mathematics.float4 basis1 = Unity.Mathematics.math.normalize(to - project(to, Unity.Mathematics.math.normalize(from)));
return(rotate(from, basis0, basis1, theta));
}
/// <summary>
/// Assume the following structure, return the determinant coeficients for v0, v1, v2, v3
/// v0 v1 v2 v3
/// x00 x01 x02 x03
/// x10 x11 x12 x13
/// x20 x21 x22 x23
/// </summary>
/// <param name="matrix"></param>
/// <returns></returns>
private static Unity.Mathematics.float4 determinantCoef(Unity.Mathematics.float4x3 matrix)
{
Unity.Mathematics.float4 bottomRow = matrix.c2;
float[] determinants = Determinant2X2(matrix.c0, matrix.c1);
return(new Unity.Mathematics.float4(
bottomRow.y * determinants[5] - bottomRow.z * determinants[4] + bottomRow.w * determinants[3],
-(bottomRow.x * determinants[5] - bottomRow.z * determinants[2] +
bottomRow.w * determinants[3]),
bottomRow.x * determinants[4] - bottomRow.y * determinants[2] + bottomRow.w * determinants[0],
-(bottomRow.x * determinants[3] - bottomRow.y * determinants[1] + bottomRow.z * determinants[0])
));
}
private static float[] Determinant2X2(Unity.Mathematics.float4 v0, Unity.Mathematics.float4 v1)
{
//find largest determinant of 2x2
float[] determinants = new float[6];
determinants[0] = v0.x * v1.y - v0.y * v1.x;
determinants[1] = v0.x * v1.z - v0.z * v1.x;
determinants[2] = v0.x * v1.w - v0.w * v1.x;
determinants[3] = v0.y * v1.z - v0.z * v1.y;
determinants[4] = v0.y * v1.w - v0.w * v1.y;
determinants[5] = v0.z * v1.w - v0.w * v1.z;
return(determinants);
}
/// <summary>
/// Compresses a quaternion from an unpacked Unity Quaternion to a packed uint32.
/// </summary>
/// <param name="quaternion">
/// The Unity Quaternion to compress.
/// </param>
/// <remarks>
/// <para>
/// A quaternion's length must equal 1, so the largest value can be dropped and recalculated later using
/// the "smallest three" trick. The target type is a uint, therefore 2 bits are budgeted for the index
/// of the largest component and 10 bits per smallest component (including sign bit).
/// </para>
/// <para>
/// This method is marked as unsafe due to the use of stack allocation.
/// </para>
/// </remarks>
/// <returns>
/// A uint32 representation of a quaternion.
/// </returns>
public static CompressedQuaternion FromUnityQuaternion(UnityEngine.Quaternion quaternion)
{
// Ensure we have a unit quaternion before compression.
quaternion = quaternion.normalized;
var q = new Unity.Mathematics.float4(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
// Iterate through quaternion to find the index of the largest component.
int largestIndex = 0;
var largestValue = Mathf.Abs(q[largestIndex]);
for (int i = 1; i < 4; ++i)
{
var componentAbsolute = Mathf.Abs(q[i]);
if (componentAbsolute > largestValue)
{
largestIndex = i;
largestValue = componentAbsolute;
}
}
// Since -q == q, transform the quaternion so that the largest component is positive. This means the sign
// bit of the largest component does not need to be sent.
uint negativeBit = quaternion[largestIndex] < 0 ? 1u : 0u;
// Initialise a uint with the index of the largest component. The variable is shifted left after each
// section of the uint is populated. At the end of the loop, the uint has the following structure:
// |largest index (2)|signbit (1) + component (9)|signbit (1) + component (9)|signbit (1) + component (9)|
uint compressedQuaternion = (uint)largestIndex;
for (int i = 0; i < 4; i++)
{
if (i != largestIndex)
{
// If quaternion needs to be transformed, flip the sign bit.
uint signBit = (q[i] < 0 ? 1u : 0u) ^ negativeBit;
// The maximum possible value of the second largest component in a unit quaternion is 1/sqrt(2), so
// translate the value from the range [0, 1/sqrt(2)] to [0, 1] for higher precision. Add 0.5f for
// rounding up the value before casting to a uint.
uint magnitude = (uint)(((1u << 9) - 1u) * (Mathf.Abs(q[i]) / SqrtHalf) + 0.5f);
// Shift uint by 10 bits then place the component's sign bit and 9-bit magnitude in the gap.
compressedQuaternion = (compressedQuaternion << 10) | (signBit << 9) | magnitude;
}
}
return(new CompressedQuaternion(compressedQuaternion));
}
public static Unity.Mathematics.float4 cross(Unity.Mathematics.float4 v, Unity.Mathematics.float4 w,
Unity.Mathematics.float4 u)
{
//from https://github.com/hollasch/ray4/blob/master/wire4/v4cross.c
float A, B, C, D, E, F; /* Intermediate Values */
A = v.x * w.y - v.y * w.x;
B = v.x * w.z - v.z * w.x;
C = v.x * w.w - v.w * w.x;
D = v.y * w.z - v.z * w.y;
E = v.y * w.w - v.w * w.y;
F = v.z * w.w - v.w * w.z;
return(new Unity.Mathematics.float4(
u[1] * F - u[2] * E + u[3] * D,
-(u[0] * F) + u[2] * C - u[3] * B,
u[0] * E - u[1] * C + u[3] * A,
-(u[0] * D) + u[1] * B - u[2] * A
));
}
public static Unity.Mathematics.float4 cross(Unity.Mathematics.float4 v, Unity.Mathematics.float4 w,
Unity.Mathematics.float4 u)
{
//from https://github.com/hollasch/ray4/blob/master/wire4/v4cross.c
float A, B, C, D, E, F; /* Intermediate Values */
A = (v.x * w.y) - (v.y * w.x);
B = (v.x * w.z) - (v.z * w.x);
C = (v.x * w.w) - (v.w * w.x);
D = (v.y * w.z) - (v.z * w.y);
E = (v.y * w.w) - (v.w * w.y);
F = (v.z * w.w) - (v.w * w.z);
return(new Unity.Mathematics.float4(
((u[1] * F) - (u[2] * E)) + (u[3] * D),
(-(u[0] * F) + (u[2] * C)) - (u[3] * B),
((u[0] * E) - (u[1] * C)) + (u[3] * A),
(-(u[0] * D) + (u[1] * B)) - (u[2] * A)
));
}
public static Unity.Mathematics.float4 project(Unity.Mathematics.float4 v, Unity.Mathematics.float4 dir)
{
return(Unity.Mathematics.math.dot(v, dir) * Unity.Mathematics.math.normalize(dir));
}
public static float magnitude(Unity.Mathematics.float4 v)
{
return(Unity.Mathematics.math.sqrt(v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w));
}
/// <summary>
/// Finds the point of intersection between a line and a plane
/// </summary>
/// <param name="linePos">A point on the line</param>
/// <param name="lineDir">The normalDirection of the line</param>
/// <param name="planePos">A point on the plane</param>
/// <param name="planeNorm">The normal normalDirection of the plane</param>
/// <returns></returns>
internal static Unity.Mathematics.float4 SegmentPlaneIntersection(Unity.Mathematics.float4 segmentA,
Unity.Mathematics.float4 segmentB, Unity.Mathematics.float4 planePos, Unity.Mathematics.float4 planeNorm)
{
// 0 = disjoint (no intersection)
// 1 = intersection in the unique point *I0
// 2 = the segment lies in the plane
int type = 0;
Unity.Mathematics.float4 result;
float tolerance = .00001f;
//segmenta and segmentb are endpoints of a segment
Unity.Mathematics.float4 u = segmentB - segmentA;
Unity.Mathematics.float4 w = segmentA - planePos;
float D = Unity.Mathematics.math.dot(planeNorm, u);
float N = -Unity.Mathematics.math.dot(planeNorm, w);
if (UnityEngine.Mathf.Abs(D) < tolerance)
{
// segment is parallel to plane
if (N == 0f)
{
// segment lies in plane
type = 2;
result = new Unity.Mathematics.float4(UnityEngine.Mathf.Infinity, UnityEngine.Mathf.Infinity,
UnityEngine.Mathf.Infinity, UnityEngine.Mathf.Infinity);
}
else
{
type = 0; // no intersection
result = new Unity.Mathematics.float4(UnityEngine.Mathf.Infinity, UnityEngine.Mathf.Infinity,
UnityEngine.Mathf.Infinity, UnityEngine.Mathf.Infinity);
}
}
else
{
// they are not parallel
// compute intersect param
float sI = N / D;
if (sI < 0 || sI > 1)
{
type = 0; // no intersection
result = new Unity.Mathematics.float4(UnityEngine.Mathf.Infinity, UnityEngine.Mathf.Infinity,
UnityEngine.Mathf.Infinity, UnityEngine.Mathf.Infinity);
}
else
{
result = segmentA + sI * u; // compute segment intersect point
type = 1;
}
}
/* if (type != 1)
* {
* Debug.Log(type);
* }*/
return(result);
}
public static float Angle(Unity.Mathematics.float4 from, Unity.Mathematics.float4 to)
{
return(Unity.Mathematics.math.acos(Unity.Mathematics.math.dot(Unity.Mathematics.math.normalize(from),
Unity.Mathematics.math.normalize(to))) * UnityEngine.Mathf.Rad2Deg);
}
/// <summary>
/// Return the basis of a hyper plane orthagonal to a given vector
/// </summary>
/// <param name="v"></param>
/// <returns></returns>
public static Unity.Mathematics.float4x3 basisSystem(this Unity.Mathematics.float4 v)
{
Unity.Mathematics.math.normalize(v);
//use method described here: https://www.geometrictools.com/Documentation/OrthonormalSets.pdf
if (v.x == 0 && v.y == 0 && v.z == 0 && v.w == 0)
{
UnityEngine.Debug.LogError("Can't form basis from zero vector");
}
//the vector is the first basis vector for the 4-space, orthag to the hyperplane
Unity.Mathematics.math.normalize(v);
//establish a second basis vector
Unity.Mathematics.float4 basis0;
if (v.x != 0 || v.y != 0)
{
basis0 = new UnityEngine.Vector4(v.y, v.x, 0, 0);
}
else
{
basis0 = new UnityEngine.Vector4(0, 0, v.w, v.z);
}
Unity.Mathematics.math.normalize(basis0);
float[] determinants = Determinant2X2(v, basis0);
//index of largest determinant
int idx = 0;
for (int i = 0; i < 6; i++)
{
if (determinants[i] > determinants[idx])
{
idx = i;
}
}
if (determinants[idx] != 0)
{
UnityEngine.Debug.LogError("No non-zero determinant");
}
//choose bottom row of det matrix to generate next basis vector
Unity.Mathematics.float4 bottomRow;
if (idx == 0 || idx == 1 || idx == 3)
{
bottomRow = new Unity.Mathematics.float4(0, 0, 0, 1);
}
else if (idx == 2 || idx == 4)
{
bottomRow = new Unity.Mathematics.float4(0, 0, 1, 0);
}
else
{
bottomRow = new Unity.Mathematics.float4(0, 1, 0, 0);
}
Unity.Mathematics.float4 basis1 = determinantCoef(new Unity.Mathematics.float4x3(v, basis0, bottomRow));
Unity.Mathematics.math.normalize(basis1);
Unity.Mathematics.float4 basis2 = determinantCoef(new Unity.Mathematics.float4x3(v, basis0, basis1));
Unity.Mathematics.math.normalize(basis2);
//returns the basis that spans the hyperplane orthogonal to v
Unity.Mathematics.float4x3 basis = new Unity.Mathematics.float4x3(basis0, basis1, basis2);
//check that v is orthogonal.
// v.projectDownDimension(basis, ProjectionMethod.parallel, null, null, null);
// if (v.x != 0 || v.y != 0 || v.z != 0) UnityEngine.Debug.LogError("Basis is not orthogonal to v");
return(basis);
}
/// <summary>
/// Projects a vector onto another vector using a dot product.
/// </summary>
/// <param name="v"></param>
/// <param name="axis"></param>
/// <returns></returns>
public static Unity.Mathematics.float4 project(this Unity.Mathematics.float4 v, Unity.Mathematics.float4 axis)
{
Unity.Mathematics.math.normalize(axis);
return(Unity.Mathematics.math.dot(v, axis) * axis);
}
