public matrix4d inverse()
{
matrix4d ret = new matrix4d();
ret.m[0] = m[5] * m[10] * m[15] + m[6] * m[11] * m[13] + m[7] * m[9] * m[14] - m[5] * m[11] * m[14] - m[6] * m[9] * m[15] - m[7] * m[10] * m[13];
ret.m[1] = m[1] * m[11] * m[14] + m[2] * m[9] * m[15] + m[3] * m[10] * m[13] - m[1] * m[10] * m[15] - m[2] * m[11] * m[13] - m[3] * m[9] * m[14];
ret.m[2] = m[1] * m[6] * m[15] + m[2] * m[7] * m[13] + m[3] * m[5] * m[14] - m[1] * m[7] * m[14] - m[2] * m[5] * m[15] - m[3] * m[6] * m[13];
ret.m[3] = m[1] * m[7] * m[10] + m[2] * m[5] * m[11] + m[3] * m[6] * m[9] - m[1] * m[6] * m[11] - m[2] * m[7] * m[9] - m[3] * m[5] * m[10];
ret.m[4] = m[4] * m[11] * m[14] + m[6] * m[8] * m[15] + m[7] * m[10] * m[12] - m[4] * m[10] * m[15] - m[6] * m[11] * m[12] - m[7] * m[8] * m[14];
ret.m[5] = m[0] * m[10] * m[15] + m[2] * m[11] * m[12] + m[3] * m[8] * m[14] - m[0] * m[11] * m[14] - m[2] * m[8] * m[15] - m[3] * m[10] * m[12];
ret.m[6] = m[0] * m[7] * m[14] + m[2] * m[4] * m[15] + m[3] * m[6] * m[12] - m[0] * m[6] * m[15] - m[2] * m[7] * m[12] - m[3] * m[4] * m[14];
ret.m[7] = m[0] * m[6] * m[11] + m[2] * m[7] * m[8] + m[3] * m[4] * m[10] - m[0] * m[7] * m[10] - m[2] * m[4] * m[11] - m[3] * m[6] * m[8];
ret.m[8] = m[4] * m[9] * m[15] + m[5] * m[11] * m[12] + m[7] * m[8] * m[13] - m[4] * m[11] * m[13] - m[5] * m[8] * m[15] - m[7] * m[9] * m[12];
ret.m[9] = m[0] * m[11] * m[13] + m[1] * m[8] * m[15] + m[3] * m[9] * m[12] - m[0] * m[9] * m[15] - m[1] * m[11] * m[12] - m[3] * m[8] * m[13];
ret.m[10] = m[0] * m[5] * m[15] + m[1] * m[7] * m[12] + m[3] * m[4] * m[13] - m[0] * m[7] * m[13] - m[1] * m[4] * m[15] - m[3] * m[5] * m[12];
ret.m[11] = m[0] * m[7] * m[9] + m[1] * m[4] * m[11] + m[3] * m[5] * m[8] - m[0] * m[5] * m[11] - m[1] * m[7] * m[8] - m[3] * m[4] * m[9];
ret.m[12] = m[4] * m[10] * m[13] + m[5] * m[8] * m[14] + m[6] * m[9] * m[12] - m[4] * m[9] * m[14] - m[5] * m[10] * m[12] - m[6] * m[8] * m[13];
ret.m[13] = m[0] * m[9] * m[14] + m[1] * m[10] * m[12] + m[2] * m[8] * m[13] - m[0] * m[10] * m[13] - m[1] * m[8] * m[14] - m[2] * m[9] * m[12];
ret.m[14] = m[0] * m[6] * m[13] + m[1] * m[4] * m[14] + m[2] * m[5] * m[12] - m[0] * m[5] * m[14] - m[1] * m[6] * m[12] - m[2] * m[4] * m[13];
ret.m[15] = m[0] * m[5] * m[10] + m[1] * m[6] * m[8] + m[2] * m[4] * m[9] - m[0] * m[6] * m[9] - m[1] * m[4] * m[10] - m[2] * m[5] * m[8];
double det_inv = 1.0 / determinant();
for (int i = 0; i < 16; i++)
{
ret.m[i] *= det_inv;
}
return(ret);
}
public matrix4d transpose()
{
matrix4d ret = new matrix4d();
ret.m[0] = m[0];
ret.m[5] = m[5];
ret.m[10] = m[10];
ret.m[15] = m[15];
ret.m[1] = m[4];
ret.m[6] = m[9];
ret.m[11] = m[14];
ret.m[2] = m[8];
ret.m[7] = m[13];
ret.m[3] = m[12];
ret.m[4] = m[1];
ret.m[9] = m[6];
ret.m[14] = m[11];
ret.m[8] = m[2];
ret.m[13] = m[7];
ret.m[12] = m[3];
return(ret);
}
public static matrix4d rotaxis(ref vect3d axis, double rad)
{
double c = System.Math.Cos(rad);
double s = System.Math.Sin(rad);
double t = 1 - c;
double x = axis.x;
double y = axis.y;
double z = axis.z;
matrix4d ret = new matrix4d();
ret.m[0] = t * x * x + c;
ret.m[1] = t * x * y - s * z;
ret.m[2] = t * x * z + s * y;
ret.m[4] = t * x * y + s * z;
ret.m[5] = t * y * y + c;
ret.m[6] = t * y * z - s * x;
ret.m[8] = t * x * z - s * y;
ret.m[9] = t * y * z + s * x;
ret.m[10] = t * z * z + c;
return ret;
}
public static matrix4d rotaxis(ref vect3d axis, double rad)
{
double c = System.Math.Cos(rad);
double s = System.Math.Sin(rad);
double t = 1 - c;
double x = axis.x;
double y = axis.y;
double z = axis.z;
matrix4d ret = new matrix4d();
ret.m[0] = t * x * x + c;
ret.m[1] = t * x * y - s * z;
ret.m[2] = t * x * z + s * y;
ret.m[4] = t * x * y + s * z;
ret.m[5] = t * y * y + c;
ret.m[6] = t * y * z - s * x;
ret.m[8] = t * x * z - s * y;
ret.m[9] = t * y * z + s * x;
ret.m[10] = t * z * z + c;
return(ret);
}
public matrix4d mult(matrix4d mat)
{
matrix4d ret = new matrix4d();
ret.m[0] = mat.m[0] * m[0] + mat.m[1] * m[4] + mat.m[2] * m[8] + mat.m[3] * m[12];
ret.m[1] = mat.m[0] * m[1] + mat.m[1] * m[5] + mat.m[2] * m[9] + mat.m[3] * m[13];
ret.m[2] = mat.m[0] * m[2] + mat.m[1] * m[6] + mat.m[2] * m[10] + mat.m[3] * m[14];
ret.m[3] = mat.m[0] * m[3] + mat.m[1] * m[7] + mat.m[2] * m[11] + mat.m[3] * m[15];
ret.m[4] = mat.m[4] * m[0] + mat.m[5] * m[4] + mat.m[6] * m[8] + mat.m[7] * m[12];
ret.m[5] = mat.m[4] * m[1] + mat.m[5] * m[5] + mat.m[6] * m[9] + mat.m[7] * m[13];
ret.m[6] = mat.m[4] * m[2] + mat.m[5] * m[6] + mat.m[6] * m[10] + mat.m[7] * m[14];
ret.m[7] = mat.m[4] * m[3] + mat.m[5] * m[7] + mat.m[6] * m[11] + mat.m[7] * m[15];
ret.m[8] = mat.m[8] * m[0] + mat.m[9] * m[4] + mat.m[10] * m[8] + mat.m[11] * m[12];
ret.m[9] = mat.m[8] * m[1] + mat.m[9] * m[5] + mat.m[10] * m[9] + mat.m[11] * m[13];
ret.m[10] = mat.m[8] * m[2] + mat.m[9] * m[6] + mat.m[10] * m[10] + mat.m[11] * m[14];
ret.m[11] = mat.m[8] * m[3] + mat.m[9] * m[7] + mat.m[10] * m[11] + mat.m[11] * m[15];
ret.m[12] = mat.m[12] * m[0] + mat.m[13] * m[4] + mat.m[14] * m[8] + mat.m[15] * m[12];
ret.m[13] = mat.m[12] * m[1] + mat.m[13] * m[5] + mat.m[14] * m[9] + mat.m[15] * m[13];
ret.m[14] = mat.m[12] * m[2] + mat.m[13] * m[6] + mat.m[14] * m[10] + mat.m[15] * m[14];
ret.m[15] = mat.m[12] * m[3] + mat.m[13] * m[7] + mat.m[14] * m[11] + mat.m[15] * m[15];
return(ret);
}
public matrix4d getMatrix()
{
double wx, wy, wz, xx, xy, xz, yy, yz, zz;
wx = 2 * v.x * s;
wy = 2 * v.y * s;
wz = 2 * v.z * s;
xx = 2 * v.x * v.x;
xy = 2 * v.x * v.y;
xz = 2 * v.x * v.z;
yy = 2 * v.y * v.y;
yz = 2 * v.y * v.z;
zz = 2 * v.z * v.z;
matrix4d mat = new matrix4d();
mat.m[0 * 4 + 0] = 1.0 - (yy + zz);
mat.m[0 * 4 + 1] = xy - wz;
mat.m[0 * 4 + 2] = xz + wy;
mat.m[1 * 4 + 0] = xy + wz;
mat.m[1 * 4 + 1] = 1.0 - (xx + zz);
mat.m[1 * 4 + 2] = yz - wx;
mat.m[2 * 4 + 0] = xz - wy;
mat.m[2 * 4 + 1] = yz + wx;
mat.m[2 * 4 + 2] = 1.0 - (xx + yy);
return(mat);
}
public matrix4d(matrix4d a)
{
m = new double[16];
for (int i = 0; i < 16; i++)
{
m[i] = a.m[i];
}
}
matrix4d translate(ref vect3d t)
{
matrix4d ret = new matrix4d();
ret.m[3] = t.x;
ret.m[7] = t.y;
ret.m[11] = t.z;
return(ret);
}
public static matrix4d scale(double s)
{
matrix4d ret = new matrix4d();
ret.m[0] = s;
ret.m[5] = s;
ret.m[10] = s;
return ret;
}
public static matrix4d scale(ref vect3d s)
{
matrix4d ret = new matrix4d();
ret.m[0] = s.x;
ret.m[5] = s.y;
ret.m[10] = s.z;
return ret;
}
public static matrix4d scale(double x, double y, double z)
{
matrix4d ret = new matrix4d();
ret.m[0] = x;
ret.m[5] = y;
ret.m[10] = z;
return ret;
}
matrix4d translate(double x, double y, double z)
{
matrix4d ret = new matrix4d();
ret.m[3] = x;
ret.m[7] = y;
ret.m[11] = z;
return(ret);
}
public static matrix4d scale(ref vect3d s)
{
matrix4d ret = new matrix4d();
ret.m[0] = s.x;
ret.m[5] = s.y;
ret.m[10] = s.z;
return(ret);
}
public static matrix4d scale(double s)
{
matrix4d ret = new matrix4d();
ret.m[0] = s;
ret.m[5] = s;
ret.m[10] = s;
return(ret);
}
public static matrix4d scale(double x, double y, double z)
{
matrix4d ret = new matrix4d();
ret.m[0] = x;
ret.m[5] = y;
ret.m[10] = z;
return(ret);
}
public matrix4d transpose()
{
matrix4d ret = new matrix4d();
ret.m[0] = m[0];
ret.m[5] = m[5];
ret.m[10] = m[10];
ret.m[15] = m[15];
ret.m[1] = m[4];
ret.m[6] = m[9];
ret.m[11] = m[14];
ret.m[2] = m[8];
ret.m[7] = m[13];
ret.m[3] = m[12];
ret.m[4] = m[1];
ret.m[9] = m[6];
ret.m[14] = m[11];
ret.m[8] = m[2];
ret.m[13] = m[7];
ret.m[12] = m[3];
return ret;
}
matrix4d translate(double x, double y, double z)
{
matrix4d ret = new matrix4d();
ret.m[3] = x;
ret.m[7] = y;
ret.m[11] = z;
return ret;
}
matrix4d translate(ref vect3d t)
{
matrix4d ret = new matrix4d();
ret.m[3] = t.x;
ret.m[7] = t.y;
ret.m[11] = t.z;
return ret;
}
public matrix4d getMatrix()
{
double wx, wy, wz, xx, xy, xz, yy, yz, zz;
wx = 2 * v.x * s;
wy = 2 * v.y * s;
wz = 2 * v.z * s;
xx = 2 * v.x * v.x;
xy = 2 * v.x * v.y;
xz = 2 * v.x * v.z;
yy = 2 * v.y * v.y;
yz = 2 * v.y * v.z;
zz = 2 * v.z * v.z;
matrix4d mat = new matrix4d();
mat.m[0 * 4 + 0] = 1.0 - (yy + zz);
mat.m[0 * 4 + 1] = xy - wz;
mat.m[0 * 4 + 2] = xz + wy;
mat.m[1 * 4 + 0] = xy + wz;
mat.m[1 * 4 + 1] = 1.0 - (xx + zz);
mat.m[1 * 4 + 2] = yz - wx;
mat.m[2 * 4 + 0] = xz - wy;
mat.m[2 * 4 + 1] = yz + wx;
mat.m[2 * 4 + 2] = 1.0 - (xx + yy);
return mat;
}
public matrix4d(matrix4d a)
{
m = new double[16];
for (int i = 0; i < 16; i++)
m[i] = a.m[i];
}
public void setMatrix(matrix4d mat)
{
double tr, s;
int i = 0;
double max;
tr = mat.m[0 * 4 + 0] + mat.m[1 * 4 + 1] + mat.m[2 * 4 + 2];
max = tr;
if (mat.m[0 * 4 + 0] > max)
{
max = mat.m[0 * 4 + 0];
i = 1;
}
if (mat.m[1 * 4 + 1] > max)
{
max = mat.m[1 * 4 + 1];
i = 2;
}
if (mat.m[2 * 4 + 2] > max)
{
i = 3;
}
switch (i)
{
case 0:
s = 0.5 * System.Math.Sqrt(tr + 1);
s = 1.0 / (4.0 * s);
v.x = (mat.m[2 * 4 + 1] - mat.m[1 * 4 + 2]) * s;
v.y = (mat.m[0 * 4 + 2] - mat.m[2 * 4 + 0]) * s;
v.z = (mat.m[1 * 4 + 0] - mat.m[0 * 4 + 1]) * s;
break;
case 1:
v.x = .5 * System.Math.Sqrt(2 * mat.m[0 * 4 + 0] - tr + 1);
s = 1.0 / (4.0 * v.x);
s = (mat.m[2 * 4 + 1] - mat.m[1 * 4 + 2]) * s;
v.y = (mat.m[1 * 4 + 0] + mat.m[0 * 4 + 1]) * s;
v.z = (mat.m[0 * 4 + 2] + mat.m[2 * 4 + 0]) * s;
break;
case 2:
v.y = .5 * System.Math.Sqrt(2 * mat.m[1 * 4 + 1] - tr + 1);
s = 1.0 / (4.0 * v.y);
s = (mat.m[0 * 4 + 2] - mat.m[2 * 4 + 0]) * s;
v.x = (mat.m[1 * 4 + 0] + mat.m[0 * 4 + 1]) * s;
v.z = (mat.m[2 * 4 + 1] + mat.m[1 * 4 + 2]) * s;
break;
case 3:
v.z = .5 * System.Math.Sqrt(2 * mat.m[2 * 4 + 2] - tr + 1);
s = 1.0 / (4.0 * v.z);
s = (mat.m[1 * 4 + 0] - mat.m[0 * 4 + 1]) * s;
v.x = (mat.m[0 * 4 + 2] + mat.m[2 * 4 + 0]) * s;
v.y = (mat.m[2 * 4 + 1] + mat.m[1 * 4 + 2]) * s;
break;
}
}
public matrix4d mult(matrix4d mat)
{
matrix4d ret = new matrix4d();
ret.m[0] = mat.m[0] * m[0] + mat.m[1] * m[4] + mat.m[2] * m[8] + mat.m[3] * m[12];
ret.m[1] = mat.m[0] * m[1] + mat.m[1] * m[5] + mat.m[2] * m[9] + mat.m[3] * m[13];
ret.m[2] = mat.m[0] * m[2] + mat.m[1] * m[6] + mat.m[2] * m[10] + mat.m[3] * m[14];
ret.m[3] = mat.m[0] * m[3] + mat.m[1] * m[7] + mat.m[2] * m[11] + mat.m[3] * m[15];
ret.m[4] = mat.m[4] * m[0] + mat.m[5] * m[4] + mat.m[6] * m[8] + mat.m[7] * m[12];
ret.m[5] = mat.m[4] * m[1] + mat.m[5] * m[5] + mat.m[6] * m[9] + mat.m[7] * m[13];
ret.m[6] = mat.m[4] * m[2] + mat.m[5] * m[6] + mat.m[6] * m[10] + mat.m[7] * m[14];
ret.m[7] = mat.m[4] * m[3] + mat.m[5] * m[7] + mat.m[6] * m[11] + mat.m[7] * m[15];
ret.m[8] = mat.m[8] * m[0] + mat.m[9] * m[4] + mat.m[10] * m[8] + mat.m[11] * m[12];
ret.m[9] = mat.m[8] * m[1] + mat.m[9] * m[5] + mat.m[10] * m[9] + mat.m[11] * m[13];
ret.m[10] = mat.m[8] * m[2] + mat.m[9] * m[6] + mat.m[10] * m[10] + mat.m[11] * m[14];
ret.m[11] = mat.m[8] * m[3] + mat.m[9] * m[7] + mat.m[10] * m[11] + mat.m[11] * m[15];
ret.m[12] = mat.m[12] * m[0] + mat.m[13] * m[4] + mat.m[14] * m[8] + mat.m[15] * m[12];
ret.m[13] = mat.m[12] * m[1] + mat.m[13] * m[5] + mat.m[14] * m[9] + mat.m[15] * m[13];
ret.m[14] = mat.m[12] * m[2] + mat.m[13] * m[6] + mat.m[14] * m[10] + mat.m[15] * m[14];
ret.m[15] = mat.m[12] * m[3] + mat.m[13] * m[7] + mat.m[14] * m[11] + mat.m[15] * m[15];
return ret;
}
public void setMatrix(matrix4d mat)
{
double tr, s;
int i = 0;
double max;
tr = mat.m[0 * 4 + 0] + mat.m[1 * 4 + 1] + mat.m[2 * 4 + 2];
max = tr;
if (mat.m[0 * 4 + 0] > max)
{
max = mat.m[0 * 4 + 0];
i = 1;
}
if (mat.m[1 * 4 + 1] > max)
{
max = mat.m[1 * 4 + 1];
i = 2;
}
if (mat.m[2 * 4 + 2] > max)
{
i = 3;
}
switch (i)
{
case 0:
s = 0.5 * System.Math.Sqrt(tr + 1);
s = 1.0 / (4.0 * s);
v.x = (mat.m[2 * 4 + 1] - mat.m[1 * 4 + 2]) * s;
v.y = (mat.m[0 * 4 + 2] - mat.m[2 * 4 + 0]) * s;
v.z = (mat.m[1 * 4 + 0] - mat.m[0 * 4 + 1]) * s;
break;
case 1:
v.x = .5 * System.Math.Sqrt(2 * mat.m[0 * 4 + 0] - tr + 1);
s = 1.0 / (4.0 * v.x);
s = (mat.m[2 * 4 + 1] - mat.m[1 * 4 + 2]) * s;
v.y = (mat.m[1 * 4 + 0] + mat.m[0 * 4 + 1]) * s;
v.z = (mat.m[0 * 4 + 2] + mat.m[2 * 4 + 0]) * s;
break;
case 2:
v.y = .5 * System.Math.Sqrt(2 * mat.m[1 * 4 + 1] - tr + 1);
s = 1.0 / (4.0 * v.y);
s = (mat.m[0 * 4 + 2] - mat.m[2 * 4 + 0]) * s;
v.x = (mat.m[1 * 4 + 0] + mat.m[0 * 4 + 1]) * s;
v.z = (mat.m[2 * 4 + 1] + mat.m[1 * 4 + 2]) * s;
break;
case 3:
v.z = .5 * System.Math.Sqrt(2 * mat.m[2 * 4 + 2] - tr + 1);
s = 1.0 / (4.0 * v.z);
s = (mat.m[1 * 4 + 0] - mat.m[0 * 4 + 1]) * s;
v.x = (mat.m[0 * 4 + 2] + mat.m[2 * 4 + 0]) * s;
v.y = (mat.m[2 * 4 + 1] + mat.m[1 * 4 + 2]) * s;
break;
}
}
public matrix4d inverse()
{
matrix4d ret = new matrix4d();
ret.m[0] = m[5] * m[10] * m[15] + m[6] * m[11] * m[13] + m[7] * m[9] * m[14] - m[5] * m[11] * m[14] - m[6] * m[9] * m[15] - m[7] * m[10] * m[13];
ret.m[1] = m[1] * m[11] * m[14] + m[2] * m[9] * m[15] + m[3] * m[10] * m[13] - m[1] * m[10] * m[15] - m[2] * m[11] * m[13] - m[3] * m[9] * m[14];
ret.m[2] = m[1] * m[6] * m[15] + m[2] * m[7] * m[13] + m[3] * m[5] * m[14] - m[1] * m[7] * m[14] - m[2] * m[5] * m[15] - m[3] * m[6] * m[13];
ret.m[3] = m[1] * m[7] * m[10] + m[2] * m[5] * m[11] + m[3] * m[6] * m[9] - m[1] * m[6] * m[11] - m[2] * m[7] * m[9] - m[3] * m[5] * m[10];
ret.m[4] = m[4] * m[11] * m[14] + m[6] * m[8] * m[15] + m[7] * m[10] * m[12] - m[4] * m[10] * m[15] - m[6] * m[11] * m[12] - m[7] * m[8] * m[14];
ret.m[5] = m[0] * m[10] * m[15] + m[2] * m[11] * m[12] + m[3] * m[8] * m[14] - m[0] * m[11] * m[14] - m[2] * m[8] * m[15] - m[3] * m[10] * m[12];
ret.m[6] = m[0] * m[7] * m[14] + m[2] * m[4] * m[15] + m[3] * m[6] * m[12] - m[0] * m[6] * m[15] - m[2] * m[7] * m[12] - m[3] * m[4] * m[14];
ret.m[7] = m[0] * m[6] * m[11] + m[2] * m[7] * m[8] + m[3] * m[4] * m[10] - m[0] * m[7] * m[10] - m[2] * m[4] * m[11] - m[3] * m[6] * m[8];
ret.m[8] = m[4] * m[9] * m[15] + m[5] * m[11] * m[12] + m[7] * m[8] * m[13] - m[4] * m[11] * m[13] - m[5] * m[8] * m[15] - m[7] * m[9] * m[12];
ret.m[9] = m[0] * m[11] * m[13] + m[1] * m[8] * m[15] + m[3] * m[9] * m[12] - m[0] * m[9] * m[15] - m[1] * m[11] * m[12] - m[3] * m[8] * m[13];
ret.m[10] = m[0] * m[5] * m[15] + m[1] * m[7] * m[12] + m[3] * m[4] * m[13] - m[0] * m[7] * m[13] - m[1] * m[4] * m[15] - m[3] * m[5] * m[12];
ret.m[11] = m[0] * m[7] * m[9] + m[1] * m[4] * m[11] + m[3] * m[5] * m[8] - m[0] * m[5] * m[11] - m[1] * m[7] * m[8] - m[3] * m[4] * m[9];
ret.m[12] = m[4] * m[10] * m[13] + m[5] * m[8] * m[14] + m[6] * m[9] * m[12] - m[4] * m[9] * m[14] - m[5] * m[10] * m[12] - m[6] * m[8] * m[13];
ret.m[13] = m[0] * m[9] * m[14] + m[1] * m[10] * m[12] + m[2] * m[8] * m[13] - m[0] * m[10] * m[13] - m[1] * m[8] * m[14] - m[2] * m[9] * m[12];
ret.m[14] = m[0] * m[6] * m[13] + m[1] * m[4] * m[14] + m[2] * m[5] * m[12] - m[0] * m[5] * m[14] - m[1] * m[6] * m[12] - m[2] * m[4] * m[13];
ret.m[15] = m[0] * m[5] * m[10] + m[1] * m[6] * m[8] + m[2] * m[4] * m[9] - m[0] * m[6] * m[9] - m[1] * m[4] * m[10] - m[2] * m[5] * m[8];
double det_inv = 1.0 / determinant();
for (int i = 0; i < 16; i++)
ret.m[i] *= det_inv;
return ret;
}
