diff --git a/Math/Matrix/Mat22.cs b/Math/Matrix/Mat22.cs index 094892c51..54989c99c 100644 --- a/Math/Matrix/Mat22.cs +++ b/Math/Matrix/Mat22.cs @@ -26,7 +26,7 @@ namespace Vintagestory.API.MathTools /// /// 2x2 Matrix /// - public class Mat22 + public partial class Mat22 { /// /// Creates a new identity mat2 diff --git a/Math/Matrix/Mat23.cs b/Math/Matrix/Mat23.cs index b39379b7e..e83df59e3 100644 --- a/Math/Matrix/Mat23.cs +++ b/Math/Matrix/Mat23.cs @@ -39,7 +39,7 @@ namespace Vintagestory.API.MathTools /// * /// * The last column is ignored so the array is shorter and operations are faster. /// - public class Mat23 + public partial class Mat23 { /// /// Creates a new identity mat2d @@ -177,7 +177,7 @@ public static float[] Multiply(float[] output, float[] a, float[] b) } /// - /// Alias for {@link mat2d.multiply} @function + /// Alias for /// /// /// diff --git a/Math/Matrix/Mat3d.cs b/Math/Matrix/Mat3d.cs index 90f4add63..1384b885e 100644 --- a/Math/Matrix/Mat3d.cs +++ b/Math/Matrix/Mat3d.cs @@ -23,19 +23,12 @@ namespace Vintagestory.API.MathTools { - public class Mat3d + public partial class Mat3d { - // /** - // * @class 3x3 Matrix - // * @name mat3 - // */ - //var mat3 = {}; - - ///** - // * Creates a new identity mat3 - // * - // * @returns {mat3} a new 3x3 matrix - // */ + /// + /// Creates a new identity mat3 + /// + /// new 3x3 matrix public static double[] Create() { double[] output = new double[9]; @@ -51,13 +44,12 @@ public static double[] Create() return output; } - ///** - // * Copies the upper-left 3x3 values into the given mat3. - // * - // * @param {mat3} output the receiving 3x3 matrix - // * @param {mat4} a the source 4x4 matrix - // * @returns {mat3} output - // */ + /// + /// Copies the upper-left 3x3 values into the given mat3. + /// + /// the receiving 3x3 matrix + /// the source 4x4 matrix + /// output public static double[] FromMat4(double[] output, double[] a) { output[0] = a[0]; @@ -72,12 +64,11 @@ public static double[] FromMat4(double[] output, double[] a) return output; } - ///** - // * Creates a new mat3 initialized with values from an existing matrix - // * - // * @param {mat3} a matrix to clone - // * @returns {mat3} a new 3x3 matrix - // */ + /// + /// Creates a new mat3 initialized with values from an existing matrix + /// + /// matrix to clone + /// new 3x3 matrix public static double[] CloneIt(double[] a) { double[] output = new double[9]; @@ -93,13 +84,12 @@ public static double[] CloneIt(double[] a) return output; } - ///** - // * Copy the values from one mat3 to another - // * - // * @param {mat3} output the receiving matrix - // * @param {mat3} a the source matrix - // * @returns {mat3} output - // */ + /// + /// Copy the values from one mat3 to another + /// + /// the receiving matrix + /// the source matrix + /// output public static double[] Copy(double[] output, double[] a) { output[0] = a[0]; @@ -114,12 +104,11 @@ public static double[] Copy(double[] output, double[] a) return output; } - ///** - // * Set a mat3 to the identity matrix - // * - // * @param {mat3} output the receiving matrix - // * @returns {mat3} output - // */ + /// + /// Set a mat3 to the identity matrix + /// + /// the receiving matrix + /// output public static double[] Identity_(double[] output) { output[0] = 1; @@ -134,13 +123,12 @@ public static double[] Identity_(double[] output) return output; } - ///** - // * Transpose the values of a mat3 - // * - // * @param {mat3} output the receiving matrix - // * @param {mat3} a the source matrix - // * @returns {mat3} output - // */ + /// + /// Transpose the values of a mat3 + /// + /// the receiving matrix + /// the source matrix + /// output public static double[] Transpose(double[] output, double[] a) { // If we are transposing ourselves we can skip a few steps but have to cache some values @@ -172,13 +160,12 @@ public static double[] Transpose(double[] output, double[] a) return output; } - ///** - // * Inverts a mat3 - // * - // * @param {mat3} output the receiving matrix - // * @param {mat3} a the source matrix - // * @returns {mat3} output - // */ + /// + /// Inverts a mat3 + /// + /// the receiving matrix + /// the source matrix + /// output public static double[] Invert(double[] output, double[] a) { double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; @@ -211,13 +198,12 @@ public static double[] Invert(double[] output, double[] a) return output; } - ///** - // * Calculates the adjugate of a mat3 - // * - // * @param {mat3} output the receiving matrix - // * @param {mat3} a the source matrix - // * @returns {mat3} output - // */ + /// + /// Calculates the adjugate of a mat3 + /// + /// the receiving matrix + /// the source matrix + /// output public static double[] Adjoint(double[] output, double[] a) { double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; @@ -236,12 +222,11 @@ public static double[] Adjoint(double[] output, double[] a) return output; } - ///** - // * Calculates the determinant of a mat3 - // * - // * @param {mat3} a the source matrix - // * @returns {Number} determinant of a - // */ + /// + /// Calculates the determinant of a mat3 + /// + /// the source matrix + /// determinant of a public static double Determinant(double[] a) { double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; @@ -251,14 +236,13 @@ public static double Determinant(double[] a) return a00 * (a22 * a11 - a12 * a21) + a01 * (-a22 * a10 + a12 * a20) + a02 * (a21 * a10 - a11 * a20); } - ///** - // * Multiplies two mat3's - // * - // * @param {mat3} output the receiving matrix - // * @param {mat3} a the first operand - // * @param {mat3} b the second operand - // * @returns {mat3} output - // */ + /// + /// Multiplies two mat3's + /// + /// the receiving matrix + /// the first operand + /// the second operand + /// output public static double[] Multiply(double[] output, double[] a, double[] b) { double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; @@ -283,22 +267,21 @@ public static double[] Multiply(double[] output, double[] a, double[] b) return output; } - ///** - // * Alias for {@link mat3.multiply} - // * @function - // */ + /// + /// Alias for + /// public static double[] Mul(double[] output, double[] a, double[] b) { return Multiply(output, a, b); } - ///** - // * Translate a mat3 by the given vector - // * - // * @param {mat3} output the receiving matrix - // * @param {mat3} a the matrix to translate - // * @param {vec2} v vector to translate by - // * @returns {mat3} output - // */ + + /// + /// Translate a mat3 by the given vector + /// + /// the receiving matrix + /// the matrix to translate + /// vector to translate by + /// output public static double[] Translate(double[] output, double[] a, double[] v) { double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; @@ -320,14 +303,13 @@ public static double[] Translate(double[] output, double[] a, double[] v) return output; } - ///** - // * Rotates a mat3 by the given angle - // * - // * @param {mat3} output the receiving matrix - // * @param {mat3} a the matrix to rotate - // * @param {Number} rad the angle to rotate the matrix by - // * @returns {mat3} output - // */ + /// + /// Rotates a mat3 by the given angle + /// + /// the receiving matrix + /// the matrix to rotate + /// the angle to rotate the matrix by + /// output public static double[] Rotate(double[] output, double[] a, double rad) { double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; @@ -351,14 +333,13 @@ public static double[] Rotate(double[] output, double[] a, double rad) return output; } - ///** - // * Scales the mat3 by the dimensions in the given vec2 - // * - // * @param {mat3} output the receiving matrix - // * @param {mat3} a the matrix to rotate - // * @param {vec2} v the vec2 to scale the matrix by - // * @returns {mat3} output - // **/ + /// + /// Scales the mat3 by the dimensions in the given vec2 + /// + /// the receiving matrix + /// the matrix to rotate + /// the vec2 to scale the matrix by + /// output public static double[] Scale(double[] output, double[] a, double[] v) { double x = v[0]; double y = v[1]; @@ -377,13 +358,12 @@ public static double[] Scale(double[] output, double[] a, double[] v) return output; } - ///** - // * Copies the values from a mat2d into a mat3 - // * - // * @param {mat3} output the receiving matrix - // * @param {mat2d} a the matrix to copy - // * @returns {mat3} output - // **/ + /// + /// Copies the values from a mat2d into a mat3 + /// + /// the receiving matrix + /// the matrix to copy + /// output public static double[] FromMat2d(double[] output, double[] a) { output[0] = a[0]; @@ -400,14 +380,12 @@ public static double[] FromMat2d(double[] output, double[] a) return output; } - ///** - //* Calculates a 3x3 matrix from the given quaternion - //* - //* @param {mat3} output mat3 receiving operation result - //* @param {quat} q Quaternion to create matrix from - //* - //* @returns {mat3} output - //*/ + /// + /// Calculates a 3x3 matrix from the given quaternion + /// + /// mat3 receiving operation result + /// Quaternion to create matrix from + /// output public static double[] FromQuat(double[] output, double[] q) { double x = q[0]; double y = q[1]; double z = q[2]; double w = q[3]; @@ -440,14 +418,12 @@ public static double[] FromQuat(double[] output, double[] q) return output; } - ///** - //* Calculates a 3x3 normal matrix (transpose inverse) from the 4x4 matrix - //* - //* @param {mat3} output mat3 receiving operation result - //* @param {mat4} a Mat4 to derive the normal matrix from - //* - //* @returns {mat3} output - //*/ + /// + /// Calculates a 3x3 normal matrix (transpose inverse) from the 4x4 matrix + /// + /// mat3 receiving operation result + /// Mat4 to derive the normal matrix from + /// output public static double[] NormalFromMat4(double[] output, double[] a) { double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; double a03 = a[3]; @@ -492,25 +468,5 @@ public static double[] NormalFromMat4(double[] output, double[] a) return output; } - - ///** - // * Returns a string representation of a mat3 - // * - // * @param {mat3} mat matrix to represent as a string - // * @returns {String} string representation of the matrix - // */ - //mat3.str = function (a) { - // return 'mat3(' + a[0] + ', ' + a[1] + ', ' + a[2] + ', ' + - // a[3] + ', ' + a[4] + ', ' + a[5] + ', ' + - // a[6] + ', ' + a[7] + ', ' + a[8] + ')'; - //}; - - //if(typeof(exports) !== 'undefined') { - // exports.mat3 = mat3; - //} - void f() - { - } } - } diff --git a/Math/Matrix/Mat3f.cs b/Math/Matrix/Mat3f.cs index 76965e92f..ff71f980e 100644 --- a/Math/Matrix/Mat3f.cs +++ b/Math/Matrix/Mat3f.cs @@ -23,19 +23,12 @@ namespace Vintagestory.API.MathTools { - public class Mat3f + public partial class Mat3f { - // /** - // * @class 3x3 Matrix - // * @name mat3 - // */ - //var mat3 = {}; - - ///** - // * Creates a new identity mat3 - // * - // * @returns {mat3} a new 3x3 matrix - // */ + /// + /// Creates a new identity mat3 + /// + /// new 3x3 matrix public static float[] Create() { float[] output = new float[9]; @@ -51,13 +44,12 @@ public static float[] Create() return output; } - ///** - // * Copies the upper-left 3x3 values into the given mat3. - // * - // * @param {mat3} output the receiving 3x3 matrix - // * @param {mat4} a the source 4x4 matrix - // * @returns {mat3} output - // */ + /// + /// Copies the upper-left 3x3 values into the given mat3. + /// + /// the receiving 3x3 matrix + /// the source 4x4 matrix + /// output public static float[] FromMat4(float[] output, float[] a) { output[0] = a[0]; @@ -72,12 +64,11 @@ public static float[] FromMat4(float[] output, float[] a) return output; } - ///** - // * Creates a new mat3 initialized with values from an existing matrix - // * - // * @param {mat3} a matrix to clone - // * @returns {mat3} a new 3x3 matrix - // */ + /// + /// Creates a new mat3 initialized with values from an existing matrix + /// + /// matrix to clone + /// new 3x3 matrix public static float[] CloneIt(float[] a) { float[] output = new float[9]; @@ -93,13 +84,12 @@ public static float[] CloneIt(float[] a) return output; } - ///** - // * Copy the values from one mat3 to another - // * - // * @param {mat3} output the receiving matrix - // * @param {mat3} a the source matrix - // * @returns {mat3} output - // */ + /// + /// Copy the values from one mat3 to another + /// + /// the receiving matrix + /// the source matrix + /// output public static float[] Copy(float[] output, float[] a) { output[0] = a[0]; @@ -114,12 +104,11 @@ public static float[] Copy(float[] output, float[] a) return output; } - ///** - // * Set a mat3 to the identity matrix - // * - // * @param {mat3} output the receiving matrix - // * @returns {mat3} output - // */ + /// + /// Set a mat3 to the identity matrix + /// + /// the receiving matrix + /// output public static float[] Identity_(float[] output) { output[0] = 1; @@ -134,13 +123,12 @@ public static float[] Identity_(float[] output) return output; } - ///** - // * Transpose the values of a mat3 - // * - // * @param {mat3} output the receiving matrix - // * @param {mat3} a the source matrix - // * @returns {mat3} output - // */ + /// + /// Transpose the values of a mat3 + /// + /// the receiving matrix + /// the source matrix + /// output public static float[] Transpose(float[] output, float[] a) { // If we are transposing ourselves we can skip a few steps but have to cache some values @@ -172,13 +160,12 @@ public static float[] Transpose(float[] output, float[] a) return output; } - ///** - // * Inverts a mat3 - // * - // * @param {mat3} output the receiving matrix - // * @param {mat3} a the source matrix - // * @returns {mat3} output - // */ + /// + /// Inverts a mat3 + /// + /// the receiving matrix + /// the source matrix + /// output public static float[] Invert(float[] output, float[] a) { float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; @@ -211,13 +198,12 @@ public static float[] Invert(float[] output, float[] a) return output; } - ///** - // * Calculates the adjugate of a mat3 - // * - // * @param {mat3} output the receiving matrix - // * @param {mat3} a the source matrix - // * @returns {mat3} output - // */ + /// + /// Calculates the adjugate of a mat3 + /// + /// the receiving matrix + /// the source matrix + /// output public static float[] Adjoint(float[] output, float[] a) { float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; @@ -236,12 +222,11 @@ public static float[] Adjoint(float[] output, float[] a) return output; } - ///** - // * Calculates the determinant of a mat3 - // * - // * @param {mat3} a the source matrix - // * @returns {Number} determinant of a - // */ + /// + /// Calculates the determinant of a mat3 + /// + /// the source matrix + /// determinant of a public static float Determinant(float[] a) { float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; @@ -251,14 +236,13 @@ public static float Determinant(float[] a) return a00 * (a22 * a11 - a12 * a21) + a01 * (-a22 * a10 + a12 * a20) + a02 * (a21 * a10 - a11 * a20); } - ///** - // * Multiplies two mat3's - // * - // * @param {mat3} output the receiving matrix - // * @param {mat3} a the first operand - // * @param {mat3} b the second operand - // * @returns {mat3} output - // */ + /// + /// Multiplies two mat3's + /// + /// the receiving matrix + /// the first operand + /// the second operand + /// output public static float[] Multiply(float[] output, float[] a, float[] b) { float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; @@ -283,22 +267,21 @@ public static float[] Multiply(float[] output, float[] a, float[] b) return output; } - ///** - // * Alias for {@link mat3.multiply} - // * @function - // */ + /// + /// Alias for + /// public static float[] Mul(float[] output, float[] a, float[] b) { return Multiply(output, a, b); } - ///** - // * Translate a mat3 by the given vector - // * - // * @param {mat3} output the receiving matrix - // * @param {mat3} a the matrix to translate - // * @param {vec2} v vector to translate by - // * @returns {mat3} output - // */ + + /// + /// Translate a mat3 by the given vector + /// + /// the receiving matrix + /// the matrix to translate + /// vector to translate by + /// output public static float[] Translate(float[] output, float[] a, float[] v) { float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; @@ -320,14 +303,13 @@ public static float[] Translate(float[] output, float[] a, float[] v) return output; } - ///** - // * Rotates a mat3 by the given angle - // * - // * @param {mat3} output the receiving matrix - // * @param {mat3} a the matrix to rotate - // * @param {Number} rad the angle to rotate the matrix by - // * @returns {mat3} output - // */ + /// + /// Rotates a mat3 by the given angle + /// + /// the receiving matrix + /// the matrix to rotate + /// the angle to rotate the matrix by + /// output public static float[] Rotate(float[] output, float[] a, float rad) { float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; @@ -351,14 +333,13 @@ public static float[] Rotate(float[] output, float[] a, float rad) return output; } - ///** - // * Scales the mat3 by the dimensions in the given vec2 - // * - // * @param {mat3} output the receiving matrix - // * @param {mat3} a the matrix to rotate - // * @param {vec2} v the vec2 to scale the matrix by - // * @returns {mat3} output - // **/ + /// + /// Scales the mat3 by the dimensions in the given vec2 + /// + /// the receiving matrix + /// the matrix to rotate + /// the vec2 to scale the matrix by + /// output public static float[] Scale(float[] output, float[] a, float[] v) { float x = v[0]; float y = v[1]; @@ -377,13 +358,13 @@ public static float[] Scale(float[] output, float[] a, float[] v) return output; } - ///** - // * Copies the values from a mat2d into a mat3 - // * - // * @param {mat3} output the receiving matrix - // * @param {mat2d} a the matrix to copy - // * @returns {mat3} output - // **/ + /// + /// Copies the values from a mat2d into a mat3 + /// + /// the receiving matrix + /// the matrix to copy + /// output + /// */ public static float[] FromMat2d(float[] output, float[] a) { output[0] = a[0]; @@ -400,14 +381,12 @@ public static float[] FromMat2d(float[] output, float[] a) return output; } - ///** - //* Calculates a 3x3 matrix from the given quaternion - //* - //* @param {mat3} output mat3 receiving operation result - //* @param {quat} q Quaternion to create matrix from - //* - //* @returns {mat3} output - //*/ + /// + /// Calculates a 3x3 matrix from the given quaternion + /// + /// mat3 receiving operation result + /// Quaternion to create matrix from + /// output public static float[] FromQuat(float[] output, float[] q) { float x = q[0]; float y = q[1]; float z = q[2]; float w = q[3]; @@ -440,14 +419,12 @@ public static float[] FromQuat(float[] output, float[] q) return output; } - ///** - //* Calculates a 3x3 normal matrix (transpose inverse) from the 4x4 matrix - //* - //* @param {mat3} output mat3 receiving operation result - //* @param {mat4} a Mat4 to derive the normal matrix from - //* - //* @returns {mat3} output - //*/ + /// + /// Calculates a 3x3 normal matrix (transpose inverse) from the 4x4 matrix + /// + /// mat3 receiving operation result + /// Mat4 to derive the normal matrix from + /// output public static float[] NormalFromMat4(float[] output, float[] a) { float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; float a03 = a[3]; @@ -492,25 +469,5 @@ public static float[] NormalFromMat4(float[] output, float[] a) return output; } - - ///** - // * Returns a string representation of a mat3 - // * - // * @param {mat3} mat matrix to represent as a string - // * @returns {String} string representation of the matrix - // */ - //mat3.str = function (a) { - // return 'mat3(' + a[0] + ', ' + a[1] + ', ' + a[2] + ', ' + - // a[3] + ', ' + a[4] + ', ' + a[5] + ', ' + - // a[6] + ', ' + a[7] + ', ' + a[8] + ')'; - //}; - - //if(typeof(exports) !== 'undefined') { - // exports.mat3 = mat3; - //} - void f() - { - } } - } diff --git a/Math/Matrix/Mat4d.cs b/Math/Matrix/Mat4d.cs index 99998bf53..f38879cd9 100644 --- a/Math/Matrix/Mat4d.cs +++ b/Math/Matrix/Mat4d.cs @@ -26,7 +26,7 @@ namespace Vintagestory.API.MathTools /// /// 4x4 Matrix Math /// - public class Mat4d + public partial class Mat4d { /// /// Creates a new identity mat4 @@ -35,7 +35,7 @@ public class Mat4d /// 2 6 10 14 /// 3 7 11 15 /// - /// {mat4} a new 4x4 matrix + /// new 4x4 matrix public static double[] Create() { return new double[16] { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; @@ -67,7 +67,7 @@ public static float[] ToMat4f(float[] output, double[] input) /// Creates a new mat4 initialized with values from an existing matrix /// /// a matrix to clone - /// {mat4} a new 4x4 matrix + /// new 4x4 matrix public static double[] CloneIt(double[] a) { double[] output = new double[16]; @@ -93,9 +93,9 @@ public static double[] CloneIt(double[] a) /// /// Copy the values from one mat4 to another /// - /// {mat4} out the receiving matrix - /// {mat4} a the source matrix - /// {mat4} out + /// the receiving matrix + /// the source matrix + /// public static double[] Copy(double[] output, double[] a) { for (int i = 0; i < output.Length; i += 4) @@ -111,8 +111,8 @@ public static double[] Copy(double[] output, double[] a) /// /// Set a mat4 to the identity matrix /// - /// {mat4} out the receiving matrix - /// {mat4} out + /// the receiving matrix + /// public static double[] Identity(double[] output) { output[0] = 1; @@ -137,9 +137,9 @@ public static double[] Identity(double[] output) /// /// Transpose the values of a mat4 /// - /// {mat4} out the receiving matrix - /// {mat4} a the source matrix - /// {mat4} out + /// the receiving matrix + /// the source matrix + /// public static double[] Transpose(double[] output, double[] a) { // If we are transposing ourselves we can skip a few steps but have to cache some values @@ -188,9 +188,9 @@ public static double[] Transpose(double[] output, double[] a) /// /// Inverts a mat4 /// - /// {mat4} out the receiving matrix - /// {mat4} a the source matrix - /// {mat4} out + /// the receiving matrix + /// the source matrix + /// public static double[] Invert(double[] output, double[] a) { double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; double a03 = a[3]; @@ -244,9 +244,9 @@ public static double[] Invert(double[] output, double[] a) /// /// Calculates the adjugate of a mat4 /// - /// {mat4} out the receiving matrix - /// {mat4} a the source matrix - /// {mat4} out + /// the receiving matrix + /// the source matrix + /// public static double[] Adjoint(double[] output, double[] a) { double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; double a03 = a[3]; @@ -276,8 +276,8 @@ public static double[] Adjoint(double[] output, double[] a) /// /// Calculates the determinant of a mat4 /// - /// {mat4} a the source matrix - /// {Number} determinant of a + /// the source matrix + /// of a public static double Determinant(double[] a) { double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; double a03 = a[3]; @@ -306,10 +306,10 @@ public static double Determinant(double[] a) /// Multiplies two mat4's /// /// - /// {mat4} out the receiving matrix - /// {mat4} a the first operand - /// {mat4} b the second operand - /// {mat4} out + /// the receiving matrix + /// the first operand + /// the second operand + /// public static double[] Multiply(double[] output, double[] a, double[] b) { double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; double a03 = a[3]; @@ -350,10 +350,10 @@ public static double[] Multiply(double[] output, double[] a, double[] b) /// Multiplies two mat4's /// /// - /// {mat4} out the receiving matrix - /// {mat4} a the first operand - /// {mat4} b the second operand - /// {mat4} out + /// the receiving matrix + /// the first operand + /// the second operand + /// public static double[] Multiply(double[] output, float[] a, double[] b) { double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; double a03 = a[3]; @@ -438,12 +438,12 @@ public static bool IsTranslationOnly(double[] matrix) /// /// Translate a mat4 by the given vector /// - /// {mat4} out the receiving matrix - /// {mat4} a the matrix to translate - /// {vec3} v vector to translate by + /// the receiving matrix + /// the matrix to translate + /// vector to translate by /// /// - /// {mat4} out + /// public static double[] Translate(double[] output, double[] input, double x, double y, double z) { if (input == output) @@ -479,10 +479,10 @@ public static double[] Translate(double[] output, double[] input, double x, doub /// /// Translate a mat4 by the given vector /// - /// {mat4} out the receiving matrix - /// {mat4} a the matrix to translate - /// {vec3} v vector to translate by - /// {mat4} out + /// the receiving matrix + /// the matrix to translate + /// vector to translate by + /// public static double[] Translate(double[] output, double[] input, double[] translate) { double x = translate[0]; double y = translate[1]; double z = translate[2]; @@ -519,10 +519,10 @@ public static double[] Translate(double[] output, double[] input, double[] trans /// /// Scales the mat4 by the dimensions in the given vec3 /// - /// {mat4} out the receiving matrix - /// {mat4} a the matrix to scale - /// {vec3} v the vec3 to scale the matrix by - /// {mat4} out + /// the receiving matrix + /// the matrix to scale + /// the vec3 to scale the matrix by + /// public static double[] Scale(double[] output, double[] a, double[] v) { double x = v[0]; double y = v[1]; double z = v[2]; @@ -565,11 +565,11 @@ public static void Scale(double[] matrix, double x, double y, double z) /// /// Rotates a mat4 by the given angle /// - /// {mat4} out the receiving matrix - /// {mat4} a the matrix to rotate - /// {Number} rad the angle to rotate the matrix by - /// {vec3} axis the axis to rotate around - /// {mat4} out + /// the receiving matrix + /// the matrix to rotate + /// the angle to rotate the matrix by + /// the axis to rotate around + /// public static double[] Rotate(double[] output, double[] a, double rad, double[] axis) { double x = axis[0]; double y = axis[1]; double z = axis[2]; @@ -645,10 +645,10 @@ public static double[] Rotate(double[] output, double[] a, double rad, double x, /// /// Rotates a matrix by the given angle around the X axis /// - /// {mat4} out the receiving matrix - /// {mat4} a the matrix to rotate - /// {Number} rad the angle to rotate the matrix by - /// {mat4} out + /// the receiving matrix + /// the matrix to rotate + /// the angle to rotate the matrix by + /// public static double[] RotateX(double[] output, double[] a, double rad) { double s = GameMath.Sin(rad); @@ -690,10 +690,10 @@ public static double[] RotateX(double[] output, double[] a, double rad) /// /// Rotates a matrix by the given angle around the Y axis /// - /// {mat4} out the receiving matrix - /// {mat4} a the matrix to rotate - /// {Number} rad the angle to rotate the matrix by - /// {mat4} out + /// the receiving matrix + /// the matrix to rotate + /// the angle to rotate the matrix by + /// public static double[] RotateY(double[] output, double[] a, double rad) { double s = GameMath.Sin(rad); @@ -735,10 +735,10 @@ public static double[] RotateY(double[] output, double[] a, double rad) /// /// Rotates a matrix by the given angle around the Z axis /// - /// {mat4} out the receiving matrix - /// {mat4} a the matrix to rotate - /// {Number} rad the angle to rotate the matrix by - /// {mat4} out + /// the receiving matrix + /// the matrix to rotate + /// the angle to rotate the matrix by + /// public static double[] RotateZ(double[] output, double[] a, double rad) { double s = GameMath.Sin(rad); @@ -786,10 +786,10 @@ public static double[] RotateZ(double[] output, double[] a, double rad) /// quat4.toMat4(quat, quatMat); /// mat4.multiply(dest, quatMat); /// - /// {mat4} out mat4 receiving operation result - /// {quat4} q Rotation quaternion - /// {vec3} v Translation vector - /// {mat4} out + /// mat4 receiving operation result + /// Rotation quaternion + /// Translation vector + /// public static double[] FromRotationTranslation(double[] output, double[] q, double[] v) { // Quaternion math @@ -831,9 +831,9 @@ public static double[] FromRotationTranslation(double[] output, double[] q, doub /// /// Calculates a 4x4 matrix from the given quaternion /// - /// {mat4} out mat4 receiving operation result - /// {quat} q Quaternion to create matrix from - /// {mat4} out + /// mat4 receiving operation result + /// Quaternion to create matrix from + /// public static double[] FromQuat(double[] output, double[] q) { double x = q[0]; double y = q[1]; double z = q[2]; double w = q[3]; @@ -877,14 +877,14 @@ public static double[] FromQuat(double[] output, double[] q) /// /// Generates a frustum matrix with the given bounds /// - /// {mat4} out mat4 frustum matrix will be written into - /// {Number} left Left bound of the frustum - /// {Number} right Right bound of the frustum - /// {Number} bottom Bottom bound of the frustum - /// {Number} top Top bound of the frustum - /// {Number} near Near bound of the frustum - /// {Number} far Far bound of the frustum - /// {mat4} out + /// mat4 frustum matrix will be written into + /// Left bound of the frustum + /// Right bound of the frustum + /// Bottom bound of the frustum + /// Top bound of the frustum + /// Near bound of the frustum + /// Far bound of the frustum + /// public static double[] Frustum(double[] output, double left, double right, double bottom, double top, double near, double far) { double rl = 1 / (right - left); @@ -912,12 +912,12 @@ public static double[] Frustum(double[] output, double left, double right, doubl /// /// Generates a perspective projection matrix with the given bounds /// - /// {mat4} out mat4 frustum matrix will be written into - /// {number} fovy Vertical field of view in radians - /// {number} aspect Aspect ratio. typically viewport width/height - /// {number} near Near bound of the frustum - /// {number} far Far bound of the frustum - /// {mat4} out + /// mat4 frustum matrix will be written into + /// Vertical field of view in radians + /// Aspect ratio. typically viewport width/height + /// Near bound of the frustum + /// Far bound of the frustum + /// public static double[] Perspective(double[] output, double fovy, double aspect, double near, double far) { double one = 1; @@ -945,14 +945,14 @@ public static double[] Perspective(double[] output, double fovy, double aspect, /// /// Generates a orthogonal projection matrix with the given bounds /// - /// {mat4} out mat4 frustum matrix will be written into - /// {number} left Left bound of the frustum - /// {number} right Right bound of the frustum - /// {number} bottom Bottom bound of the frustum - /// {number} top Top bound of the frustum - /// {number} near Near bound of the frustum - /// {number} far Far bound of the frustum - /// {mat4} out + /// mat4 frustum matrix will be written into + /// Left bound of the frustum + /// Right bound of the frustum + /// Bottom bound of the frustum + /// Top bound of the frustum + /// Near bound of the frustum + /// Far bound of the frustum + /// public static double[] Ortho(double[] output, double left, double right, double bottom, double top, double near, double far) { double lr = 1 / (left - right); @@ -980,11 +980,11 @@ public static double[] Ortho(double[] output, double left, double right, double /// /// Generates a look-at matrix with the given eye position, focal point, and up axis /// - /// {mat4} out mat4 frustum matrix will be written into - /// {vec3} eye Position of the viewer - /// {vec3} center Point the viewer is looking at - /// {vec3} up vec3 pointing up - /// {mat4} out + /// mat4 frustum matrix will be written into + /// Position of the viewer + /// Point the viewer is looking at + /// vec3 pointing up + /// public static double[] LookAt(double[] output, double[] eye, double[] center, double[] up) { double x0; double x1; double x2; double y0; double y1; double y2; double z0; double z1; double z2; double len; diff --git a/Math/Matrix/Mat4f.cs b/Math/Matrix/Mat4f.cs index 145250baa..69badec31 100644 --- a/Math/Matrix/Mat4f.cs +++ b/Math/Matrix/Mat4f.cs @@ -29,7 +29,7 @@ namespace Vintagestory.API.MathTools /// /// 4x4 Matrix Math /// - public class Mat4f + public partial class Mat4f { /// /// Creates a new identity mat4 @@ -38,7 +38,7 @@ public class Mat4f /// 2 6 10 14 /// 3 7 11 15 /// - /// {mat4} a new 4x4 matrix + /// new 4x4 matrix public static float[] Create() { float[] output = new float[16]; // a new array is automatically initialised to the default float value of 0f @@ -54,7 +54,7 @@ public static float[] Create() /// Creates a new mat4 initialized with values from an existing matrix /// /// a matrix to clone - /// {mat4} a new 4x4 matrix + /// new 4x4 matrix public static float[] CloneIt(float[] a) { float[] output = new float[16]; @@ -80,9 +80,9 @@ public static float[] CloneIt(float[] a) /// /// Copy the values from one mat4 to another /// - /// {mat4} out the receiving matrix - /// {mat4} a the source matrix - /// {mat4} out + /// the receiving matrix + /// the source matrix + /// public static float[] Copy(float[] output, float[] a) { output[0] = a[0]; @@ -107,8 +107,8 @@ public static float[] Copy(float[] output, float[] a) /// /// Set a mat4 to the identity matrix /// - /// {mat4} out the receiving matrix - /// {mat4} out + /// the receiving matrix + /// public static float[] Identity(float[] output) { output[0] = 1; @@ -134,9 +134,9 @@ public static float[] Identity(float[] output) /// /// Set a mat4 to the identity matrix with a scale applied /// - /// {mat4} out the receiving matrix + /// the receiving matrix /// - /// {mat4} out + /// public static float[] Identity_Scaled(float[] output, float scale) { output[0] = scale; @@ -162,9 +162,9 @@ public static float[] Identity_Scaled(float[] output, float scale) /// /// Transpose the values of a mat4 /// - /// {mat4} out the receiving matrix - /// {mat4} a the source matrix - /// {mat4} out + /// the receiving matrix + /// the source matrix + /// public static float[] Transpose(float[] output, float[] a) { // If we are transposing ourselves we can skip a few steps but have to cache some values @@ -211,11 +211,11 @@ public static float[] Transpose(float[] output, float[] a) } /// - /// Inverts a mat4 + /// Inverts a mat4 /// - /// {mat4} out the receiving matrix - /// {mat4} a the source matrix - /// {mat4} out + /// the receiving matrix + /// the source matrix + /// public static float[] Invert(float[] output, float[] a) { float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; float a03 = a[3]; @@ -269,9 +269,9 @@ public static float[] Invert(float[] output, float[] a) /// /// Calculates the adjugate of a mat4 /// - /// {mat4} out the receiving matrix - /// {mat4} a the source matrix - /// {mat4} out + /// the receiving matrix + /// the source matrix + /// public static float[] Adjoint(float[] output, float[] a) { float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; float a03 = a[3]; @@ -301,8 +301,8 @@ public static float[] Adjoint(float[] output, float[] a) /// /// Calculates the determinant of a mat4 /// - /// {mat4} a the source matrix - /// {Number} determinant of a + /// the source matrix + /// of a public static float Determinant(float[] a) { float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; float a03 = a[3]; @@ -328,13 +328,13 @@ public static float Determinant(float[] a) } /// - /// Multiplies two mat4's + /// Multiplies two mat4's /// /// - /// {mat4} out the receiving matrix - /// {mat4} a the first operand - /// {mat4} b the second operand - /// {mat4} out + /// the receiving matrix + /// the first operand + /// the second operand + /// public static float[] Multiply(float[] output, float[] a, float[] b) { float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; float a03 = a[3]; @@ -375,7 +375,7 @@ public static float[] Multiply(float[] output, float[] a, float[] b) /// /// /// - /// + /// public static float[] Mul(float[] output, float[] a, float[] b) { return Multiply(output, a, b); @@ -385,12 +385,12 @@ public static float[] Mul(float[] output, float[] a, float[] b) /// /// Translate a mat4 by the given vector /// - /// {mat4} out the receiving matrix - /// {mat4} a the matrix to translate - /// {vec3} v vector to translate by + /// the receiving matrix + /// the matrix to translate + /// vector to translate by /// /// - /// {mat4} out + /// public static float[] Translate(float[] output, float[] input, float x, float y, float z) { if (input == output) @@ -426,10 +426,10 @@ public static float[] Translate(float[] output, float[] input, float x, float y, /// /// Translate a mat4 by the given vector /// - /// {mat4} out the receiving matrix - /// {mat4} a the matrix to translate - /// {vec3} v vector to translate by - /// {mat4} out + /// the receiving matrix + /// the matrix to translate + /// vector to translate by + /// public static float[] Translate(float[] output, float[] input, float[] translate) { float x = translate[0]; float y = translate[1]; float z = translate[2]; @@ -466,10 +466,10 @@ public static float[] Translate(float[] output, float[] input, float[] translate /// /// Scales the mat4 by the dimensions in the given vec3 /// - /// {mat4} out the receiving matrix - /// {mat4} a the matrix to scale - /// {vec3} v the vec3 to scale the matrix by - /// {mat4} out + /// the receiving matrix + /// the matrix to scale + /// the vec3 to scale the matrix by + /// public static float[] Scale(float[] output, float[] a, float[] v) { float x = v[0]; float y = v[1]; float z = v[2]; @@ -506,12 +506,12 @@ public static void SimpleScaleMatrix(Span matrix, float x, float y, float /// /// Scales the mat4 by the dimensions in the given vec3 /// - /// {mat4} out the receiving matrix - /// {mat4} a the matrix to scale + /// the receiving matrix + /// the matrix to scale /// /// /// - /// {mat4} out + /// public static float[] Scale(float[] output, float[] a, float xScale, float yScale, float zScale) { output[0] = a[0] * xScale; @@ -536,11 +536,11 @@ public static float[] Scale(float[] output, float[] a, float xScale, float yScal /// /// Rotates a mat4 by the given angle /// - /// {mat4} out the receiving matrix - /// {mat4} a the matrix to rotate - /// {Number} rad the angle to rotate the matrix by - /// {vec3} axis the axis to rotate around - /// {mat4} out + /// the receiving matrix + /// the matrix to rotate + /// the angle to rotate the matrix by + /// the axis to rotate around + /// public static float[] Rotate(float[] output, float[] a, float rad, float[] axis) { float x = axis[0]; float y = axis[1]; float z = axis[2]; @@ -601,10 +601,10 @@ public static float[] Rotate(float[] output, float[] a, float rad, float[] axis) /// /// Rotates a matrix by the given angle around the X axis /// - /// {mat4} out the receiving matrix - /// {mat4} a the matrix to rotate - /// {Number} rad the angle to rotate the matrix by - /// {mat4} out + /// the receiving matrix + /// the matrix to rotate + /// the angle to rotate the matrix by + /// public static float[] RotateX(float[] output, float[] a, float rad) { float s = GameMath.Sin(rad); @@ -646,10 +646,10 @@ public static float[] RotateX(float[] output, float[] a, float rad) /// /// Rotates a matrix by the given angle around the Y axis /// - /// {mat4} out the receiving matrix - /// {mat4} a the matrix to rotate - /// {Number} rad the angle to rotate the matrix by - /// {mat4} out + /// the receiving matrix + /// the matrix to rotate + /// the angle to rotate the matrix by + /// public static float[] RotateY(float[] output, float[] a, float rad) { float s = GameMath.Sin(rad); @@ -691,10 +691,10 @@ public static float[] RotateY(float[] output, float[] a, float rad) /// /// Rotates a matrix by the given angle around the Z axis /// - /// {mat4} out the receiving matrix - /// {mat4} a the matrix to rotate - /// {Number} rad the angle to rotate the matrix by - /// {mat4} out + /// the receiving matrix + /// the matrix to rotate + /// the angle to rotate the matrix by + /// public static float[] RotateZ(float[] output, float[] a, float rad) { float s = GameMath.Sin(rad); @@ -771,18 +771,18 @@ public static void RotateXYZ(Span matrix, float radX, float radY, float r } /// - /// Creates a matrix from a quaternion rotation and vector translation - /// This is equivalent to (but much faster than): - /// mat4.identity(dest); - /// mat4.translate(dest, vec); - /// var quatMat = mat4.create(); - /// quat4.toMat4(quat, quatMat); + /// Creates a matrix from a quaternion rotation and vector translation + /// This is equivalent to (but much faster than): + /// mat4.identity(dest); + /// mat4.translate(dest, vec); + /// var quatMat = mat4.create(); + /// quat4.toMat4(quat, quatMat); /// mat4.multiply(dest, quatMat); /// - /// {mat4} out mat4 receiving operation result - /// {quat4} q Rotation quaternion - /// {vec3} v Translation vector - /// {mat4} out + /// mat4 receiving operation result + /// Rotation quaternion + /// Translation vector + /// public static float[] FromRotationTranslation(float[] output, float[] q, float[] v) { // Quaternion math @@ -824,9 +824,9 @@ public static float[] FromRotationTranslation(float[] output, float[] q, float[] /// /// Calculates a 4x4 matrix from the given quaternion /// - /// {mat4} out mat4 receiving operation result - /// {quat} q Quaternion to create matrix from - /// {mat4} out + /// mat4 receiving operation result + /// Quaternion to create matrix from + /// public static float[] FromQuat(float[] output, float[] q) { float x = q[0]; float y = q[1]; float z = q[2]; float w = q[3]; @@ -870,14 +870,14 @@ public static float[] FromQuat(float[] output, float[] q) /// /// Generates a frustum matrix with the given bounds /// - /// {mat4} out mat4 frustum matrix will be written into - /// {Number} left Left bound of the frustum - /// {Number} right Right bound of the frustum - /// {Number} bottom Bottom bound of the frustum - /// {Number} top Top bound of the frustum - /// {Number} near Near bound of the frustum - /// {Number} far Far bound of the frustum - /// {mat4} out + /// mat4 frustum matrix will be written into + /// Left bound of the frustum + /// Right bound of the frustum + /// Bottom bound of the frustum + /// Top bound of the frustum + /// Near bound of the frustum + /// Far bound of the frustum + /// public static float[] Frustum(float[] output, float left, float right, float bottom, float top, float near, float far) { float rl = 1 / (right - left); @@ -905,12 +905,12 @@ public static float[] Frustum(float[] output, float left, float right, float bot /// /// Generates a perspective projection matrix with the given bounds /// - /// {mat4} out mat4 frustum matrix will be written into - /// {number} fovy Vertical field of view in radians - /// {number} aspect Aspect ratio. typically viewport width/height - /// {number} near Near bound of the frustum - /// {number} far Far bound of the frustum - /// {mat4} out + /// mat4 frustum matrix will be written into + /// Vertical field of view in radians + /// Aspect ratio. typically viewport width/height + /// Near bound of the frustum + /// Far bound of the frustum + /// public static float[] Perspective(float[] output, float fovy, float aspect, float near, float far) { float one = 1; @@ -938,14 +938,14 @@ public static float[] Perspective(float[] output, float fovy, float aspect, floa /// /// Generates a orthogonal projection matrix with the given bounds /// - /// {mat4} out mat4 frustum matrix will be written into - /// {number} left Left bound of the frustum - /// {number} right Right bound of the frustum - /// {number} bottom Bottom bound of the frustum - /// {number} top Top bound of the frustum - /// {number} near Near bound of the frustum - /// {number} far Far bound of the frustum - /// {mat4} out + /// mat4 frustum matrix will be written into + /// Left bound of the frustum + /// Right bound of the frustum + /// Bottom bound of the frustum + /// Top bound of the frustum + /// Near bound of the frustum + /// Far bound of the frustum + /// public static float[] Ortho(float[] output, float left, float right, float bottom, float top, float near, float far) { float lr = 1 / (left - right); @@ -973,11 +973,11 @@ public static float[] Ortho(float[] output, float left, float right, float botto /// /// Generates a look-at matrix with the given eye position, focal point, and up axis /// - /// {mat4} out mat4 frustum matrix will be written into - /// {vec3} eye Position of the viewer - /// {vec3} center Point the viewer is looking at - /// {vec3} up vec3 pointing up - /// {mat4} out + /// mat4 frustum matrix will be written into + /// Position of the viewer + /// Point the viewer is looking at + /// vec3 pointing up + /// public static float[] LookAt(float[] output, float[] eye, float[] center, float[] up) { float x0; float x1; float x2; float y0; float y1; float y2; float z0; float z1; float z2; float len; @@ -1070,7 +1070,7 @@ public static float[] LookAt(float[] output, float[] eye, float[] center, float[ /// /// /// - /// + /// public static float[] MulWithVec4(float[] matrix, float[] vec4) { float[] output = new float[] { 0, 0, 0, 0 }; @@ -1273,7 +1273,7 @@ public static double[] MulWithVec4(float[] matrix, double[] vec4) /// /// /// - /// + /// public static void MulWithVec4(float[] matrix, float[] vec4, Vec4f outVal) { outVal.Set(0, 0, 0, 0); @@ -1294,7 +1294,7 @@ public static void MulWithVec4(float[] matrix, float[] vec4, Vec4f outVal) /// /// /// - /// + /// public static void MulWithVec4(float[] matrix, Vec4d inVal, Vec4d outVal) { outVal.Set(0, 0, 0, 0); @@ -1316,7 +1316,7 @@ public static void MulWithVec4(float[] matrix, Vec4d inVal, Vec4d outVal) /// /// /// - /// + /// public static void MulWithVec4(float[] matrix, Vec4f inVal, Vec4f outVal) { outVal.Set(0, 0, 0, 0); diff --git a/Math/Matrix/MatrixTools.cs b/Math/Matrix/MatrixTools.cs index d6d16ffd7..81746483a 100644 --- a/Math/Matrix/MatrixTools.cs +++ b/Math/Matrix/MatrixTools.cs @@ -2,7 +2,7 @@ namespace Vintagestory.API.Client { - public class MatrixToolsd + public partial class MatrixToolsd { public static Vec3d Project(Vec3d pos, double[] projection, double[] view, int viewportWidth, int viewportHeight) { diff --git a/Math/Matrix/Quaterniond.cs b/Math/Matrix/Quaterniond.cs index f5b805b0a..706602716 100644 --- a/Math/Matrix/Quaterniond.cs +++ b/Math/Matrix/Quaterniond.cs @@ -26,13 +26,12 @@ namespace Vintagestory.API.MathTools { - public class Quaterniond + public partial class Quaterniond { - ///** - // * Creates a new identity quat - // * - // * @returns {quat} a new quaternion - // */ + /// + /// Creates a new identity quat + /// + /// new quaternion public static double[] Create() { double[] output = new double[4]; @@ -43,17 +42,16 @@ public static double[] Create() return output; } - ///** - // * Sets a quaternion to represent the shortest rotation from one - // * vector to another. - // * - // * Both vectors are assumed to be unit length. - // * - // * @param {quat} output the receiving quaternion. - // * @param {vec3} a the initial vector - // * @param {vec3} b the destination vector - // * @returns {quat} output - // */ + /// + /// Sets a quaternion to represent the shortest rotation from one + /// vector to another. + /// + /// Both vectors are assumed to be unit length. + /// + /// the receiving quaternion. + /// the initial vector + /// the destination vector + /// output public static double[] RotationTo(double[] output, double[] a, double[] b) { double[] tmpvec3 = Vec3Utilsd.Create(); @@ -98,16 +96,15 @@ public static double[] RotationTo(double[] output, double[] a, double[] b) // }; } - ///** - // * Sets the specified quaternion with values corresponding to the given - // * axes. Each axis is a vec3 and is expected to be unit length and - // * perpendicular to all other specified axes. - // * - // * @param {vec3} view the vector representing the viewing direction - // * @param {vec3} right the vector representing the local "right" direction - // * @param {vec3} up the vector representing the local "up" direction - // * @returns {quat} output - // */ + /// + /// Sets the specified quaternion with values corresponding to the given + /// axes. Each axis is a vec3 and is expected to be unit length and + /// perpendicular to all other specified axes. + /// + /// the vector representing the viewing direction + /// the vector representing the local "right" direction + /// the vector representing the local "up" direction + /// output public static double[] SetAxes(double[] output, double[] view, double[] right, double[] up) { double[] matr = Mat3d.Create(); @@ -126,71 +123,61 @@ public static double[] SetAxes(double[] output, double[] view, double[] right, d matr[8] = view[2]; return Quaterniond.Normalize(output, Quaterniond.FromMat3(output, matr)); - // }; } - ///** - // * Creates a new quat initialized with values from an existing quaternion - // * - // * @param {quat} a quaternion to clone - // * @returns {quat} a new quaternion - // * @function - // */ + /// + /// Creates a new quat initialized with values from an existing quaternion + /// + /// quaternion to clone + /// new quaternion public static double[] CloneIt(double[] a) { return QVec4d.CloneIt(a); } - ///** - // * Creates a new quat initialized with the given values - // * - // * @param {Number} x X component - // * @param {Number} y Y component - // * @param {Number} z Z component - // * @param {Number} w W component - // * @returns {quat} a new quaternion - // * @function - // */ + /// + /// Creates a new quat initialized with the given values + /// + /// X component + /// Y component + /// Z component + /// W component + /// new quaternion public static double[] FromValues(double x, double y, double z, double w) { return QVec4d.FromValues(x, y, z, w); } - ///** - // * Copy the values from one quat to another - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a the source quaternion - // * @returns {quat} output - // * @function - // */ + /// + /// Copy the values from one quat to another + /// + /// the receiving quaternion + /// the source quaternion + /// output public static double[] Copy(double[] output, double[] a) { return QVec4d.Copy(output, a); } - ///** - // * Set the components of a quat to the given values - // * - // * @param {quat} output the receiving quaternion - // * @param {Number} x X component - // * @param {Number} y Y component - // * @param {Number} z Z component - // * @param {Number} w W component - // * @returns {quat} output - // * @function - // */ + /// + /// Set the components of a quat to the given values + /// + /// the receiving quaternion + /// X component + /// Y component + /// Z component + /// W component + /// output public static double[] Set(double[] output, double x, double y, double z, double w) { return QVec4d.Set(output, x, y, z, w); } - ///** - // * Set a quat to the identity quaternion - // * - // * @param {quat} output the receiving quaternion - // * @returns {quat} output - // */ + /// + /// Set a quat to the identity quaternion + /// + /// the receiving quaternion + /// output public static double[] Identity_(double[] output) { output[0] = 0; @@ -200,15 +187,14 @@ public static double[] Identity_(double[] output) return output; } - ///** - // * Sets a quat from the given angle and rotation axis, - // * then returns it. - // * - // * @param {quat} output the receiving quaternion - // * @param {vec3} axis the axis around which to rotate - // * @param {Number} rad the angle in radians - // * @returns {quat} output - // **/ + /// + /// Sets a quat from the given angle and rotation axis, + /// then returns it. + /// + /// the receiving quaternion + /// the axis around which to rotate + /// the angle in radians + /// output public static double[] SetAxisAngle(double[] output, double[] axis, double rad) { rad = rad / 2; @@ -220,29 +206,25 @@ public static double[] SetAxisAngle(double[] output, double[] axis, double rad) return output; } - ///** - // * Adds two quat's - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a the first operand - // * @param {quat} b the second operand - // * @returns {quat} output - // * @function - // */ - //quat.add = QVec4d.add; + /// + /// Adds two quat's + /// + /// the receiving quaternion + /// the first operand + /// the second operand + /// output public static double[] Add(double[] output, double[] a, double[] b) { return QVec4d.Add(output, a, b); } - ///** - // * Multiplies two quat's - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a the first operand - // * @param {quat} b the second operand - // * @returns {quat} output - // */ + /// + /// Multiplies two quat's + /// + /// the receiving quaternion + /// the first operand + /// the second operand + /// output public static double[] Multiply(double[] output, double[] a, double[] b) { double ax = a[0]; double ay = a[1]; double az = a[2]; double aw = a[3]; @@ -255,29 +237,25 @@ public static double[] Multiply(double[] output, double[] a, double[] b) return output; } - ///** - // * Scales a quat by a scalar number - // * - // * @param {quat} output the receiving vector - // * @param {quat} a the vector to scale - // * @param {Number} b amount to scale the vector by - // * @returns {quat} output - // * @function - // */ - //quat.scale = QVec4d.scale; + /// + /// Scales a quat by a scalar number + /// + /// the receiving vector + /// the vector to scale + /// amount to scale the vector by + /// output public static double[] Scale(double[] output, double[] a, double b) { return QVec4d.Scale(output, a, b); } - ///** - // * Rotates a quaternion by the given angle aboutput the X axis - // * - // * @param {quat} output quat receiving operation result - // * @param {quat} a quat to rotate - // * @param {number} rad angle (in radians) to rotate - // * @returns {quat} output - // */ + /// + /// Rotates a quaternion by the given angle aboutput the X axis + /// + /// quat receiving operation result + /// quat to rotate + /// angle (in radians) to rotate + /// output public static double[] RotateX(double[] output, double[] a, double rad) { rad /= 2; @@ -292,14 +270,13 @@ public static double[] RotateX(double[] output, double[] a, double rad) return output; } - ///** - // * Rotates a quaternion by the given angle aboutput the Y axis - // * - // * @param {quat} output quat receiving operation result - // * @param {quat} a quat to rotate - // * @param {number} rad angle (in radians) to rotate - // * @returns {quat} output - // */ + /// + /// Rotates a quaternion by the given angle aboutput the Y axis + /// + /// quat receiving operation result + /// quat to rotate + /// angle (in radians) to rotate + /// output public static double[] RotateY(double[] output, double[] a, double rad) { rad /= 2; @@ -314,14 +291,13 @@ public static double[] RotateY(double[] output, double[] a, double rad) return output; } - ///** - // * Rotates a quaternion by the given angle aboutput the Z axis - // * - // * @param {quat} output quat receiving operation result - // * @param {quat} a quat to rotate - // * @param {number} rad angle (in radians) to rotate - // * @returns {quat} output - // */ + /// + /// Rotates a quaternion by the given angle aboutput the Z axis + /// + /// quat receiving operation result + /// quat to rotate + /// angle (in radians) to rotate + /// output public static double[] RotateZ(double[] output, double[] a, double rad) { rad /= 2; @@ -336,15 +312,14 @@ public static double[] RotateZ(double[] output, double[] a, double rad) return output; } - ///** - // * Calculates the W component of a quat from the X, Y, and Z components. - // * Assumes that quaternion is 1 unit in length. - // * Any existing W component will be ignored. - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a quat to calculate W component of - // * @returns {quat} output - // */ + /// + /// Calculates the W component of a quat from the X, Y, and Z components. + /// Assumes that quaternion is 1 unit in length. + /// Any existing W component will be ignored. + /// + /// the receiving quaternion + /// quat to calculate W component of + /// output public static double[] CalculateW(double[] output, double[] a) { double x = a[0]; double y = a[1]; double z = a[2]; @@ -357,14 +332,12 @@ public static double[] CalculateW(double[] output, double[] a) return output; } - ///** - // * Calculates the dot product of two quat's - // * - // * @param {quat} a the first operand - // * @param {quat} b the second operand - // * @returns {Number} dot product of a and b - // * @function - // */ + /// + /// Calculates the dot product of two quat's + /// + /// the first operand + /// the second operand + /// dot product of a and b public static double Dot(double[] a, double[] b) { return QVec4d.Dot(a, b); @@ -395,30 +368,27 @@ public static float[] ToEulerAngles(double[] quat) return angles; } - ///** - // * Performs a linear interpolation between two quat's - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a the first operand - // * @param {quat} b the second operand - // * @param {Number} t interpolation amount between the two inputs - // * @returns {quat} output - // * @function - // */ + /// + /// Performs a linear interpolation between two quat's + /// + /// the receiving quaternion + /// the first operand + /// the second operand + /// interpolation amount between the two inputs + /// output public static double[] Lerp(double[] output, double[] a, double[] b, double t) { return QVec4d.Lerp(output, a, b, t); } - ///** - // * Performs a spherical linear interpolation between two quat - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a the first operand - // * @param {quat} b the second operand - // * @param {Number} t interpolation amount between the two inputs - // * @returns {quat} output - // */ + /// + /// Performs a spherical linear interpolation between two quat + /// + /// the receiving quaternion + /// the first operand + /// the second operand + /// interpolation amount between the two inputs + /// output //quat.slerp = function (output, a, b, t) { public static double[] Slerp(double[] output, double[] a, double[] b, double t) { @@ -468,13 +438,12 @@ public static double[] Slerp(double[] output, double[] a, double[] b, double t) return output; } - ///** - // * Calculates the inverse of a quat - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a quat to calculate inverse of - // * @returns {quat} output - // */ + /// + /// Calculates the inverse of a quat + /// + /// the receiving quaternion + /// quat to calculate inverse of + /// output public double[] Invert(double[] output, double[] a) { double a0 = a[0]; double a1 = a[1]; double a2 = a[2]; double a3 = a[3]; @@ -491,14 +460,13 @@ public double[] Invert(double[] output, double[] a) return output; } - ///** - // * Calculates the conjugate of a quat - // * If the quaternion is normalized, this function is faster than quat.inverse and produces the same result. - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a quat to calculate conjugate of - // * @returns {quat} output - // */ + /// + /// Calculates the conjugate of a quat + /// If the quaternion is normalized, this function is faster than quat.inverse and produces the same result. + /// + /// the receiving quaternion + /// quat to calculate conjugate of + /// output public double[] Conjugate(double[] output, double[] a) { output[0] = -a[0]; @@ -508,56 +476,48 @@ public double[] Conjugate(double[] output, double[] a) return output; } - ///** - // * Calculates the length of a quat - // * - // * @param {quat} a vector to calculate length of - // * @returns {Number} length of a - // * @function - // */ + /// + /// Calculates the length of a quat + /// + /// vector to calculate length of + /// length of a //quat.length = QVec4d.length; public static double Length_(double[] a) { return QVec4d.Length_(a); } - ///** - // * Calculates the squared length of a quat - // * - // * @param {quat} a vector to calculate squared length of - // * @returns {Number} squared length of a - // * @function - // */ + /// + /// Calculates the squared length of a quat + /// + /// vector to calculate squared length of + /// squared length of a public static double SquaredLength(double[] a) { return QVec4d.SquaredLength(a); } - ///** - // * Normalize a quat - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a quaternion to normalize - // * @returns {quat} output - // * @function - // */ + /// + /// Normalize a quat + /// + /// the receiving quaternion + /// quaternion to normalize + /// output public static double[] Normalize(double[] output, double[] a) { return QVec4d.Normalize(output, a); } - ///** - // * Creates a quaternion from the given 3x3 rotation matrix. - // * - // * NOTE: The resultant quaternion is not normalized, so you should be sure - // * to renormalize the quaternion yourself where necessary. - // * - // * @param {quat} output the receiving quaternion - // * @param {mat3} m rotation matrix - // * @returns {quat} output - // * @function - // */ + /// + /// Creates a quaternion from the given 3x3 rotation matrix. + /// + /// NOTE: The resultant quaternion is not normalized, so you should be sure + /// to renormalize the quaternion yourself where necessary. + /// + /// the receiving quaternion + /// rotation matrix + /// output public static double[] FromMat3(double[] output, double[] m) { // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes @@ -602,14 +562,13 @@ public static double[] FromMat3(double[] output, double[] m) } - class QVec4d + partial class QVec4d { - - ///** - // * Creates a new, empty QVec4d - // * - // * @returns {QVec4d} a new 4D vector - // */ + + /// + /// Creates a new, empty QVec4d + /// + /// new 4D vector public static double[] Create() { double[] output = new double[4]; @@ -620,12 +579,11 @@ public static double[] Create() return output; } - ///** - // * Creates a new QVec4d initialized with values from an existing vector - // * - // * @param {QVec4d} a vector to clone - // * @returns {QVec4d} a new 4D vector - // */ + /// + /// Creates a new QVec4d initialized with values from an existing vector + /// + /// vector to clone + /// new 4D vector public static double[] CloneIt(double[] a) { double[] output = new double[4]; @@ -636,15 +594,14 @@ public static double[] CloneIt(double[] a) return output; } - ///** - // * Creates a new QVec4d initialized with the given values - // * - // * @param {Number} x X component - // * @param {Number} y Y component - // * @param {Number} z Z component - // * @param {Number} w W component - // * @returns {QVec4d} a new 4D vector - // */ + /// + /// Creates a new QVec4d initialized with the given values + /// + /// X component + /// Y component + /// Z component + /// W component + /// new 4D vector public static double[] FromValues(double x, double y, double z, double w) { double[] output = new double[4]; @@ -655,13 +612,12 @@ public static double[] FromValues(double x, double y, double z, double w) return output; } - ///** - // * Copy the values from one QVec4d to another - // * - // * @param {QVec4d} output the receiving vector - // * @param {QVec4d} a the source vector - // * @returns {QVec4d} output - // */ + /// + /// Copy the values from one QVec4d to another + /// + /// the receiving vector + /// the source vector + /// output public static double[] Copy(double[] output, double[] a) { output[0] = a[0]; @@ -671,16 +627,15 @@ public static double[] Copy(double[] output, double[] a) return output; } - ///** - // * Set the components of a QVec4d to the given values - // * - // * @param {QVec4d} output the receiving vector - // * @param {Number} x X component - // * @param {Number} y Y component - // * @param {Number} z Z component - // * @param {Number} w W component - // * @returns {QVec4d} output - // */ + /// + /// Set the components of a QVec4d to the given values + /// + /// the receiving vector + /// X component + /// Y component + /// Z component + /// W component + /// output public static double[] Set(double[] output, double x, double y, double z, double w) { output[0] = x; @@ -690,14 +645,13 @@ public static double[] Set(double[] output, double x, double y, double z, double return output; } - ///** - // * Adds two QVec4d's - // * - // * @param {QVec4d} output the receiving vector - // * @param {QVec4d} a the first operand - // * @param {QVec4d} b the second operand - // * @returns {QVec4d} output - // */ + /// + /// Adds two QVec4d's + /// + /// the receiving vector + /// the first operand + /// the second operand + /// output public static double[] Add(double[] output, double[] a, double[] b) { output[0] = a[0] + b[0]; @@ -707,14 +661,13 @@ public static double[] Add(double[] output, double[] a, double[] b) return output; } - ///** - // * Subtracts vector b from vector a - // * - // * @param {QVec4d} output the receiving vector - // * @param {QVec4d} a the first operand - // * @param {QVec4d} b the second operand - // * @returns {QVec4d} output - // */ + /// + /// Subtracts vector b from vector a + /// + /// the receiving vector + /// the first operand + /// the second operand + /// output public static double[] Subtract(double[] output, double[] a, double[] b) { output[0] = a[0] - b[0]; @@ -725,14 +678,13 @@ public static double[] Subtract(double[] output, double[] a, double[] b) } - ///** - // * Multiplies two QVec4d's - // * - // * @param {QVec4d} output the receiving vector - // * @param {QVec4d} a the first operand - // * @param {QVec4d} b the second operand - // * @returns {QVec4d} output - // */ + /// + /// Multiplies two QVec4d's + /// + /// the receiving vector + /// the first operand + /// the second operand + /// output public static double[] Multiply(double[] output, double[] a, double[] b) { output[0] = a[0] * b[0]; @@ -743,14 +695,13 @@ public static double[] Multiply(double[] output, double[] a, double[] b) } - ///** - // * Divides two QVec4d's - // * - // * @param {QVec4d} output the receiving vector - // * @param {QVec4d} a the first operand - // * @param {QVec4d} b the second operand - // * @returns {QVec4d} output - // */ + /// + /// Divides two QVec4d's + /// + /// the receiving vector + /// the first operand + /// the second operand + /// output public static double[] Divide(double[] output, double[] a, double[] b) { output[0] = a[0] / b[0]; @@ -761,14 +712,13 @@ public static double[] Divide(double[] output, double[] a, double[] b) } - ///** - // * Returns the minimum of two QVec4d's - // * - // * @param {QVec4d} output the receiving vector - // * @param {QVec4d} a the first operand - // * @param {QVec4d} b the second operand - // * @returns {QVec4d} output - // */ + /// + /// Returns the minimum of two QVec4d's + /// + /// the receiving vector + /// the first operand + /// the second operand + /// output public static double[] Min(double[] output, double[] a, double[] b) { output[0] = Math.Min(a[0], b[0]); @@ -778,14 +728,13 @@ public static double[] Min(double[] output, double[] a, double[] b) return output; } - ///** - // * Returns the maximum of two QVec4d's - // * - // * @param {QVec4d} output the receiving vector - // * @param {QVec4d} a the first operand - // * @param {QVec4d} b the second operand - // * @returns {QVec4d} output - // */ + /// + /// Returns the maximum of two QVec4d's + /// + /// the receiving vector + /// the first operand + /// the second operand + /// output public static double[] Max(double[] output, double[] a, double[] b) { output[0] = Math.Max(a[0], b[0]); @@ -795,14 +744,13 @@ public static double[] Max(double[] output, double[] a, double[] b) return output; } - ///** - // * Scales a QVec4d by a scalar number - // * - // * @param {QVec4d} output the receiving vector - // * @param {QVec4d} a the vector to scale - // * @param {Number} b amount to scale the vector by - // * @returns {QVec4d} output - // */ + /// + /// Scales a QVec4d by a scalar number + /// + /// the receiving vector + /// the vector to scale + /// amount to scale the vector by + /// output public static double[] Scale(double[] output, double[] a, double b) { output[0] = a[0] * b; @@ -812,15 +760,14 @@ public static double[] Scale(double[] output, double[] a, double b) return output; } - ///** - // * Adds two QVec4d's after scaling the second operand by a scalar value - // * - // * @param {QVec4d} output the receiving vector - // * @param {QVec4d} a the first operand - // * @param {QVec4d} b the second operand - // * @param {Number} scale the amount to scale b by before adding - // * @returns {QVec4d} output - // */ + /// + /// Adds two QVec4d's after scaling the second operand by a scalar value + /// + /// the receiving vector + /// the first operand + /// the second operand + /// the amount to scale b by before adding + /// output public static double[] ScaleAndAdd(double[] output, double[] a, double[] b, double scale) { output[0] = a[0] + (b[0] * scale); @@ -830,13 +777,12 @@ public static double[] ScaleAndAdd(double[] output, double[] a, double[] b, doub return output; } - ///** - // * Calculates the euclidian distance between two QVec4d's - // * - // * @param {QVec4d} a the first operand - // * @param {QVec4d} b the second operand - // * @returns {Number} distance between a and b - // */ + /// + /// Calculates the euclidian distance between two QVec4d's + /// + /// the first operand + /// the second operand + /// distance between a and b public static double Distance(double[] a, double[] b) { double x = b[0] - a[0]; @@ -847,13 +793,12 @@ public static double Distance(double[] a, double[] b) } - ///** - // * Calculates the squared euclidian distance between two QVec4d's - // * - // * @param {QVec4d} a the first operand - // * @param {QVec4d} b the second operand - // * @returns {Number} squared distance between a and b - // */ + /// + /// Calculates the squared euclidian distance between two QVec4d's + /// + /// the first operand + /// the second operand + /// squared distance between a and b public static double SquaredDistance(double[] a, double[] b) { double x = b[0] - a[0]; @@ -863,12 +808,11 @@ public static double SquaredDistance(double[] a, double[] b) return x * x + y * y + z * z + w * w; } - ///** - // * Calculates the length of a QVec4d - // * - // * @param {QVec4d} a vector to calculate length of - // * @returns {Number} length of a - // */ + /// + /// Calculates the length of a QVec4d + /// + /// vector to calculate length of + /// length of a public static double Length_(double[] a) { double x = a[0]; @@ -879,12 +823,11 @@ public static double Length_(double[] a) } - ///** - // * Calculates the squared length of a QVec4d - // * - // * @param {QVec4d} a vector to calculate squared length of - // * @returns {Number} squared length of a - // */ + /// + /// Calculates the squared length of a QVec4d + /// + /// vector to calculate squared length of + /// squared length of a public static double SquaredLength(double[] a) { double x = a[0]; @@ -895,13 +838,12 @@ public static double SquaredLength(double[] a) } - ///** - // * Negates the components of a QVec4d - // * - // * @param {QVec4d} output the receiving vector - // * @param {QVec4d} a vector to negate - // * @returns {QVec4d} output - // */ + /// + /// Negates the components of a QVec4d + /// + /// the receiving vector + /// vector to negate + /// output public static double[] Negate(double[] output, double[] a) { output[0] = -a[0]; @@ -911,13 +853,12 @@ public static double[] Negate(double[] output, double[] a) return output; } - ///** - // * Normalize a QVec4d - // * - // * @param {QVec4d} output the receiving vector - // * @param {QVec4d} a vector to normalize - // * @returns {QVec4d} output - // */ + /// + /// Normalize a QVec4d + /// + /// the receiving vector + /// vector to normalize + /// output public static double[] Normalize(double[] output, double[] a) { double x = a[0]; @@ -937,27 +878,25 @@ public static double[] Normalize(double[] output, double[] a) return output; } - ///** - // * Calculates the dot product of two QVec4d's - // * - // * @param {QVec4d} a the first operand - // * @param {QVec4d} b the second operand - // * @returns {Number} dot product of a and b - // */ + /// + /// Calculates the dot product of two QVec4d's + /// + /// the first operand + /// the second operand + /// dot product of a and b public static double Dot(double[] a, double[] b) { return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]; } - ///** - // * Performs a linear interpolation between two QVec4d's - // * - // * @param {QVec4d} output the receiving vector - // * @param {QVec4d} a the first operand - // * @param {QVec4d} b the second operand - // * @param {Number} t interpolation amount between the two inputs - // * @returns {QVec4d} output - // */ + /// + /// Performs a linear interpolation between two QVec4d's + /// + /// the receiving vector + /// the first operand + /// the second operand + /// interpolation amount between the two inputs + /// output public static double[] Lerp(double[] output, double[] a, double[] b, double t) { double ax = a[0]; @@ -972,14 +911,13 @@ public static double[] Lerp(double[] output, double[] a, double[] b, double t) } - ///** - // * Transforms the QVec4d with a mat4. - // * - // * @param {QVec4d} output the receiving vector - // * @param {QVec4d} a the vector to transform - // * @param {mat4} m matrix to transform with - // * @returns {QVec4d} output - // */ + /// + /// Transforms the QVec4d with a mat4. + /// + /// the receiving vector + /// the vector to transform + /// matrix to transform with + /// output public static double[] TransformMat4(double[] output, double[] a, double[] m) { double x = a[0]; double y = a[1]; double z = a[2]; double w = a[3]; @@ -990,14 +928,13 @@ public static double[] TransformMat4(double[] output, double[] a, double[] m) return output; } - ///** - // * Transforms the QVec4d with a quat - // * - // * @param {QVec4d} output the receiving vector - // * @param {QVec4d} a the vector to transform - // * @param {quat} q quaternion to transform with - // * @returns {QVec4d} output - // */ + /// + /// Transforms the QVec4d with a quat + /// + /// the receiving vector + /// the vector to transform + /// quaternion to transform with + /// output public static double[] transformQuat(double[] output, double[] a, double[] q) { double x = a[0]; double y = a[1]; double z = a[2]; @@ -1023,7 +960,7 @@ public static double[] transformQuat(double[] output, double[] a, double[] q) /// /// Don't use this class unless you need it to interoperate with Mat4d /// - public class Vec3Utilsd + public partial class Vec3Utilsd { /// Creates a new, empty vec3 /// Returns {vec3} a new 3D vector. diff --git a/Math/Matrix/Quaternionf.cs b/Math/Matrix/Quaternionf.cs index 08b1115fe..fa4cbbb10 100644 --- a/Math/Matrix/Quaternionf.cs +++ b/Math/Matrix/Quaternionf.cs @@ -26,13 +26,12 @@ namespace Vintagestory.API.MathTools { - public class Quaternionf + public partial class Quaternionf { - ///** - // * Creates a new identity quat - // * - // * @returns {quat} a new quaternion - // */ + /// + /// Creates a new identity quat + /// + /// new quaternion public static float[] Create() { float[] output = new float[4]; @@ -43,17 +42,16 @@ public static float[] Create() return output; } - ///** - // * Sets a quaternion to represent the shortest rotation from one - // * vector to another. - // * - // * Both vectors are assumed to be unit length. - // * - // * @param {quat} output the receiving quaternion. - // * @param {vec3} a the initial vector - // * @param {vec3} b the destination vector - // * @returns {quat} output - // */ + /// + /// Sets a quaternion to represent the shortest rotation from one + /// vector to another. + /// + /// Both vectors are assumed to be unit length. + /// + /// the receiving quaternion. + /// the initial vector + /// the destination vector + /// output public static float[] RotationTo(float[] output, float[] a, float[] b) { float[] tmpvec3 = Vec3Utilsf.Create(); @@ -98,16 +96,15 @@ public static float[] RotationTo(float[] output, float[] a, float[] b) // }; } - ///** - // * Sets the specified quaternion with values corresponding to the given - // * axes. Each axis is a vec3 and is expected to be unit length and - // * perpendicular to all other specified axes. - // * - // * @param {vec3} view the vector representing the viewing direction - // * @param {vec3} right the vector representing the local "right" direction - // * @param {vec3} up the vector representing the local "up" direction - // * @returns {quat} output - // */ + /// + /// Sets the specified quaternion with values corresponding to the given + /// axes. Each axis is a vec3 and is expected to be unit length and + /// perpendicular to all other specified axes. + /// + /// the vector representing the viewing direction + /// the vector representing the local "right" direction + /// the vector representing the local "up" direction + /// output public static float[] SetAxes(float[] output, float[] view, float[] right, float[] up) { float[] matr = Mat3f.Create(); @@ -129,68 +126,59 @@ public static float[] SetAxes(float[] output, float[] view, float[] right, float // }; } - ///** - // * Creates a new quat initialized with values from an existing quaternion - // * - // * @param {quat} a quaternion to clone - // * @returns {quat} a new quaternion - // * @function - // */ + /// + /// Creates a new quat initialized with values from an existing quaternion + /// + /// quaternion to clone + /// new quaternion public static float[] CloneIt(float[] a) { return QVec4f.CloneIt(a); } - ///** - // * Creates a new quat initialized with the given values - // * - // * @param {Number} x X component - // * @param {Number} y Y component - // * @param {Number} z Z component - // * @param {Number} w W component - // * @returns {quat} a new quaternion - // * @function - // */ + /// + /// Creates a new quat initialized with the given values + /// + /// X component + /// Y component + /// Z component + /// W component + /// new quaternion public static float[] FromValues(float x, float y, float z, float w) { return QVec4f.FromValues(x, y, z, w); } - ///** - // * Copy the values from one quat to another - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a the source quaternion - // * @returns {quat} output - // * @function - // */ + /// + /// Copy the values from one quat to another + /// + /// the receiving quaternion + /// the source quaternion + /// output public static float[] Copy(float[] output, float[] a) { return QVec4f.Copy(output, a); } - ///** - // * Set the components of a quat to the given values - // * - // * @param {quat} output the receiving quaternion - // * @param {Number} x X component - // * @param {Number} y Y component - // * @param {Number} z Z component - // * @param {Number} w W component - // * @returns {quat} output - // * @function - // */ + /// + /// Set the components of a quat to the given values + /// + /// the receiving quaternion + /// X component + /// Y component + /// Z component + /// W component + /// output public static float[] Set(float[] output, float x, float y, float z, float w) { return QVec4f.Set(output, x, y, z, w); } - ///** - // * Set a quat to the identity quaternion - // * - // * @param {quat} output the receiving quaternion - // * @returns {quat} output - // */ + /// + /// Set a quat to the identity quaternion + /// + /// the receiving quaternion + /// output public static float[] Identity_(float[] output) { output[0] = 0; @@ -200,15 +188,14 @@ public static float[] Identity_(float[] output) return output; } - ///** - // * Sets a quat from the given angle and rotation axis, - // * then returns it. - // * - // * @param {quat} output the receiving quaternion - // * @param {vec3} axis the axis around which to rotate - // * @param {Number} rad the angle in radians - // * @returns {quat} output - // **/ + /// + /// Sets a quat from the given angle and rotation axis, + /// then returns it. + /// + /// the receiving quaternion + /// the axis around which to rotate + /// the angle in radians + /// output public static float[] SetAxisAngle(float[] output, float[] axis, float rad) { rad = rad / 2; @@ -220,29 +207,25 @@ public static float[] SetAxisAngle(float[] output, float[] axis, float rad) return output; } - ///** - // * Adds two quat's - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a the first operand - // * @param {quat} b the second operand - // * @returns {quat} output - // * @function - // */ - //quat.add = QVec4f.add; + /// + /// Adds two quat's + /// + /// the receiving quaternion + /// the first operand + /// the second operand + /// output public static float[] Add(float[] output, float[] a, float[] b) { return QVec4f.Add(output, a, b); } - ///** - // * Multiplies two quat's - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a the first operand - // * @param {quat} b the second operand - // * @returns {quat} output - // */ + /// + /// Multiplies two quat's + /// + /// the receiving quaternion + /// the first operand + /// the second operand + /// output public static float[] Multiply(float[] output, float[] a, float[] b) { float ax = a[0]; float ay = a[1]; float az = a[2]; float aw = a[3]; @@ -255,38 +238,33 @@ public static float[] Multiply(float[] output, float[] a, float[] b) return output; } - ///** - // * Alias for {@link quat.multiply} - // * @function - // */ + /// + /// Alias for + /// public static float[] Mul(float[] output, float[] a, float[] b) { return Multiply(output, a, b); } - ///** - // * Scales a quat by a scalar number - // * - // * @param {quat} output the receiving vector - // * @param {quat} a the vector to scale - // * @param {Number} b amount to scale the vector by - // * @returns {quat} output - // * @function - // */ - //quat.scale = QVec4f.scale; + /// + /// Scales a quat by a scalar number + /// + /// the receiving vector + /// the vector to scale + /// amount to scale the vector by + /// output public static float[] Scale(float[] output, float[] a, float b) { return QVec4f.Scale(output, a, b); } - ///** - // * Rotates a quaternion by the given angle aboutput the X axis - // * - // * @param {quat} output quat receiving operation result - // * @param {quat} a quat to rotate - // * @param {number} rad angle (in radians) to rotate - // * @returns {quat} output - // */ + /// + /// Rotates a quaternion by the given angle aboutput the X axis + /// + /// quat receiving operation result + /// quat to rotate + /// angle (in radians) to rotate + /// output public static float[] RotateX(float[] output, float[] a, float rad) { rad /= 2; @@ -301,14 +279,13 @@ public static float[] RotateX(float[] output, float[] a, float rad) return output; } - ///** - // * Rotates a quaternion by the given angle aboutput the Y axis - // * - // * @param {quat} output quat receiving operation result - // * @param {quat} a quat to rotate - // * @param {number} rad angle (in radians) to rotate - // * @returns {quat} output - // */ + /// + /// Rotates a quaternion by the given angle aboutput the Y axis + /// + /// quat receiving operation result + /// quat to rotate + /// angle (in radians) to rotate + /// output public static float[] RotateY(float[] output, float[] a, float rad) { rad /= 2; @@ -323,14 +300,13 @@ public static float[] RotateY(float[] output, float[] a, float rad) return output; } - ///** - // * Rotates a quaternion by the given angle aboutput the Z axis - // * - // * @param {quat} output quat receiving operation result - // * @param {quat} a quat to rotate - // * @param {number} rad angle (in radians) to rotate - // * @returns {quat} output - // */ + /// + /// Rotates a quaternion by the given angle aboutput the Z axis + /// + /// quat receiving operation result + /// quat to rotate + /// angle (in radians) to rotate + /// output public static float[] RotateZ(float[] output, float[] a, float rad) { rad /= 2; @@ -345,15 +321,14 @@ public static float[] RotateZ(float[] output, float[] a, float rad) return output; } - ///** - // * Calculates the W component of a quat from the X, Y, and Z components. - // * Assumes that quaternion is 1 unit in length. - // * Any existing W component will be ignored. - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a quat to calculate W component of - // * @returns {quat} output - // */ + /// + /// Calculates the W component of a quat from the X, Y, and Z components. + /// Assumes that quaternion is 1 unit in length. + /// Any existing W component will be ignored. + /// + /// the receiving quaternion + /// quat to calculate W component of + /// output public static float[] CalculateW(float[] output, float[] a) { float x = a[0]; float y = a[1]; float z = a[2]; @@ -366,14 +341,12 @@ public static float[] CalculateW(float[] output, float[] a) return output; } - ///** - // * Calculates the dot product of two quat's - // * - // * @param {quat} a the first operand - // * @param {quat} b the second operand - // * @returns {Number} dot product of a and b - // * @function - // */ + /// + /// Calculates the dot product of two quat's + /// + /// the first operand + /// the second operand + /// dot product of a and b public static float Dot(float[] a, float[] b) { return QVec4f.Dot(a, b); @@ -404,31 +377,27 @@ public static float[] ToEulerAngles(float[] quat) return angles; } - ///** - // * Performs a linear interpolation between two quat's - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a the first operand - // * @param {quat} b the second operand - // * @param {Number} t interpolation amount between the two inputs - // * @returns {quat} output - // * @function - // */ + /// + /// Performs a linear interpolation between two quat's + /// + /// the receiving quaternion + /// the first operand + /// the second operand + /// interpolation amount between the two inputs + /// output public static float[] Lerp(float[] output, float[] a, float[] b, float t) { return QVec4f.Lerp(output, a, b, t); } - ///** - // * Performs a spherical linear interpolation between two quat - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a the first operand - // * @param {quat} b the second operand - // * @param {Number} t interpolation amount between the two inputs - // * @returns {quat} output - // */ - //quat.slerp = function (output, a, b, t) { + /// + /// Performs a spherical linear interpolation between two quat + /// + /// the receiving quaternion + /// the first operand + /// the second operand + /// interpolation amount between the two inputs + /// output public static float[] Slerp(float[] output, float[] a, float[] b, float t) { // // benchmarks: @@ -477,13 +446,12 @@ public static float[] Slerp(float[] output, float[] a, float[] b, float t) return output; } - ///** - // * Calculates the inverse of a quat - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a quat to calculate inverse of - // * @returns {quat} output - // */ + /// + /// Calculates the inverse of a quat + /// + /// the receiving quaternion + /// quat to calculate inverse of + /// output public float[] Invert(float[] output, float[] a) { float a0 = a[0]; float a1 = a[1]; float a2 = a[2]; float a3 = a[3]; @@ -500,14 +468,13 @@ public float[] Invert(float[] output, float[] a) return output; } - ///** - // * Calculates the conjugate of a quat - // * If the quaternion is normalized, this function is faster than quat.inverse and produces the same result. - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a quat to calculate conjugate of - // * @returns {quat} output - // */ + /// + /// Calculates the conjugate of a quat + /// If the quaternion is normalized, this function is faster than quat.inverse and produces the same result. + /// + /// the receiving quaternion + /// quat to calculate conjugate of + /// output public float[] Conjugate(float[] output, float[] a) { output[0] = -a[0]; @@ -517,73 +484,63 @@ public float[] Conjugate(float[] output, float[] a) return output; } - ///** - // * Calculates the length of a quat - // * - // * @param {quat} a vector to calculate length of - // * @returns {Number} length of a - // * @function - // */ + /// + /// Calculates the length of a quat + /// + /// vector to calculate length of + /// length of a //quat.length = QVec4f.length; public static float Length_(float[] a) { return QVec4f.Length_(a); } - ///** - // * Alias for {@link quat.length} - // * @function - // */ + /// + /// Alias for + /// public static float Len(float[] a) { return Length_(a); } - ///** - // * Calculates the squared length of a quat - // * - // * @param {quat} a vector to calculate squared length of - // * @returns {Number} squared length of a - // * @function - // */ + /// + /// Calculates the squared length of a quat + /// + /// vector to calculate squared length of + /// squared length of a public static float SquaredLength(float[] a) { return QVec4f.SquaredLength(a); } - ///** - // * Alias for {@link quat.squaredLength} - // * @function - // */ + /// + /// Alias for + /// public static float SqrLen(float[] a) { return SquaredLength(a); } - ///** - // * Normalize a quat - // * - // * @param {quat} output the receiving quaternion - // * @param {quat} a quaternion to normalize - // * @returns {quat} output - // * @function - // */ + /// + /// Normalize a quat + /// + /// the receiving quaternion + /// quaternion to normalize + /// output public static float[] Normalize(float[] output, float[] a) { return QVec4f.Normalize(output, a); } - ///** - // * Creates a quaternion from the given 3x3 rotation matrix. - // * - // * NOTE: The resultant quaternion is not normalized, so you should be sure - // * to renormalize the quaternion yourself where necessary. - // * - // * @param {quat} output the receiving quaternion - // * @param {mat3} m rotation matrix - // * @returns {quat} output - // * @function - // */ + /// + /// Creates a quaternion from the given 3x3 rotation matrix. + /// + /// NOTE: The resultant quaternion is not normalized, so you should be sure + /// to renormalize the quaternion yourself where necessary. + /// + /// the receiving quaternion + /// rotation matrix + /// output public static float[] FromMat3(float[] output, float[] m) { // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes @@ -628,14 +585,13 @@ public static float[] FromMat3(float[] output, float[] m) } - class QVec4f + partial class QVec4f { - - ///** - // * Creates a new, empty QVec4f - // * - // * @returns {QVec4f} a new 4D vector - // */ + + /// + /// Creates a new, empty QVec4f + /// + /// new 4D vector public static float[] Create() { float[] output = new float[4]; @@ -646,12 +602,11 @@ public static float[] Create() return output; } - ///** - // * Creates a new QVec4f initialized with values from an existing vector - // * - // * @param {QVec4f} a vector to clone - // * @returns {QVec4f} a new 4D vector - // */ + /// + /// Creates a new QVec4f initialized with values from an existing vector + /// + /// vector to clone + /// new 4D vector public static float[] CloneIt(float[] a) { float[] output = new float[4]; @@ -662,15 +617,14 @@ public static float[] CloneIt(float[] a) return output; } - ///** - // * Creates a new QVec4f initialized with the given values - // * - // * @param {Number} x X component - // * @param {Number} y Y component - // * @param {Number} z Z component - // * @param {Number} w W component - // * @returns {QVec4f} a new 4D vector - // */ + /// + /// Creates a new QVec4f initialized with the given values + /// + /// X component + /// Y component + /// Z component + /// W component + /// new 4D vector public static float[] FromValues(float x, float y, float z, float w) { float[] output = new float[4]; @@ -681,13 +635,12 @@ public static float[] FromValues(float x, float y, float z, float w) return output; } - ///** - // * Copy the values from one QVec4f to another - // * - // * @param {QVec4f} output the receiving vector - // * @param {QVec4f} a the source vector - // * @returns {QVec4f} output - // */ + /// + /// Copy the values from one QVec4f to another + /// + /// the receiving vector + /// the source vector + /// output public static float[] Copy(float[] output, float[] a) { output[0] = a[0]; @@ -697,16 +650,15 @@ public static float[] Copy(float[] output, float[] a) return output; } - ///** - // * Set the components of a QVec4f to the given values - // * - // * @param {QVec4f} output the receiving vector - // * @param {Number} x X component - // * @param {Number} y Y component - // * @param {Number} z Z component - // * @param {Number} w W component - // * @returns {QVec4f} output - // */ + /// + /// Set the components of a QVec4f to the given values + /// + /// the receiving vector + /// X component + /// Y component + /// Z component + /// W component + /// output public static float[] Set(float[] output, float x, float y, float z, float w) { output[0] = x; @@ -716,14 +668,13 @@ public static float[] Set(float[] output, float x, float y, float z, float w) return output; } - ///** - // * Adds two QVec4f's - // * - // * @param {QVec4f} output the receiving vector - // * @param {QVec4f} a the first operand - // * @param {QVec4f} b the second operand - // * @returns {QVec4f} output - // */ + /// + /// Adds two QVec4f's + /// + /// the receiving vector + /// the first operand + /// the second operand + /// output public static float[] Add(float[] output, float[] a, float[] b) { output[0] = a[0] + b[0]; @@ -733,14 +684,13 @@ public static float[] Add(float[] output, float[] a, float[] b) return output; } - ///** - // * Subtracts vector b from vector a - // * - // * @param {QVec4f} output the receiving vector - // * @param {QVec4f} a the first operand - // * @param {QVec4f} b the second operand - // * @returns {QVec4f} output - // */ + /// + /// Subtracts vector b from vector a + /// + /// the receiving vector + /// the first operand + /// the second operand + /// output public static float[] Subtract(float[] output, float[] a, float[] b) { output[0] = a[0] - b[0]; @@ -750,23 +700,21 @@ public static float[] Subtract(float[] output, float[] a, float[] b) return output; } - ///** - // * Alias for {@link QVec4f.subtract} - // * @function - // */ + /// + /// Alias for + /// public static float[] Sub(float[] output, float[] a, float[] b) { return Subtract(output, a, b); } - ///** - // * Multiplies two QVec4f's - // * - // * @param {QVec4f} output the receiving vector - // * @param {QVec4f} a the first operand - // * @param {QVec4f} b the second operand - // * @returns {QVec4f} output - // */ + /// + /// Multiplies two QVec4f's + /// + /// the receiving vector + /// the first operand + /// the second operand + /// output public static float[] Multiply(float[] output, float[] a, float[] b) { output[0] = a[0] * b[0]; @@ -776,24 +724,21 @@ public static float[] Multiply(float[] output, float[] a, float[] b) return output; } - ///** - // * Alias for {@link QVec4f.multiply} - // * @function - // */ - //QVec4f.mul = QVec4f.multiply; + /// + /// Alias for + /// public static float[] Mul(float[] output, float[] a, float[] b) { return Multiply(output, a, b); } - ///** - // * Divides two QVec4f's - // * - // * @param {QVec4f} output the receiving vector - // * @param {QVec4f} a the first operand - // * @param {QVec4f} b the second operand - // * @returns {QVec4f} output - // */ + /// + /// Divides two QVec4f's + /// + /// the receiving vector + /// the first operand + /// the second operand + /// output public static float[] Divide(float[] output, float[] a, float[] b) { output[0] = a[0] / b[0]; @@ -803,24 +748,21 @@ public static float[] Divide(float[] output, float[] a, float[] b) return output; } - ///** - // * Alias for {@link QVec4f.divide} - // * @function - // */ - //QVec4f.div = QVec4f.divide; + /// + /// Alias for + /// public static float[] Div(float[] output, float[] a, float[] b) { return Divide(output, a, b); } - ///** - // * Returns the minimum of two QVec4f's - // * - // * @param {QVec4f} output the receiving vector - // * @param {QVec4f} a the first operand - // * @param {QVec4f} b the second operand - // * @returns {QVec4f} output - // */ + /// + /// Returns the minimum of two QVec4f's + /// + /// the receiving vector + /// the first operand + /// the second operand + /// output public static float[] Min(float[] output, float[] a, float[] b) { output[0] = Math.Min(a[0], b[0]); @@ -830,14 +772,13 @@ public static float[] Min(float[] output, float[] a, float[] b) return output; } - ///** - // * Returns the maximum of two QVec4f's - // * - // * @param {QVec4f} output the receiving vector - // * @param {QVec4f} a the first operand - // * @param {QVec4f} b the second operand - // * @returns {QVec4f} output - // */ + /// + /// Returns the maximum of two QVec4f's + /// + /// the receiving vector + /// the first operand + /// the second operand + /// output public static float[] Max(float[] output, float[] a, float[] b) { output[0] = Math.Max(a[0], b[0]); @@ -847,14 +788,13 @@ public static float[] Max(float[] output, float[] a, float[] b) return output; } - ///** - // * Scales a QVec4f by a scalar number - // * - // * @param {QVec4f} output the receiving vector - // * @param {QVec4f} a the vector to scale - // * @param {Number} b amount to scale the vector by - // * @returns {QVec4f} output - // */ + /// + /// Scales a QVec4f by a scalar number + /// + /// the receiving vector + /// the vector to scale + /// amount to scale the vector by + /// output public static float[] Scale(float[] output, float[] a, float b) { output[0] = a[0] * b; @@ -864,15 +804,14 @@ public static float[] Scale(float[] output, float[] a, float b) return output; } - ///** - // * Adds two QVec4f's after scaling the second operand by a scalar value - // * - // * @param {QVec4f} output the receiving vector - // * @param {QVec4f} a the first operand - // * @param {QVec4f} b the second operand - // * @param {Number} scale the amount to scale b by before adding - // * @returns {QVec4f} output - // */ + /// + /// Adds two QVec4f's after scaling the second operand by a scalar value + /// + /// the receiving vector + /// the first operand + /// the second operand + /// the amount to scale b by before adding + /// output public static float[] ScaleAndAdd(float[] output, float[] a, float[] b, float scale) { output[0] = a[0] + (b[0] * scale); @@ -882,13 +821,12 @@ public static float[] ScaleAndAdd(float[] output, float[] a, float[] b, float sc return output; } - ///** - // * Calculates the euclidian distance between two QVec4f's - // * - // * @param {QVec4f} a the first operand - // * @param {QVec4f} b the second operand - // * @returns {Number} distance between a and b - // */ + /// + /// Calculates the euclidian distance between two QVec4f's + /// + /// the first operand + /// the second operand + /// distance between a and b public static float Distance(float[] a, float[] b) { float x = b[0] - a[0]; @@ -898,23 +836,20 @@ public static float Distance(float[] a, float[] b) return GameMath.Sqrt(x * x + y * y + z * z + w * w); } - ///** - // * Alias for {@link QVec4f.distance} - // * @function - // */ - //QVec4f.dist = QVec4f.distance; + /// + /// Alias for + /// public static float Dist(float[] a, float[] b) { return Distance(a, b); } - ///** - // * Calculates the squared euclidian distance between two QVec4f's - // * - // * @param {QVec4f} a the first operand - // * @param {QVec4f} b the second operand - // * @returns {Number} squared distance between a and b - // */ + /// + /// Calculates the squared euclidian distance between two QVec4f's + /// + /// the first operand + /// the second operand + /// squared distance between a and b public static float SquaredDistance(float[] a, float[] b) { float x = b[0] - a[0]; @@ -924,20 +859,19 @@ public static float SquaredDistance(float[] a, float[] b) return x * x + y * y + z * z + w * w; } - ///** - // * Alias for {@link QVec4f.squaredDistance} - // * @function - // */ + /// + /// Alias for + /// public static float SqrDist(float[] a, float[] b) { return SquaredDistance(a, b); } - ///** - // * Calculates the length of a QVec4f - // * - // * @param {QVec4f} a vector to calculate length of - // * @returns {Number} length of a - // */ + + /// + /// Calculates the length of a QVec4f + /// + /// vector to calculate length of + /// length of a public static float Length_(float[] a) { float x = a[0]; @@ -947,21 +881,19 @@ public static float Length_(float[] a) return GameMath.Sqrt(x * x + y * y + z * z + w * w); } - ///** - // * Alias for {@link QVec4f.length} - // * @function - // */ + /// + /// Alias for + /// public static float Len(float[] a) { return Length_(a); } - ///** - // * Calculates the squared length of a QVec4f - // * - // * @param {QVec4f} a vector to calculate squared length of - // * @returns {Number} squared length of a - // */ + /// + /// Calculates the squared length of a QVec4f + /// + /// vector to calculate squared length of + /// squared length of a public static float SquaredLength(float[] a) { float x = a[0]; @@ -971,23 +903,20 @@ public static float SquaredLength(float[] a) return x * x + y * y + z * z + w * w; } - ///** - // * Alias for {@link QVec4f.squaredLength} - // * @function - // */ - //QVec4f.sqrLen = QVec4f.squaredLength; + /// + /// Alias for + /// public static float SqrLen(float[] a) { return SquaredLength(a); } - ///** - // * Negates the components of a QVec4f - // * - // * @param {QVec4f} output the receiving vector - // * @param {QVec4f} a vector to negate - // * @returns {QVec4f} output - // */ + /// + /// Negates the components of a QVec4f + /// + /// the receiving vector + /// vector to negate + /// output public static float[] Negate(float[] output, float[] a) { output[0] = -a[0]; @@ -997,13 +926,12 @@ public static float[] Negate(float[] output, float[] a) return output; } - ///** - // * Normalize a QVec4f - // * - // * @param {QVec4f} output the receiving vector - // * @param {QVec4f} a vector to normalize - // * @returns {QVec4f} output - // */ + /// + /// Normalize a QVec4f + /// + /// the receiving vector + /// vector to normalize + /// output public static float[] Normalize(float[] output, float[] a) { float x = a[0]; @@ -1023,27 +951,25 @@ public static float[] Normalize(float[] output, float[] a) return output; } - ///** - // * Calculates the dot product of two QVec4f's - // * - // * @param {QVec4f} a the first operand - // * @param {QVec4f} b the second operand - // * @returns {Number} dot product of a and b - // */ + /// + /// Calculates the dot product of two QVec4f's + /// + /// the first operand + /// the second operand + /// dot product of a and b public static float Dot(float[] a, float[] b) { return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]; } - ///** - // * Performs a linear interpolation between two QVec4f's - // * - // * @param {QVec4f} output the receiving vector - // * @param {QVec4f} a the first operand - // * @param {QVec4f} b the second operand - // * @param {Number} t interpolation amount between the two inputs - // * @returns {QVec4f} output - // */ + /// + /// Performs a linear interpolation between two QVec4f's + /// + /// the receiving vector + /// the first operand + /// the second operand + /// interpolation amount between the two inputs + /// output public static float[] Lerp(float[] output, float[] a, float[] b, float t) { float ax = a[0]; @@ -1058,14 +984,13 @@ public static float[] Lerp(float[] output, float[] a, float[] b, float t) } - ///** - // * Transforms the QVec4f with a mat4. - // * - // * @param {QVec4f} output the receiving vector - // * @param {QVec4f} a the vector to transform - // * @param {mat4} m matrix to transform with - // * @returns {QVec4f} output - // */ + /// + /// Transforms the QVec4f with a mat4. + /// + /// the receiving vector + /// the vector to transform + /// matrix to transform with + /// output public static float[] TransformMat4(float[] output, float[] a, float[] m) { float x = a[0]; float y = a[1]; float z = a[2]; float w = a[3]; @@ -1076,14 +1001,13 @@ public static float[] TransformMat4(float[] output, float[] a, float[] m) return output; } - ///** - // * Transforms the QVec4f with a quat - // * - // * @param {QVec4f} output the receiving vector - // * @param {QVec4f} a the vector to transform - // * @param {quat} q quaternion to transform with - // * @returns {QVec4f} output - // */ + /// + /// Transforms the QVec4f with a quat + /// + /// the receiving vector + /// the vector to transform + /// quaternion to transform with + /// output public static float[] transformQuat(float[] output, float[] a, float[] q) { float x = a[0]; float y = a[1]; float z = a[2]; @@ -1109,7 +1033,7 @@ public static float[] transformQuat(float[] output, float[] a, float[] q) /// /// Don't use this class unless you need it to interoperate with Mat4d /// - public class Vec3Utilsf + public partial class Vec3Utilsf { /// Creates a new, empty vec3 /// Returns {vec3} a new 3D vector. diff --git a/Math/Matrix/SpanMat22.cs b/Math/Matrix/SpanMat22.cs new file mode 100644 index 000000000..8f9c0e28b --- /dev/null +++ b/Math/Matrix/SpanMat22.cs @@ -0,0 +1,185 @@ +//glMatrix license: +//Copyright (c) 2013, Brandon Jones, Colin MacKenzie IV. All rights reserved. + +//Redistribution and use in source and binary forms, with or without modification, +//are permitted provided that the following conditions are met: + +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. + +//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +//ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +//WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +//DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +//ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +//ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +//(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +//SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +using System; +using System.Runtime.CompilerServices; + +namespace Vintagestory.API.MathTools +{ + /// + /// 2x2 Matrix + /// + public partial class Mat22 + { + /// + /// Set a mat2 to the identity matrix + /// Returns output + /// + /// the receiving matrix + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Identity(Span output) + { + output[0] = 1; + output[1] = 0; + output[2] = 0; + output[3] = 1; + } + + /// + /// Transpose the values of a mat2 + /// Returns output + /// + /// the receiving matrix + /// the source matrix + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Transpose(Span output, ReadOnlySpan a) + { + output[0] = a[0]; + output[1] = a[2]; + output[2] = a[1]; + output[3] = a[3]; + } + + /// + /// Inverts a mat2 + /// Returns output + /// + /// the receiving matrix + /// the source matrix + /// if the operation was successful + public static bool Invert(Span output, ReadOnlySpan a) + { + float a0 = a[0]; float a1 = a[1]; float a2 = a[2]; float a3 = a[3]; + + // Calculate the determinant + float det = a0 * a3 - a2 * a1; + + if (det == 0) + { + return false; + } + float one = 1; + det = one / det; + + output[0] = a3 * det; + output[1] = -a1 * det; + output[2] = -a2 * det; + output[3] = a0 * det; + + return true; + } + + /// + /// Calculates the adjugate of a mat2 + /// Returns output + /// + /// the receiving matrix + /// the source matrix + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Adjoint(Span output, ReadOnlySpan a) + { + // Caching this value is nessecary if output == a + float a0 = a[0]; + output[0] = a[3]; + output[1] = -a[1]; + output[2] = -a[2]; + output[3] = a0; + } + + /// + /// Calculates the determinant of a mat2 + /// Returns determinant of a + /// + /// the source matrix + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Determinant(ReadOnlySpan a) + { + return a[0] * a[3] - a[2] * a[1]; + } + + /// + /// Multiplies two mat2's + /// Returns output + /// + /// the receiving matrix + /// the first operand + /// the second operand + public static void Multiply(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + float a0 = a[0]; float a1 = a[1]; float a2 = a[2]; float a3 = a[3]; + float b0 = b[0]; float b1 = b[1]; float b2 = b[2]; float b3 = b[3]; + output[0] = a0 * b0 + a1 * b2; + output[1] = a0 * b1 + a1 * b3; + output[2] = a2 * b0 + a3 * b2; + output[3] = a2 * b1 + a3 * b3; + } + + /// + /// Alias for + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Mul(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Multiply(output, a, b); + } + + /// + /// Rotates a mat2 by the given angle + /// Returns output + /// + /// the receiving matrix + /// the matrix to rotate + /// the angle to rotate the matrix by + public static void Rotate(Span output, ReadOnlySpan a, float rad) + { + float a0 = a[0]; float a1 = a[1]; float a2 = a[2]; float a3 = a[3]; + float s = GameMath.Sin(rad); + float c = GameMath.Cos(rad); + output[0] = a0 * c + a1 * s; + output[1] = a0 * -s + a1 * c; + output[2] = a2 * c + a3 * s; + output[3] = a2 * -s + a3 * c; + } + + /// + /// Scales the mat2 by the dimensions in the given vec2 + /// Returns output + /// + /// the receiving matrix + /// the matrix to rotate + /// the vec2 to scale the matrix by + public static void Scale(Span output, ReadOnlySpan a, ReadOnlySpan v) + { + float a0 = a[0]; float a1 = a[1]; float a2 = a[2]; float a3 = a[3]; + float v0 = v[0]; float v1 = v[1]; + output[0] = a0 * v0; + output[1] = a1 * v1; + output[2] = a2 * v0; + output[3] = a3 * v1; + } + } +} diff --git a/Math/Matrix/SpanMat23.cs b/Math/Matrix/SpanMat23.cs new file mode 100644 index 000000000..2302461a7 --- /dev/null +++ b/Math/Matrix/SpanMat23.cs @@ -0,0 +1,202 @@ +//glMatrix license: +//Copyright (c) 2013, Brandon Jones, Colin MacKenzie IV. All rights reserved. + +//Redistribution and use in source and binary forms, with or without modification, +//are permitted provided that the following conditions are met: + +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. + +//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +//ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +//WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +//DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +//ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +//ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +//(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +//SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +using System; +using System.Runtime.CompilerServices; + +namespace Vintagestory.API.MathTools +{ + /// + /// 2x3 Matrix + /// * A mat2d contains six elements defined as: + /// * 
+    /// * [a, b,
+    /// *  c, d,
+    /// *  tx,ty]
+    /// *
+ /// * This is a short form for the 3x3 matrix: + /// * 
+    /// * [a, b, 0
+    /// *  c, d, 0
+    /// *  tx,ty,1]
+    /// *
+ /// * The last column is ignored so the array is shorter and operations are faster. + /// + public partial class Mat23 + { + /// + /// Set a mat2d to the identity matrix + /// Returns output + /// + /// the receiving matrix + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Identity(Span output) + { + output[0] = 1; + output[1] = 0; + output[2] = 0; + output[3] = 1; + output[4] = 0; + output[5] = 0; + } + + /// + /// Inverts a mat2d + /// Returns output + /// + /// the receiving matrix + /// the source matrix + /// if the operation was successful + public static bool Invert(Span output, ReadOnlySpan a) + { + float aa = a[0]; float ab = a[1]; float ac = a[2]; float ad = a[3]; + float atx = a[4]; float aty = a[5]; + + float det = aa * ad - ab * ac; + if (det == 0) + { + return false; + } + float one = 1; + det = one / det; + + output[0] = ad * det; + output[1] = -ab * det; + output[2] = -ac * det; + output[3] = aa * det; + output[4] = (ac * aty - ad * atx) * det; + output[5] = (ab * atx - aa * aty) * det; + return true; + } + + /// + /// Calculates the determinant of a mat2d + /// Returns determinant of a + /// + /// the source matrix + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Determinant(ReadOnlySpan a) + { + return a[0] * a[3] - a[1] * a[2]; + } + + /// + /// Multiplies two mat2d's + /// Returns output + /// + /// the receiving matrix + /// the first operand + /// the second operand + public static void Multiply(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + float aa = a[0]; float ab = a[1]; float ac = a[2]; float ad = a[3]; + float atx = a[4]; float aty = a[5]; + float ba = b[0]; float bb = b[1]; float bc = b[2]; float bd = b[3]; + float btx = b[4]; float bty = b[5]; + + output[0] = aa * ba + ab * bc; + output[1] = aa * bb + ab * bd; + output[2] = ac * ba + ad * bc; + output[3] = ac * bb + ad * bd; + output[4] = ba * atx + bc * aty + btx; + output[5] = bb * atx + bd * aty + bty; + } + + /// + /// Alias for + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Mul(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Multiply(output, a, b); + } + + + /// + /// Rotates a mat2d by the given angle + /// Returns output + /// + /// the receiving matrix + /// the matrix to rotate + /// the angle to rotate the matrix by + public static void Rotate(Span output, ReadOnlySpan a, float rad) + { + float aa = a[0]; + float ab = a[1]; + float ac = a[2]; + float ad = a[3]; + float atx = a[4]; + float aty = a[5]; + float st = GameMath.Sin(rad); + float ct = GameMath.Cos(rad); + + output[0] = aa * ct + ab * st; + output[1] = -aa * st + ab * ct; + output[2] = ac * ct + ad * st; + output[3] = -ac * st + ct * ad; + output[4] = ct * atx + st * aty; + output[5] = ct * aty - st * atx; + } + + /// + /// Scales the mat2d by the dimensions in the given vec2 + /// Returns output + /// + /// the receiving matrix + /// the matrix to translate + /// the vec2 to scale the matrix by + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Scale(Span output, ReadOnlySpan a, ReadOnlySpan v) + { + float vx = v[0]; float vy = v[1]; + output[0] = a[0] * vx; + output[1] = a[1] * vy; + output[2] = a[2] * vx; + output[3] = a[3] * vy; + output[4] = a[4] * vx; + output[5] = a[5] * vy; + } + + /// + /// Translates the mat2d by the dimensions in the given vec2 + /// Returns output + /// + /// the receiving matrix + /// the matrix to translate + /// the vec2 to translate the matrix by + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Translate(Span output, ReadOnlySpan a, ReadOnlySpan v) + { + output[0] = a[0]; + output[1] = a[1]; + output[2] = a[2]; + output[3] = a[3]; + output[4] = a[4] + v[0]; + output[5] = a[5] + v[1]; + } + } +} diff --git a/Math/Matrix/SpanMat3d.cs b/Math/Matrix/SpanMat3d.cs new file mode 100644 index 000000000..6532e4323 --- /dev/null +++ b/Math/Matrix/SpanMat3d.cs @@ -0,0 +1,398 @@ +//glMatrix license: +//Copyright (c) 2013, Brandon Jones, Colin MacKenzie IV. All rights reserved. + +//Redistribution and use in source and binary forms, with or without modification, +//are permitted provided that the following conditions are met: + +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. + +//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +//ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +//WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +//DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +//ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +//ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +//(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +//SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +using System; +using System.Runtime.CompilerServices; + +namespace Vintagestory.API.MathTools +{ + public partial class Mat3d + { + /// + /// Copies the upper-left 3x3 values into the given mat3. + /// + /// the receiving 3x3 matrix + /// the source 4x4 matrix + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void FromMat4(Span output, ReadOnlySpan a) + { + output[0] = a[0]; + output[1] = a[1]; + output[2] = a[2]; + output[3] = a[4]; + output[4] = a[5]; + output[5] = a[6]; + output[6] = a[8]; + output[7] = a[9]; + output[8] = a[10]; + } + + /// + /// Set a mat3 to the identity matrix + /// + /// the receiving matrix + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Identity(Span output) + { + output[0] = 1.0; + output[1] = 0.0; + output[2] = 0.0; + output[3] = 0.0; + output[4] = 1.0; + output[5] = 0.0; + output[6] = 0.0; + output[7] = 0.0; + output[8] = 1.0; + } + + /// + /// Transpose the values of a mat3 + /// + /// the receiving matrix + /// the source matrix + public static void Transpose(Span output, ReadOnlySpan a) + { + // If we are transposing ourselves we can skip a few steps but have to cache some values + if (output == a) + { + double a01 = a[1]; + double a02 = a[2]; + double a12 = a[5]; + output[1] = a[3]; + output[2] = a[6]; + output[3] = a01; + output[5] = a[7]; + output[6] = a02; + output[7] = a12; + } + else + { + output[0] = a[0]; + output[1] = a[3]; + output[2] = a[6]; + output[3] = a[1]; + output[4] = a[4]; + output[5] = a[7]; + output[6] = a[2]; + output[7] = a[5]; + output[8] = a[8]; + } + } + + /// + /// Inverts a mat3 + /// + /// the receiving matrix + /// the source matrix + /// if the operation was successful + public static bool Invert(Span output, ReadOnlySpan a) + { + double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; + double a10 = a[3]; double a11 = a[4]; double a12 = a[5]; + double a20 = a[6]; double a21 = a[7]; double a22 = a[8]; + + double b01 = a22 * a11 - a12 * a21; + double b11 = -a22 * a10 + a12 * a20; + double b21 = a21 * a10 - a11 * a20; + + // Calculate the determinant + double det = a00 * b01 + a01 * b11 + a02 * b21; + + if (det == 0.0) + { + return false; + } + double one = 1.0; + det = one / det; + + output[0] = b01 * det; + output[1] = (-a22 * a01 + a02 * a21) * det; + output[2] = (a12 * a01 - a02 * a11) * det; + output[3] = b11 * det; + output[4] = (a22 * a00 - a02 * a20) * det; + output[5] = (-a12 * a00 + a02 * a10) * det; + output[6] = b21 * det; + output[7] = (-a21 * a00 + a01 * a20) * det; + output[8] = (a11 * a00 - a01 * a10) * det; + return true; + } + + /// + /// Calculates the adjugate of a mat3 + /// + /// the receiving matrix + /// the source matrix + public static void Adjoint(Span output, ReadOnlySpan a) + { + double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; + double a10 = a[3]; double a11 = a[4]; double a12 = a[5]; + double a20 = a[6]; double a21 = a[7]; double a22 = a[8]; + + output[0] = (a11 * a22 - a12 * a21); + output[1] = (a02 * a21 - a01 * a22); + output[2] = (a01 * a12 - a02 * a11); + output[3] = (a12 * a20 - a10 * a22); + output[4] = (a00 * a22 - a02 * a20); + output[5] = (a02 * a10 - a00 * a12); + output[6] = (a10 * a21 - a11 * a20); + output[7] = (a01 * a20 - a00 * a21); + output[8] = (a00 * a11 - a01 * a10); + } + + /// + /// Calculates the determinant of a mat3 + /// + /// the source matrix + public static double Determinant(ReadOnlySpan a) + { + double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; + double a10 = a[3]; double a11 = a[4]; double a12 = a[5]; + double a20 = a[6]; double a21 = a[7]; double a22 = a[8]; + + return a00 * (a22 * a11 - a12 * a21) + a01 * (-a22 * a10 + a12 * a20) + a02 * (a21 * a10 - a11 * a20); + } + + /// + /// Multiplies two mat3's + /// + /// the receiving matrix + /// the first operand + /// the second operand + public static void Multiply(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; + double a10 = a[3]; double a11 = a[4]; double a12 = a[5]; + double a20 = a[6]; double a21 = a[7]; double a22 = a[8]; + + double b00 = b[0]; double b01 = b[1]; double b02 = b[2]; + double b10 = b[3]; double b11 = b[4]; double b12 = b[5]; + double b20 = b[6]; double b21 = b[7]; double b22 = b[8]; + + output[0] = b00 * a00 + b01 * a10 + b02 * a20; + output[1] = b00 * a01 + b01 * a11 + b02 * a21; + output[2] = b00 * a02 + b01 * a12 + b02 * a22; + + output[3] = b10 * a00 + b11 * a10 + b12 * a20; + output[4] = b10 * a01 + b11 * a11 + b12 * a21; + output[5] = b10 * a02 + b11 * a12 + b12 * a22; + + output[6] = b20 * a00 + b21 * a10 + b22 * a20; + output[7] = b20 * a01 + b21 * a11 + b22 * a21; + output[8] = b20 * a02 + b21 * a12 + b22 * a22; + } + + /// + /// Alias for + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Mul(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Multiply(output, a, b); + } + + /// + /// Translate a mat3 by the given vector + /// + /// the receiving matrix + /// the matrix to translate + /// vector to translate by + public static void Translate(Span output, ReadOnlySpan a, ReadOnlySpan v) + { + double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; + double a10 = a[3]; double a11 = a[4]; double a12 = a[5]; + double a20 = a[6]; double a21 = a[7]; double a22 = a[8]; + double x = v[0]; double y = v[1]; + + output[0] = a00; + output[1] = a01; + output[2] = a02; + + output[3] = a10; + output[4] = a11; + output[5] = a12; + + output[6] = x * a00 + y * a10 + a20; + output[7] = x * a01 + y * a11 + a21; + output[8] = x * a02 + y * a12 + a22; + } + + /// + /// Rotates a mat3 by the given angle + /// + /// the receiving matrix + /// the matrix to rotate + /// the angle to rotate the matrix by + public static void Rotate(Span output, ReadOnlySpan a, double rad) + { + double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; + double a10 = a[3]; double a11 = a[4]; double a12 = a[5]; + double a20 = a[6]; double a21 = a[7]; double a22 = a[8]; + + double s = GameMath.Sin(rad); + double c = GameMath.Cos(rad); + + output[0] = c * a00 + s * a10; + output[1] = c * a01 + s * a11; + output[2] = c * a02 + s * a12; + + output[3] = c * a10 - s * a00; + output[4] = c * a11 - s * a01; + output[5] = c * a12 - s * a02; + + output[6] = a20; + output[7] = a21; + output[8] = a22; + } + + /// + /// Scales the mat3 by the dimensions in the given vec2 + /// + /// the receiving matrix + /// the matrix to rotate + /// the vec2 to scale the matrix by + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Scale(Span output, ReadOnlySpan a, ReadOnlySpan v) + { + double x = v[0]; double y = v[1]; + + output[0] = x * a[0]; + output[1] = x * a[1]; + output[2] = x * a[2]; + + output[3] = y * a[3]; + output[4] = y * a[4]; + output[5] = y * a[5]; + + output[6] = a[6]; + output[7] = a[7]; + output[8] = a[8]; + } + + /// + /// Copies the values from a mat2d into a mat3 + /// + /// the receiving matrix + /// the matrix to copy + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void FromMat2d(Span output, ReadOnlySpan a) + { + output[0] = a[0]; + output[1] = a[1]; + output[2] = 0.0; + + output[3] = a[2]; + output[4] = a[3]; + output[5] = 0.0; + + output[6] = a[4]; + output[7] = a[5]; + output[8] = 1.0; + } + + /// + /// Calculates a 3x3 matrix from the given quaternion + /// + /// mat3 receiving operation result + /// Quaternion to create matrix from + public static void FromQuat(Span output, ReadOnlySpan q) + { + double x = q[0]; double y = q[1]; double z = q[2]; double w = q[3]; + double x2 = x + x; + double y2 = y + y; + double z2 = z + z; + + double xx = x * x2; + double xy = x * y2; + double xz = x * z2; + double yy = y * y2; + double yz = y * z2; + double zz = z * z2; + double wx = w * x2; + double wy = w * y2; + double wz = w * z2; + + output[0] = 1.0 - (yy + zz); + output[3] = xy + wz; + output[6] = xz - wy; + + output[1] = xy - wz; + output[4] = 1.0 - (xx + zz); + output[7] = yz + wx; + + output[2] = xz + wy; + output[5] = yz - wx; + output[8] = 1.0 - (xx + yy); + } + + /// + /// Calculates a 3x3 normal matrix (transpose inverse) from the 4x4 matrix + /// + /// mat3 receiving operation result + /// Mat4 to derive the normal matrix from + /// if the operation was successful + public static bool NormalFromMat4(Span output, ReadOnlySpan a) + { + double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; double a03 = a[3]; + double a10 = a[4]; double a11 = a[5]; double a12 = a[6]; double a13 = a[7]; + double a20 = a[8]; double a21 = a[9]; double a22 = a[10]; double a23 = a[11]; + double a30 = a[12]; double a31 = a[13]; double a32 = a[14]; double a33 = a[15]; + + double b00 = a00 * a11 - a01 * a10; + double b01 = a00 * a12 - a02 * a10; + double b02 = a00 * a13 - a03 * a10; + double b03 = a01 * a12 - a02 * a11; + double b04 = a01 * a13 - a03 * a11; + double b05 = a02 * a13 - a03 * a12; + double b06 = a20 * a31 - a21 * a30; + double b07 = a20 * a32 - a22 * a30; + double b08 = a20 * a33 - a23 * a30; + double b09 = a21 * a32 - a22 * a31; + double b10 = a21 * a33 - a23 * a31; + double b11 = a22 * a33 - a23 * a32; + + // Calculate the determinant + double det = b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06; + + if (det == 0.0) + { + return false; + } + double one = 1.0; + det = one / det; + + output[0] = (a11 * b11 - a12 * b10 + a13 * b09) * det; + output[1] = (a12 * b08 - a10 * b11 - a13 * b07) * det; + output[2] = (a10 * b10 - a11 * b08 + a13 * b06) * det; + + output[3] = (a02 * b10 - a01 * b11 - a03 * b09) * det; + output[4] = (a00 * b11 - a02 * b08 + a03 * b07) * det; + output[5] = (a01 * b08 - a00 * b10 - a03 * b06) * det; + + output[6] = (a31 * b05 - a32 * b04 + a33 * b03) * det; + output[7] = (a32 * b02 - a30 * b05 - a33 * b01) * det; + output[8] = (a30 * b04 - a31 * b02 + a33 * b00) * det; + + return true; + } + } +} diff --git a/Math/Matrix/SpanMat3f.cs b/Math/Matrix/SpanMat3f.cs new file mode 100644 index 000000000..c9bd83d05 --- /dev/null +++ b/Math/Matrix/SpanMat3f.cs @@ -0,0 +1,398 @@ +//glMatrix license: +//Copyright (c) 2013, Brandon Jones, Colin MacKenzie IV. All rights reserved. + +//Redistribution and use in source and binary forms, with or without modification, +//are permitted provided that the following conditions are met: + +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. + +//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +//ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +//WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +//DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +//ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +//ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +//(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +//SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +using System; +using System.Runtime.CompilerServices; + +namespace Vintagestory.API.MathTools +{ + public partial class Mat3f + { + /// + /// Copies the upper-left 3x3 values into the given mat3. + /// + /// the receiving 3x3 matrix + /// the source 4x4 matrix + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void FromMat4(Span output, ReadOnlySpan a) + { + output[0] = a[0]; + output[1] = a[1]; + output[2] = a[2]; + output[3] = a[4]; + output[4] = a[5]; + output[5] = a[6]; + output[6] = a[8]; + output[7] = a[9]; + output[8] = a[10]; + } + + /// + /// Set a mat3 to the identity matrix + /// + /// the receiving matrix + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Identity(Span output) + { + output[0] = 1f; + output[1] = 0f; + output[2] = 0f; + output[3] = 0f; + output[4] = 1f; + output[5] = 0f; + output[6] = 0f; + output[7] = 0f; + output[8] = 1f; + } + + /// + /// Transpose the values of a mat3 + /// + /// the receiving matrix + /// the source matrix + public static void Transpose(Span output, ReadOnlySpan a) + { + // If we are transposing ourselves we can skip a few steps but have to cache some values + if (output == a) + { + float a01 = a[1]; + float a02 = a[2]; + float a12 = a[5]; + output[1] = a[3]; + output[2] = a[6]; + output[3] = a01; + output[5] = a[7]; + output[6] = a02; + output[7] = a12; + } + else + { + output[0] = a[0]; + output[1] = a[3]; + output[2] = a[6]; + output[3] = a[1]; + output[4] = a[4]; + output[5] = a[7]; + output[6] = a[2]; + output[7] = a[5]; + output[8] = a[8]; + } + } + + /// + /// Inverts a mat3 + /// + /// the receiving matrix + /// the source matrix + /// if the operation was successful + public static bool Invert(Span output, ReadOnlySpan a) + { + float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; + float a10 = a[3]; float a11 = a[4]; float a12 = a[5]; + float a20 = a[6]; float a21 = a[7]; float a22 = a[8]; + + float b01 = a22 * a11 - a12 * a21; + float b11 = -a22 * a10 + a12 * a20; + float b21 = a21 * a10 - a11 * a20; + + // Calculate the determinant + float det = a00 * b01 + a01 * b11 + a02 * b21; + + if (det == 0f) + { + return false; + } + float one = 1f; + det = one / det; + + output[0] = b01 * det; + output[1] = (-a22 * a01 + a02 * a21) * det; + output[2] = (a12 * a01 - a02 * a11) * det; + output[3] = b11 * det; + output[4] = (a22 * a00 - a02 * a20) * det; + output[5] = (-a12 * a00 + a02 * a10) * det; + output[6] = b21 * det; + output[7] = (-a21 * a00 + a01 * a20) * det; + output[8] = (a11 * a00 - a01 * a10) * det; + return true; + } + + /// + /// Calculates the adjugate of a mat3 + /// + /// the receiving matrix + /// the source matrix + public static void Adjoint(Span output, ReadOnlySpan a) + { + float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; + float a10 = a[3]; float a11 = a[4]; float a12 = a[5]; + float a20 = a[6]; float a21 = a[7]; float a22 = a[8]; + + output[0] = (a11 * a22 - a12 * a21); + output[1] = (a02 * a21 - a01 * a22); + output[2] = (a01 * a12 - a02 * a11); + output[3] = (a12 * a20 - a10 * a22); + output[4] = (a00 * a22 - a02 * a20); + output[5] = (a02 * a10 - a00 * a12); + output[6] = (a10 * a21 - a11 * a20); + output[7] = (a01 * a20 - a00 * a21); + output[8] = (a00 * a11 - a01 * a10); + } + + /// + /// Calculates the determinant of a mat3 + /// + /// the source matrix + public static float Determinant(ReadOnlySpan a) + { + float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; + float a10 = a[3]; float a11 = a[4]; float a12 = a[5]; + float a20 = a[6]; float a21 = a[7]; float a22 = a[8]; + + return a00 * (a22 * a11 - a12 * a21) + a01 * (-a22 * a10 + a12 * a20) + a02 * (a21 * a10 - a11 * a20); + } + + /// + /// Multiplies two mat3's + /// + /// the receiving matrix + /// the first operand + /// the second operand + public static void Multiply(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; + float a10 = a[3]; float a11 = a[4]; float a12 = a[5]; + float a20 = a[6]; float a21 = a[7]; float a22 = a[8]; + + float b00 = b[0]; float b01 = b[1]; float b02 = b[2]; + float b10 = b[3]; float b11 = b[4]; float b12 = b[5]; + float b20 = b[6]; float b21 = b[7]; float b22 = b[8]; + + output[0] = b00 * a00 + b01 * a10 + b02 * a20; + output[1] = b00 * a01 + b01 * a11 + b02 * a21; + output[2] = b00 * a02 + b01 * a12 + b02 * a22; + + output[3] = b10 * a00 + b11 * a10 + b12 * a20; + output[4] = b10 * a01 + b11 * a11 + b12 * a21; + output[5] = b10 * a02 + b11 * a12 + b12 * a22; + + output[6] = b20 * a00 + b21 * a10 + b22 * a20; + output[7] = b20 * a01 + b21 * a11 + b22 * a21; + output[8] = b20 * a02 + b21 * a12 + b22 * a22; + } + + /// + /// Alias for + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Mul(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Multiply(output, a, b); + } + + /// + /// Translate a mat3 by the given vector + /// + /// the receiving matrix + /// the matrix to translate + /// vector to translate by + public static void Translate(Span output, ReadOnlySpan a, ReadOnlySpan v) + { + float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; + float a10 = a[3]; float a11 = a[4]; float a12 = a[5]; + float a20 = a[6]; float a21 = a[7]; float a22 = a[8]; + float x = v[0]; float y = v[1]; + + output[0] = a00; + output[1] = a01; + output[2] = a02; + + output[3] = a10; + output[4] = a11; + output[5] = a12; + + output[6] = x * a00 + y * a10 + a20; + output[7] = x * a01 + y * a11 + a21; + output[8] = x * a02 + y * a12 + a22; + } + + /// + /// Rotates a mat3 by the given angle + /// + /// the receiving matrix + /// the matrix to rotate + /// the angle to rotate the matrix by + public static void Rotate(Span output, ReadOnlySpan a, float rad) + { + float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; + float a10 = a[3]; float a11 = a[4]; float a12 = a[5]; + float a20 = a[6]; float a21 = a[7]; float a22 = a[8]; + + float s = GameMath.Sin(rad); + float c = GameMath.Cos(rad); + + output[0] = c * a00 + s * a10; + output[1] = c * a01 + s * a11; + output[2] = c * a02 + s * a12; + + output[3] = c * a10 - s * a00; + output[4] = c * a11 - s * a01; + output[5] = c * a12 - s * a02; + + output[6] = a20; + output[7] = a21; + output[8] = a22; + } + + /// + /// Scales the mat3 by the dimensions in the given vec2 + /// + /// the receiving matrix + /// the matrix to rotate + /// the vec2 to scale the matrix by + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Scale(Span output, ReadOnlySpan a, ReadOnlySpan v) + { + float x = v[0]; float y = v[1]; + + output[0] = x * a[0]; + output[1] = x * a[1]; + output[2] = x * a[2]; + + output[3] = y * a[3]; + output[4] = y * a[4]; + output[5] = y * a[5]; + + output[6] = a[6]; + output[7] = a[7]; + output[8] = a[8]; + } + + /// + /// Copies the values from a mat2d into a mat3 + /// + /// the receiving matrix + /// the matrix to copy + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void FromMat2d(Span output, ReadOnlySpan a) + { + output[0] = a[0]; + output[1] = a[1]; + output[2] = 0f; + + output[3] = a[2]; + output[4] = a[3]; + output[5] = 0f; + + output[6] = a[4]; + output[7] = a[5]; + output[8] = 1f; + } + + /// + /// Calculates a 3x3 matrix from the given quaternion + /// + /// mat3 receiving operation result + /// Quaternion to create matrix from + public static void FromQuat(Span output, ReadOnlySpan q) + { + float x = q[0]; float y = q[1]; float z = q[2]; float w = q[3]; + float x2 = x + x; + float y2 = y + y; + float z2 = z + z; + + float xx = x * x2; + float xy = x * y2; + float xz = x * z2; + float yy = y * y2; + float yz = y * z2; + float zz = z * z2; + float wx = w * x2; + float wy = w * y2; + float wz = w * z2; + + output[0] = 1f - (yy + zz); + output[3] = xy + wz; + output[6] = xz - wy; + + output[1] = xy - wz; + output[4] = 1f - (xx + zz); + output[7] = yz + wx; + + output[2] = xz + wy; + output[5] = yz - wx; + output[8] = 1f - (xx + yy); + } + + /// + /// Calculates a 3x3 normal matrix (transpose inverse) from the 4x4 matrix + /// + /// mat3 receiving operation result + /// Mat4 to derive the normal matrix from + /// if the operation was successful + public static bool NormalFromMat4(Span output, ReadOnlySpan a) + { + float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; float a03 = a[3]; + float a10 = a[4]; float a11 = a[5]; float a12 = a[6]; float a13 = a[7]; + float a20 = a[8]; float a21 = a[9]; float a22 = a[10]; float a23 = a[11]; + float a30 = a[12]; float a31 = a[13]; float a32 = a[14]; float a33 = a[15]; + + float b00 = a00 * a11 - a01 * a10; + float b01 = a00 * a12 - a02 * a10; + float b02 = a00 * a13 - a03 * a10; + float b03 = a01 * a12 - a02 * a11; + float b04 = a01 * a13 - a03 * a11; + float b05 = a02 * a13 - a03 * a12; + float b06 = a20 * a31 - a21 * a30; + float b07 = a20 * a32 - a22 * a30; + float b08 = a20 * a33 - a23 * a30; + float b09 = a21 * a32 - a22 * a31; + float b10 = a21 * a33 - a23 * a31; + float b11 = a22 * a33 - a23 * a32; + + // Calculate the determinant + float det = b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06; + + if (det == 0f) + { + return false; + } + float one = 1f; + det = one / det; + + output[0] = (a11 * b11 - a12 * b10 + a13 * b09) * det; + output[1] = (a12 * b08 - a10 * b11 - a13 * b07) * det; + output[2] = (a10 * b10 - a11 * b08 + a13 * b06) * det; + + output[3] = (a02 * b10 - a01 * b11 - a03 * b09) * det; + output[4] = (a00 * b11 - a02 * b08 + a03 * b07) * det; + output[5] = (a01 * b08 - a00 * b10 - a03 * b06) * det; + + output[6] = (a31 * b05 - a32 * b04 + a33 * b03) * det; + output[7] = (a32 * b02 - a30 * b05 - a33 * b01) * det; + output[8] = (a30 * b04 - a31 * b02 + a33 * b00) * det; + + return true; + } + } +} diff --git a/Math/Matrix/SpanMat4d.cs b/Math/Matrix/SpanMat4d.cs new file mode 100644 index 000000000..bcd56cb48 --- /dev/null +++ b/Math/Matrix/SpanMat4d.cs @@ -0,0 +1,1040 @@ +//glMatrix license: +//Copyright (c) 2013, Brandon Jones, Colin MacKenzie IV. All rights reserved. + +//Redistribution and use in source and binary forms, with or without modification, +//are permitted provided that the following conditions are met: + +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. + +//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +//ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +//WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +//DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +//ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +//ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +//(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +//SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +using System; +using System.Runtime.CompilerServices; + +namespace Vintagestory.API.MathTools +{ + /// + /// 4x4 Matrix Math + /// + public partial class Mat4d + { + private const double GLMAT_EPSILON = 1.0 / 1000000.0; + + /// + /// Set a mat4 to the identity matrix + /// + /// {mat4} out the receiving matrix + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Identity(Span output) + { + output[0] = 1; + output[1] = 0; + output[2] = 0; + output[3] = 0; + output[4] = 0; + output[5] = 1; + output[6] = 0; + output[7] = 0; + output[8] = 0; + output[9] = 0; + output[10] = 1; + output[11] = 0; + output[12] = 0; + output[13] = 0; + output[14] = 0; + output[15] = 1; + } + + + /// + /// Set a mat4 to the identity matrix with a scale applied + /// + /// {mat4} out the receiving matrix + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Identity_Scaled(Span output, double scale) + { + output[0] = scale; + output[1] = 0; + output[2] = 0; + output[3] = 0; + output[4] = 0; + output[5] = scale; + output[6] = 0; + output[7] = 0; + output[8] = 0; + output[9] = 0; + output[10] = scale; + output[11] = 0; + output[12] = 0; + output[13] = 0; + output[14] = 0; + output[15] = 1; + } + + + /// + /// Transpose the values of a mat4 + /// + /// {mat4} out the receiving matrix + /// {mat4} a the source matrix + public static void Transpose(Span output, ReadOnlySpan a) + { + // If we are transposing ourselves we can skip a few steps but have to cache some values + if (output == a) + { + double a01 = a[1]; double a02 = a[2]; double a03 = a[3]; + double a12 = a[6]; double a13 = a[7]; + double a23 = a[11]; + + output[1] = a[4]; + output[2] = a[8]; + output[3] = a[12]; + output[4] = a01; + output[6] = a[9]; + output[7] = a[13]; + output[8] = a02; + output[9] = a12; + output[11] = a[14]; + output[12] = a03; + output[13] = a13; + output[14] = a23; + } + else + { + output[0] = a[0]; + output[1] = a[4]; + output[2] = a[8]; + output[3] = a[12]; + output[4] = a[1]; + output[5] = a[5]; + output[6] = a[9]; + output[7] = a[13]; + output[8] = a[2]; + output[9] = a[6]; + output[10] = a[10]; + output[11] = a[14]; + output[12] = a[3]; + output[13] = a[7]; + output[14] = a[11]; + output[15] = a[15]; + } + } + + /// + /// Inverts a mat4 + /// + /// {mat4} out the receiving matrix + /// {mat4} a the source matrix + /// if the operation was successful + public static bool Invert(Span output, ReadOnlySpan a) + { + double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; double a03 = a[3]; + double a10 = a[4]; double a11 = a[5]; double a12 = a[6]; double a13 = a[7]; + double a20 = a[8]; double a21 = a[9]; double a22 = a[10]; double a23 = a[11]; + double a30 = a[12]; double a31 = a[13]; double a32 = a[14]; double a33 = a[15]; + + double b00 = a00 * a11 - a01 * a10; + double b01 = a00 * a12 - a02 * a10; + double b02 = a00 * a13 - a03 * a10; + double b03 = a01 * a12 - a02 * a11; + double b04 = a01 * a13 - a03 * a11; + double b05 = a02 * a13 - a03 * a12; + double b06 = a20 * a31 - a21 * a30; + double b07 = a20 * a32 - a22 * a30; + double b08 = a20 * a33 - a23 * a30; + double b09 = a21 * a32 - a22 * a31; + double b10 = a21 * a33 - a23 * a31; + double b11 = a22 * a33 - a23 * a32; + + // Calculate the determinant + double det = b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06; + + if (det == 0) + { + return false; + } + double one = 1; + det = one / det; + + output[0] = (a11 * b11 - a12 * b10 + a13 * b09) * det; + output[1] = (a02 * b10 - a01 * b11 - a03 * b09) * det; + output[2] = (a31 * b05 - a32 * b04 + a33 * b03) * det; + output[3] = (a22 * b04 - a21 * b05 - a23 * b03) * det; + output[4] = (a12 * b08 - a10 * b11 - a13 * b07) * det; + output[5] = (a00 * b11 - a02 * b08 + a03 * b07) * det; + output[6] = (a32 * b02 - a30 * b05 - a33 * b01) * det; + output[7] = (a20 * b05 - a22 * b02 + a23 * b01) * det; + output[8] = (a10 * b10 - a11 * b08 + a13 * b06) * det; + output[9] = (a01 * b08 - a00 * b10 - a03 * b06) * det; + output[10] = (a30 * b04 - a31 * b02 + a33 * b00) * det; + output[11] = (a21 * b02 - a20 * b04 - a23 * b00) * det; + output[12] = (a11 * b07 - a10 * b09 - a12 * b06) * det; + output[13] = (a00 * b09 - a01 * b07 + a02 * b06) * det; + output[14] = (a31 * b01 - a30 * b03 - a32 * b00) * det; + output[15] = (a20 * b03 - a21 * b01 + a22 * b00) * det; + + return true; + } + + /// + /// Calculates the adjugate of a mat4 + /// + /// {mat4} out the receiving matrix + /// {mat4} a the source matrix + public static void Adjoint(Span output, ReadOnlySpan a) + { + double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; double a03 = a[3]; + double a10 = a[4]; double a11 = a[5]; double a12 = a[6]; double a13 = a[7]; + double a20 = a[8]; double a21 = a[9]; double a22 = a[10]; double a23 = a[11]; + double a30 = a[12]; double a31 = a[13]; double a32 = a[14]; double a33 = a[15]; + + output[0] = (a11 * (a22 * a33 - a23 * a32) - a21 * (a12 * a33 - a13 * a32) + a31 * (a12 * a23 - a13 * a22)); + output[1] = -(a01 * (a22 * a33 - a23 * a32) - a21 * (a02 * a33 - a03 * a32) + a31 * (a02 * a23 - a03 * a22)); + output[2] = (a01 * (a12 * a33 - a13 * a32) - a11 * (a02 * a33 - a03 * a32) + a31 * (a02 * a13 - a03 * a12)); + output[3] = -(a01 * (a12 * a23 - a13 * a22) - a11 * (a02 * a23 - a03 * a22) + a21 * (a02 * a13 - a03 * a12)); + output[4] = -(a10 * (a22 * a33 - a23 * a32) - a20 * (a12 * a33 - a13 * a32) + a30 * (a12 * a23 - a13 * a22)); + output[5] = (a00 * (a22 * a33 - a23 * a32) - a20 * (a02 * a33 - a03 * a32) + a30 * (a02 * a23 - a03 * a22)); + output[6] = -(a00 * (a12 * a33 - a13 * a32) - a10 * (a02 * a33 - a03 * a32) + a30 * (a02 * a13 - a03 * a12)); + output[7] = (a00 * (a12 * a23 - a13 * a22) - a10 * (a02 * a23 - a03 * a22) + a20 * (a02 * a13 - a03 * a12)); + output[8] = (a10 * (a21 * a33 - a23 * a31) - a20 * (a11 * a33 - a13 * a31) + a30 * (a11 * a23 - a13 * a21)); + output[9] = -(a00 * (a21 * a33 - a23 * a31) - a20 * (a01 * a33 - a03 * a31) + a30 * (a01 * a23 - a03 * a21)); + output[10] = (a00 * (a11 * a33 - a13 * a31) - a10 * (a01 * a33 - a03 * a31) + a30 * (a01 * a13 - a03 * a11)); + output[11] = -(a00 * (a11 * a23 - a13 * a21) - a10 * (a01 * a23 - a03 * a21) + a20 * (a01 * a13 - a03 * a11)); + output[12] = -(a10 * (a21 * a32 - a22 * a31) - a20 * (a11 * a32 - a12 * a31) + a30 * (a11 * a22 - a12 * a21)); + output[13] = (a00 * (a21 * a32 - a22 * a31) - a20 * (a01 * a32 - a02 * a31) + a30 * (a01 * a22 - a02 * a21)); + output[14] = -(a00 * (a11 * a32 - a12 * a31) - a10 * (a01 * a32 - a02 * a31) + a30 * (a01 * a12 - a02 * a11)); + output[15] = (a00 * (a11 * a22 - a12 * a21) - a10 * (a01 * a22 - a02 * a21) + a20 * (a01 * a12 - a02 * a11)); + } + + /// + /// Calculates the determinant of a mat4 + /// + /// {mat4} a the source matrix + /// {Number} determinant of a + public static double Determinant(ReadOnlySpan a) + { + double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; double a03 = a[3]; + double a10 = a[4]; double a11 = a[5]; double a12 = a[6]; double a13 = a[7]; + double a20 = a[8]; double a21 = a[9]; double a22 = a[10]; double a23 = a[11]; + double a30 = a[12]; double a31 = a[13]; double a32 = a[14]; double a33 = a[15]; + + double b00 = a00 * a11 - a01 * a10; + double b01 = a00 * a12 - a02 * a10; + double b02 = a00 * a13 - a03 * a10; + double b03 = a01 * a12 - a02 * a11; + double b04 = a01 * a13 - a03 * a11; + double b05 = a02 * a13 - a03 * a12; + double b06 = a20 * a31 - a21 * a30; + double b07 = a20 * a32 - a22 * a30; + double b08 = a20 * a33 - a23 * a30; + double b09 = a21 * a32 - a22 * a31; + double b10 = a21 * a33 - a23 * a31; + double b11 = a22 * a33 - a23 * a32; + + // Calculate the determinant + return b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06; + } + + /// + /// Multiplies two mat4's + /// + /// {mat4} out the receiving matrix + /// {mat4} a the first operand + /// {mat4} b the second operand + public static void Multiply(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + double a00 = a[0]; double a01 = a[1]; double a02 = a[2]; double a03 = a[3]; + double a10 = a[4]; double a11 = a[5]; double a12 = a[6]; double a13 = a[7]; + double a20 = a[8]; double a21 = a[9]; double a22 = a[10]; double a23 = a[11]; + double a30 = a[12]; double a31 = a[13]; double a32 = a[14]; double a33 = a[15]; + + // Cache only the current line of the second matrix + double b0 = b[0]; double b1 = b[1]; double b2 = b[2]; double b3 = b[3]; + output[0] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; + output[1] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; + output[2] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; + output[3] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; + + b0 = b[4]; b1 = b[5]; b2 = b[6]; b3 = b[7]; + output[4] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; + output[5] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; + output[6] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; + output[7] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; + + b0 = b[8]; b1 = b[9]; b2 = b[10]; b3 = b[11]; + output[8] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; + output[9] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; + output[10] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; + output[11] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; + + b0 = b[12]; b1 = b[13]; b2 = b[14]; b3 = b[15]; + output[12] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; + output[13] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; + output[14] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; + output[15] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; + } + + /// + /// mat4.multiply + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Mul(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Multiply(output, a, b); + } + + /// + /// Translate a mat4 by the given vector + /// + /// {mat4} out the receiving matrix + /// {mat4} a the matrix to translate + /// {vec3} v vector to translate by + /// + /// + public static void Translate(Span output, ReadOnlySpan input, double x, double y, double z) + { + if (input == output) + { + output[12] = input[0] * x + input[4] * y + input[8] * z + input[12]; + output[13] = input[1] * x + input[5] * y + input[9] * z + input[13]; + output[14] = input[2] * x + input[6] * y + input[10] * z + input[14]; + output[15] = input[3] * x + input[7] * y + input[11] * z + input[15]; + } + else + { + double a00; double a01; double a02; double a03; + double a10; double a11; double a12; double a13; + double a20; double a21; double a22; double a23; + + a00 = input[0]; a01 = input[1]; a02 = input[2]; a03 = input[3]; + a10 = input[4]; a11 = input[5]; a12 = input[6]; a13 = input[7]; + a20 = input[8]; a21 = input[9]; a22 = input[10]; a23 = input[11]; + + output[0] = a00; output[1] = a01; output[2] = a02; output[3] = a03; + output[4] = a10; output[5] = a11; output[6] = a12; output[7] = a13; + output[8] = a20; output[9] = a21; output[10] = a22; output[11] = a23; + + output[12] = a00 * x + a10 * y + a20 * z + input[12]; + output[13] = a01 * x + a11 * y + a21 * z + input[13]; + output[14] = a02 * x + a12 * y + a22 * z + input[14]; + output[15] = a03 * x + a13 * y + a23 * z + input[15]; + } + } + + /// + /// Translate a mat4 by the given vector + /// + /// {mat4} out the receiving matrix + /// {mat4} a the matrix to translate + /// {vec3} v vector to translate by + public static void Translate(Span output, ReadOnlySpan input, ReadOnlySpan translate) + { + double x = translate[0]; double y = translate[1]; double z = translate[2]; + if (input == output) + { + output[12] = input[0] * x + input[4] * y + input[8] * z + input[12]; + output[13] = input[1] * x + input[5] * y + input[9] * z + input[13]; + output[14] = input[2] * x + input[6] * y + input[10] * z + input[14]; + output[15] = input[3] * x + input[7] * y + input[11] * z + input[15]; + } + else + { + double a00; double a01; double a02; double a03; + double a10; double a11; double a12; double a13; + double a20; double a21; double a22; double a23; + + a00 = input[0]; a01 = input[1]; a02 = input[2]; a03 = input[3]; + a10 = input[4]; a11 = input[5]; a12 = input[6]; a13 = input[7]; + a20 = input[8]; a21 = input[9]; a22 = input[10]; a23 = input[11]; + + output[0] = a00; output[1] = a01; output[2] = a02; output[3] = a03; + output[4] = a10; output[5] = a11; output[6] = a12; output[7] = a13; + output[8] = a20; output[9] = a21; output[10] = a22; output[11] = a23; + + output[12] = a00 * x + a10 * y + a20 * z + input[12]; + output[13] = a01 * x + a11 * y + a21 * z + input[13]; + output[14] = a02 * x + a12 * y + a22 * z + input[14]; + output[15] = a03 * x + a13 * y + a23 * z + input[15]; + } + } + + /// + /// Scales the mat4 by the dimensions in the given vec3 + /// + /// {mat4} out the receiving matrix + /// {mat4} a the matrix to scale + /// {vec3} v the vec3 to scale the matrix by + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Scale(Span output, ReadOnlySpan a, ReadOnlySpan v) + { + double x = v[0]; double y = v[1]; double z = v[2]; + + output[0] = a[0] * x; + output[1] = a[1] * x; + output[2] = a[2] * x; + output[3] = a[3] * x; + output[4] = a[4] * y; + output[5] = a[5] * y; + output[6] = a[6] * y; + output[7] = a[7] * y; + output[8] = a[8] * z; + output[9] = a[9] * z; + output[10] = a[10] * z; + output[11] = a[11] * z; + output[12] = a[12]; + output[13] = a[13]; + output[14] = a[14]; + output[15] = a[15]; + } + + /// + /// Scales the mat4 by the dimensions in the given vec3 + /// + /// {mat4} out the receiving matrix + /// {mat4} a the matrix to scale + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Scale(Span output, ReadOnlySpan a, double xScale, double yScale, double zScale) + { + output[0] = a[0] * xScale; + output[1] = a[1] * xScale; + output[2] = a[2] * xScale; + output[3] = a[3] * xScale; + output[4] = a[4] * yScale; + output[5] = a[5] * yScale; + output[6] = a[6] * yScale; + output[7] = a[7] * yScale; + output[8] = a[8] * zScale; + output[9] = a[9] * zScale; + output[10] = a[10] * zScale; + output[11] = a[11] * zScale; + output[12] = a[12]; + output[13] = a[13]; + output[14] = a[14]; + output[15] = a[15]; + } + + /// + /// Rotates a mat4 by the given angle + /// + /// {mat4} out the receiving matrix + /// {mat4} a the matrix to rotate + /// {Number} rad the angle to rotate the matrix by + /// {vec3} axis the axis to rotate around + /// if the operation was successful + public static bool Rotate(Span output, ReadOnlySpan a, double rad, ReadOnlySpan axis) + { + double x = axis[0]; double y = axis[1]; double z = axis[2]; + double len = GameMath.Sqrt(x * x + y * y + z * z); + double s; double c; double t; + double a00; double a01; double a02; double a03; + double a10; double a11; double a12; double a13; + double a20; double a21; double a22; double a23; + double b00; double b01; double b02; + double b10; double b11; double b12; + double b20; double b21; double b22; + + if (Math.Abs(len) < GLMAT_EPSILON) { return false; } + + len = 1 / len; + x *= len; + y *= len; + z *= len; + + s = GameMath.Sin(rad); + c = GameMath.Cos(rad); + t = 1 - c; + + a00 = a[0]; a01 = a[1]; a02 = a[2]; a03 = a[3]; + a10 = a[4]; a11 = a[5]; a12 = a[6]; a13 = a[7]; + a20 = a[8]; a21 = a[9]; a22 = a[10]; a23 = a[11]; + + // Construct the elements of the rotation matrix + b00 = x * x * t + c; b01 = y * x * t + z * s; b02 = z * x * t - y * s; + b10 = x * y * t - z * s; b11 = y * y * t + c; b12 = z * y * t + x * s; + b20 = x * z * t + y * s; b21 = y * z * t - x * s; b22 = z * z * t + c; + + // Perform rotation-specific matrix multiplication + output[0] = a00 * b00 + a10 * b01 + a20 * b02; + output[1] = a01 * b00 + a11 * b01 + a21 * b02; + output[2] = a02 * b00 + a12 * b01 + a22 * b02; + output[3] = a03 * b00 + a13 * b01 + a23 * b02; + output[4] = a00 * b10 + a10 * b11 + a20 * b12; + output[5] = a01 * b10 + a11 * b11 + a21 * b12; + output[6] = a02 * b10 + a12 * b11 + a22 * b12; + output[7] = a03 * b10 + a13 * b11 + a23 * b12; + output[8] = a00 * b20 + a10 * b21 + a20 * b22; + output[9] = a01 * b20 + a11 * b21 + a21 * b22; + output[10] = a02 * b20 + a12 * b21 + a22 * b22; + output[11] = a03 * b20 + a13 * b21 + a23 * b22; + + if (a != output) + { + // If the source and destination differ, copy the unchanged last row + output[12] = a[12]; + output[13] = a[13]; + output[14] = a[14]; + output[15] = a[15]; + } + return true; + } + + /// + /// Rotates a matrix by the given angle around the X axis + /// + /// {mat4} out the receiving matrix + /// {mat4} a the matrix to rotate + /// {Number} rad the angle to rotate the matrix by + public static void RotateX(Span output, ReadOnlySpan a, double rad) + { + double s = GameMath.Sin(rad); + double c = GameMath.Cos(rad); + double a10 = a[4]; + double a11 = a[5]; + double a12 = a[6]; + double a13 = a[7]; + double a20 = a[8]; + double a21 = a[9]; + double a22 = a[10]; + double a23 = a[11]; + + if (a != output) + { + // If the source and destination differ, copy the unchanged rows + output[0] = a[0]; + output[1] = a[1]; + output[2] = a[2]; + output[3] = a[3]; + output[12] = a[12]; + output[13] = a[13]; + output[14] = a[14]; + output[15] = a[15]; + } + + // Perform axis-specific matrix multiplication + output[4] = a10 * c + a20 * s; + output[5] = a11 * c + a21 * s; + output[6] = a12 * c + a22 * s; + output[7] = a13 * c + a23 * s; + output[8] = a20 * c - a10 * s; + output[9] = a21 * c - a11 * s; + output[10] = a22 * c - a12 * s; + output[11] = a23 * c - a13 * s; + } + + /// + /// Rotates a matrix by the given angle around the Y axis + /// + /// {mat4} out the receiving matrix + /// {mat4} a the matrix to rotate + /// {Number} rad the angle to rotate the matrix by + public static void RotateY(Span output, ReadOnlySpan a, double rad) + { + double s = GameMath.Sin(rad); + double c = GameMath.Cos(rad); + double a00 = a[0]; + double a01 = a[1]; + double a02 = a[2]; + double a03 = a[3]; + double a20 = a[8]; + double a21 = a[9]; + double a22 = a[10]; + double a23 = a[11]; + + if (a != output) + { + // If the source and destination differ, copy the unchanged rows + output[4] = a[4]; + output[5] = a[5]; + output[6] = a[6]; + output[7] = a[7]; + output[12] = a[12]; + output[13] = a[13]; + output[14] = a[14]; + output[15] = a[15]; + } + + // Perform axis-specific matrix multiplication + output[0] = a00 * c - a20 * s; + output[1] = a01 * c - a21 * s; + output[2] = a02 * c - a22 * s; + output[3] = a03 * c - a23 * s; + output[8] = a00 * s + a20 * c; + output[9] = a01 * s + a21 * c; + output[10] = a02 * s + a22 * c; + output[11] = a03 * s + a23 * c; + } + + /// + /// Rotates a matrix by the given angle around the Z axis + /// + /// {mat4} out the receiving matrix + /// {mat4} a the matrix to rotate + /// {Number} rad the angle to rotate the matrix by + public static void RotateZ(Span output, ReadOnlySpan a, double rad) + { + double s = GameMath.Sin(rad); + double c = GameMath.Cos(rad); + double a00 = a[0]; + double a01 = a[1]; + double a02 = a[2]; + double a03 = a[3]; + double a10 = a[4]; + double a11 = a[5]; + double a12 = a[6]; + double a13 = a[7]; + + if (a != output) + { + // If the source and destination differ, copy the unchanged last row + output[8] = a[8]; + output[9] = a[9]; + output[10] = a[10]; + output[11] = a[11]; + output[12] = a[12]; + output[13] = a[13]; + output[14] = a[14]; + output[15] = a[15]; + } + + // Perform axis-specific matrix multiplication + output[0] = a00 * c + a10 * s; + output[1] = a01 * c + a11 * s; + output[2] = a02 * c + a12 * s; + output[3] = a03 * c + a13 * s; + output[4] = a10 * c - a00 * s; + output[5] = a11 * c - a01 * s; + output[6] = a12 * c - a02 * s; + output[7] = a13 * c - a03 * s; + } + + /// + /// Creates a matrix from a quaternion rotation and vector translation + /// This is equivalent to (but much faster than): + /// mat4.identity(dest); + /// mat4.translate(dest, vec); + /// var quatMat = mat4.create(); + /// quat4.toMat4(quat, quatMat); + /// mat4.multiply(dest, quatMat); + /// + /// {mat4} out mat4 receiving operation result + /// {quat4} q Rotation quaternion + /// {vec3} v Translation vector + public static void FromRotationTranslation(Span output, ReadOnlySpan q, ReadOnlySpan v) + { + // Quaternion math + double x = q[0]; double y = q[1]; double z = q[2]; double w = q[3]; + double x2 = x + x; + double y2 = y + y; + double z2 = z + z; + + double xx = x * x2; + double xy = x * y2; + double xz = x * z2; + double yy = y * y2; + double yz = y * z2; + double zz = z * z2; + double wx = w * x2; + double wy = w * y2; + double wz = w * z2; + + output[0] = 1 - (yy + zz); + output[1] = xy + wz; + output[2] = xz - wy; + output[3] = 0; + output[4] = xy - wz; + output[5] = 1 - (xx + zz); + output[6] = yz + wx; + output[7] = 0; + output[8] = xz + wy; + output[9] = yz - wx; + output[10] = 1 - (xx + yy); + output[11] = 0; + output[12] = v[0]; + output[13] = v[1]; + output[14] = v[2]; + output[15] = 1; + } + + /// + /// Calculates a 4x4 matrix from the given quaternion + /// + /// {mat4} out mat4 receiving operation result + /// {quat} q Quaternion to create matrix from + public static void FromQuat(Span output, ReadOnlySpan q) + { + double x = q[0]; double y = q[1]; double z = q[2]; double w = q[3]; + double x2 = x + x; + double y2 = y + y; + double z2 = z + z; + + double xx = x * x2; + double xy = x * y2; + double xz = x * z2; + double yy = y * y2; + double yz = y * z2; + double zz = z * z2; + double wx = w * x2; + double wy = w * y2; + double wz = w * z2; + + output[0] = 1 - (yy + zz); + output[1] = xy + wz; + output[2] = xz - wy; + output[3] = 0; + + output[4] = xy - wz; + output[5] = 1 - (xx + zz); + output[6] = yz + wx; + output[7] = 0; + + output[8] = xz + wy; + output[9] = yz - wx; + output[10] = 1 - (xx + yy); + output[11] = 0; + + output[12] = 0; + output[13] = 0; + output[14] = 0; + output[15] = 1; + } + + /// + /// Generates a frustum matrix with the given bounds + /// + /// {mat4} out mat4 frustum matrix will be written into + /// {Number} left Left bound of the frustum + /// {Number} right Right bound of the frustum + /// {Number} bottom Bottom bound of the frustum + /// {Number} top Top bound of the frustum + /// {Number} near Near bound of the frustum + /// {Number} far Far bound of the frustum + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Frustum(Span output, double left, double right, double bottom, double top, double near, double far) + { + double rl = 1 / (right - left); + double tb = 1 / (top - bottom); + double nf = 1 / (near - far); + output[0] = (near * 2.0) * rl; + output[1] = 0; + output[2] = 0; + output[3] = 0; + output[4] = 0; + output[5] = (near * 2.0) * tb; + output[6] = 0; + output[7] = 0; + output[8] = (right + left) * rl; + output[9] = (top + bottom) * tb; + output[10] = (far + near) * nf; + output[11] = -1; + output[12] = 0; + output[13] = 0; + output[14] = (far * near * 2.0) * nf; + output[15] = 0; + } + + /// + /// Generates a perspective projection matrix with the given bounds + /// + /// {mat4} out mat4 frustum matrix will be written into + /// {number} fovy Vertical field of view in radians + /// {number} aspect Aspect ratio. typically viewport width/height + /// {number} near Near bound of the frustum + /// {number} far Far bound of the frustum + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Perspective(Span output, double fovy, double aspect, double near, double far) + { + double one = 1; + double f = one / GameMath.Tan(fovy / 2.0); + double nf = 1 / (near - far); + output[0] = f / aspect; + output[1] = 0; + output[2] = 0; + output[3] = 0; + output[4] = 0; + output[5] = f; + output[6] = 0; + output[7] = 0; + output[8] = 0; + output[9] = 0; + output[10] = (far + near) * nf; + output[11] = -1; + output[12] = 0; + output[13] = 0; + output[14] = (2.0 * far * near) * nf; + output[15] = 0; + } + + /// + /// Generates a orthogonal projection matrix with the given bounds + /// + /// {mat4} out mat4 frustum matrix will be written into + /// {number} left Left bound of the frustum + /// {number} right Right bound of the frustum + /// {number} bottom Bottom bound of the frustum + /// {number} top Top bound of the frustum + /// {number} near Near bound of the frustum + /// {number} far Far bound of the frustum + public static void Ortho(Span output, double left, double right, double bottom, double top, double near, double far) + { + double lr = 1 / (left - right); + double bt = 1 / (bottom - top); + double nf = 1 / (near - far); + output[0] = -2.0 * lr; + output[1] = 0; + output[2] = 0; + output[3] = 0; + output[4] = 0; + output[5] = -2.0 * bt; + output[6] = 0; + output[7] = 0; + output[8] = 0; + output[9] = 0; + output[10] = 2.0 * nf; + output[11] = 0; + output[12] = (left + right) * lr; + output[13] = (top + bottom) * bt; + output[14] = (far + near) * nf; + output[15] = 1; + } + + /// + /// Generates a look-at matrix with the given eye position, focal point, and up axis + /// + /// {mat4} out mat4 frustum matrix will be written into + /// {vec3} eye Position of the viewer + /// {vec3} center Point the viewer is looking at + /// {vec3} up vec3 pointing up + public static void LookAt(Span output, ReadOnlySpan eye, ReadOnlySpan center, ReadOnlySpan up) + { + double x0; double x1; double x2; double y0; double y1; double y2; double z0; double z1; double z2; double len; + double eyex = eye[0]; + double eyey = eye[1]; + double eyez = eye[2]; + double upx = up[0]; + double upy = up[1]; + double upz = up[2]; + double centerx = center[0]; + double centery = center[1]; + double centerz = center[2]; + + if (Math.Abs(eyex - centerx) < GLMAT_EPSILON && + Math.Abs(eyey - centery) < GLMAT_EPSILON && + Math.Abs(eyez - centerz) < GLMAT_EPSILON) + { + Identity(output); + return; + } + + z0 = eyex - centerx; + z1 = eyey - centery; + z2 = eyez - centerz; + + len = 1 / GameMath.Sqrt(z0 * z0 + z1 * z1 + z2 * z2); + z0 *= len; + z1 *= len; + z2 *= len; + + x0 = upy * z2 - upz * z1; + x1 = upz * z0 - upx * z2; + x2 = upx * z1 - upy * z0; + len = GameMath.Sqrt(x0 * x0 + x1 * x1 + x2 * x2); + if (len == 0) + { + x0 = 0; + x1 = 0; + x2 = 0; + } + else + { + len = 1 / len; + x0 *= len; + x1 *= len; + x2 *= len; + } + + y0 = z1 * x2 - z2 * x1; + y1 = z2 * x0 - z0 * x2; + y2 = z0 * x1 - z1 * x0; + + len = GameMath.Sqrt(y0 * y0 + y1 * y1 + y2 * y2); + if (len == 0) + { + y0 = 0; + y1 = 0; + y2 = 0; + } + else + { + len = 1 / len; + y0 *= len; + y1 *= len; + y2 *= len; + } + + output[0] = x0; + output[1] = y0; + output[2] = z0; + output[3] = 0; + output[4] = x1; + output[5] = y1; + output[6] = z1; + output[7] = 0; + output[8] = x2; + output[9] = y2; + output[10] = z2; + output[11] = 0; + output[12] = -(x0 * eyex + x1 * eyey + x2 * eyez); + output[13] = -(y0 * eyex + y1 * eyey + y2 * eyez); + output[14] = -(z0 * eyex + z1 * eyey + z2 * eyez); + output[15] = 1; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void MulVec4(Span output, ReadOnlySpan matrix, ReadOnlySpan vec) + { + double vx = vec[0]; + double vy = vec[1]; + double vz = vec[2]; + double va = vec[3]; + output[0] = matrix[0] * vx + matrix[4] * vy + matrix[8] * vz + matrix[12] * va; + output[1] = matrix[1] * vx + matrix[5] * vy + matrix[9] * vz + matrix[13] * va; + output[2] = matrix[2] * vx + matrix[6] * vy + matrix[10] * vz + matrix[14] * va; + output[3] = matrix[3] * vx + matrix[7] * vy + matrix[11] * vz + matrix[15] * va; + } + + /// + /// Used for vec3 representing a direction or normal - as a vec4 this would have the 4th element set to 0, so that applying a matrix transform with a translation would have *no* effect + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void MulVec3(Span output, ReadOnlySpan matrix, ReadOnlySpan vec) + { + double x = vec[0]; + double y = vec[1]; + double z = vec[2]; + output[0] = matrix[0] * x + matrix[4] * y + matrix[8] * z; + output[1] = matrix[1] * x + matrix[5] * y + matrix[9] * z; + output[2] = matrix[2] * x + matrix[6] * y + matrix[10] * z; + } + + /// + /// Used for vec3 representing an x,y,z position - as a vec4 this would have the 4th element set to 1, so that applying a matrix transform with a translation would have an effect + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void MulVec3_Position(Span output, ReadOnlySpan matrix, ReadOnlySpan vec) + { + double x = vec[0]; + double y = vec[1]; + double z = vec[2]; + output[0] = matrix[0] * x + matrix[4] * y + matrix[8] * z + matrix[12]; + output[1] = matrix[1] * x + matrix[5] * y + matrix[9] * z + matrix[13]; + output[2] = matrix[2] * x + matrix[6] * y + matrix[10] * z + matrix[14]; + } + + /// + /// Used for vec3 representing an x,y,z position - as a vec4 this would have the 4th element set to 1, so that applying a matrix transform with a translation would have an effect + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void MulVec3_Position(Span output, ReadOnlySpan matrix, double x, double y, double z) + { + output[0] = matrix[0] * x + matrix[4] * y + matrix[8] * z + matrix[12]; + output[1] = matrix[1] * x + matrix[5] * y + matrix[9] * z + matrix[13]; + output[2] = matrix[2] * x + matrix[6] * y + matrix[10] * z + matrix[14]; + } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void MulVec3_Position_AndScale(Span output, ReadOnlySpan matrix, ReadOnlySpan vec, double scaleFactor) + { + double x = (vec[0] - 0.5) * scaleFactor + 0.5; + double y = vec[1] * scaleFactor; + double z = (vec[2] - 0.5) * scaleFactor + 0.5; + output[0] = matrix[0] * x + matrix[4] * y + matrix[8] * z + matrix[12]; + output[1] = matrix[1] * x + matrix[5] * y + matrix[9] * z + matrix[13]; + output[2] = matrix[2] * x + matrix[6] * y + matrix[10] * z + matrix[14]; + } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void MulVec3_Position_AndScaleXY(Span output, ReadOnlySpan matrix, ReadOnlySpan vec, double scaleFactor) + { + double x = (vec[0] - 0.5) * scaleFactor + 0.5; + double y = vec[1]; + double z = (vec[2] - 0.5) * scaleFactor + 0.5; + output[0] = matrix[0] * x + matrix[4] * y + matrix[8] * z + matrix[12]; + output[1] = matrix[1] * x + matrix[5] * y + matrix[9] * z + matrix[13]; + output[2] = matrix[2] * x + matrix[6] * y + matrix[10] * z + matrix[14]; + } + + /// + /// Used for vec3 representing an x,y,z position - as a vec4 this would have the 4th element set to 1, so that applying a matrix transform with a translation would have an effect + /// The offset is used to index within the original and output arrays - e.g. in MeshData.xyz + /// The origin is the origin for the rotation + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void MulVec3_Position_WithOrigin(Span output, ReadOnlySpan matrix, ReadOnlySpan vec, ReadOnlySpan origin) + { + double vx = vec[0] - origin[0]; + double vy = vec[1] - origin[1]; + double vz = vec[2] - origin[2]; + output[0] = origin[0] + matrix[0] * vx + matrix[4] * vy + matrix[8] * vz + matrix[12]; + output[1] = origin[1] + matrix[1] * vx + matrix[5] * vy + matrix[9] * vz + matrix[13]; + output[2] = origin[2] + matrix[2] * vx + matrix[6] * vy + matrix[10] * vz + matrix[14]; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ExtractEulerAngles(ReadOnlySpan m, ref double thetaX, ref double thetaY, ref double thetaZ) + { + double sinY = m[8]; + if (Math.Abs(sinY) == 0) + { + thetaX = sinY * Math.Atan2(m[1], m[5]); + thetaY = sinY * GameMath.PIHALF; + thetaZ = 0; + } + else + { + thetaX = Math.Atan2(-m[9], m[10]); + thetaY = GameMath.Asin(sinY); + thetaZ = Math.Atan2(-m[4], m[0]); + } + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ExtractEulerAngles(Span output, ReadOnlySpan m) + { + double sinY = m[8]; + if (Math.Abs(sinY) == 1) + { + output[0] = sinY * Math.Atan2(m[1], m[5]); + output[1] = sinY * GameMath.PIHALF; + output[2] = 0; + } + else + { + output[0] = Math.Atan2(-m[9], m[10]); + output[1] = GameMath.Asin(sinY); + output[2] = Math.Atan2(-m[4], m[0]); + } + } + } +} diff --git a/Math/Matrix/SpanMat4f.cs b/Math/Matrix/SpanMat4f.cs new file mode 100644 index 000000000..96a67421c --- /dev/null +++ b/Math/Matrix/SpanMat4f.cs @@ -0,0 +1,1040 @@ +//glMatrix license: +//Copyright (c) 2013, Brandon Jones, Colin MacKenzie IV. All rights reserved. + +//Redistribution and use in source and binary forms, with or without modification, +//are permitted provided that the following conditions are met: + +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. + +//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +//ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +//WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +//DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +//ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +//ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +//(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +//SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +using System; +using System.Runtime.CompilerServices; + +namespace Vintagestory.API.MathTools +{ + /// + /// 4x4 Matrix Math + /// + public partial class Mat4f + { + private const float GLMAT_EPSILON = 1.0f / 1000000.0f; + + /// + /// Set a mat4 to the identity matrix + /// + /// {mat4} out the receiving matrix + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Identity(Span output) + { + output[0] = 1; + output[1] = 0; + output[2] = 0; + output[3] = 0; + output[4] = 0; + output[5] = 1; + output[6] = 0; + output[7] = 0; + output[8] = 0; + output[9] = 0; + output[10] = 1; + output[11] = 0; + output[12] = 0; + output[13] = 0; + output[14] = 0; + output[15] = 1; + } + + + /// + /// Set a mat4 to the identity matrix with a scale applied + /// + /// {mat4} out the receiving matrix + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Identity_Scaled(Span output, float scale) + { + output[0] = scale; + output[1] = 0; + output[2] = 0; + output[3] = 0; + output[4] = 0; + output[5] = scale; + output[6] = 0; + output[7] = 0; + output[8] = 0; + output[9] = 0; + output[10] = scale; + output[11] = 0; + output[12] = 0; + output[13] = 0; + output[14] = 0; + output[15] = 1; + } + + + /// + /// Transpose the values of a mat4 + /// + /// {mat4} out the receiving matrix + /// {mat4} a the source matrix + public static void Transpose(Span output, ReadOnlySpan a) + { + // If we are transposing ourselves we can skip a few steps but have to cache some values + if (output == a) + { + float a01 = a[1]; float a02 = a[2]; float a03 = a[3]; + float a12 = a[6]; float a13 = a[7]; + float a23 = a[11]; + + output[1] = a[4]; + output[2] = a[8]; + output[3] = a[12]; + output[4] = a01; + output[6] = a[9]; + output[7] = a[13]; + output[8] = a02; + output[9] = a12; + output[11] = a[14]; + output[12] = a03; + output[13] = a13; + output[14] = a23; + } + else + { + output[0] = a[0]; + output[1] = a[4]; + output[2] = a[8]; + output[3] = a[12]; + output[4] = a[1]; + output[5] = a[5]; + output[6] = a[9]; + output[7] = a[13]; + output[8] = a[2]; + output[9] = a[6]; + output[10] = a[10]; + output[11] = a[14]; + output[12] = a[3]; + output[13] = a[7]; + output[14] = a[11]; + output[15] = a[15]; + } + } + + /// + /// Inverts a mat4 + /// + /// {mat4} out the receiving matrix + /// {mat4} a the source matrix + /// if the operation was successful + public static bool Invert(Span output, ReadOnlySpan a) + { + float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; float a03 = a[3]; + float a10 = a[4]; float a11 = a[5]; float a12 = a[6]; float a13 = a[7]; + float a20 = a[8]; float a21 = a[9]; float a22 = a[10]; float a23 = a[11]; + float a30 = a[12]; float a31 = a[13]; float a32 = a[14]; float a33 = a[15]; + + float b00 = a00 * a11 - a01 * a10; + float b01 = a00 * a12 - a02 * a10; + float b02 = a00 * a13 - a03 * a10; + float b03 = a01 * a12 - a02 * a11; + float b04 = a01 * a13 - a03 * a11; + float b05 = a02 * a13 - a03 * a12; + float b06 = a20 * a31 - a21 * a30; + float b07 = a20 * a32 - a22 * a30; + float b08 = a20 * a33 - a23 * a30; + float b09 = a21 * a32 - a22 * a31; + float b10 = a21 * a33 - a23 * a31; + float b11 = a22 * a33 - a23 * a32; + + // Calculate the determinant + float det = b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06; + + if (det == 0) + { + return false; + } + float one = 1; + det = one / det; + + output[0] = (a11 * b11 - a12 * b10 + a13 * b09) * det; + output[1] = (a02 * b10 - a01 * b11 - a03 * b09) * det; + output[2] = (a31 * b05 - a32 * b04 + a33 * b03) * det; + output[3] = (a22 * b04 - a21 * b05 - a23 * b03) * det; + output[4] = (a12 * b08 - a10 * b11 - a13 * b07) * det; + output[5] = (a00 * b11 - a02 * b08 + a03 * b07) * det; + output[6] = (a32 * b02 - a30 * b05 - a33 * b01) * det; + output[7] = (a20 * b05 - a22 * b02 + a23 * b01) * det; + output[8] = (a10 * b10 - a11 * b08 + a13 * b06) * det; + output[9] = (a01 * b08 - a00 * b10 - a03 * b06) * det; + output[10] = (a30 * b04 - a31 * b02 + a33 * b00) * det; + output[11] = (a21 * b02 - a20 * b04 - a23 * b00) * det; + output[12] = (a11 * b07 - a10 * b09 - a12 * b06) * det; + output[13] = (a00 * b09 - a01 * b07 + a02 * b06) * det; + output[14] = (a31 * b01 - a30 * b03 - a32 * b00) * det; + output[15] = (a20 * b03 - a21 * b01 + a22 * b00) * det; + + return true; + } + + /// + /// Calculates the adjugate of a mat4 + /// + /// {mat4} out the receiving matrix + /// {mat4} a the source matrix + public static void Adjoint(Span output, ReadOnlySpan a) + { + float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; float a03 = a[3]; + float a10 = a[4]; float a11 = a[5]; float a12 = a[6]; float a13 = a[7]; + float a20 = a[8]; float a21 = a[9]; float a22 = a[10]; float a23 = a[11]; + float a30 = a[12]; float a31 = a[13]; float a32 = a[14]; float a33 = a[15]; + + output[0] = (a11 * (a22 * a33 - a23 * a32) - a21 * (a12 * a33 - a13 * a32) + a31 * (a12 * a23 - a13 * a22)); + output[1] = -(a01 * (a22 * a33 - a23 * a32) - a21 * (a02 * a33 - a03 * a32) + a31 * (a02 * a23 - a03 * a22)); + output[2] = (a01 * (a12 * a33 - a13 * a32) - a11 * (a02 * a33 - a03 * a32) + a31 * (a02 * a13 - a03 * a12)); + output[3] = -(a01 * (a12 * a23 - a13 * a22) - a11 * (a02 * a23 - a03 * a22) + a21 * (a02 * a13 - a03 * a12)); + output[4] = -(a10 * (a22 * a33 - a23 * a32) - a20 * (a12 * a33 - a13 * a32) + a30 * (a12 * a23 - a13 * a22)); + output[5] = (a00 * (a22 * a33 - a23 * a32) - a20 * (a02 * a33 - a03 * a32) + a30 * (a02 * a23 - a03 * a22)); + output[6] = -(a00 * (a12 * a33 - a13 * a32) - a10 * (a02 * a33 - a03 * a32) + a30 * (a02 * a13 - a03 * a12)); + output[7] = (a00 * (a12 * a23 - a13 * a22) - a10 * (a02 * a23 - a03 * a22) + a20 * (a02 * a13 - a03 * a12)); + output[8] = (a10 * (a21 * a33 - a23 * a31) - a20 * (a11 * a33 - a13 * a31) + a30 * (a11 * a23 - a13 * a21)); + output[9] = -(a00 * (a21 * a33 - a23 * a31) - a20 * (a01 * a33 - a03 * a31) + a30 * (a01 * a23 - a03 * a21)); + output[10] = (a00 * (a11 * a33 - a13 * a31) - a10 * (a01 * a33 - a03 * a31) + a30 * (a01 * a13 - a03 * a11)); + output[11] = -(a00 * (a11 * a23 - a13 * a21) - a10 * (a01 * a23 - a03 * a21) + a20 * (a01 * a13 - a03 * a11)); + output[12] = -(a10 * (a21 * a32 - a22 * a31) - a20 * (a11 * a32 - a12 * a31) + a30 * (a11 * a22 - a12 * a21)); + output[13] = (a00 * (a21 * a32 - a22 * a31) - a20 * (a01 * a32 - a02 * a31) + a30 * (a01 * a22 - a02 * a21)); + output[14] = -(a00 * (a11 * a32 - a12 * a31) - a10 * (a01 * a32 - a02 * a31) + a30 * (a01 * a12 - a02 * a11)); + output[15] = (a00 * (a11 * a22 - a12 * a21) - a10 * (a01 * a22 - a02 * a21) + a20 * (a01 * a12 - a02 * a11)); + } + + /// + /// Calculates the determinant of a mat4 + /// + /// {mat4} a the source matrix + /// {Number} determinant of a + public static float Determinant(ReadOnlySpan a) + { + float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; float a03 = a[3]; + float a10 = a[4]; float a11 = a[5]; float a12 = a[6]; float a13 = a[7]; + float a20 = a[8]; float a21 = a[9]; float a22 = a[10]; float a23 = a[11]; + float a30 = a[12]; float a31 = a[13]; float a32 = a[14]; float a33 = a[15]; + + float b00 = a00 * a11 - a01 * a10; + float b01 = a00 * a12 - a02 * a10; + float b02 = a00 * a13 - a03 * a10; + float b03 = a01 * a12 - a02 * a11; + float b04 = a01 * a13 - a03 * a11; + float b05 = a02 * a13 - a03 * a12; + float b06 = a20 * a31 - a21 * a30; + float b07 = a20 * a32 - a22 * a30; + float b08 = a20 * a33 - a23 * a30; + float b09 = a21 * a32 - a22 * a31; + float b10 = a21 * a33 - a23 * a31; + float b11 = a22 * a33 - a23 * a32; + + // Calculate the determinant + return b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06; + } + + /// + /// Multiplies two mat4's + /// + /// {mat4} out the receiving matrix + /// {mat4} a the first operand + /// {mat4} b the second operand + public static void Multiply(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; float a03 = a[3]; + float a10 = a[4]; float a11 = a[5]; float a12 = a[6]; float a13 = a[7]; + float a20 = a[8]; float a21 = a[9]; float a22 = a[10]; float a23 = a[11]; + float a30 = a[12]; float a31 = a[13]; float a32 = a[14]; float a33 = a[15]; + + // Cache only the current line of the second matrix + float b0 = b[0]; float b1 = b[1]; float b2 = b[2]; float b3 = b[3]; + output[0] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; + output[1] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; + output[2] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; + output[3] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; + + b0 = b[4]; b1 = b[5]; b2 = b[6]; b3 = b[7]; + output[4] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; + output[5] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; + output[6] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; + output[7] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; + + b0 = b[8]; b1 = b[9]; b2 = b[10]; b3 = b[11]; + output[8] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; + output[9] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; + output[10] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; + output[11] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; + + b0 = b[12]; b1 = b[13]; b2 = b[14]; b3 = b[15]; + output[12] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; + output[13] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; + output[14] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; + output[15] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; + } + + /// + /// mat4.multiply + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Mul(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Multiply(output, a, b); + } + + /// + /// Translate a mat4 by the given vector + /// + /// {mat4} out the receiving matrix + /// {mat4} a the matrix to translate + /// {vec3} v vector to translate by + /// + /// + public static void Translate(Span output, ReadOnlySpan input, float x, float y, float z) + { + if (input == output) + { + output[12] = input[0] * x + input[4] * y + input[8] * z + input[12]; + output[13] = input[1] * x + input[5] * y + input[9] * z + input[13]; + output[14] = input[2] * x + input[6] * y + input[10] * z + input[14]; + output[15] = input[3] * x + input[7] * y + input[11] * z + input[15]; + } + else + { + float a00; float a01; float a02; float a03; + float a10; float a11; float a12; float a13; + float a20; float a21; float a22; float a23; + + a00 = input[0]; a01 = input[1]; a02 = input[2]; a03 = input[3]; + a10 = input[4]; a11 = input[5]; a12 = input[6]; a13 = input[7]; + a20 = input[8]; a21 = input[9]; a22 = input[10]; a23 = input[11]; + + output[0] = a00; output[1] = a01; output[2] = a02; output[3] = a03; + output[4] = a10; output[5] = a11; output[6] = a12; output[7] = a13; + output[8] = a20; output[9] = a21; output[10] = a22; output[11] = a23; + + output[12] = a00 * x + a10 * y + a20 * z + input[12]; + output[13] = a01 * x + a11 * y + a21 * z + input[13]; + output[14] = a02 * x + a12 * y + a22 * z + input[14]; + output[15] = a03 * x + a13 * y + a23 * z + input[15]; + } + } + + /// + /// Translate a mat4 by the given vector + /// + /// {mat4} out the receiving matrix + /// {mat4} a the matrix to translate + /// {vec3} v vector to translate by + public static void Translate(Span output, ReadOnlySpan input, ReadOnlySpan translate) + { + float x = translate[0]; float y = translate[1]; float z = translate[2]; + if (input == output) + { + output[12] = input[0] * x + input[4] * y + input[8] * z + input[12]; + output[13] = input[1] * x + input[5] * y + input[9] * z + input[13]; + output[14] = input[2] * x + input[6] * y + input[10] * z + input[14]; + output[15] = input[3] * x + input[7] * y + input[11] * z + input[15]; + } + else + { + float a00; float a01; float a02; float a03; + float a10; float a11; float a12; float a13; + float a20; float a21; float a22; float a23; + + a00 = input[0]; a01 = input[1]; a02 = input[2]; a03 = input[3]; + a10 = input[4]; a11 = input[5]; a12 = input[6]; a13 = input[7]; + a20 = input[8]; a21 = input[9]; a22 = input[10]; a23 = input[11]; + + output[0] = a00; output[1] = a01; output[2] = a02; output[3] = a03; + output[4] = a10; output[5] = a11; output[6] = a12; output[7] = a13; + output[8] = a20; output[9] = a21; output[10] = a22; output[11] = a23; + + output[12] = a00 * x + a10 * y + a20 * z + input[12]; + output[13] = a01 * x + a11 * y + a21 * z + input[13]; + output[14] = a02 * x + a12 * y + a22 * z + input[14]; + output[15] = a03 * x + a13 * y + a23 * z + input[15]; + } + } + + /// + /// Scales the mat4 by the dimensions in the given vec3 + /// + /// {mat4} out the receiving matrix + /// {mat4} a the matrix to scale + /// {vec3} v the vec3 to scale the matrix by + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Scale(Span output, ReadOnlySpan a, ReadOnlySpan v) + { + float x = v[0]; float y = v[1]; float z = v[2]; + + output[0] = a[0] * x; + output[1] = a[1] * x; + output[2] = a[2] * x; + output[3] = a[3] * x; + output[4] = a[4] * y; + output[5] = a[5] * y; + output[6] = a[6] * y; + output[7] = a[7] * y; + output[8] = a[8] * z; + output[9] = a[9] * z; + output[10] = a[10] * z; + output[11] = a[11] * z; + output[12] = a[12]; + output[13] = a[13]; + output[14] = a[14]; + output[15] = a[15]; + } + + /// + /// Scales the mat4 by the dimensions in the given vec3 + /// + /// {mat4} out the receiving matrix + /// {mat4} a the matrix to scale + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Scale(Span output, ReadOnlySpan a, float xScale, float yScale, float zScale) + { + output[0] = a[0] * xScale; + output[1] = a[1] * xScale; + output[2] = a[2] * xScale; + output[3] = a[3] * xScale; + output[4] = a[4] * yScale; + output[5] = a[5] * yScale; + output[6] = a[6] * yScale; + output[7] = a[7] * yScale; + output[8] = a[8] * zScale; + output[9] = a[9] * zScale; + output[10] = a[10] * zScale; + output[11] = a[11] * zScale; + output[12] = a[12]; + output[13] = a[13]; + output[14] = a[14]; + output[15] = a[15]; + } + + /// + /// Rotates a mat4 by the given angle + /// + /// {mat4} out the receiving matrix + /// {mat4} a the matrix to rotate + /// {Number} rad the angle to rotate the matrix by + /// {vec3} axis the axis to rotate around + /// if the operation was successful + public static bool Rotate(Span output, ReadOnlySpan a, float rad, ReadOnlySpan axis) + { + float x = axis[0]; float y = axis[1]; float z = axis[2]; + float len = GameMath.Sqrt(x * x + y * y + z * z); + float s; float c; float t; + float a00; float a01; float a02; float a03; + float a10; float a11; float a12; float a13; + float a20; float a21; float a22; float a23; + float b00; float b01; float b02; + float b10; float b11; float b12; + float b20; float b21; float b22; + + if (Math.Abs(len) < GLMAT_EPSILON) { return false; } + + len = 1 / len; + x *= len; + y *= len; + z *= len; + + s = GameMath.Sin(rad); + c = GameMath.Cos(rad); + t = 1 - c; + + a00 = a[0]; a01 = a[1]; a02 = a[2]; a03 = a[3]; + a10 = a[4]; a11 = a[5]; a12 = a[6]; a13 = a[7]; + a20 = a[8]; a21 = a[9]; a22 = a[10]; a23 = a[11]; + + // Construct the elements of the rotation matrix + b00 = x * x * t + c; b01 = y * x * t + z * s; b02 = z * x * t - y * s; + b10 = x * y * t - z * s; b11 = y * y * t + c; b12 = z * y * t + x * s; + b20 = x * z * t + y * s; b21 = y * z * t - x * s; b22 = z * z * t + c; + + // Perform rotation-specific matrix multiplication + output[0] = a00 * b00 + a10 * b01 + a20 * b02; + output[1] = a01 * b00 + a11 * b01 + a21 * b02; + output[2] = a02 * b00 + a12 * b01 + a22 * b02; + output[3] = a03 * b00 + a13 * b01 + a23 * b02; + output[4] = a00 * b10 + a10 * b11 + a20 * b12; + output[5] = a01 * b10 + a11 * b11 + a21 * b12; + output[6] = a02 * b10 + a12 * b11 + a22 * b12; + output[7] = a03 * b10 + a13 * b11 + a23 * b12; + output[8] = a00 * b20 + a10 * b21 + a20 * b22; + output[9] = a01 * b20 + a11 * b21 + a21 * b22; + output[10] = a02 * b20 + a12 * b21 + a22 * b22; + output[11] = a03 * b20 + a13 * b21 + a23 * b22; + + if (a != output) + { + // If the source and destination differ, copy the unchanged last row + output[12] = a[12]; + output[13] = a[13]; + output[14] = a[14]; + output[15] = a[15]; + } + return true; + } + + /// + /// Rotates a matrix by the given angle around the X axis + /// + /// {mat4} out the receiving matrix + /// {mat4} a the matrix to rotate + /// {Number} rad the angle to rotate the matrix by + public static void RotateX(Span output, ReadOnlySpan a, float rad) + { + float s = GameMath.Sin(rad); + float c = GameMath.Cos(rad); + float a10 = a[4]; + float a11 = a[5]; + float a12 = a[6]; + float a13 = a[7]; + float a20 = a[8]; + float a21 = a[9]; + float a22 = a[10]; + float a23 = a[11]; + + if (a != output) + { + // If the source and destination differ, copy the unchanged rows + output[0] = a[0]; + output[1] = a[1]; + output[2] = a[2]; + output[3] = a[3]; + output[12] = a[12]; + output[13] = a[13]; + output[14] = a[14]; + output[15] = a[15]; + } + + // Perform axis-specific matrix multiplication + output[4] = a10 * c + a20 * s; + output[5] = a11 * c + a21 * s; + output[6] = a12 * c + a22 * s; + output[7] = a13 * c + a23 * s; + output[8] = a20 * c - a10 * s; + output[9] = a21 * c - a11 * s; + output[10] = a22 * c - a12 * s; + output[11] = a23 * c - a13 * s; + } + + /// + /// Rotates a matrix by the given angle around the Y axis + /// + /// {mat4} out the receiving matrix + /// {mat4} a the matrix to rotate + /// {Number} rad the angle to rotate the matrix by + public static void RotateY(Span output, ReadOnlySpan a, float rad) + { + float s = GameMath.Sin(rad); + float c = GameMath.Cos(rad); + float a00 = a[0]; + float a01 = a[1]; + float a02 = a[2]; + float a03 = a[3]; + float a20 = a[8]; + float a21 = a[9]; + float a22 = a[10]; + float a23 = a[11]; + + if (a != output) + { + // If the source and destination differ, copy the unchanged rows + output[4] = a[4]; + output[5] = a[5]; + output[6] = a[6]; + output[7] = a[7]; + output[12] = a[12]; + output[13] = a[13]; + output[14] = a[14]; + output[15] = a[15]; + } + + // Perform axis-specific matrix multiplication + output[0] = a00 * c - a20 * s; + output[1] = a01 * c - a21 * s; + output[2] = a02 * c - a22 * s; + output[3] = a03 * c - a23 * s; + output[8] = a00 * s + a20 * c; + output[9] = a01 * s + a21 * c; + output[10] = a02 * s + a22 * c; + output[11] = a03 * s + a23 * c; + } + + /// + /// Rotates a matrix by the given angle around the Z axis + /// + /// {mat4} out the receiving matrix + /// {mat4} a the matrix to rotate + /// {Number} rad the angle to rotate the matrix by + public static void RotateZ(Span output, ReadOnlySpan a, float rad) + { + float s = GameMath.Sin(rad); + float c = GameMath.Cos(rad); + float a00 = a[0]; + float a01 = a[1]; + float a02 = a[2]; + float a03 = a[3]; + float a10 = a[4]; + float a11 = a[5]; + float a12 = a[6]; + float a13 = a[7]; + + if (a != output) + { + // If the source and destination differ, copy the unchanged last row + output[8] = a[8]; + output[9] = a[9]; + output[10] = a[10]; + output[11] = a[11]; + output[12] = a[12]; + output[13] = a[13]; + output[14] = a[14]; + output[15] = a[15]; + } + + // Perform axis-specific matrix multiplication + output[0] = a00 * c + a10 * s; + output[1] = a01 * c + a11 * s; + output[2] = a02 * c + a12 * s; + output[3] = a03 * c + a13 * s; + output[4] = a10 * c - a00 * s; + output[5] = a11 * c - a01 * s; + output[6] = a12 * c - a02 * s; + output[7] = a13 * c - a03 * s; + } + + /// + /// Creates a matrix from a quaternion rotation and vector translation + /// This is equivalent to (but much faster than): + /// mat4.identity(dest); + /// mat4.translate(dest, vec); + /// var quatMat = mat4.create(); + /// quat4.toMat4(quat, quatMat); + /// mat4.multiply(dest, quatMat); + /// + /// {mat4} out mat4 receiving operation result + /// {quat4} q Rotation quaternion + /// {vec3} v Translation vector + public static void FromRotationTranslation(Span output, ReadOnlySpan q, ReadOnlySpan v) + { + // Quaternion math + float x = q[0]; float y = q[1]; float z = q[2]; float w = q[3]; + float x2 = x + x; + float y2 = y + y; + float z2 = z + z; + + float xx = x * x2; + float xy = x * y2; + float xz = x * z2; + float yy = y * y2; + float yz = y * z2; + float zz = z * z2; + float wx = w * x2; + float wy = w * y2; + float wz = w * z2; + + output[0] = 1 - (yy + zz); + output[1] = xy + wz; + output[2] = xz - wy; + output[3] = 0; + output[4] = xy - wz; + output[5] = 1 - (xx + zz); + output[6] = yz + wx; + output[7] = 0; + output[8] = xz + wy; + output[9] = yz - wx; + output[10] = 1 - (xx + yy); + output[11] = 0; + output[12] = v[0]; + output[13] = v[1]; + output[14] = v[2]; + output[15] = 1; + } + + /// + /// Calculates a 4x4 matrix from the given quaternion + /// + /// {mat4} out mat4 receiving operation result + /// {quat} q Quaternion to create matrix from + public static void FromQuat(Span output, ReadOnlySpan q) + { + float x = q[0]; float y = q[1]; float z = q[2]; float w = q[3]; + float x2 = x + x; + float y2 = y + y; + float z2 = z + z; + + float xx = x * x2; + float xy = x * y2; + float xz = x * z2; + float yy = y * y2; + float yz = y * z2; + float zz = z * z2; + float wx = w * x2; + float wy = w * y2; + float wz = w * z2; + + output[0] = 1 - (yy + zz); + output[1] = xy + wz; + output[2] = xz - wy; + output[3] = 0; + + output[4] = xy - wz; + output[5] = 1 - (xx + zz); + output[6] = yz + wx; + output[7] = 0; + + output[8] = xz + wy; + output[9] = yz - wx; + output[10] = 1 - (xx + yy); + output[11] = 0; + + output[12] = 0; + output[13] = 0; + output[14] = 0; + output[15] = 1; + } + + /// + /// Generates a frustum matrix with the given bounds + /// + /// {mat4} out mat4 frustum matrix will be written into + /// {Number} left Left bound of the frustum + /// {Number} right Right bound of the frustum + /// {Number} bottom Bottom bound of the frustum + /// {Number} top Top bound of the frustum + /// {Number} near Near bound of the frustum + /// {Number} far Far bound of the frustum + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Frustum(Span output, float left, float right, float bottom, float top, float near, float far) + { + float rl = 1 / (right - left); + float tb = 1 / (top - bottom); + float nf = 1 / (near - far); + output[0] = (near * 2f) * rl; + output[1] = 0; + output[2] = 0; + output[3] = 0; + output[4] = 0; + output[5] = (near * 2f) * tb; + output[6] = 0; + output[7] = 0; + output[8] = (right + left) * rl; + output[9] = (top + bottom) * tb; + output[10] = (far + near) * nf; + output[11] = -1; + output[12] = 0; + output[13] = 0; + output[14] = (far * near * 2f) * nf; + output[15] = 0; + } + + /// + /// Generates a perspective projection matrix with the given bounds + /// + /// {mat4} out mat4 frustum matrix will be written into + /// {number} fovy Vertical field of view in radians + /// {number} aspect Aspect ratio. typically viewport width/height + /// {number} near Near bound of the frustum + /// {number} far Far bound of the frustum + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Perspective(Span output, float fovy, float aspect, float near, float far) + { + float one = 1; + float f = one / GameMath.Tan(fovy / 2f); + float nf = 1 / (near - far); + output[0] = f / aspect; + output[1] = 0; + output[2] = 0; + output[3] = 0; + output[4] = 0; + output[5] = f; + output[6] = 0; + output[7] = 0; + output[8] = 0; + output[9] = 0; + output[10] = (far + near) * nf; + output[11] = -1; + output[12] = 0; + output[13] = 0; + output[14] = (2f * far * near) * nf; + output[15] = 0; + } + + /// + /// Generates a orthogonal projection matrix with the given bounds + /// + /// {mat4} out mat4 frustum matrix will be written into + /// {number} left Left bound of the frustum + /// {number} right Right bound of the frustum + /// {number} bottom Bottom bound of the frustum + /// {number} top Top bound of the frustum + /// {number} near Near bound of the frustum + /// {number} far Far bound of the frustum + public static void Ortho(Span output, float left, float right, float bottom, float top, float near, float far) + { + float lr = 1 / (left - right); + float bt = 1 / (bottom - top); + float nf = 1 / (near - far); + output[0] = -2f * lr; + output[1] = 0; + output[2] = 0; + output[3] = 0; + output[4] = 0; + output[5] = -2f * bt; + output[6] = 0; + output[7] = 0; + output[8] = 0; + output[9] = 0; + output[10] = 2f * nf; + output[11] = 0; + output[12] = (left + right) * lr; + output[13] = (top + bottom) * bt; + output[14] = (far + near) * nf; + output[15] = 1; + } + + /// + /// Generates a look-at matrix with the given eye position, focal point, and up axis + /// + /// {mat4} out mat4 frustum matrix will be written into + /// {vec3} eye Position of the viewer + /// {vec3} center Point the viewer is looking at + /// {vec3} up vec3 pointing up + public static void LookAt(Span output, ReadOnlySpan eye, ReadOnlySpan center, ReadOnlySpan up) + { + float x0; float x1; float x2; float y0; float y1; float y2; float z0; float z1; float z2; float len; + float eyex = eye[0]; + float eyey = eye[1]; + float eyez = eye[2]; + float upx = up[0]; + float upy = up[1]; + float upz = up[2]; + float centerx = center[0]; + float centery = center[1]; + float centerz = center[2]; + + if (Math.Abs(eyex - centerx) < GLMAT_EPSILON && + Math.Abs(eyey - centery) < GLMAT_EPSILON && + Math.Abs(eyez - centerz) < GLMAT_EPSILON) + { + Identity(output); + return; + } + + z0 = eyex - centerx; + z1 = eyey - centery; + z2 = eyez - centerz; + + len = 1 / GameMath.Sqrt(z0 * z0 + z1 * z1 + z2 * z2); + z0 *= len; + z1 *= len; + z2 *= len; + + x0 = upy * z2 - upz * z1; + x1 = upz * z0 - upx * z2; + x2 = upx * z1 - upy * z0; + len = GameMath.Sqrt(x0 * x0 + x1 * x1 + x2 * x2); + if (len == 0) + { + x0 = 0; + x1 = 0; + x2 = 0; + } + else + { + len = 1 / len; + x0 *= len; + x1 *= len; + x2 *= len; + } + + y0 = z1 * x2 - z2 * x1; + y1 = z2 * x0 - z0 * x2; + y2 = z0 * x1 - z1 * x0; + + len = GameMath.Sqrt(y0 * y0 + y1 * y1 + y2 * y2); + if (len == 0) + { + y0 = 0; + y1 = 0; + y2 = 0; + } + else + { + len = 1 / len; + y0 *= len; + y1 *= len; + y2 *= len; + } + + output[0] = x0; + output[1] = y0; + output[2] = z0; + output[3] = 0; + output[4] = x1; + output[5] = y1; + output[6] = z1; + output[7] = 0; + output[8] = x2; + output[9] = y2; + output[10] = z2; + output[11] = 0; + output[12] = -(x0 * eyex + x1 * eyey + x2 * eyez); + output[13] = -(y0 * eyex + y1 * eyey + y2 * eyez); + output[14] = -(z0 * eyex + z1 * eyey + z2 * eyez); + output[15] = 1; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void MulVec4(Span output, ReadOnlySpan matrix, ReadOnlySpan vec) + { + float vx = vec[0]; + float vy = vec[1]; + float vz = vec[2]; + float va = vec[3]; + output[0] = matrix[0] * vx + matrix[4] * vy + matrix[8] * vz + matrix[12] * va; + output[1] = matrix[1] * vx + matrix[5] * vy + matrix[9] * vz + matrix[13] * va; + output[2] = matrix[2] * vx + matrix[6] * vy + matrix[10] * vz + matrix[14] * va; + output[3] = matrix[3] * vx + matrix[7] * vy + matrix[11] * vz + matrix[15] * va; + } + + /// + /// Used for vec3 representing a direction or normal - as a vec4 this would have the 4th element set to 0, so that applying a matrix transform with a translation would have *no* effect + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void MulVec3(Span output, ReadOnlySpan matrix, ReadOnlySpan vec) + { + float x = vec[0]; + float y = vec[1]; + float z = vec[2]; + output[0] = matrix[0] * x + matrix[4] * y + matrix[8] * z; + output[1] = matrix[1] * x + matrix[5] * y + matrix[9] * z; + output[2] = matrix[2] * x + matrix[6] * y + matrix[10] * z; + } + + /// + /// Used for vec3 representing an x,y,z position - as a vec4 this would have the 4th element set to 1, so that applying a matrix transform with a translation would have an effect + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void MulVec3_Position(Span output, ReadOnlySpan matrix, ReadOnlySpan vec) + { + float x = vec[0]; + float y = vec[1]; + float z = vec[2]; + output[0] = matrix[0] * x + matrix[4] * y + matrix[8] * z + matrix[12]; + output[1] = matrix[1] * x + matrix[5] * y + matrix[9] * z + matrix[13]; + output[2] = matrix[2] * x + matrix[6] * y + matrix[10] * z + matrix[14]; + } + + /// + /// Used for vec3 representing an x,y,z position - as a vec4 this would have the 4th element set to 1, so that applying a matrix transform with a translation would have an effect + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void MulVec3_Position(Span output, ReadOnlySpan matrix, float x, float y, float z) + { + output[0] = matrix[0] * x + matrix[4] * y + matrix[8] * z + matrix[12]; + output[1] = matrix[1] * x + matrix[5] * y + matrix[9] * z + matrix[13]; + output[2] = matrix[2] * x + matrix[6] * y + matrix[10] * z + matrix[14]; + } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void MulVec3_Position_AndScale(Span output, ReadOnlySpan matrix, ReadOnlySpan vec, float scaleFactor) + { + float x = (vec[0] - 0.5f) * scaleFactor + 0.5f; + float y = vec[1] * scaleFactor; + float z = (vec[2] - 0.5f) * scaleFactor + 0.5f; + output[0] = matrix[0] * x + matrix[4] * y + matrix[8] * z + matrix[12]; + output[1] = matrix[1] * x + matrix[5] * y + matrix[9] * z + matrix[13]; + output[2] = matrix[2] * x + matrix[6] * y + matrix[10] * z + matrix[14]; + } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void MulVec3_Position_AndScaleXY(Span output, ReadOnlySpan matrix, ReadOnlySpan vec, float scaleFactor) + { + float x = (vec[0] - 0.5f) * scaleFactor + 0.5f; + float y = vec[1]; + float z = (vec[2] - 0.5f) * scaleFactor + 0.5f; + output[0] = matrix[0] * x + matrix[4] * y + matrix[8] * z + matrix[12]; + output[1] = matrix[1] * x + matrix[5] * y + matrix[9] * z + matrix[13]; + output[2] = matrix[2] * x + matrix[6] * y + matrix[10] * z + matrix[14]; + } + + /// + /// Used for vec3 representing an x,y,z position - as a vec4 this would have the 4th element set to 1, so that applying a matrix transform with a translation would have an effect + /// The offset is used to index within the original and output arrays - e.g. in MeshData.xyz + /// The origin is the origin for the rotation + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void MulVec3_Position_WithOrigin(Span output, ReadOnlySpan matrix, ReadOnlySpan vec, ReadOnlySpan origin) + { + float vx = vec[0] - origin[0]; + float vy = vec[1] - origin[1]; + float vz = vec[2] - origin[2]; + output[0] = origin[0] + matrix[0] * vx + matrix[4] * vy + matrix[8] * vz + matrix[12]; + output[1] = origin[1] + matrix[1] * vx + matrix[5] * vy + matrix[9] * vz + matrix[13]; + output[2] = origin[2] + matrix[2] * vx + matrix[6] * vy + matrix[10] * vz + matrix[14]; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ExtractEulerAngles(ReadOnlySpan m, ref float thetaX, ref float thetaY, ref float thetaZ) + { + float sinY = m[8]; + if (Math.Abs(sinY) == 0) + { + thetaX = sinY * (float)Math.Atan2(m[1], m[5]); + thetaY = sinY * GameMath.PIHALF; + thetaZ = 0; + } + else + { + thetaX = (float)Math.Atan2(-m[9], m[10]); + thetaY = GameMath.Asin(sinY); + thetaZ = (float)Math.Atan2(-m[4], m[0]); + } + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ExtractEulerAngles(Span output, ReadOnlySpan m) + { + float sinY = m[8]; + if (Math.Abs(sinY) == 1) + { + output[0] = sinY * (float)Math.Atan2(m[1], m[5]); + output[1] = sinY * GameMath.PIHALF; + output[2] = 0; + } + else + { + output[0] = (float)Math.Atan2(-m[9], m[10]); + output[1] = GameMath.Asin(sinY); + output[2] = (float)Math.Atan2(-m[4], m[0]); + } + } + } +} diff --git a/Math/Matrix/SpanMatrixTools.cs b/Math/Matrix/SpanMatrixTools.cs new file mode 100644 index 000000000..742d67f87 --- /dev/null +++ b/Math/Matrix/SpanMatrixTools.cs @@ -0,0 +1,102 @@ +using System; +using System.Runtime.CompilerServices; +using Vintagestory.API.MathTools; + +namespace Vintagestory.API.Client +{ + public partial class MatrixToolsd + { + public static void Project(Span output, ReadOnlySpan pos, + ReadOnlySpan projection, ReadOnlySpan view, + int viewportWidth, int viewportHeight) + { + Span outmat = stackalloc double[16]; + Mat4d.Mul(outmat, projection, view); + + ReadOnlySpan pos4 = stackalloc double[4] { pos[0], pos[1], pos[2], 1 }; + Span outpos = stackalloc double[4]; + Mat4d.MulVec4(outpos, outmat, pos4); + + output[0] = (outpos[0] / outpos[3] + 1) * (viewportWidth / 2); + output[1] = (outpos[1] / outpos[3] + 1) * (viewportHeight / 2); + output[2] = outpos[2]; + } + + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void MatFollowPlayer(Span m) + { + m[12] = 0; + m[13] = 0; + m[14] = 0; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void LoadPlayerFacingMatrix(Span m) + { + // http://stackoverflow.com/a/5487981 + // | d 0 0 T.x | + // | 0 d 0 T.y | + // | 0 0 d T.z | + // | 0 0 0 1 | + double d = GameMath.Sqrt(m[0] * m[0] + m[1] * m[1] + m[2] * m[2]); + + m[0] = d; + m[1] = 0; + m[2] = 0; + m[3] = 0; + + m[4] = 0; + m[5] = d; + m[6] = 0; + m[7] = 0; + + m[8] = 0; + m[9] = 0; + m[10] = d; + m[11] = 0; + + m[12] = m[12]; + m[13] = m[13]; + m[14] = m[14]; + m[15] = 1; + + Mat4d.RotateX(m, m, GameMath.PI); + + // game.GlLoadMatrix(m); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void MatFacePlayer(Span m) + { + // http://stackoverflow.com/a/5487981 + // | d 0 0 T.x | + // | 0 d 0 T.y | + // | 0 0 d T.z | + // | 0 0 0 1 | + double d = GameMath.Sqrt(m[0] * m[0] + m[1] * m[1] + m[2] * m[2]); + + m[0] = d; + m[1] = 0; + m[2] = 0; + m[3] = 0; + + m[4] = 0; + m[5] = d; + m[6] = 0; + m[7] = 0; + + m[8] = 0; + m[9] = 0; + m[10] = d; + m[11] = 0; + + m[12] = m[12]; + m[13] = m[13]; + m[14] = m[14]; + m[15] = 1; + + Mat4d.RotateX(m, m, GameMath.PI); + } + } +} diff --git a/Math/Matrix/SpanQuaterniond.cs b/Math/Matrix/SpanQuaterniond.cs new file mode 100644 index 000000000..fe82083b9 --- /dev/null +++ b/Math/Matrix/SpanQuaterniond.cs @@ -0,0 +1,1243 @@ +//glMatrix license: +//Copyright (c) 2013, Brandon Jones, Colin MacKenzie IV. All rights reserved. + +//Redistribution and use in source and binary forms, with or without modification, +//are permitted provided that the following conditions are met: + +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. + +//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +//ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +//WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +//DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +//ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +//ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +//(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +//SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +using System; +using System.Runtime.CompilerServices; + +namespace Vintagestory.API.MathTools +{ + public partial class Quaterniond + { + /// + /// Sets a quaternion to represent the shortest rotation from one + /// vector to another. + /// + /// Both vectors are assumed to be unit length. + /// + /// the receiving quaternion. + /// the initial vector + /// the destination vector + public static void RotationTo(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Span tmpvec3 = stackalloc double[3]; + Span xUnitVec3 = stackalloc double[3] { 1.0, 0.0, 0.0 }; + Span yUnitVec3 = stackalloc double[3] { 0.0, 1.0, 0.0 }; + + double dot = Vec3Utilsd.Dot(a, b); + + double nines = 999999.0 / 1000000.0; // 0.999999 + + double epsilon = 1.0 / 1000000.0; // 0.000001 + + if (dot < -nines) + { + Vec3Utilsd.Cross(tmpvec3, xUnitVec3, a); + if (Vec3Utilsd.Length(tmpvec3) < epsilon) + Vec3Utilsd.Cross(tmpvec3, yUnitVec3, a); + Vec3Utilsd.Normalize(tmpvec3, tmpvec3); + SetAxisAngle(output, tmpvec3, GameMath.PI); + return; + } + else if (dot > nines) + { + output[0] = 0.0; + output[1] = 0.0; + output[2] = 0.0; + output[3] = 1.0; + return; + } + else + { + Vec3Utilsd.Cross(tmpvec3, a, b); + output[0] = tmpvec3[0]; + output[1] = tmpvec3[1]; + output[2] = tmpvec3[2]; + output[3] = 1.0 + dot; + Normalize(output, output); + } + } + + /// + /// Sets the specified quaternion with values corresponding to the given + /// axes. Each axis is a vec3 and is expected to be unit length and + /// perpendicular to all other specified axes. + /// + /// the vector representing the viewing direction + /// the vector representing the local "right" direction + /// the vector representing the local "up" direction + public static void SetAxes(Span output, ReadOnlySpan view, ReadOnlySpan right, ReadOnlySpan up) + { + Span matr = stackalloc double[9]; + + matr[0] = right[0]; + matr[3] = right[1]; + matr[6] = right[2]; + + matr[1] = up[0]; + matr[4] = up[1]; + matr[7] = up[2]; + + matr[2] = view[0]; + matr[5] = view[1]; + matr[8] = view[2]; + FromMat3(output, matr); + Normalize(output, output); + } + + /// + /// Set the components of a quat to the given values + /// + /// the receiving quaternion + /// X component + /// Y component + /// Z component + /// W component + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Set(Span output, double x, double y, double z, double w) + { + QVec4d.Set(output, x, y, z, w); + } + + /// + /// Set a quat to the identity quaternion + /// + /// the receiving quaternion + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Identity(Span output) + { + output[0] = 0.0; + output[1] = 0.0; + output[2] = 0.0; + output[3] = 1.0; + } + + /// + /// Sets a quat from the given angle and rotation axis, + /// then returns it. + /// + /// the receiving quaternion + /// the axis around which to rotate + /// the angle in radians + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void SetAxisAngle(Span output, ReadOnlySpan axis, double rad) + { + rad /= 2.0; + double s = GameMath.Sin(rad); + output[0] = s * axis[0]; + output[1] = s * axis[1]; + output[2] = s * axis[2]; + output[3] = GameMath.Cos(rad); + } + + /// + /// Adds two quat's + /// + /// the receiving quaternion + /// the first operand + /// the second operand + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Add(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + QVec4d.Add(output, a, b); + } + + /// + /// Multiplies two quat's + /// + /// the receiving quaternion + /// the first operand + /// the second operand + public static void Multiply(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + double ax = a[0]; double ay = a[1]; double az = a[2]; double aw = a[3]; + double bx = b[0]; double by = b[1]; double bz = b[2]; double bw = b[3]; + + output[0] = ax * bw + aw * bx + ay * bz - az * by; + output[1] = ay * bw + aw * by + az * bx - ax * bz; + output[2] = az * bw + aw * bz + ax * by - ay * bx; + output[3] = aw * bw - ax * bx - ay * by - az * bz; + } + + /// + /// Alias for + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Mul(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Multiply(output, a, b); + } + + /// + /// Scales a quat by a scalar number + /// + /// the receiving vector + /// the vector to scale + /// amount to scale the vector by + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Scale(Span output, ReadOnlySpan a, double b) + { + QVec4d.Scale(output, a, b); + } + + /// + /// Rotates a quaternion by the given angle aboutput the X axis + /// + /// quat receiving operation result + /// quat to rotate + /// angle (in radians) to rotate + public static void RotateX(Span output, ReadOnlySpan a, double rad) + { + rad /= 2.0; + + double ax = a[0]; double ay = a[1]; double az = a[2]; double aw = a[3]; + double bx = GameMath.Sin(rad); double bw = GameMath.Cos(rad); + + output[0] = ax * bw + aw * bx; + output[1] = ay * bw + az * bx; + output[2] = az * bw - ay * bx; + output[3] = aw * bw - ax * bx; + } + + /// + /// Rotates a quaternion by the given angle aboutput the Y axis + /// + /// quat receiving operation result + /// quat to rotate + /// angle (in radians) to rotate + public static void RotateY(Span output, ReadOnlySpan a, double rad) + { + rad /= 2.0; + + double ax = a[0]; double ay = a[1]; double az = a[2]; double aw = a[3]; + double by = GameMath.Sin(rad); double bw = GameMath.Cos(rad); + + output[0] = ax * bw - az * by; + output[1] = ay * bw + aw * by; + output[2] = az * bw + ax * by; + output[3] = aw * bw - ay * by; + } + + /// + /// Rotates a quaternion by the given angle aboutput the Z axis + /// + /// quat receiving operation result + /// quat to rotate + /// angle (in radians) to rotate + public static void RotateZ(Span output, ReadOnlySpan a, double rad) + { + rad /= 2.0; + + double ax = a[0]; double ay = a[1]; double az = a[2]; double aw = a[3]; + double bz = GameMath.Sin(rad); double bw = GameMath.Cos(rad); + + output[0] = ax * bw + ay * bz; + output[1] = ay * bw - ax * bz; + output[2] = az * bw + aw * bz; + output[3] = aw * bw - az * bz; + } + + /// + /// Calculates the W component of a quat from the X, Y, and Z components. + /// Assumes that quaternion is 1 unit in length. + /// Any existing W component will be ignored. + /// + /// the receiving quaternion + /// quat to calculate W component of + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void CalculateW(Span output, ReadOnlySpan a) + { + double x = a[0]; double y = a[1]; double z = a[2]; + + output[0] = x; + output[1] = y; + output[2] = z; + output[3] = -GameMath.Sqrt(Math.Abs(1.0 - x * x - y * y - z * z)); + } + + /// + /// Calculates the dot product of two quat's + /// + /// the first operand + /// the second operand + /// dot product of a and b + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double Dot(ReadOnlySpan a, ReadOnlySpan b) + { + return QVec4d.Dot(a, b); + } + + public static void ToEulerAngles(Span output, ReadOnlySpan quat) + { + // roll (x-axis rotation) + double sinr_cosp = 2.0 * (quat[3] * quat[0] + quat[1] * quat[2]); + double cosr_cosp = 1.0 - 2.0 * (quat[0] * quat[0] + quat[1] * quat[1]); + output[2] = Math.Atan2(sinr_cosp, cosr_cosp); + + // pitch (y-axis rotation) + double sinp = 2.0 * (quat[3] * quat[1] - quat[2] * quat[0]); + if (Math.Abs(sinp) >= 1.0) + output[1] = Math.PI / 2 * Math.Sign(sinp); // use 90 degrees if out of range + else + output[1] = Math.Asin(sinp); + + // yaw (z-axis rotation) + double siny_cosp = 2.0 * (quat[3] * quat[2] + quat[0] * quat[1]); + double cosy_cosp = 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2]); + output[0] = Math.Atan2(siny_cosp, cosy_cosp); + } + + /// + /// Performs a linear interpolation between two quat's + /// + /// the receiving quaternion + /// the first operand + /// the second operand + /// interpolation amount between the two inputs + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Lerp(Span output, ReadOnlySpan a, ReadOnlySpan b, double t) + { + QVec4d.Lerp(output, a, b, t); + } + + /// + /// Performs a spherical linear interpolation between two quat + /// + /// the receiving quaternion + /// the first operand + /// the second operand + /// interpolation amount between the two inputs + public static void Slerp(Span output, ReadOnlySpan a, ReadOnlySpan b, double t) + { + // // benchmarks: + // // http://jsperf.com/quaternion-slerp-implementations + + double ax = a[0]; double ay = a[1]; double az = a[2]; double aw = a[3]; + double bx = b[0]; double by = b[1]; double bz = b[2]; double bw = b[3]; + + double omega; double cosom; double sinom; double scale0; double scale1; + + // calc cosine + cosom = ax * bx + ay * by + az * bz + aw * bw; + // adjust signs (if necessary) + if (cosom < 0.0) + { + cosom = -cosom; + bx = -bx; + by = -by; + bz = -bz; + bw = -bw; + } + double one = 1.0; + double epsilon = one / 1000000; + // calculate coefficients + if ((one - cosom) > epsilon) + { + // standard case (slerp) + omega = GameMath.Acos(cosom); + sinom = GameMath.Sin(omega); + scale0 = GameMath.Sin((one - t) * omega) / sinom; + scale1 = GameMath.Sin(t * omega) / sinom; + } + else + { + // "from" and "to" quaternions are very close + // ... so we can do a linear interpolation + scale0 = one - t; + scale1 = t; + } + // calculate final values + output[0] = scale0 * ax + scale1 * bx; + output[1] = scale0 * ay + scale1 * by; + output[2] = scale0 * az + scale1 * bz; + output[3] = scale0 * aw + scale1 * bw; + } + + /// + /// Calculates the inverse of a quat + /// + /// the receiving quaternion + /// quat to calculate inverse of + public static void Invert(Span output, ReadOnlySpan a) + { + double a0 = a[0]; double a1 = a[1]; double a2 = a[2]; double a3 = a[3]; + double dot = a0 * a0 + a1 * a1 + a2 * a2 + a3 * a3; + double one = 1.0; + double invDot = (dot != 0.0) ? one / dot : 0.0; + + // TODO: Would be faster to return [0,0,0,0] immediately if dot == 0 + + output[0] = -a0 * invDot; + output[1] = -a1 * invDot; + output[2] = -a2 * invDot; + output[3] = a3 * invDot; + } + + /// + /// Calculates the conjugate of a quat + /// If the quaternion is normalized, this function is faster than quat.inverse and produces the same result. + /// + /// the receiving quaternion + /// quat to calculate conjugate of + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Conjugate(Span output, ReadOnlySpan a) + { + output[0] = -a[0]; + output[1] = -a[1]; + output[2] = -a[2]; + output[3] = a[3]; + } + + /// + /// Calculates the length of a quat + /// + /// vector to calculate length of + /// length of a + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double Length(ReadOnlySpan a) + { + return QVec4d.Length(a); + } + + /// + /// Alias for + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double Len(ReadOnlySpan a) + { + return Length(a); + } + + /// + /// Calculates the squared length of a quat + /// + /// vector to calculate squared length of + /// squared length of a + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double SquaredLength(ReadOnlySpan a) + { + return QVec4d.SquaredLength(a); + } + + /// + /// Alias for + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double SqrLen(ReadOnlySpan a) + { + return SquaredLength(a); + } + + /// + /// Normalize a quat + /// + /// the receiving quaternion + /// quaternion to normalize + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Normalize(Span output, ReadOnlySpan a) + { + QVec4d.Normalize(output, a); + } + + /// + /// Creates a quaternion from the given 3x3 rotation matrix. + /// + /// NOTE: The resultant quaternion is not normalized, so you should be sure + /// to renormalize the quaternion yourself where necessary. + /// + /// the receiving quaternion + /// rotation matrix + public static void FromMat3(Span output, ReadOnlySpan m) + { + // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes + // article "Quaternion Calculus and Fast Animation". + double fTrace = m[0] + m[4] + m[8]; + double fRoot; + + double zero = 0; + double one = 1; + double half = one / 2; + if (fTrace > zero) + { + // |w| > 1/2, may as well choose w > 1/2 + fRoot = GameMath.Sqrt(fTrace + one); // 2w + output[3] = half * fRoot; + fRoot = half / fRoot; // 1/(4w) + output[0] = (m[7] - m[5]) * fRoot; + output[1] = (m[2] - m[6]) * fRoot; + output[2] = (m[3] - m[1]) * fRoot; + } + else + { + // |w| <= 1/2 + int i = 0; + if (m[4] > m[0]) + i = 1; + if (m[8] > m[i * 3 + i]) + i = 2; + int j = (i + 1) % 3; + int k = (i + 2) % 3; + + fRoot = GameMath.Sqrt(m[i * 3 + i] - m[j * 3 + j] - m[k * 3 + k] + one); + output[i] = half * fRoot; + fRoot = half / fRoot; + output[3] = (m[k * 3 + j] - m[j * 3 + k]) * fRoot; + output[j] = (m[j * 3 + i] + m[i * 3 + j]) * fRoot; + output[k] = (m[k * 3 + i] + m[i * 3 + k]) * fRoot; + } + } + } + + partial class QVec4d + { + /// + /// Set the components of a QVec4d to the given values + /// + /// the receiving vector + /// X component + /// Y component + /// Z component + /// W component + public static void Set(Span output, double x, double y, double z, double w) + { + output[0] = x; + output[1] = y; + output[2] = z; + output[3] = w; + } + + /// + /// Adds two QVec4d's + /// + /// the receiving vector + /// the first operand + /// the second operand + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Add(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = a[0] + b[0]; + output[1] = a[1] + b[1]; + output[2] = a[2] + b[2]; + output[3] = a[3] + b[3]; + } + + /// + /// Subtracts vector b from vector a + /// + /// the receiving vector + /// the first operand + /// the second operand + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Subtract(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = a[0] - b[0]; + output[1] = a[1] - b[1]; + output[2] = a[2] - b[2]; + output[3] = a[3] - b[3]; + } + + /// + /// Alias for + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Sub(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Subtract(output, a, b); + } + + /// + /// Multiplies two QVec4d's + /// + /// the receiving vector + /// the first operand + /// the second operand + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Multiply(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = a[0] * b[0]; + output[1] = a[1] * b[1]; + output[2] = a[2] * b[2]; + output[3] = a[3] * b[3]; + } + + /// + /// Alias for + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Mul(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Multiply(output, a, b); + } + + /// + /// Divides two QVec4d's + /// + /// the receiving vector + /// the first operand + /// the second operand + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Divide(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = a[0] / b[0]; + output[1] = a[1] / b[1]; + output[2] = a[2] / b[2]; + output[3] = a[3] / b[3]; + } + + /// + /// Alias for + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Div(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Divide(output, a, b); + } + + /// + /// Returns the minimum of two QVec4d's + /// + /// the receiving vector + /// the first operand + /// the second operand + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Min(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = Math.Min(a[0], b[0]); + output[1] = Math.Min(a[1], b[1]); + output[2] = Math.Min(a[2], b[2]); + output[3] = Math.Min(a[3], b[3]); + } + + /// + /// Returns the maximum of two QVec4d's + /// + /// the receiving vector + /// the first operand + /// the second operand + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Max(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = Math.Max(a[0], b[0]); + output[1] = Math.Max(a[1], b[1]); + output[2] = Math.Max(a[2], b[2]); + output[3] = Math.Max(a[3], b[3]); + } + + /// + /// Scales a QVec4d by a scalar number + /// + /// the receiving vector + /// the vector to scale + /// amount to scale the vector by + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Scale(Span output, ReadOnlySpan a, double b) + { + output[0] = a[0] * b; + output[1] = a[1] * b; + output[2] = a[2] * b; + output[3] = a[3] * b; + } + + /// + /// Adds two QVec4d's after scaling the second operand by a scalar value + /// + /// the receiving vector + /// the first operand + /// the second operand + /// the amount to scale b by before adding + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ScaleAndAdd(Span output, ReadOnlySpan a, ReadOnlySpan b, double scale) + { + output[0] = a[0] + (b[0] * scale); + output[1] = a[1] + (b[1] * scale); + output[2] = a[2] + (b[2] * scale); + output[3] = a[3] + (b[3] * scale); + } + + /// + /// Calculates the euclidian distance between two QVec4d's + /// + /// the first operand + /// the second operand + /// distance between a and b + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double Distance(ReadOnlySpan a, ReadOnlySpan b) + { + double x = b[0] - a[0]; + double y = b[1] - a[1]; + double z = b[2] - a[2]; + double w = b[3] - a[3]; + return GameMath.Sqrt(x * x + y * y + z * z + w * w); + } + + /// + /// Alias for + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double Dist(ReadOnlySpan a, ReadOnlySpan b) + { + return Distance(a, b); + } + + /// + /// Calculates the squared euclidian distance between two QVec4d's + /// + /// the first operand + /// the second operand + /// squared distance between a and b + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double SquaredDistance(ReadOnlySpan a, ReadOnlySpan b) + { + double x = b[0] - a[0]; + double y = b[1] - a[1]; + double z = b[2] - a[2]; + double w = b[3] - a[3]; + return x * x + y * y + z * z + w * w; + } + + /// + /// Alias for + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double SqrDist(ReadOnlySpan a, ReadOnlySpan b) + { + return SquaredDistance(a, b); + } + /// + /// Calculates the length of a QVec4d + /// + /// vector to calculate length of + /// length of a + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double Length(ReadOnlySpan a) + { + double x = a[0]; + double y = a[1]; + double z = a[2]; + double w = a[3]; + return GameMath.Sqrt(x * x + y * y + z * z + w * w); + } + + /// + /// Alias for + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double Len(ReadOnlySpan a) + { + return Length(a); + } + + /// + /// Calculates the squared length of a QVec4d + /// + /// vector to calculate squared length of + /// squared length of a + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double SquaredLength(ReadOnlySpan a) + { + double x = a[0]; + double y = a[1]; + double z = a[2]; + double w = a[3]; + return x * x + y * y + z * z + w * w; + } + + /// + /// Alias for + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double SqrLen(ReadOnlySpan a) + { + return SquaredLength(a); + } + + /// + /// Negates the components of a QVec4d + /// + /// the receiving vector + /// vector to negate + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Negate(Span output, ReadOnlySpan a) + { + output[0] = -a[0]; + output[1] = -a[1]; + output[2] = -a[2]; + output[3] = -a[3]; + } + + /// + /// Normalize a QVec4d + /// + /// the receiving vector + /// vector to normalize + public static void Normalize(Span output, ReadOnlySpan a) + { + double x = a[0]; + double y = a[1]; + double z = a[2]; + double w = a[3]; + double len = x * x + y * y + z * z + w * w; + if (len > 0.0) + { + double one = 1.0; + len = one / GameMath.Sqrt(len); + output[0] = x * len; + output[1] = y * len; + output[2] = z * len; + output[3] = w * len; + } + } + + /// + /// Calculates the dot product of two QVec4d's + /// + /// the first operand + /// the second operand + /// dot product of a and b + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double Dot(ReadOnlySpan a, ReadOnlySpan b) + { + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]; + } + + /// + /// Performs a linear interpolation between two QVec4d's + /// + /// the receiving vector + /// the first operand + /// the second operand + /// interpolation amount between the two inputs + public static void Lerp(Span output, ReadOnlySpan a, ReadOnlySpan b, double t) + { + double ax = a[0]; + double ay = a[1]; + double az = a[2]; + double aw = a[3]; + output[0] = ax + t * (b[0] - ax); + output[1] = ay + t * (b[1] - ay); + output[2] = az + t * (b[2] - az); + output[3] = aw + t * (b[3] - aw); + } + + + /// + /// Transforms the QVec4d with a mat4. + /// + /// the receiving vector + /// the vector to transform + /// matrix to transform with + public static void TransformMat4(Span output, ReadOnlySpan a, ReadOnlySpan m) + { + double x = a[0]; double y = a[1]; double z = a[2]; double w = a[3]; + output[0] = m[0] * x + m[4] * y + m[8] * z + m[12] * w; + output[1] = m[1] * x + m[5] * y + m[9] * z + m[13] * w; + output[2] = m[2] * x + m[6] * y + m[10] * z + m[14] * w; + output[3] = m[3] * x + m[7] * y + m[11] * z + m[15] * w; + } + + /// + /// Transforms the QVec4d with a quat + /// + /// the receiving vector + /// the vector to transform + /// quaternion to transform with + public static void transformQuat(Span output, ReadOnlySpan a, ReadOnlySpan q) + { + double x = a[0]; double y = a[1]; double z = a[2]; + double qx = q[0]; double qy = q[1]; double qz = q[2]; double qw = q[3]; + + // calculate quat * vec + double ix = qw * x + qy * z - qz * y; + double iy = qw * y + qz * x - qx * z; + double iz = qw * z + qx * y - qy * x; + double iw = -qx * x - qy * y - qz * z; + + // calculate result * inverse quat + output[0] = ix * qw + iw * -qx + iy * -qz - iz * -qy; + output[1] = iy * qw + iw * -qy + iz * -qx - ix * -qz; + output[2] = iz * qw + iw * -qz + ix * -qy - iy * -qx; + } + } + + /// + /// Don't use this class unless you need it to interoperate with Mat4d + /// + public partial class Vec3Utilsd + { + /// + /// Set the components of a vec3 to the given values + /// + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Set(Span output, double x, double y, double z) + { + output[0] = x; + output[1] = y; + output[2] = z; + } + + /// + /// Adds two vec3's + /// + /// the receiving vector + /// the first operand + /// the second operand + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Add(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = a[0] + b[0]; + output[1] = a[1] + b[1]; + output[2] = a[2] + b[2]; + } + + /// + /// Subtracts vector b from vector a + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Substract(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = a[0] - b[0]; + output[1] = a[1] - b[1]; + output[2] = a[2] - b[2]; + } + + + /// + /// Multiplies two vec3's + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Multiply(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = a[0] * b[0]; + output[1] = a[1] * b[1]; + output[2] = a[2] * b[2]; + } + + /// + /// Alias for + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Mul(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Multiply(output, a, b); + } + + /// + /// Divides two vec3's + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Divide(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = a[0] / b[0]; + output[1] = a[1] / b[1]; + output[2] = a[2] / b[2]; + } + + /// + /// Returns the minimum of two vec3's + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Min(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = Math.Min(a[0], b[0]); + output[1] = Math.Min(a[1], b[1]); + output[2] = Math.Min(a[2], b[2]); + } + + /// + /// Returns the maximum of two vec3's + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Max(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = Math.Max(a[0], b[0]); + output[1] = Math.Max(a[1], b[1]); + output[2] = Math.Max(a[2], b[2]); + } + + /// + /// Scales a vec3 by a scalar number + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Scale(Span output, ReadOnlySpan a, double b) + { + output[0] = a[0] * b; + output[1] = a[1] * b; + output[2] = a[2] * b; + } + + /// + /// Adds two vec3's after scaling the second operand by a scalar value + /// + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ScaleAndAdd(Span output, ReadOnlySpan a, ReadOnlySpan b, double scale) + { + output[0] = a[0] + (b[0] * scale); + output[1] = a[1] + (b[1] * scale); + output[2] = a[2] + (b[2] * scale); + } + + /// + /// Calculates the euclidian distance between two vec3's. Returns {Number} distance between a and b + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double Distance(ReadOnlySpan a, ReadOnlySpan b) + { + double x = b[0] - a[0]; + double y = b[1] - a[1]; + double z = b[2] - a[2]; + return GameMath.Sqrt(x * x + y * y + z * z); + } + + /// + /// Calculates the squared euclidian distance between two vec3's. Returns {Number} squared distance between a and b + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double SquaredDistance(ReadOnlySpan a, ReadOnlySpan b) + { + double x = b[0] - a[0]; + double y = b[1] - a[1]; + double z = b[2] - a[2]; + return x * x + y * y + z * z; + } + + /// + /// Calculates the length of a vec3 + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double Length(ReadOnlySpan a) + { + double x = a[0]; + double y = a[1]; + double z = a[2]; + return GameMath.Sqrt(x * x + y * y + z * z); + } + + /// + /// Calculates the squared length of a vec3. Returns {Number} squared length of a + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double SquaredLength(ReadOnlySpan a) + { + double x = a[0]; + double y = a[1]; + double z = a[2]; + return x * x + y * y + z * z; + } + + + + /// + /// Negates the components of a vec3 + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Negate(Span output, ReadOnlySpan a) + { + output[0] = -a[0]; + output[1] = -a[1]; + output[2] = -a[2]; + } + + /// + /// Normalize a vec3 + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Normalize(Span output, ReadOnlySpan a) + { + double x = a[0]; + double y = a[1]; + double z = a[2]; + double len = x * x + y * y + z * z; + if (len > 0.0) + { + double one = 1.0; + len = one / GameMath.Sqrt(len); + output[0] = a[0] * len; + output[1] = a[1] * len; + output[2] = a[2] * len; + } + } + + /// + /// Calculates the dot product of two vec3's. Returns {Number} dot product of a and b + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double Dot(ReadOnlySpan a, ReadOnlySpan b) + { + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; + } + + /// + /// Computes the cross product of two vec3's. Returns {vec3} out + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Cross(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + double ax = a[0]; + double ay = a[1]; + double az = a[2]; + double bx = b[0]; + double by = b[1]; + double bz = b[2]; + + output[0] = ay * bz - az * by; + output[1] = az * bx - ax * bz; + output[2] = ax * by - ay * bx; + } + + /// + /// Performs a linear interpolation between two vec3's + /// + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Lerp(Span output, ReadOnlySpan a, ReadOnlySpan b, double t) + { + double ax = a[0]; + double ay = a[1]; + double az = a[2]; + output[0] = ax + t * (b[0] - ax); + output[1] = ay + t * (b[1] - ay); + output[2] = az + t * (b[2] - az); + } + + + /// + /// Transforms the vec3 with a mat4. 4th vector component is implicitly '1'. Returns {vec3} out + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void TransformMat4(Span output, ReadOnlySpan a, ReadOnlySpan m) + { + double x = a[0]; + double y = a[1]; + double z = a[2]; + output[0] = m[0] * x + m[4] * y + m[8] * z + m[12]; + output[1] = m[1] * x + m[5] * y + m[9] * z + m[13]; + output[2] = m[2] * x + m[6] * y + m[10] * z + m[14]; + } + + /// + /// Transforms the vec3 with a mat3. Returns {vec3} out + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void TransformMat3(Span output, ReadOnlySpan a, ReadOnlySpan m) + { + double x = a[0]; + double y = a[1]; + double z = a[2]; + output[0] = x * m[0] + y * m[3] + z * m[6]; + output[1] = x * m[1] + y * m[4] + z * m[7]; + output[2] = x * m[2] + y * m[5] + z * m[8]; + } + + /// + /// Transforms the vec3 with a quat + /// + /// + /// + /// + public static void TransformQuat(Span output, ReadOnlySpan a, ReadOnlySpan q) + { + double x = a[0]; + double y = a[1]; + double z = a[2]; + + double qx = q[0]; + double qy = q[1]; + double qz = q[2]; + double qw = q[3]; + + // calculate quat * vec + double ix = qw * x + qy * z - qz * y; + double iy = qw * y + qz * x - qx * z; + double iz = qw * z + qx * y - qy * x; + double iw = -qx * x - qy * y - qz * z; + + // calculate result * inverse quat + output[0] = ix * qw + iw * -qx + iy * -qz - iz * -qy; + output[1] = iy * qw + iw * -qy + iz * -qx - ix * -qz; + output[2] = iz * qw + iw * -qz + ix * -qy - iy * -qx; + } + } +} diff --git a/Math/Matrix/SpanQuaternionf.cs b/Math/Matrix/SpanQuaternionf.cs new file mode 100644 index 000000000..6b3c801b4 --- /dev/null +++ b/Math/Matrix/SpanQuaternionf.cs @@ -0,0 +1,1244 @@ +//glMatrix license: +//Copyright (c) 2013, Brandon Jones, Colin MacKenzie IV. All rights reserved. + +//Redistribution and use in source and binary forms, with or without modification, +//are permitted provided that the following conditions are met: + +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. + +//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +//ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +//WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +//DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +//ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +//ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +//(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +//SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +using System; +using System.Runtime.CompilerServices; + +namespace Vintagestory.API.MathTools +{ + public partial class Quaternionf + { + /// + /// Sets a quaternion to represent the shortest rotation from one + /// vector to another. + /// + /// Both vectors are assumed to be unit length. + /// + /// the receiving quaternion. + /// the initial vector + /// the destination vector + public static void RotationTo(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Span tmpvec3 = stackalloc float[3]; + Span xUnitVec3 = stackalloc float[3] { 1f, 0f, 0f }; + Span yUnitVec3 = stackalloc float[3] { 0f, 1f, 0f }; + + float dot = Vec3Utilsf.Dot(a, b); + + float nines = 999999f / 1000000f; // 0.999999 + + float epsilon = 1f / 1000000f; // 0.000001 + + if (dot < -nines) + { + Vec3Utilsf.Cross(tmpvec3, xUnitVec3, a); + if (Vec3Utilsf.Length(tmpvec3) < epsilon) + Vec3Utilsf.Cross(tmpvec3, yUnitVec3, a); + Vec3Utilsf.Normalize(tmpvec3, tmpvec3); + SetAxisAngle(output, tmpvec3, GameMath.PI); + return; + } + else if (dot > nines) + { + output[0] = 0f; + output[1] = 0f; + output[2] = 0f; + output[3] = 1f; + return; + } + else + { + Vec3Utilsf.Cross(tmpvec3, a, b); + output[0] = tmpvec3[0]; + output[1] = tmpvec3[1]; + output[2] = tmpvec3[2]; + output[3] = 1f + dot; + Normalize(output, output); + } + } + + /// + /// Sets the specified quaternion with values corresponding to the given + /// axes. Each axis is a vec3 and is expected to be unit length and + /// perpendicular to all other specified axes. + /// + /// the vector representing the viewing direction + /// the vector representing the local "right" direction + /// the vector representing the local "up" direction + public static void SetAxes(Span output, ReadOnlySpan view, ReadOnlySpan right, ReadOnlySpan up) + { + Span matr = stackalloc float[9]; + + matr[0] = right[0]; + matr[3] = right[1]; + matr[6] = right[2]; + + matr[1] = up[0]; + matr[4] = up[1]; + matr[7] = up[2]; + + matr[2] = view[0]; + matr[5] = view[1]; + matr[8] = view[2]; + + FromMat3(output, matr); + Normalize(output, output); + } + + /// + /// Set the components of a quat to the given values + /// + /// the receiving quaternion + /// X component + /// Y component + /// Z component + /// W component + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Set(Span output, float x, float y, float z, float w) + { + QVec4f.Set(output, x, y, z, w); + } + + /// + /// Set a quat to the identity quaternion + /// + /// the receiving quaternion + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Identity(Span output) + { + output[0] = 0f; + output[1] = 0f; + output[2] = 0f; + output[3] = 1f; + } + + /// + /// Sets a quat from the given angle and rotation axis, + /// then returns it. + /// + /// the receiving quaternion + /// the axis around which to rotate + /// the angle in radians + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void SetAxisAngle(Span output, ReadOnlySpan axis, float rad) + { + rad /= 2f; + float s = GameMath.Sin(rad); + output[0] = s * axis[0]; + output[1] = s * axis[1]; + output[2] = s * axis[2]; + output[3] = GameMath.Cos(rad); + } + + /// + /// Adds two quat's + /// + /// the receiving quaternion + /// the first operand + /// the second operand + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Add(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + QVec4f.Add(output, a, b); + } + + /// + /// Multiplies two quat's + /// + /// the receiving quaternion + /// the first operand + /// the second operand + public static void Multiply(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + float ax = a[0]; float ay = a[1]; float az = a[2]; float aw = a[3]; + float bx = b[0]; float by = b[1]; float bz = b[2]; float bw = b[3]; + + output[0] = ax * bw + aw * bx + ay * bz - az * by; + output[1] = ay * bw + aw * by + az * bx - ax * bz; + output[2] = az * bw + aw * bz + ax * by - ay * bx; + output[3] = aw * bw - ax * bx - ay * by - az * bz; + } + + /// + /// Alias for + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Mul(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Multiply(output, a, b); + } + + /// + /// Scales a quat by a scalar number + /// + /// the receiving vector + /// the vector to scale + /// amount to scale the vector by + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Scale(Span output, ReadOnlySpan a, float b) + { + QVec4f.Scale(output, a, b); + } + + /// + /// Rotates a quaternion by the given angle aboutput the X axis + /// + /// quat receiving operation result + /// quat to rotate + /// angle (in radians) to rotate + public static void RotateX(Span output, ReadOnlySpan a, float rad) + { + rad /= 2f; + + float ax = a[0]; float ay = a[1]; float az = a[2]; float aw = a[3]; + float bx = GameMath.Sin(rad); float bw = GameMath.Cos(rad); + + output[0] = ax * bw + aw * bx; + output[1] = ay * bw + az * bx; + output[2] = az * bw - ay * bx; + output[3] = aw * bw - ax * bx; + } + + /// + /// Rotates a quaternion by the given angle aboutput the Y axis + /// + /// quat receiving operation result + /// quat to rotate + /// angle (in radians) to rotate + public static void RotateY(Span output, ReadOnlySpan a, float rad) + { + rad /= 2f; + + float ax = a[0]; float ay = a[1]; float az = a[2]; float aw = a[3]; + float by = GameMath.Sin(rad); float bw = GameMath.Cos(rad); + + output[0] = ax * bw - az * by; + output[1] = ay * bw + aw * by; + output[2] = az * bw + ax * by; + output[3] = aw * bw - ay * by; + } + + /// + /// Rotates a quaternion by the given angle aboutput the Z axis + /// + /// quat receiving operation result + /// quat to rotate + /// angle (in radians) to rotate + public static void RotateZ(Span output, ReadOnlySpan a, float rad) + { + rad /= 2f; + + float ax = a[0]; float ay = a[1]; float az = a[2]; float aw = a[3]; + float bz = GameMath.Sin(rad); float bw = GameMath.Cos(rad); + + output[0] = ax * bw + ay * bz; + output[1] = ay * bw - ax * bz; + output[2] = az * bw + aw * bz; + output[3] = aw * bw - az * bz; + } + + /// + /// Calculates the W component of a quat from the X, Y, and Z components. + /// Assumes that quaternion is 1 unit in length. + /// Any existing W component will be ignored. + /// + /// the receiving quaternion + /// quat to calculate W component of + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void CalculateW(Span output, ReadOnlySpan a) + { + float x = a[0]; float y = a[1]; float z = a[2]; + + output[0] = x; + output[1] = y; + output[2] = z; + output[3] = -GameMath.Sqrt(Math.Abs(1f - x * x - y * y - z * z)); + } + + /// + /// Calculates the dot product of two quat's + /// + /// the first operand + /// the second operand + /// dot product of a and b + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Dot(ReadOnlySpan a, ReadOnlySpan b) + { + return QVec4f.Dot(a, b); + } + + public static void ToEulerAngles(Span output, ReadOnlySpan quat) + { + // roll (x-axis rotation) + float sinr_cosp = 2f * (quat[3] * quat[0] + quat[1] * quat[2]); + float cosr_cosp = 1f - 2f * (quat[0] * quat[0] + quat[1] * quat[1]); + output[2] = (float)Math.Atan2(sinr_cosp, cosr_cosp); + + // pitch (y-axis rotation) + float sinp = 2f * (quat[3] * quat[1] - quat[2] * quat[0]); + if (Math.Abs(sinp) >= 1f) + output[1] = (float)Math.PI / 2 * Math.Sign(sinp); // use 90 degrees if out of range + else + output[1] = (float)Math.Asin(sinp); + + // yaw (z-axis rotation) + float siny_cosp = 2f * (quat[3] * quat[2] + quat[0] * quat[1]); + float cosy_cosp = 1f - 2f * (quat[1] * quat[1] + quat[2] * quat[2]); + output[0] = (float)Math.Atan2(siny_cosp, cosy_cosp); + } + + /// + /// Performs a linear interpolation between two quat's + /// + /// the receiving quaternion + /// the first operand + /// the second operand + /// interpolation amount between the two inputs + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Lerp(Span output, ReadOnlySpan a, ReadOnlySpan b, float t) + { + QVec4f.Lerp(output, a, b, t); + } + + /// + /// Performs a spherical linear interpolation between two quat + /// + /// the receiving quaternion + /// the first operand + /// the second operand + /// interpolation amount between the two inputs + public static void Slerp(Span output, ReadOnlySpan a, ReadOnlySpan b, float t) + { + // // benchmarks: + // // http://jsperf.com/quaternion-slerp-implementations + + float ax = a[0]; float ay = a[1]; float az = a[2]; float aw = a[3]; + float bx = b[0]; float by = b[1]; float bz = b[2]; float bw = b[3]; + + float omega; float cosom; float sinom; float scale0; float scale1; + + // calc cosine + cosom = ax * bx + ay * by + az * bz + aw * bw; + // adjust signs (if necessary) + if (cosom < 0f) + { + cosom = -cosom; + bx = -bx; + by = -by; + bz = -bz; + bw = -bw; + } + float one = 1f; + float epsilon = one / 1000000; + // calculate coefficients + if ((one - cosom) > epsilon) + { + // standard case (slerp) + omega = GameMath.Acos(cosom); + sinom = GameMath.Sin(omega); + scale0 = GameMath.Sin((one - t) * omega) / sinom; + scale1 = GameMath.Sin(t * omega) / sinom; + } + else + { + // "from" and "to" quaternions are very close + // ... so we can do a linear interpolation + scale0 = one - t; + scale1 = t; + } + // calculate final values + output[0] = scale0 * ax + scale1 * bx; + output[1] = scale0 * ay + scale1 * by; + output[2] = scale0 * az + scale1 * bz; + output[3] = scale0 * aw + scale1 * bw; + } + + /// + /// Calculates the inverse of a quat + /// + /// the receiving quaternion + /// quat to calculate inverse of + public static void Invert(Span output, ReadOnlySpan a) + { + float a0 = a[0]; float a1 = a[1]; float a2 = a[2]; float a3 = a[3]; + float dot = a0 * a0 + a1 * a1 + a2 * a2 + a3 * a3; + float one = 1f; + float invDot = (dot != 0f) ? one / dot : 0f; + + // TODO: Would be faster to return [0,0,0,0] immediately if dot == 0 + + output[0] = -a0 * invDot; + output[1] = -a1 * invDot; + output[2] = -a2 * invDot; + output[3] = a3 * invDot; + } + + /// + /// Calculates the conjugate of a quat + /// If the quaternion is normalized, this function is faster than quat.inverse and produces the same result. + /// + /// the receiving quaternion + /// quat to calculate conjugate of + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Conjugate(Span output, ReadOnlySpan a) + { + output[0] = -a[0]; + output[1] = -a[1]; + output[2] = -a[2]; + output[3] = a[3]; + } + + /// + /// Calculates the length of a quat + /// + /// vector to calculate length of + /// length of a + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Length(ReadOnlySpan a) + { + return QVec4f.Length(a); + } + + /// + /// Alias for + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Len(ReadOnlySpan a) + { + return Length(a); + } + + /// + /// Calculates the squared length of a quat + /// + /// vector to calculate squared length of + /// squared length of a + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float SquaredLength(ReadOnlySpan a) + { + return QVec4f.SquaredLength(a); + } + + /// + /// Alias for + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float SqrLen(ReadOnlySpan a) + { + return SquaredLength(a); + } + + /// + /// Normalize a quat + /// + /// the receiving quaternion + /// quaternion to normalize + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Normalize(Span output, ReadOnlySpan a) + { + QVec4f.Normalize(output, a); + } + + /// + /// Creates a quaternion from the given 3x3 rotation matrix. + /// + /// NOTE: The resultant quaternion is not normalized, so you should be sure + /// to renormalize the quaternion yourself where necessary. + /// + /// the receiving quaternion + /// rotation matrix + public static void FromMat3(Span output, ReadOnlySpan m) + { + // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes + // article "Quaternion Calculus and Fast Animation". + float fTrace = m[0] + m[4] + m[8]; + float fRoot; + + float zero = 0; + float one = 1; + float half = one / 2; + if (fTrace > zero) + { + // |w| > 1/2, may as well choose w > 1/2 + fRoot = GameMath.Sqrt(fTrace + one); // 2w + output[3] = half * fRoot; + fRoot = half / fRoot; // 1/(4w) + output[0] = (m[7] - m[5]) * fRoot; + output[1] = (m[2] - m[6]) * fRoot; + output[2] = (m[3] - m[1]) * fRoot; + } + else + { + // |w| <= 1/2 + int i = 0; + if (m[4] > m[0]) + i = 1; + if (m[8] > m[i * 3 + i]) + i = 2; + int j = (i + 1) % 3; + int k = (i + 2) % 3; + + fRoot = GameMath.Sqrt(m[i * 3 + i] - m[j * 3 + j] - m[k * 3 + k] + one); + output[i] = half * fRoot; + fRoot = half / fRoot; + output[3] = (m[k * 3 + j] - m[j * 3 + k]) * fRoot; + output[j] = (m[j * 3 + i] + m[i * 3 + j]) * fRoot; + output[k] = (m[k * 3 + i] + m[i * 3 + k]) * fRoot; + } + } + } + + partial class QVec4f + { + /// + /// Set the components of a QVec4f to the given values + /// + /// the receiving vector + /// X component + /// Y component + /// Z component + /// W component + public static void Set(Span output, float x, float y, float z, float w) + { + output[0] = x; + output[1] = y; + output[2] = z; + output[3] = w; + } + + /// + /// Adds two QVec4f's + /// + /// the receiving vector + /// the first operand + /// the second operand + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Add(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = a[0] + b[0]; + output[1] = a[1] + b[1]; + output[2] = a[2] + b[2]; + output[3] = a[3] + b[3]; + } + + /// + /// Subtracts vector b from vector a + /// + /// the receiving vector + /// the first operand + /// the second operand + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Subtract(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = a[0] - b[0]; + output[1] = a[1] - b[1]; + output[2] = a[2] - b[2]; + output[3] = a[3] - b[3]; + } + + /// + /// Alias for + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Sub(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Subtract(output, a, b); + } + + /// + /// Multiplies two QVec4f's + /// + /// the receiving vector + /// the first operand + /// the second operand + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Multiply(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = a[0] * b[0]; + output[1] = a[1] * b[1]; + output[2] = a[2] * b[2]; + output[3] = a[3] * b[3]; + } + + /// + /// Alias for + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Mul(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Multiply(output, a, b); + } + + /// + /// Divides two QVec4f's + /// + /// the receiving vector + /// the first operand + /// the second operand + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Divide(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = a[0] / b[0]; + output[1] = a[1] / b[1]; + output[2] = a[2] / b[2]; + output[3] = a[3] / b[3]; + } + + /// + /// Alias for + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Div(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Divide(output, a, b); + } + + /// + /// Returns the minimum of two QVec4f's + /// + /// the receiving vector + /// the first operand + /// the second operand + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Min(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = Math.Min(a[0], b[0]); + output[1] = Math.Min(a[1], b[1]); + output[2] = Math.Min(a[2], b[2]); + output[3] = Math.Min(a[3], b[3]); + } + + /// + /// Returns the maximum of two QVec4f's + /// + /// the receiving vector + /// the first operand + /// the second operand + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Max(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = Math.Max(a[0], b[0]); + output[1] = Math.Max(a[1], b[1]); + output[2] = Math.Max(a[2], b[2]); + output[3] = Math.Max(a[3], b[3]); + } + + /// + /// Scales a QVec4f by a scalar number + /// + /// the receiving vector + /// the vector to scale + /// amount to scale the vector by + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Scale(Span output, ReadOnlySpan a, float b) + { + output[0] = a[0] * b; + output[1] = a[1] * b; + output[2] = a[2] * b; + output[3] = a[3] * b; + } + + /// + /// Adds two QVec4f's after scaling the second operand by a scalar value + /// + /// the receiving vector + /// the first operand + /// the second operand + /// the amount to scale b by before adding + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ScaleAndAdd(Span output, ReadOnlySpan a, ReadOnlySpan b, float scale) + { + output[0] = a[0] + (b[0] * scale); + output[1] = a[1] + (b[1] * scale); + output[2] = a[2] + (b[2] * scale); + output[3] = a[3] + (b[3] * scale); + } + + /// + /// Calculates the euclidian distance between two QVec4f's + /// + /// the first operand + /// the second operand + /// distance between a and b + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Distance(ReadOnlySpan a, ReadOnlySpan b) + { + float x = b[0] - a[0]; + float y = b[1] - a[1]; + float z = b[2] - a[2]; + float w = b[3] - a[3]; + return GameMath.Sqrt(x * x + y * y + z * z + w * w); + } + + /// + /// Alias for + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Dist(ReadOnlySpan a, ReadOnlySpan b) + { + return Distance(a, b); + } + + /// + /// Calculates the squared euclidian distance between two QVec4f's + /// + /// the first operand + /// the second operand + /// squared distance between a and b + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float SquaredDistance(ReadOnlySpan a, ReadOnlySpan b) + { + float x = b[0] - a[0]; + float y = b[1] - a[1]; + float z = b[2] - a[2]; + float w = b[3] - a[3]; + return x * x + y * y + z * z + w * w; + } + + /// + /// Alias for + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float SqrDist(ReadOnlySpan a, ReadOnlySpan b) + { + return SquaredDistance(a, b); + } + /// + /// Calculates the length of a QVec4f + /// + /// vector to calculate length of + /// length of a + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Length(ReadOnlySpan a) + { + float x = a[0]; + float y = a[1]; + float z = a[2]; + float w = a[3]; + return GameMath.Sqrt(x * x + y * y + z * z + w * w); + } + + /// + /// Alias for + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Len(ReadOnlySpan a) + { + return Length(a); + } + + /// + /// Calculates the squared length of a QVec4f + /// + /// vector to calculate squared length of + /// squared length of a + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float SquaredLength(ReadOnlySpan a) + { + float x = a[0]; + float y = a[1]; + float z = a[2]; + float w = a[3]; + return x * x + y * y + z * z + w * w; + } + + /// + /// Alias for + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float SqrLen(ReadOnlySpan a) + { + return SquaredLength(a); + } + + /// + /// Negates the components of a QVec4f + /// + /// the receiving vector + /// vector to negate + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Negate(Span output, ReadOnlySpan a) + { + output[0] = -a[0]; + output[1] = -a[1]; + output[2] = -a[2]; + output[3] = -a[3]; + } + + /// + /// Normalize a QVec4f + /// + /// the receiving vector + /// vector to normalize + public static void Normalize(Span output, ReadOnlySpan a) + { + float x = a[0]; + float y = a[1]; + float z = a[2]; + float w = a[3]; + float len = x * x + y * y + z * z + w * w; + if (len > 0f) + { + float one = 1f; + len = one / GameMath.Sqrt(len); + output[0] = x * len; + output[1] = y * len; + output[2] = z * len; + output[3] = w * len; + } + } + + /// + /// Calculates the dot product of two QVec4f's + /// + /// the first operand + /// the second operand + /// dot product of a and b + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Dot(ReadOnlySpan a, ReadOnlySpan b) + { + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]; + } + + /// + /// Performs a linear interpolation between two QVec4f's + /// + /// the receiving vector + /// the first operand + /// the second operand + /// interpolation amount between the two inputs + public static void Lerp(Span output, ReadOnlySpan a, ReadOnlySpan b, float t) + { + float ax = a[0]; + float ay = a[1]; + float az = a[2]; + float aw = a[3]; + output[0] = ax + t * (b[0] - ax); + output[1] = ay + t * (b[1] - ay); + output[2] = az + t * (b[2] - az); + output[3] = aw + t * (b[3] - aw); + } + + + /// + /// Transforms the QVec4f with a mat4. + /// + /// the receiving vector + /// the vector to transform + /// matrix to transform with + public static void TransformMat4(Span output, ReadOnlySpan a, ReadOnlySpan m) + { + float x = a[0]; float y = a[1]; float z = a[2]; float w = a[3]; + output[0] = m[0] * x + m[4] * y + m[8] * z + m[12] * w; + output[1] = m[1] * x + m[5] * y + m[9] * z + m[13] * w; + output[2] = m[2] * x + m[6] * y + m[10] * z + m[14] * w; + output[3] = m[3] * x + m[7] * y + m[11] * z + m[15] * w; + } + + /// + /// Transforms the QVec4f with a quat + /// + /// the receiving vector + /// the vector to transform + /// quaternion to transform with + public static void transformQuat(Span output, ReadOnlySpan a, ReadOnlySpan q) + { + float x = a[0]; float y = a[1]; float z = a[2]; + float qx = q[0]; float qy = q[1]; float qz = q[2]; float qw = q[3]; + + // calculate quat * vec + float ix = qw * x + qy * z - qz * y; + float iy = qw * y + qz * x - qx * z; + float iz = qw * z + qx * y - qy * x; + float iw = -qx * x - qy * y - qz * z; + + // calculate result * inverse quat + output[0] = ix * qw + iw * -qx + iy * -qz - iz * -qy; + output[1] = iy * qw + iw * -qy + iz * -qx - ix * -qz; + output[2] = iz * qw + iw * -qz + ix * -qy - iy * -qx; + } + } + + /// + /// Don't use this class unless you need it to interoperate with Mat4d + /// + public partial class Vec3Utilsf + { + /// + /// Set the components of a vec3 to the given values + /// + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Set(Span output, float x, float y, float z) + { + output[0] = x; + output[1] = y; + output[2] = z; + } + + /// + /// Adds two vec3's + /// + /// the receiving vector + /// the first operand + /// the second operand + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Add(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = a[0] + b[0]; + output[1] = a[1] + b[1]; + output[2] = a[2] + b[2]; + } + + /// + /// Subtracts vector b from vector a + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Substract(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = a[0] - b[0]; + output[1] = a[1] - b[1]; + output[2] = a[2] - b[2]; + } + + + /// + /// Multiplies two vec3's + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Multiply(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = a[0] * b[0]; + output[1] = a[1] * b[1]; + output[2] = a[2] * b[2]; + } + + /// + /// Alias for + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Mul(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + Multiply(output, a, b); + } + + /// + /// Divides two vec3's + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Divide(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = a[0] / b[0]; + output[1] = a[1] / b[1]; + output[2] = a[2] / b[2]; + } + + /// + /// Returns the minimum of two vec3's + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Min(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = Math.Min(a[0], b[0]); + output[1] = Math.Min(a[1], b[1]); + output[2] = Math.Min(a[2], b[2]); + } + + /// + /// Returns the maximum of two vec3's + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Max(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + output[0] = Math.Max(a[0], b[0]); + output[1] = Math.Max(a[1], b[1]); + output[2] = Math.Max(a[2], b[2]); + } + + /// + /// Scales a vec3 by a scalar number + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Scale(Span output, ReadOnlySpan a, float b) + { + output[0] = a[0] * b; + output[1] = a[1] * b; + output[2] = a[2] * b; + } + + /// + /// Adds two vec3's after scaling the second operand by a scalar value + /// + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void ScaleAndAdd(Span output, ReadOnlySpan a, ReadOnlySpan b, float scale) + { + output[0] = a[0] + (b[0] * scale); + output[1] = a[1] + (b[1] * scale); + output[2] = a[2] + (b[2] * scale); + } + + /// + /// Calculates the euclidian distance between two vec3's. Returns {Number} distance between a and b + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Distance(ReadOnlySpan a, ReadOnlySpan b) + { + float x = b[0] - a[0]; + float y = b[1] - a[1]; + float z = b[2] - a[2]; + return GameMath.Sqrt(x * x + y * y + z * z); + } + + /// + /// Calculates the squared euclidian distance between two vec3's. Returns {Number} squared distance between a and b + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float SquaredDistance(ReadOnlySpan a, ReadOnlySpan b) + { + float x = b[0] - a[0]; + float y = b[1] - a[1]; + float z = b[2] - a[2]; + return x * x + y * y + z * z; + } + + /// + /// Calculates the length of a vec3 + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Length(ReadOnlySpan a) + { + float x = a[0]; + float y = a[1]; + float z = a[2]; + return GameMath.Sqrt(x * x + y * y + z * z); + } + + /// + /// Calculates the squared length of a vec3. Returns {Number} squared length of a + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float SquaredLength(ReadOnlySpan a) + { + float x = a[0]; + float y = a[1]; + float z = a[2]; + return x * x + y * y + z * z; + } + + + + /// + /// Negates the components of a vec3 + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Negate(Span output, ReadOnlySpan a) + { + output[0] = -a[0]; + output[1] = -a[1]; + output[2] = -a[2]; + } + + /// + /// Normalize a vec3 + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Normalize(Span output, ReadOnlySpan a) + { + float x = a[0]; + float y = a[1]; + float z = a[2]; + float len = x * x + y * y + z * z; + if (len > 0f) + { + float one = 1f; + len = one / GameMath.Sqrt(len); + output[0] = a[0] * len; + output[1] = a[1] * len; + output[2] = a[2] * len; + } + } + + /// + /// Calculates the dot product of two vec3's. Returns {Number} dot product of a and b + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Dot(ReadOnlySpan a, ReadOnlySpan b) + { + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; + } + + /// + /// Computes the cross product of two vec3's. Returns {vec3} out + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Cross(Span output, ReadOnlySpan a, ReadOnlySpan b) + { + float ax = a[0]; + float ay = a[1]; + float az = a[2]; + float bx = b[0]; + float by = b[1]; + float bz = b[2]; + + output[0] = ay * bz - az * by; + output[1] = az * bx - ax * bz; + output[2] = ax * by - ay * bx; + } + + /// + /// Performs a linear interpolation between two vec3's + /// + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void Lerp(Span output, ReadOnlySpan a, ReadOnlySpan b, float t) + { + float ax = a[0]; + float ay = a[1]; + float az = a[2]; + output[0] = ax + t * (b[0] - ax); + output[1] = ay + t * (b[1] - ay); + output[2] = az + t * (b[2] - az); + } + + + /// + /// Transforms the vec3 with a mat4. 4th vector component is implicitly '1'. Returns {vec3} out + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void TransformMat4(Span output, ReadOnlySpan a, ReadOnlySpan m) + { + float x = a[0]; + float y = a[1]; + float z = a[2]; + output[0] = m[0] * x + m[4] * y + m[8] * z + m[12]; + output[1] = m[1] * x + m[5] * y + m[9] * z + m[13]; + output[2] = m[2] * x + m[6] * y + m[10] * z + m[14]; + } + + /// + /// Transforms the vec3 with a mat3. Returns {vec3} out + /// + /// + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void TransformMat3(Span output, ReadOnlySpan a, ReadOnlySpan m) + { + float x = a[0]; + float y = a[1]; + float z = a[2]; + output[0] = x * m[0] + y * m[3] + z * m[6]; + output[1] = x * m[1] + y * m[4] + z * m[7]; + output[2] = x * m[2] + y * m[5] + z * m[8]; + } + + /// + /// Transforms the vec3 with a quat + /// + /// + /// + /// + public static void TransformQuat(Span output, ReadOnlySpan a, ReadOnlySpan q) + { + float x = a[0]; + float y = a[1]; + float z = a[2]; + + float qx = q[0]; + float qy = q[1]; + float qz = q[2]; + float qw = q[3]; + + // calculate quat * vec + float ix = qw * x + qy * z - qz * y; + float iy = qw * y + qz * x - qx * z; + float iz = qw * z + qx * y - qy * x; + float iw = -qx * x - qy * y - qz * z; + + // calculate result * inverse quat + output[0] = ix * qw + iw * -qx + iy * -qz - iz * -qy; + output[1] = iy * qw + iw * -qy + iz * -qx - ix * -qz; + output[2] = iz * qw + iw * -qz + ix * -qy - iy * -qx; + } + } +} diff --git a/Math/Vector/FastVec3d.cs b/Math/Vector/FastVec3d.cs index 5488bfacd..1457f82c4 100644 --- a/Math/Vector/FastVec3d.cs +++ b/Math/Vector/FastVec3d.cs @@ -1,11 +1,13 @@ using System; using System.IO; +using System.Runtime.InteropServices; namespace Vintagestory.API.MathTools { /// /// Represents a vector of 3 doubles /// + [StructLayout(LayoutKind.Sequential, Pack = 8)] public struct FastVec3d { /// diff --git a/Math/Vector/FastVec3f.cs b/Math/Vector/FastVec3f.cs index b67ede36c..5c5addf4d 100644 --- a/Math/Vector/FastVec3f.cs +++ b/Math/Vector/FastVec3f.cs @@ -1,11 +1,13 @@ using System; using System.IO; +using System.Runtime.InteropServices; namespace Vintagestory.API.MathTools { /// /// Represents a vector of 3 floats. Go bug Tyron of you need more utility methods in this class. /// + [StructLayout(LayoutKind.Sequential, Pack = 4)] public struct FastVec3f { /// diff --git a/Math/Vector/FastVec3i.cs b/Math/Vector/FastVec3i.cs index a061debc5..5f21a6122 100644 --- a/Math/Vector/FastVec3i.cs +++ b/Math/Vector/FastVec3i.cs @@ -1,12 +1,14 @@ using System; using System.Collections.Generic; using System.IO; +using System.Runtime.InteropServices; namespace Vintagestory.API.MathTools { /// /// Represents a vector of 3 ints, similar to a Vec3i or a BlockPos but this is a struct /// + [StructLayout(LayoutKind.Sequential, Pack = 4)] public struct FastVec3i { /// diff --git a/Math/Vector/Vec2d.cs b/Math/Vector/Vec2d.cs index c87b8ff0c..0fa765176 100644 --- a/Math/Vector/Vec2d.cs +++ b/Math/Vector/Vec2d.cs @@ -1,10 +1,12 @@ using System; +using System.Runtime.InteropServices; namespace Vintagestory.API.MathTools { /// /// Represents a vector of 2 doubles. Go bug Tyron of you need more utility methods in this class. /// + [StructLayout(LayoutKind.Sequential, Pack = 8)] public class Vec2d { public double X; diff --git a/Math/Vector/Vec2f.cs b/Math/Vector/Vec2f.cs index ed034410a..7a8bfed56 100644 --- a/Math/Vector/Vec2f.cs +++ b/Math/Vector/Vec2f.cs @@ -1,6 +1,7 @@ using Newtonsoft.Json; using ProtoBuf; using System; +using System.Runtime.InteropServices; namespace Vintagestory.API.MathTools { @@ -27,6 +28,7 @@ public AngleConstraint(float centerRad, float rangeRad) : base(centerRad, rangeR [DocumentAsJson] [JsonObject(MemberSerialization.OptIn)] [ProtoContract] + [StructLayout(LayoutKind.Sequential, Pack = 4)] public class Vec2f { public static readonly Vec2f Zero = new Vec2f(0, 0); diff --git a/Math/Vector/Vec2i.cs b/Math/Vector/Vec2i.cs index 495b80737..bc9eceea5 100644 --- a/Math/Vector/Vec2i.cs +++ b/Math/Vector/Vec2i.cs @@ -1,5 +1,6 @@ using ProtoBuf; using System; +using System.Runtime.InteropServices; namespace Vintagestory.API.MathTools { @@ -7,6 +8,7 @@ namespace Vintagestory.API.MathTools /// Represents a vector of 2 ints. Go bug Tyron if you need more utility methods in this class. /// [ProtoContract] + [StructLayout(LayoutKind.Sequential, Pack = 4)] public class Vec2i : IEquatable { [ProtoMember(1)] diff --git a/Math/Vector/Vec3d.cs b/Math/Vector/Vec3d.cs index ed3bd0385..c5178f67c 100644 --- a/Math/Vector/Vec3d.cs +++ b/Math/Vector/Vec3d.cs @@ -2,6 +2,7 @@ using ProtoBuf; using System; using System.IO; +using System.Runtime.InteropServices; using Vintagestory.API.Client; namespace Vintagestory.API.MathTools @@ -11,6 +12,7 @@ namespace Vintagestory.API.MathTools /// [ProtoContract] + [StructLayout(LayoutKind.Sequential, Pack = 8)] public class Vec3d : IVec3, IEquatable { [ProtoMember(1)] diff --git a/Math/Vector/Vec3f.cs b/Math/Vector/Vec3f.cs index d410a0ecb..9005c85e8 100644 --- a/Math/Vector/Vec3f.cs +++ b/Math/Vector/Vec3f.cs @@ -2,6 +2,7 @@ using ProtoBuf; using System; using System.IO; +using System.Runtime.InteropServices; using Vintagestory.API.Client; namespace Vintagestory.API.MathTools @@ -11,6 +12,7 @@ namespace Vintagestory.API.MathTools /// [JsonObject(MemberSerialization.OptIn)] [ProtoContract] + [StructLayout(LayoutKind.Sequential, Pack = 4)] public class Vec3f : IVec3, IEquatable { /// diff --git a/Math/Vector/Vec3i.cs b/Math/Vector/Vec3i.cs index 5bc133684..278397ef4 100644 --- a/Math/Vector/Vec3i.cs +++ b/Math/Vector/Vec3i.cs @@ -2,6 +2,7 @@ using ProtoBuf; using System; using System.Collections.Generic; +using System.Runtime.InteropServices; namespace Vintagestory.API.MathTools { @@ -10,6 +11,7 @@ namespace Vintagestory.API.MathTools /// [ProtoContract] [JsonObject(MemberSerialization.OptIn)] + [StructLayout(LayoutKind.Sequential, Pack = 4)] public class Vec3i : IEquatable, IVec3 { [ProtoMember(1)] diff --git a/Math/Vector/Vec4Us.cs b/Math/Vector/Vec4Us.cs index f872b3838..e08196571 100644 --- a/Math/Vector/Vec4Us.cs +++ b/Math/Vector/Vec4Us.cs @@ -1,10 +1,12 @@ using System; +using System.Runtime.InteropServices; namespace Vintagestory.API.MathTools { /// /// Represents a vector of 4 unsigned shorts. Go bug Tyron if you need more utility methods in this class. /// + [StructLayout(LayoutKind.Sequential, Pack = 2)] public class Vec4us : IEquatable { public ushort X; diff --git a/Math/Vector/Vec4d.cs b/Math/Vector/Vec4d.cs index 0c4612a72..47f7a21aa 100644 --- a/Math/Vector/Vec4d.cs +++ b/Math/Vector/Vec4d.cs @@ -1,5 +1,8 @@ -namespace Vintagestory.API.MathTools +using System.Runtime.InteropServices; + +namespace Vintagestory.API.MathTools { + [StructLayout(LayoutKind.Sequential, Pack = 8)] public class Vec4d { public double X; diff --git a/Math/Vector/Vec4f.cs b/Math/Vector/Vec4f.cs index a43000ee5..94915b559 100644 --- a/Math/Vector/Vec4f.cs +++ b/Math/Vector/Vec4f.cs @@ -1,7 +1,9 @@ using System; +using System.Runtime.InteropServices; namespace Vintagestory.API.MathTools { + [StructLayout(LayoutKind.Sequential, Pack = 4)] public class Vec4f { public float X; diff --git a/Math/Vector/Vec4i.cs b/Math/Vector/Vec4i.cs index 3d717dc4d..31ce72806 100644 --- a/Math/Vector/Vec4i.cs +++ b/Math/Vector/Vec4i.cs @@ -1,5 +1,6 @@ using ProtoBuf; using System; +using System.Runtime.InteropServices; namespace Vintagestory.API.MathTools { @@ -7,6 +8,7 @@ namespace Vintagestory.API.MathTools /// Represents a vector of 4 ints. Go bug Tyron if you need more utility methods in this class. /// [ProtoContract] + [StructLayout(LayoutKind.Sequential, Pack = 4)] public class Vec4i : IEquatable { [ProtoMember(1)] diff --git a/Math/Vector/Vec4s.cs b/Math/Vector/Vec4s.cs index 4d9f23ba5..af8cc1691 100644 --- a/Math/Vector/Vec4s.cs +++ b/Math/Vector/Vec4s.cs @@ -1,10 +1,12 @@ using System; +using System.Runtime.InteropServices; namespace Vintagestory.API.MathTools { /// /// Represents a vector of 4 shorts. Go bug Tyron if you need more utility methods in this class. /// + [StructLayout(LayoutKind.Sequential, Pack = 2)] public class Vec4s : IEquatable { public short X; diff --git a/Math/Vector/VecSpanExt.cs b/Math/Vector/VecSpanExt.cs new file mode 100644 index 000000000..f71656b4b --- /dev/null +++ b/Math/Vector/VecSpanExt.cs @@ -0,0 +1,179 @@ +using System; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +namespace Vintagestory.API.MathTools +{ + public static class VecSpanExt + { + /// + /// Returns the span representation of the vector + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Span AsSpan(this ref FastVec3f vec) + { + return MemoryMarshal.CreateSpan(ref vec.X, 3); + } + + /// + /// Returns the span representation of the vector + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Span AsSpan(this ref FastVec3d vec) + { + return MemoryMarshal.CreateSpan(ref vec.X, 3); + } + + /// + /// Returns the span representation of the vector + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Span AsSpan(this ref FastVec3i vec) + { + return MemoryMarshal.CreateSpan(ref vec.X, 3); + } + + /// + /// Returns the span representation of the vector + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static ReadOnlySpan AsReadOnlySpan(this in FastVec3f vec) + { + return MemoryMarshal.CreateReadOnlySpan(ref Unsafe.AsRef(vec.X), 3); + } + + /// + /// Returns the span representation of the vector + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static ReadOnlySpan AsReadOnlySpan(this in FastVec3d vec) + { + return MemoryMarshal.CreateReadOnlySpan(ref Unsafe.AsRef(vec.X), 3); + } + + /// + /// Returns the span representation of the vector + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static ReadOnlySpan AsReadOnlySpan(this in FastVec3i vec) + { + return MemoryMarshal.CreateReadOnlySpan(ref Unsafe.AsRef(vec.X), 3); + } + + /// + /// Returns the span representation of the vector + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Span AsSpan(this Vec2f vec) + { + return MemoryMarshal.CreateSpan(ref vec.X, 2); + } + + /// + /// Returns the span representation of the vector + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Span AsSpan(this Vec2d vec) + { + return MemoryMarshal.CreateSpan(ref vec.X, 2); + } + + /// + /// Returns the span representation of the vector + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Span AsSpan(this Vec2i vec) + { + return MemoryMarshal.CreateSpan(ref vec.X, 2); + } + + /// + /// Returns the span representation of the vector + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Span AsSpan(this Vec3f vec) + { + return MemoryMarshal.CreateSpan(ref vec.X, 3); + } + + /// + /// Returns the span representation of the vector + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Span AsSpan(this Vec3d vec) + { + return MemoryMarshal.CreateSpan(ref vec.X, 3); + } + + /// + /// Returns the span representation of the vector + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Span AsSpan(this Vec3i vec) + { + return MemoryMarshal.CreateSpan(ref vec.X, 3); + } + + /// + /// Returns the span representation of the vector + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Span AsSpan(this Vec4f vec) + { + return MemoryMarshal.CreateSpan(ref vec.X, 4); + } + + /// + /// Returns the span representation of the vector + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Span AsSpan(this Vec4d vec) + { + return MemoryMarshal.CreateSpan(ref vec.X, 4); + } + + /// + /// Returns the span representation of the vector + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Span AsSpan(this Vec4i vec) + { + return MemoryMarshal.CreateSpan(ref vec.X, 4); + } + + /// + /// Returns the span representation of the vector + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Span AsSpan(this Vec4s vec) + { + return MemoryMarshal.CreateSpan(ref vec.X, 4); + } + + /// + /// Returns the span representation of the vector + /// + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Span AsSpan(this Vec4us vec) + { + return MemoryMarshal.CreateSpan(ref vec.X, 4); + } + } +}