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