|
@@ -0,0 +1,313 @@
|
|
|
+using System;
|
|
|
+
|
|
|
+namespace BabylonExport.Entities
|
|
|
+{
|
|
|
+ public class BabylonMatrix
|
|
|
+ {
|
|
|
+ public float[] m = new float[16];
|
|
|
+
|
|
|
+ /**
|
|
|
+ * Returns a new Matrix as the passed inverted one.
|
|
|
+ */
|
|
|
+ public static BabylonMatrix Invert(BabylonMatrix source) {
|
|
|
+ var result = new BabylonMatrix();
|
|
|
+ source.invertToRef(result);
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+
|
|
|
+ /**
|
|
|
+ * Inverts in place the Matrix.
|
|
|
+ * Returns the Matrix inverted.
|
|
|
+ */
|
|
|
+ public BabylonMatrix invert() {
|
|
|
+ this.invertToRef(this);
|
|
|
+ return this;
|
|
|
+ }
|
|
|
+
|
|
|
+ /**
|
|
|
+ * Sets the passed matrix with the current inverted Matrix.
|
|
|
+ * Returns the unmodified current Matrix.
|
|
|
+ */
|
|
|
+ public BabylonMatrix invertToRef(BabylonMatrix other) {
|
|
|
+ var l1 = this.m[0];
|
|
|
+ var l2 = this.m[1];
|
|
|
+ var l3 = this.m[2];
|
|
|
+ var l4 = this.m[3];
|
|
|
+ var l5 = this.m[4];
|
|
|
+ var l6 = this.m[5];
|
|
|
+ var l7 = this.m[6];
|
|
|
+ var l8 = this.m[7];
|
|
|
+ var l9 = this.m[8];
|
|
|
+ var l10 = this.m[9];
|
|
|
+ var l11 = this.m[10];
|
|
|
+ var l12 = this.m[11];
|
|
|
+ var l13 = this.m[12];
|
|
|
+ var l14 = this.m[13];
|
|
|
+ var l15 = this.m[14];
|
|
|
+ var l16 = this.m[15];
|
|
|
+ var l17 = (l11 * l16) - (l12 * l15);
|
|
|
+ var l18 = (l10 * l16) - (l12 * l14);
|
|
|
+ var l19 = (l10 * l15) - (l11 * l14);
|
|
|
+ var l20 = (l9 * l16) - (l12 * l13);
|
|
|
+ var l21 = (l9 * l15) - (l11 * l13);
|
|
|
+ var l22 = (l9 * l14) - (l10 * l13);
|
|
|
+ var l23 = ((l6 * l17) - (l7 * l18)) + (l8 * l19);
|
|
|
+ var l24 = -(((l5 * l17) - (l7 * l20)) + (l8 * l21));
|
|
|
+ var l25 = ((l5 * l18) - (l6 * l20)) + (l8 * l22);
|
|
|
+ var l26 = -(((l5 * l19) - (l6 * l21)) + (l7 * l22));
|
|
|
+ var l27 = 1.0f / ((((l1 * l23) + (l2 * l24)) + (l3 * l25)) + (l4 * l26));
|
|
|
+ var l28 = (l7 * l16) - (l8 * l15);
|
|
|
+ var l29 = (l6 * l16) - (l8 * l14);
|
|
|
+ var l30 = (l6 * l15) - (l7 * l14);
|
|
|
+ var l31 = (l5 * l16) - (l8 * l13);
|
|
|
+ var l32 = (l5 * l15) - (l7 * l13);
|
|
|
+ var l33 = (l5 * l14) - (l6 * l13);
|
|
|
+ var l34 = (l7 * l12) - (l8 * l11);
|
|
|
+ var l35 = (l6 * l12) - (l8 * l10);
|
|
|
+ var l36 = (l6 * l11) - (l7 * l10);
|
|
|
+ var l37 = (l5 * l12) - (l8 * l9);
|
|
|
+ var l38 = (l5 * l11) - (l7 * l9);
|
|
|
+ var l39 = (l5 * l10) - (l6 * l9);
|
|
|
+
|
|
|
+ other.m[0] = l23* l27;
|
|
|
+ other.m[4] = l24* l27;
|
|
|
+ other.m[8] = l25* l27;
|
|
|
+ other.m[12] = l26* l27;
|
|
|
+ other.m[1] = -(((l2* l17) - (l3* l18)) + (l4* l19)) * l27;
|
|
|
+ other.m[5] = (((l1* l17) - (l3* l20)) + (l4* l21)) * l27;
|
|
|
+ other.m[9] = -(((l1* l18) - (l2* l20)) + (l4* l22)) * l27;
|
|
|
+ other.m[13] = (((l1* l19) - (l2* l21)) + (l3* l22)) * l27;
|
|
|
+ other.m[2] = (((l2* l28) - (l3* l29)) + (l4* l30)) * l27;
|
|
|
+ other.m[6] = -(((l1* l28) - (l3* l31)) + (l4* l32)) * l27;
|
|
|
+ other.m[10] = (((l1* l29) - (l2* l31)) + (l4* l33)) * l27;
|
|
|
+ other.m[14] = -(((l1* l30) - (l2* l32)) + (l3* l33)) * l27;
|
|
|
+ other.m[3] = -(((l2* l34) - (l3* l35)) + (l4* l36)) * l27;
|
|
|
+ other.m[7] = (((l1* l34) - (l3* l37)) + (l4* l38)) * l27;
|
|
|
+ other.m[11] = -(((l1* l35) - (l2* l37)) + (l4* l39)) * l27;
|
|
|
+ other.m[15] = (((l1* l36) - (l2* l38)) + (l3* l39)) * l27;
|
|
|
+
|
|
|
+ return this;
|
|
|
+ }
|
|
|
+
|
|
|
+ /**
|
|
|
+ * Returns a new Matrix composed by the passed scale (vector3), rotation (quaternion) and translation (vector3).
|
|
|
+ */
|
|
|
+ public static BabylonMatrix Compose(BabylonVector3 scale, BabylonQuaternion rotation, BabylonVector3 translation)
|
|
|
+ {
|
|
|
+ var result = BabylonMatrix.Identity();
|
|
|
+ BabylonMatrix.ComposeToRef(scale, rotation, translation, result);
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+
|
|
|
+ /**
|
|
|
+ * Update a Matrix with values composed by the passed scale (vector3), rotation (quaternion) and translation (vector3).
|
|
|
+ */
|
|
|
+ public static void ComposeToRef(BabylonVector3 scale, BabylonQuaternion rotation, BabylonVector3 translation, BabylonMatrix result) {
|
|
|
+ var matrix1 = new BabylonMatrix();
|
|
|
+ BabylonMatrix.FromValuesToRef(scale.X, 0, 0, 0,
|
|
|
+ 0, scale.Y, 0, 0,
|
|
|
+ 0, 0, scale.Z, 0,
|
|
|
+ 0, 0, 0, 1, matrix1);
|
|
|
+
|
|
|
+ var matrix0 = new BabylonMatrix();
|
|
|
+ rotation.toRotationMatrix(matrix0);
|
|
|
+ matrix1.multiplyToRef(matrix0, result);
|
|
|
+
|
|
|
+ result.setTranslation(translation);
|
|
|
+ }
|
|
|
+
|
|
|
+ /**
|
|
|
+ * Returns a new indentity Matrix.
|
|
|
+ */
|
|
|
+ public static BabylonMatrix Identity()
|
|
|
+ {
|
|
|
+ var matrix = new BabylonMatrix();
|
|
|
+ BabylonMatrix.FromValuesToRef(
|
|
|
+ 1.0f, 0.0f, 0.0f, 0.0f,
|
|
|
+ 0.0f, 1.0f, 0.0f, 0.0f,
|
|
|
+ 0.0f, 0.0f, 1.0f, 0.0f,
|
|
|
+ 0.0f, 0.0f, 0.0f, 1.0f, matrix);
|
|
|
+ return matrix;
|
|
|
+ }
|
|
|
+
|
|
|
+ public static BabylonMatrix operator *(BabylonMatrix a, BabylonMatrix b)
|
|
|
+ {
|
|
|
+ return a.multiply(b);
|
|
|
+ }
|
|
|
+
|
|
|
+ /**
|
|
|
+ * Returns a new Matrix set with the multiplication result of the current Matrix and the passed one.
|
|
|
+ */
|
|
|
+ public BabylonMatrix multiply(BabylonMatrix other) {
|
|
|
+ var result = new BabylonMatrix();
|
|
|
+ this.multiplyToRef(other, result);
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+
|
|
|
+ /**
|
|
|
+ * Sets the passed matrix "result" with the multiplication result of the current Matrix and the passed one.
|
|
|
+ */
|
|
|
+ public BabylonMatrix multiplyToRef(BabylonMatrix other, BabylonMatrix result) {
|
|
|
+ this.multiplyToArray(other, result.m, 0);
|
|
|
+ return this;
|
|
|
+ }
|
|
|
+
|
|
|
+ /**
|
|
|
+ * Sets the Float32Array "result" from the passed index "offset" with the multiplication result of the current Matrix and the passed one.
|
|
|
+ */
|
|
|
+ public BabylonMatrix multiplyToArray(BabylonMatrix other, float[] result, int offset)
|
|
|
+ {
|
|
|
+ var tm0 = this.m[0];
|
|
|
+ var tm1 = this.m[1];
|
|
|
+ var tm2 = this.m[2];
|
|
|
+ var tm3 = this.m[3];
|
|
|
+ var tm4 = this.m[4];
|
|
|
+ var tm5 = this.m[5];
|
|
|
+ var tm6 = this.m[6];
|
|
|
+ var tm7 = this.m[7];
|
|
|
+ var tm8 = this.m[8];
|
|
|
+ var tm9 = this.m[9];
|
|
|
+ var tm10 = this.m[10];
|
|
|
+ var tm11 = this.m[11];
|
|
|
+ var tm12 = this.m[12];
|
|
|
+ var tm13 = this.m[13];
|
|
|
+ var tm14 = this.m[14];
|
|
|
+ var tm15 = this.m[15];
|
|
|
+
|
|
|
+ var om0 = other.m[0];
|
|
|
+ var om1 = other.m[1];
|
|
|
+ var om2 = other.m[2];
|
|
|
+ var om3 = other.m[3];
|
|
|
+ var om4 = other.m[4];
|
|
|
+ var om5 = other.m[5];
|
|
|
+ var om6 = other.m[6];
|
|
|
+ var om7 = other.m[7];
|
|
|
+ var om8 = other.m[8];
|
|
|
+ var om9 = other.m[9];
|
|
|
+ var om10 = other.m[10];
|
|
|
+ var om11 = other.m[11];
|
|
|
+ var om12 = other.m[12];
|
|
|
+ var om13 = other.m[13];
|
|
|
+ var om14 = other.m[14];
|
|
|
+ var om15 = other.m[15];
|
|
|
+
|
|
|
+ result[offset] = tm0* om0 + tm1* om4 + tm2* om8 + tm3* om12;
|
|
|
+ result[offset + 1] = tm0* om1 + tm1* om5 + tm2* om9 + tm3* om13;
|
|
|
+ result[offset + 2] = tm0* om2 + tm1* om6 + tm2* om10 + tm3* om14;
|
|
|
+ result[offset + 3] = tm0* om3 + tm1* om7 + tm2* om11 + tm3* om15;
|
|
|
+
|
|
|
+ result[offset + 4] = tm4* om0 + tm5* om4 + tm6* om8 + tm7* om12;
|
|
|
+ result[offset + 5] = tm4* om1 + tm5* om5 + tm6* om9 + tm7* om13;
|
|
|
+ result[offset + 6] = tm4* om2 + tm5* om6 + tm6* om10 + tm7* om14;
|
|
|
+ result[offset + 7] = tm4* om3 + tm5* om7 + tm6* om11 + tm7* om15;
|
|
|
+
|
|
|
+ result[offset + 8] = tm8* om0 + tm9* om4 + tm10* om8 + tm11* om12;
|
|
|
+ result[offset + 9] = tm8* om1 + tm9* om5 + tm10* om9 + tm11* om13;
|
|
|
+ result[offset + 10] = tm8* om2 + tm9* om6 + tm10* om10 + tm11* om14;
|
|
|
+ result[offset + 11] = tm8* om3 + tm9* om7 + tm10* om11 + tm11* om15;
|
|
|
+
|
|
|
+ result[offset + 12] = tm12* om0 + tm13* om4 + tm14* om8 + tm15* om12;
|
|
|
+ result[offset + 13] = tm12* om1 + tm13* om5 + tm14* om9 + tm15* om13;
|
|
|
+ result[offset + 14] = tm12* om2 + tm13* om6 + tm14* om10 + tm15* om14;
|
|
|
+ result[offset + 15] = tm12* om3 + tm13* om7 + tm14* om11 + tm15* om15;
|
|
|
+ return this;
|
|
|
+ }
|
|
|
+
|
|
|
+ /**
|
|
|
+ * Inserts the translation vector in the current Matrix.
|
|
|
+ * Returns the updated Matrix.
|
|
|
+ */
|
|
|
+ public BabylonMatrix setTranslation(BabylonVector3 vector3) {
|
|
|
+ this.m[12] = vector3.X;
|
|
|
+ this.m[13] = vector3.Y;
|
|
|
+ this.m[14] = vector3.Z;
|
|
|
+
|
|
|
+ return this;
|
|
|
+ }
|
|
|
+
|
|
|
+ /**
|
|
|
+ * Decomposes the current Matrix into :
|
|
|
+ * - a scale vector3 passed as a reference to update,
|
|
|
+ * - a rotation quaternion passed as a reference to update,
|
|
|
+ * - a translation vector3 passed as a reference to update.
|
|
|
+ * Returns the boolean `true`.
|
|
|
+ */
|
|
|
+ public bool decompose(BabylonVector3 scale, BabylonQuaternion rotation, BabylonVector3 translation)
|
|
|
+ {
|
|
|
+ translation.X = this.m[12];
|
|
|
+ translation.Y = this.m[13];
|
|
|
+ translation.Z = this.m[14];
|
|
|
+
|
|
|
+ scale.X = (float)Math.Sqrt(this.m[0] * this.m[0] + this.m[1] * this.m[1] + this.m[2] * this.m[2]);
|
|
|
+ scale.Y = (float)Math.Sqrt(this.m[4] * this.m[4] + this.m[5] * this.m[5] + this.m[6] * this.m[6]);
|
|
|
+ scale.Z = (float)Math.Sqrt(this.m[8] * this.m[8] + this.m[9] * this.m[9] + this.m[10] * this.m[10]);
|
|
|
+
|
|
|
+ if (this.determinant() <= 0) {
|
|
|
+ scale.Y *= -1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (scale.X == 0 || scale.Y == 0 || scale.Z == 0) {
|
|
|
+ rotation.X = 0;
|
|
|
+ rotation.Y = 0;
|
|
|
+ rotation.Z = 0;
|
|
|
+ rotation.W = 1;
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ var matrix = new BabylonMatrix();
|
|
|
+
|
|
|
+ BabylonMatrix.FromValuesToRef(
|
|
|
+ this.m[0] / scale.X, this.m[1] / scale.X, this.m[2] / scale.X, 0,
|
|
|
+ this.m[4] / scale.Y, this.m[5] / scale.Y, this.m[6] / scale.Y, 0,
|
|
|
+ this.m[8] / scale.Z, this.m[9] / scale.Z, this.m[10] / scale.Z, 0,
|
|
|
+ 0, 0, 0, 1, matrix);
|
|
|
+
|
|
|
+ BabylonQuaternion.FromRotationMatrixToRef(matrix, rotation);
|
|
|
+
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+
|
|
|
+ /**
|
|
|
+ * Returns the matrix determinant (float).
|
|
|
+ */
|
|
|
+ public float determinant()
|
|
|
+ {
|
|
|
+ var temp1 = (this.m[10] * this.m[15]) - (this.m[11] * this.m[14]);
|
|
|
+ var temp2 = (this.m[9] * this.m[15]) - (this.m[11] * this.m[13]);
|
|
|
+ var temp3 = (this.m[9] * this.m[14]) - (this.m[10] * this.m[13]);
|
|
|
+ var temp4 = (this.m[8] * this.m[15]) - (this.m[11] * this.m[12]);
|
|
|
+ var temp5 = (this.m[8] * this.m[14]) - (this.m[10] * this.m[12]);
|
|
|
+ var temp6 = (this.m[8] * this.m[13]) - (this.m[9] * this.m[12]);
|
|
|
+
|
|
|
+ return ((((this.m[0] * (((this.m[5] * temp1) - (this.m[6] * temp2)) + (this.m[7] * temp3))) - (this.m[1] * (((this.m[4] * temp1) -
|
|
|
+ (this.m[6] * temp4)) + (this.m[7] * temp5)))) + (this.m[2] * (((this.m[4] * temp2) - (this.m[5] * temp4)) + (this.m[7] * temp6)))) -
|
|
|
+ (this.m[3] * (((this.m[4] * temp3) - (this.m[5] * temp5)) + (this.m[6] * temp6))));
|
|
|
+ }
|
|
|
+
|
|
|
+ /**
|
|
|
+ * Sets the passed matrix "result" with the 16 passed floats.
|
|
|
+ */
|
|
|
+ public static void FromValuesToRef(float initialM11, float initialM12, float initialM13, float initialM14,
|
|
|
+ float initialM21, float initialM22, float initialM23, float initialM24,
|
|
|
+ float initialM31, float initialM32, float initialM33, float initialM34,
|
|
|
+ float initialM41, float initialM42, float initialM43, float initialM44, BabylonMatrix result)
|
|
|
+ {
|
|
|
+ result.m[0] = initialM11;
|
|
|
+ result.m[1] = initialM12;
|
|
|
+ result.m[2] = initialM13;
|
|
|
+ result.m[3] = initialM14;
|
|
|
+ result.m[4] = initialM21;
|
|
|
+ result.m[5] = initialM22;
|
|
|
+ result.m[6] = initialM23;
|
|
|
+ result.m[7] = initialM24;
|
|
|
+ result.m[8] = initialM31;
|
|
|
+ result.m[9] = initialM32;
|
|
|
+ result.m[10] = initialM33;
|
|
|
+ result.m[11] = initialM34;
|
|
|
+ result.m[12] = initialM41;
|
|
|
+ result.m[13] = initialM42;
|
|
|
+ result.m[14] = initialM43;
|
|
|
+ result.m[15] = initialM44;
|
|
|
+ }
|
|
|
+}
|
|
|
+}
|