mirror of
https://github.com/tengge1/ShadowEditor.git
synced 2026-01-25 15:08:11 +00:00
827 lines
25 KiB
C#
827 lines
25 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.Threading.Tasks;
|
|
using _Math = System.Math;
|
|
|
|
namespace THREE
|
|
{
|
|
/// <summary>
|
|
/// @author mrdoob / http://mrdoob.com/
|
|
/// @author supereggbert / http://www.paulbrunt.co.uk/
|
|
/// @author philogb / http://blog.thejit.org/
|
|
/// @author jordi_ros / http://plattsoft.com
|
|
/// @author D1plo1d / http://github.com/D1plo1d
|
|
/// @author alteredq / http://alteredqualia.com/
|
|
/// @author mikael emtinger / http://gomo.se/
|
|
/// @author timknip / http://www.floorplanner.com/
|
|
/// @author bhouston / http://clara.io
|
|
/// @author WestLangley / http://github.com/WestLangley
|
|
/// @author tengge / https://github.com/tengge1
|
|
/// </summary>
|
|
public class Matrix4
|
|
{
|
|
public double[] elements;
|
|
|
|
public Matrix4()
|
|
{
|
|
this.elements = new double[] {
|
|
1, 0, 0, 0,
|
|
0, 1, 0, 0,
|
|
0, 0, 1, 0,
|
|
0, 0, 0, 1
|
|
};
|
|
}
|
|
|
|
public const bool isMatrix4 = true;
|
|
|
|
public Matrix4 Set(double n11, double n12, double n13, double n14, double n21, double n22, double n23, double n24, double n31, double n32, double n33, double n34, double n41, double n42, double n43, double n44)
|
|
{
|
|
var te = this.elements;
|
|
|
|
te[0] = n11; te[4] = n12; te[8] = n13; te[12] = n14;
|
|
te[1] = n21; te[5] = n22; te[9] = n23; te[13] = n24;
|
|
te[2] = n31; te[6] = n32; te[10] = n33; te[14] = n34;
|
|
te[3] = n41; te[7] = n42; te[11] = n43; te[15] = n44;
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 Identity()
|
|
{
|
|
this.Set(
|
|
1, 0, 0, 0,
|
|
0, 1, 0, 0,
|
|
0, 0, 1, 0,
|
|
0, 0, 0, 1
|
|
);
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 Clone()
|
|
{
|
|
return new Matrix4().FromArray(this.elements);
|
|
}
|
|
|
|
public Matrix4 Copy(Matrix4 m)
|
|
{
|
|
var te = this.elements;
|
|
var me = m.elements;
|
|
|
|
te[0] = me[0]; te[1] = me[1]; te[2] = me[2]; te[3] = me[3];
|
|
te[4] = me[4]; te[5] = me[5]; te[6] = me[6]; te[7] = me[7];
|
|
te[8] = me[8]; te[9] = me[9]; te[10] = me[10]; te[11] = me[11];
|
|
te[12] = me[12]; te[13] = me[13]; te[14] = me[14]; te[15] = me[15];
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 CopyPosition(Matrix4 m)
|
|
{
|
|
double[] te = this.elements, me = m.elements;
|
|
|
|
te[12] = me[12];
|
|
te[13] = me[13];
|
|
te[14] = me[14];
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 ExtractBasis(Vector3 xAxis, Vector3 yAxis, Vector3 zAxis)
|
|
{
|
|
xAxis.SetFromMatrixColumn(this, 0);
|
|
yAxis.SetFromMatrixColumn(this, 1);
|
|
zAxis.SetFromMatrixColumn(this, 2);
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 MakeBasis(Vector3 xAxis, Vector3 yAxis, Vector3 zAxis)
|
|
{
|
|
this.Set(
|
|
xAxis.x, yAxis.x, zAxis.x, 0,
|
|
xAxis.y, yAxis.y, zAxis.y, 0,
|
|
xAxis.z, yAxis.z, zAxis.z, 0,
|
|
0, 0, 0, 1
|
|
);
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 ExtractRotation(Matrix4 m)
|
|
{
|
|
var v1 = new Vector3();
|
|
|
|
// this method does not support reflection matrices
|
|
|
|
var te = this.elements;
|
|
var me = m.elements;
|
|
|
|
var scaleX = 1 / v1.SetFromMatrixColumn(m, 0).Length();
|
|
var scaleY = 1 / v1.SetFromMatrixColumn(m, 1).Length();
|
|
var scaleZ = 1 / v1.SetFromMatrixColumn(m, 2).Length();
|
|
|
|
te[0] = me[0] * scaleX;
|
|
te[1] = me[1] * scaleX;
|
|
te[2] = me[2] * scaleX;
|
|
te[3] = 0;
|
|
|
|
te[4] = me[4] * scaleY;
|
|
te[5] = me[5] * scaleY;
|
|
te[6] = me[6] * scaleY;
|
|
te[7] = 0;
|
|
|
|
te[8] = me[8] * scaleZ;
|
|
te[9] = me[9] * scaleZ;
|
|
te[10] = me[10] * scaleZ;
|
|
te[11] = 0;
|
|
|
|
te[12] = 0;
|
|
te[13] = 0;
|
|
te[14] = 0;
|
|
te[15] = 1;
|
|
|
|
return this;
|
|
|
|
}
|
|
|
|
public Matrix4 MakeRotationFromEuler(Euler euler)
|
|
{
|
|
var te = this.elements;
|
|
|
|
double x = euler._x, y = euler._y, z = euler._z;
|
|
double a = _Math.Cos(x), b = _Math.Sin(x);
|
|
double c = _Math.Cos(y), d = _Math.Sin(y);
|
|
double e = _Math.Cos(z), f = _Math.Sin(z);
|
|
|
|
if (euler._order == "XYZ")
|
|
{
|
|
double ae = a * e, af = a * f, be = b * e, bf = b * f;
|
|
|
|
te[0] = c * e;
|
|
te[4] = -c * f;
|
|
te[8] = d;
|
|
|
|
te[1] = af + be * d;
|
|
te[5] = ae - bf * d;
|
|
te[9] = -b * c;
|
|
|
|
te[2] = bf - ae * d;
|
|
te[6] = be + af * d;
|
|
te[10] = a * c;
|
|
}
|
|
else if (euler._order == "YXZ")
|
|
{
|
|
double ce = c * e, cf = c * f, de = d * e, df = d * f;
|
|
|
|
te[0] = ce + df * b;
|
|
te[4] = de * b - cf;
|
|
te[8] = a * d;
|
|
|
|
te[1] = a * f;
|
|
te[5] = a * e;
|
|
te[9] = -b;
|
|
|
|
te[2] = cf * b - de;
|
|
te[6] = df + ce * b;
|
|
te[10] = a * c;
|
|
}
|
|
else if (euler._order == "ZXY")
|
|
{
|
|
double ce = c * e, cf = c * f, de = d * e, df = d * f;
|
|
|
|
te[0] = ce - df * b;
|
|
te[4] = -a * f;
|
|
te[8] = de + cf * b;
|
|
|
|
te[1] = cf + de * b;
|
|
te[5] = a * e;
|
|
te[9] = df - ce * b;
|
|
|
|
te[2] = -a * d;
|
|
te[6] = b;
|
|
te[10] = a * c;
|
|
}
|
|
else if (euler._order == "ZYX")
|
|
{
|
|
double ae = a * e, af = a * f, be = b * e, bf = b * f;
|
|
|
|
te[0] = c * e;
|
|
te[4] = be * d - af;
|
|
te[8] = ae * d + bf;
|
|
|
|
te[1] = c * f;
|
|
te[5] = bf * d + ae;
|
|
te[9] = af * d - be;
|
|
|
|
te[2] = -d;
|
|
te[6] = b * c;
|
|
te[10] = a * c;
|
|
}
|
|
else if (euler._order == "YZX")
|
|
{
|
|
double ac = a * c, ad = a * d, bc = b * c, bd = b * d;
|
|
|
|
te[0] = c * e;
|
|
te[4] = bd - ac * f;
|
|
te[8] = bc * f + ad;
|
|
|
|
te[1] = f;
|
|
te[5] = a * e;
|
|
te[9] = -b * e;
|
|
|
|
te[2] = -d * e;
|
|
te[6] = ad * f + bc;
|
|
te[10] = ac - bd * f;
|
|
}
|
|
else if (euler._order == "XZY")
|
|
{
|
|
double ac = a * c, ad = a * d, bc = b * c, bd = b * d;
|
|
|
|
te[0] = c * e;
|
|
te[4] = -f;
|
|
te[8] = d * e;
|
|
|
|
te[1] = ac * f + bd;
|
|
te[5] = a * e;
|
|
te[9] = ad * f - bc;
|
|
|
|
te[2] = bc * f - ad;
|
|
te[6] = b * e;
|
|
te[10] = bd * f + ac;
|
|
}
|
|
|
|
// bottom row
|
|
te[3] = 0;
|
|
te[7] = 0;
|
|
te[11] = 0;
|
|
|
|
// last column
|
|
te[12] = 0;
|
|
te[13] = 0;
|
|
te[14] = 0;
|
|
te[15] = 1;
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 MakeRotationFromQuaternion(Quaternion q)
|
|
{
|
|
var zero = new Vector3(0, 0, 0);
|
|
var one = new Vector3(1, 1, 1);
|
|
|
|
return this.Compose(zero, q, one);
|
|
}
|
|
|
|
public Matrix4 LookAt(Vector3 eye, Vector3 target, Vector3 up)
|
|
{
|
|
var x = new Vector3();
|
|
var y = new Vector3();
|
|
var z = new Vector3();
|
|
|
|
var te = this.elements;
|
|
|
|
z.SubVectors(eye, target);
|
|
|
|
if (z.LengthSq() == 0)
|
|
{
|
|
// eye and target are in the same position
|
|
|
|
z.z = 1;
|
|
}
|
|
|
|
z.Normalize();
|
|
x.CrossVectors(up, z);
|
|
|
|
if (x.LengthSq() == 0)
|
|
{
|
|
|
|
// up and z are parallel
|
|
|
|
if (_Math.Abs(up.z) == 1)
|
|
{
|
|
z.x += 0.0001;
|
|
}
|
|
else
|
|
{
|
|
z.z += 0.0001;
|
|
}
|
|
|
|
z.Normalize();
|
|
x.CrossVectors(up, z);
|
|
}
|
|
|
|
x.Normalize();
|
|
y.CrossVectors(z, x);
|
|
|
|
te[0] = x.x; te[4] = y.x; te[8] = z.x;
|
|
te[1] = x.y; te[5] = y.y; te[9] = z.y;
|
|
te[2] = x.z; te[6] = y.z; te[10] = z.z;
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 Multiply(Matrix4 m, Matrix4 n = null)
|
|
{
|
|
if (n != null)
|
|
{
|
|
Console.WriteLine("THREE.Matrix4: .multiply() now only accepts one argument. Use .multiplyMatrices( a, b ) instead.");
|
|
return this.MultiplyMatrices(m, n);
|
|
}
|
|
|
|
return this.MultiplyMatrices(this, m);
|
|
}
|
|
|
|
public Matrix4 Premultiply(Matrix4 m)
|
|
{
|
|
return this.MultiplyMatrices(m, this);
|
|
}
|
|
|
|
public Matrix4 MultiplyMatrices(Matrix4 a, Matrix4 b)
|
|
{
|
|
var ae = a.elements;
|
|
var be = b.elements;
|
|
var te = this.elements;
|
|
|
|
double a11 = ae[0], a12 = ae[4], a13 = ae[8], a14 = ae[12];
|
|
double a21 = ae[1], a22 = ae[5], a23 = ae[9], a24 = ae[13];
|
|
double a31 = ae[2], a32 = ae[6], a33 = ae[10], a34 = ae[14];
|
|
double a41 = ae[3], a42 = ae[7], a43 = ae[11], a44 = ae[15];
|
|
|
|
double b11 = be[0], b12 = be[4], b13 = be[8], b14 = be[12];
|
|
double b21 = be[1], b22 = be[5], b23 = be[9], b24 = be[13];
|
|
double b31 = be[2], b32 = be[6], b33 = be[10], b34 = be[14];
|
|
double b41 = be[3], b42 = be[7], b43 = be[11], b44 = be[15];
|
|
|
|
te[0] = a11 * b11 + a12 * b21 + a13 * b31 + a14 * b41;
|
|
te[4] = a11 * b12 + a12 * b22 + a13 * b32 + a14 * b42;
|
|
te[8] = a11 * b13 + a12 * b23 + a13 * b33 + a14 * b43;
|
|
te[12] = a11 * b14 + a12 * b24 + a13 * b34 + a14 * b44;
|
|
|
|
te[1] = a21 * b11 + a22 * b21 + a23 * b31 + a24 * b41;
|
|
te[5] = a21 * b12 + a22 * b22 + a23 * b32 + a24 * b42;
|
|
te[9] = a21 * b13 + a22 * b23 + a23 * b33 + a24 * b43;
|
|
te[13] = a21 * b14 + a22 * b24 + a23 * b34 + a24 * b44;
|
|
|
|
te[2] = a31 * b11 + a32 * b21 + a33 * b31 + a34 * b41;
|
|
te[6] = a31 * b12 + a32 * b22 + a33 * b32 + a34 * b42;
|
|
te[10] = a31 * b13 + a32 * b23 + a33 * b33 + a34 * b43;
|
|
te[14] = a31 * b14 + a32 * b24 + a33 * b34 + a34 * b44;
|
|
|
|
te[3] = a41 * b11 + a42 * b21 + a43 * b31 + a44 * b41;
|
|
te[7] = a41 * b12 + a42 * b22 + a43 * b32 + a44 * b42;
|
|
te[11] = a41 * b13 + a42 * b23 + a43 * b33 + a44 * b43;
|
|
te[15] = a41 * b14 + a42 * b24 + a43 * b34 + a44 * b44;
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 MultiplyScalar(double s)
|
|
{
|
|
var te = this.elements;
|
|
|
|
te[0] *= s; te[4] *= s; te[8] *= s; te[12] *= s;
|
|
te[1] *= s; te[5] *= s; te[9] *= s; te[13] *= s;
|
|
te[2] *= s; te[6] *= s; te[10] *= s; te[14] *= s;
|
|
te[3] *= s; te[7] *= s; te[11] *= s; te[15] *= s;
|
|
|
|
return this;
|
|
}
|
|
|
|
public double Determinant()
|
|
{
|
|
var te = this.elements;
|
|
|
|
double n11 = te[0], n12 = te[4], n13 = te[8], n14 = te[12];
|
|
double n21 = te[1], n22 = te[5], n23 = te[9], n24 = te[13];
|
|
double n31 = te[2], n32 = te[6], n33 = te[10], n34 = te[14];
|
|
double n41 = te[3], n42 = te[7], n43 = te[11], n44 = te[15];
|
|
|
|
//TODO: make this more efficient
|
|
//( based on http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/fourD/index.htm )
|
|
|
|
return (
|
|
n41 * (
|
|
+n14 * n23 * n32
|
|
- n13 * n24 * n32
|
|
- n14 * n22 * n33
|
|
+ n12 * n24 * n33
|
|
+ n13 * n22 * n34
|
|
- n12 * n23 * n34
|
|
) +
|
|
n42 * (
|
|
+n11 * n23 * n34
|
|
- n11 * n24 * n33
|
|
+ n14 * n21 * n33
|
|
- n13 * n21 * n34
|
|
+ n13 * n24 * n31
|
|
- n14 * n23 * n31
|
|
) +
|
|
n43 * (
|
|
+n11 * n24 * n32
|
|
- n11 * n22 * n34
|
|
- n14 * n21 * n32
|
|
+ n12 * n21 * n34
|
|
+ n14 * n22 * n31
|
|
- n12 * n24 * n31
|
|
) +
|
|
n44 * (
|
|
-n13 * n22 * n31
|
|
- n11 * n23 * n32
|
|
+ n11 * n22 * n33
|
|
+ n13 * n21 * n32
|
|
- n12 * n21 * n33
|
|
+ n12 * n23 * n31
|
|
)
|
|
|
|
);
|
|
|
|
}
|
|
|
|
public Matrix4 Transpose()
|
|
{
|
|
var te = this.elements;
|
|
double tmp;
|
|
|
|
tmp = te[1]; te[1] = te[4]; te[4] = tmp;
|
|
tmp = te[2]; te[2] = te[8]; te[8] = tmp;
|
|
tmp = te[6]; te[6] = te[9]; te[9] = tmp;
|
|
|
|
tmp = te[3]; te[3] = te[12]; te[12] = tmp;
|
|
tmp = te[7]; te[7] = te[13]; te[13] = tmp;
|
|
tmp = te[11]; te[11] = te[14]; te[14] = tmp;
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 SetPosition(Vector3 v)
|
|
{
|
|
var te = this.elements;
|
|
|
|
te[12] = v.x;
|
|
te[13] = v.y;
|
|
te[14] = v.z;
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 GetInverse(Matrix4 m, bool throwOnDegenerate = false)
|
|
{
|
|
// based on http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/fourD/index.htm
|
|
double[] te = this.elements,
|
|
me = m.elements;
|
|
|
|
double n11 = me[0], n21 = me[1], n31 = me[2], n41 = me[3],
|
|
n12 = me[4], n22 = me[5], n32 = me[6], n42 = me[7],
|
|
n13 = me[8], n23 = me[9], n33 = me[10], n43 = me[11],
|
|
n14 = me[12], n24 = me[13], n34 = me[14], n44 = me[15],
|
|
|
|
t11 = n23 * n34 * n42 - n24 * n33 * n42 + n24 * n32 * n43 - n22 * n34 * n43 - n23 * n32 * n44 + n22 * n33 * n44,
|
|
t12 = n14 * n33 * n42 - n13 * n34 * n42 - n14 * n32 * n43 + n12 * n34 * n43 + n13 * n32 * n44 - n12 * n33 * n44,
|
|
t13 = n13 * n24 * n42 - n14 * n23 * n42 + n14 * n22 * n43 - n12 * n24 * n43 - n13 * n22 * n44 + n12 * n23 * n44,
|
|
t14 = n14 * n23 * n32 - n13 * n24 * n32 - n14 * n22 * n33 + n12 * n24 * n33 + n13 * n22 * n34 - n12 * n23 * n34;
|
|
|
|
var det = n11 * t11 + n21 * t12 + n31 * t13 + n41 * t14;
|
|
|
|
if (det == 0)
|
|
{
|
|
var msg = "THREE.Matrix4: .getInverse() can't invert matrix, determinant is 0";
|
|
|
|
if (throwOnDegenerate)
|
|
{
|
|
throw new Exception(msg);
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine(msg);
|
|
}
|
|
|
|
return this.Identity();
|
|
}
|
|
|
|
var detInv = 1 / det;
|
|
|
|
te[0] = t11 * detInv;
|
|
te[1] = (n24 * n33 * n41 - n23 * n34 * n41 - n24 * n31 * n43 + n21 * n34 * n43 + n23 * n31 * n44 - n21 * n33 * n44) * detInv;
|
|
te[2] = (n22 * n34 * n41 - n24 * n32 * n41 + n24 * n31 * n42 - n21 * n34 * n42 - n22 * n31 * n44 + n21 * n32 * n44) * detInv;
|
|
te[3] = (n23 * n32 * n41 - n22 * n33 * n41 - n23 * n31 * n42 + n21 * n33 * n42 + n22 * n31 * n43 - n21 * n32 * n43) * detInv;
|
|
|
|
te[4] = t12 * detInv;
|
|
te[5] = (n13 * n34 * n41 - n14 * n33 * n41 + n14 * n31 * n43 - n11 * n34 * n43 - n13 * n31 * n44 + n11 * n33 * n44) * detInv;
|
|
te[6] = (n14 * n32 * n41 - n12 * n34 * n41 - n14 * n31 * n42 + n11 * n34 * n42 + n12 * n31 * n44 - n11 * n32 * n44) * detInv;
|
|
te[7] = (n12 * n33 * n41 - n13 * n32 * n41 + n13 * n31 * n42 - n11 * n33 * n42 - n12 * n31 * n43 + n11 * n32 * n43) * detInv;
|
|
|
|
te[8] = t13 * detInv;
|
|
te[9] = (n14 * n23 * n41 - n13 * n24 * n41 - n14 * n21 * n43 + n11 * n24 * n43 + n13 * n21 * n44 - n11 * n23 * n44) * detInv;
|
|
te[10] = (n12 * n24 * n41 - n14 * n22 * n41 + n14 * n21 * n42 - n11 * n24 * n42 - n12 * n21 * n44 + n11 * n22 * n44) * detInv;
|
|
te[11] = (n13 * n22 * n41 - n12 * n23 * n41 - n13 * n21 * n42 + n11 * n23 * n42 + n12 * n21 * n43 - n11 * n22 * n43) * detInv;
|
|
|
|
te[12] = t14 * detInv;
|
|
te[13] = (n13 * n24 * n31 - n14 * n23 * n31 + n14 * n21 * n33 - n11 * n24 * n33 - n13 * n21 * n34 + n11 * n23 * n34) * detInv;
|
|
te[14] = (n14 * n22 * n31 - n12 * n24 * n31 - n14 * n21 * n32 + n11 * n24 * n32 + n12 * n21 * n34 - n11 * n22 * n34) * detInv;
|
|
te[15] = (n12 * n23 * n31 - n13 * n22 * n31 + n13 * n21 * n32 - n11 * n23 * n32 - n12 * n21 * n33 + n11 * n22 * n33) * detInv;
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 Scale(Vector3 v)
|
|
{
|
|
var te = this.elements;
|
|
double x = v.x, y = v.y, z = v.z;
|
|
|
|
te[0] *= x; te[4] *= y; te[8] *= z;
|
|
te[1] *= x; te[5] *= y; te[9] *= z;
|
|
te[2] *= x; te[6] *= y; te[10] *= z;
|
|
te[3] *= x; te[7] *= y; te[11] *= z;
|
|
|
|
return this;
|
|
}
|
|
|
|
public double GetMaxScaleOnAxis()
|
|
{
|
|
var te = this.elements;
|
|
|
|
var scaleXSq = te[0] * te[0] + te[1] * te[1] + te[2] * te[2];
|
|
var scaleYSq = te[4] * te[4] + te[5] * te[5] + te[6] * te[6];
|
|
var scaleZSq = te[8] * te[8] + te[9] * te[9] + te[10] * te[10];
|
|
|
|
return _Math.Sqrt(_Math.Max(_Math.Max(scaleXSq, scaleYSq), scaleZSq));
|
|
}
|
|
|
|
public Matrix4 MakeTranslation(double x, double y, double z)
|
|
{
|
|
this.Set(
|
|
1, 0, 0, x,
|
|
0, 1, 0, y,
|
|
0, 0, 1, z,
|
|
0, 0, 0, 1
|
|
|
|
);
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 MakeRotationX(double theta)
|
|
{
|
|
double c = _Math.Cos(theta), s = _Math.Sin(theta);
|
|
|
|
this.Set(
|
|
1, 0, 0, 0,
|
|
0, c, -s, 0,
|
|
0, s, c, 0,
|
|
0, 0, 0, 1
|
|
);
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 MakeRotationY(double theta)
|
|
{
|
|
double c = _Math.Cos(theta), s = _Math.Sin(theta);
|
|
|
|
this.Set(
|
|
c, 0, s, 0,
|
|
0, 1, 0, 0,
|
|
-s, 0, c, 0,
|
|
0, 0, 0, 1
|
|
);
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 MakeRotationZ(double theta)
|
|
{
|
|
double c = _Math.Cos(theta), s = _Math.Sin(theta);
|
|
|
|
this.Set(
|
|
c, -s, 0, 0,
|
|
s, c, 0, 0,
|
|
0, 0, 1, 0,
|
|
0, 0, 0, 1
|
|
);
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 MakeRotationAxis(Vector3 axis, double angle)
|
|
{
|
|
// Based on http://www.gamedev.net/reference/articles/article1199.asp
|
|
|
|
var c = _Math.Cos(angle);
|
|
var s = _Math.Sin(angle);
|
|
var t = 1 - c;
|
|
double x = axis.x, y = axis.y, z = axis.z;
|
|
double tx = t * x, ty = t * y;
|
|
|
|
this.Set(
|
|
tx * x + c, tx * y - s * z, tx * z + s * y, 0,
|
|
tx * y + s * z, ty * y + c, ty * z - s * x, 0,
|
|
tx * z - s * y, ty * z + s * x, t * z * z + c, 0,
|
|
0, 0, 0, 1
|
|
);
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 MakeScale(double x, double y, double z)
|
|
{
|
|
this.Set(
|
|
|
|
x, 0, 0, 0,
|
|
0, y, 0, 0,
|
|
0, 0, z, 0,
|
|
0, 0, 0, 1
|
|
|
|
);
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 MakeShear(double x, double y, double z)
|
|
{
|
|
this.Set(
|
|
1, y, z, 0,
|
|
x, 1, z, 0,
|
|
x, y, 1, 0,
|
|
0, 0, 0, 1
|
|
);
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 Compose(Vector3 position, Quaternion quaternion, Vector3 scale)
|
|
{
|
|
var te = this.elements;
|
|
|
|
double x = quaternion._x, y = quaternion._y, z = quaternion._z, w = quaternion._w;
|
|
double x2 = x + x, y2 = y + y, z2 = z + z;
|
|
double xx = x * x2, xy = x * y2, xz = x * z2;
|
|
double yy = y * y2, yz = y * z2, zz = z * z2;
|
|
double wx = w * x2, wy = w * y2, wz = w * z2;
|
|
|
|
double sx = scale.x, sy = scale.y, sz = scale.z;
|
|
|
|
te[0] = (1 - (yy + zz)) * sx;
|
|
te[1] = (xy + wz) * sx;
|
|
te[2] = (xz - wy) * sx;
|
|
te[3] = 0;
|
|
|
|
te[4] = (xy - wz) * sy;
|
|
te[5] = (1 - (xx + zz)) * sy;
|
|
te[6] = (yz + wx) * sy;
|
|
te[7] = 0;
|
|
|
|
te[8] = (xz + wy) * sz;
|
|
te[9] = (yz - wx) * sz;
|
|
te[10] = (1 - (xx + yy)) * sz;
|
|
te[11] = 0;
|
|
|
|
te[12] = position.x;
|
|
te[13] = position.y;
|
|
te[14] = position.z;
|
|
te[15] = 1;
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 Decompose(Vector3 position, Quaternion quaternion, Vector3 scale)
|
|
{
|
|
var vector = new Vector3();
|
|
var matrix = new Matrix4();
|
|
|
|
var te = this.elements;
|
|
|
|
var sx = vector.Set(te[0], te[1], te[2]).Length();
|
|
var sy = vector.Set(te[4], te[5], te[6]).Length();
|
|
var sz = vector.Set(te[8], te[9], te[10]).Length();
|
|
|
|
// if determine is negative, we need to invert one scale
|
|
var det = this.Determinant();
|
|
if (det < 0) sx = -sx;
|
|
|
|
position.x = te[12];
|
|
position.y = te[13];
|
|
position.z = te[14];
|
|
|
|
// scale the rotation part
|
|
matrix.Copy(this);
|
|
|
|
var invSX = 1 / sx;
|
|
var invSY = 1 / sy;
|
|
var invSZ = 1 / sz;
|
|
|
|
matrix.elements[0] *= invSX;
|
|
matrix.elements[1] *= invSX;
|
|
matrix.elements[2] *= invSX;
|
|
|
|
matrix.elements[4] *= invSY;
|
|
matrix.elements[5] *= invSY;
|
|
matrix.elements[6] *= invSY;
|
|
|
|
matrix.elements[8] *= invSZ;
|
|
matrix.elements[9] *= invSZ;
|
|
matrix.elements[10] *= invSZ;
|
|
|
|
quaternion.SetFromRotationMatrix(matrix);
|
|
|
|
scale.x = sx;
|
|
scale.y = sy;
|
|
scale.z = sz;
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 MakePerspective(double left, double right, double top, double bottom, double near, double far)
|
|
{
|
|
var te = this.elements;
|
|
var x = 2 * near / (right - left);
|
|
var y = 2 * near / (top - bottom);
|
|
|
|
var a = (right + left) / (right - left);
|
|
var b = (top + bottom) / (top - bottom);
|
|
var c = -(far + near) / (far - near);
|
|
var d = -2 * far * near / (far - near);
|
|
|
|
te[0] = x; te[4] = 0; te[8] = a; te[12] = 0;
|
|
te[1] = 0; te[5] = y; te[9] = b; te[13] = 0;
|
|
te[2] = 0; te[6] = 0; te[10] = c; te[14] = d;
|
|
te[3] = 0; te[7] = 0; te[11] = -1; te[15] = 0;
|
|
|
|
return this;
|
|
}
|
|
|
|
public Matrix4 MakeOrthographic(double left, double right, double top, double bottom, double near, double far)
|
|
{
|
|
var te = this.elements;
|
|
var w = 1.0 / (right - left);
|
|
var h = 1.0 / (top - bottom);
|
|
var p = 1.0 / (far - near);
|
|
|
|
var x = (right + left) * w;
|
|
var y = (top + bottom) * h;
|
|
var z = (far + near) * p;
|
|
|
|
te[0] = 2 * w; te[4] = 0; te[8] = 0; te[12] = -x;
|
|
te[1] = 0; te[5] = 2 * h; te[9] = 0; te[13] = -y;
|
|
te[2] = 0; te[6] = 0; te[10] = -2 * p; te[14] = -z;
|
|
te[3] = 0; te[7] = 0; te[11] = 0; te[15] = 1;
|
|
|
|
return this;
|
|
}
|
|
|
|
public bool Equals(Matrix4 matrix)
|
|
{
|
|
var te = this.elements;
|
|
var me = matrix.elements;
|
|
|
|
for (var i = 0; i < 16; i++)
|
|
{
|
|
if (te[i] != me[i]) return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
public Matrix4 FromArray(double[] array, int offset = 0)
|
|
{
|
|
for (var i = 0; i < 16; i++)
|
|
{
|
|
this.elements[i] = array[i + offset];
|
|
}
|
|
|
|
return this;
|
|
}
|
|
|
|
public double[] ToArray(double[] array = null, int offset = 0)
|
|
{
|
|
if (array == null) array = new double[16];
|
|
|
|
var te = this.elements;
|
|
|
|
array[offset] = te[0];
|
|
array[offset + 1] = te[1];
|
|
array[offset + 2] = te[2];
|
|
array[offset + 3] = te[3];
|
|
|
|
array[offset + 4] = te[4];
|
|
array[offset + 5] = te[5];
|
|
array[offset + 6] = te[6];
|
|
array[offset + 7] = te[7];
|
|
|
|
array[offset + 8] = te[8];
|
|
array[offset + 9] = te[9];
|
|
array[offset + 10] = te[10];
|
|
array[offset + 11] = te[11];
|
|
|
|
array[offset + 12] = te[12];
|
|
array[offset + 13] = te[13];
|
|
array[offset + 14] = te[14];
|
|
array[offset + 15] = te[15];
|
|
|
|
return array;
|
|
}
|
|
}
|
|
}
|