Files
EnderIO/common/crazypants/vecmath/Matrix4d.java
T

524 lines
12 KiB
Java

package crazypants.vecmath;
public class Matrix4d {
public double m00 = 0;
public double m01 = 0;
public double m02 = 0;
public double m03 = 0;
public double m10 = 0;
public double m11 = 0;
public double m12 = 0;
public double m13 = 0;
public double m20 = 0;
public double m21 = 0;
public double m22 = 0;
public double m23 = 0;
public double m30 = 0;
public double m31 = 0;
public double m32 = 0;
public double m33 = 0;
public Matrix4d() {
}
public Matrix4d(Matrix4d other) {
m00 = other.m00;
m01 = other.m01;
m02 = other.m02;
m03 = other.m03;
m10 = other.m10;
m11 = other.m11;
m12 = other.m12;
m13 = other.m13;
m20 = other.m20;
m21 = other.m21;
m22 = other.m22;
m23 = other.m23;
m30 = other.m30;
m31 = other.m31;
m32 = other.m32;
m33 = other.m33;
}
public Matrix4d(double m00, double m01, double m02, double m03,
double m10, double m11, double m12, double m13,
double m20, double m21, double m22, double m23,
double m30, double m31, double m32, double m33) {
this.m00 = m00;
this.m01 = m01;
this.m02 = m02;
this.m03 = m03;
this.m10 = m10;
this.m11 = m11;
this.m12 = m12;
this.m13 = m13;
this.m20 = m20;
this.m21 = m21;
this.m22 = m22;
this.m23 = m23;
this.m30 = m30;
this.m31 = m31;
this.m32 = m32;
this.m33 = m33;
}
public final void setIdentity() {
m00 = 1.0;
m01 = 0.0;
m02 = 0.0;
m03 = 0.0;
m10 = 0.0;
m11 = 1.0;
m12 = 0.0;
m13 = 0.0;
m20 = 0.0;
m21 = 0.0;
m22 = 1.0;
m23 = 0.0;
m30 = 0.0;
m31 = 0.0;
m32 = 0.0;
m33 = 1.0;
}
public void set(Matrix4d other) {
m00 = other.m00;
m01 = other.m01;
m02 = other.m02;
m03 = other.m03;
m10 = other.m10;
m11 = other.m11;
m12 = other.m12;
m13 = other.m13;
m20 = other.m20;
m21 = other.m21;
m22 = other.m22;
m23 = other.m23;
m30 = other.m30;
m31 = other.m31;
m32 = other.m32;
m33 = other.m33;
}
public void set(double[] values) {
m00 = values[0];
m01 = values[1];
m02 = values[2];
m03 = values[3];
m10 = values[4];
m11 = values[5];
m12 = values[6];
m13 = values[7];
m20 = values[8];
m21 = values[9];
m22 = values[10];
m23 = values[11];
m30 = values[12];
m31 = values[13];
m32 = values[14];
m33 = values[15];
}
public double getElement(int row, int col) {
switch (row) {
case 0:
switch (col) {
case 0:
return m00;
case 1:
return m01;
case 2:
return m02;
case 3:
return m03;
default:
break;
}
break;
case 1:
switch (col) {
case 0:
return m10;
case 1:
return m11;
case 2:
return m12;
case 3:
return m13;
default:
break;
}
break;
case 2:
switch (col) {
case 0:
return m20;
case 1:
return m21;
case 2:
return m22;
case 3:
return m23;
default:
break;
}
break;
case 3:
switch (col) {
case 0:
return m30;
case 1:
return m31;
case 2:
return m32;
case 3:
return m33;
default:
break;
}
break;
default:
break;
}
return 0;
}
public void setElement(int row, int column, double val) {
switch (row) {
case 0:
switch (column) {
case 0:
m00 = val;
break;
case 1:
m01 = val;
break;
case 2:
m02 = val;
break;
case 3:
m03 = val;
break;
default:
break;
}
break;
case 1:
switch (column) {
case 0:
m10 = val;
break;
case 1:
m11 = val;
break;
case 2:
m12 = val;
break;
case 3:
m13 = val;
break;
default:
break;
}
break;
case 2:
switch (column) {
case 0:
m20 = val;
break;
case 1:
m21 = val;
break;
case 2:
m22 = val;
break;
case 3:
m23 = val;
break;
default:
break;
}
break;
case 3:
switch (column) {
case 0:
m30 = val;
break;
case 1:
m31 = val;
break;
case 2:
m32 = val;
break;
case 3:
m33 = val;
break;
default:
break;
}
break;
default:
break;
}
}
public void getTranslation(Vector3d trans) {
trans.x = m03;
trans.y = m13;
trans.z = m23;
}
public void setTranslation(Vector3d trans) {
m03 = trans.x;
m13 = trans.y;
m23 = trans.z;
}
public void makeRotationX(double angle) {
setIdentity();
double sin = Math.sin(angle);
double cos = Math.cos(angle);
m11 = cos;
m12 = -sin;
m21 = sin;
m22 = cos;
}
public void makeRotationY(double angle) {
setIdentity();
double sin = Math.sin(angle);
double cos = Math.cos(angle);
m00 = cos;
m02 = sin;
m20 = -sin;
m22 = cos;
}
public void makeRotationZ(double angle) {
setIdentity();
double sin = Math.sin(angle);
double cos = Math.cos(angle);
m00 = cos;
m01 = -sin;
m10 = sin;
m11 = cos;
}
public void transform(Vector3d vec) {
double x = m00 * vec.x + m01 * vec.y + m02 * vec.z + m03;
double y = m10 * vec.x + m11 * vec.y + m12 * vec.z + m13;
vec.z = m20 * vec.x + m21 * vec.y + m22 * vec.z + m23;
vec.x = x;
vec.y = y;
}
public void transform(Vector4d vec) {
double x = m00 * vec.x + m01 * vec.y + m02 * vec.z + m03 * vec.w;
double y = m10 * vec.x + m11 * vec.y + m12 * vec.z + m13 * vec.w;
double z = m20 * vec.x + m21 * vec.y + m22 * vec.z + m23 * vec.w;
vec.w = m30 * vec.x + m31 * vec.y + m32 * vec.z + m33 * vec.w;
vec.x = x;
vec.y = y;
vec.z = z;
}
public void transformNormal(Vector3f normal) {
double x = m00 * normal.x + m01 * normal.y + m02 * normal.z;
double y = m10 * normal.x + m11 * normal.y + m12 * normal.z;
normal.z = (float) (m20 * normal.x + m21 * normal.y + m22 * normal.z);
normal.x = (float) x;
normal.y = (float) y;
}
public void transformNormal(Vector3d normal) {
double x = m00 * normal.x + m01 * normal.y + m02 * normal.z;
double y = m10 * normal.x + m11 * normal.y + m12 * normal.z;
normal.z = m20 * normal.x + m21 * normal.y + m22 * normal.z;
normal.x = x;
normal.y = y;
}
public void mul(Matrix4d m1, Matrix4d m2) {
set(m1);
mul(m2);
}
public void mul(Matrix4d m) {
double m00Tmp = m00 * m.m00 + m01 * m.m10 + m02 * m.m20 + m03 * m.m30;
double m01Tmp = m00 * m.m01 + m01 * m.m11 + m02 * m.m21 + m03 * m.m31;
double m02Tmp = m00 * m.m02 + m01 * m.m12 + m02 * m.m22 + m03 * m.m32;
double m03Tmp = m00 * m.m03 + m01 * m.m13 + m02 * m.m23 + m03 * m.m33;
double m10Tmp = m10 * m.m00 + m11 * m.m10 + m12 * m.m20 + m13 * m.m30;
double m11Tmp = m10 * m.m01 + m11 * m.m11 + m12 * m.m21 + m13 * m.m31;
double m12Tmp = m10 * m.m02 + m11 * m.m12 + m12 * m.m22 + m13 * m.m32;
double m13Tmp = m10 * m.m03 + m11 * m.m13 + m12 * m.m23 + m13 * m.m33;
double m20Tmp = m20 * m.m00 + m21 * m.m10 + m22 * m.m20 + m23 * m.m30;
double m21Tmp = m20 * m.m01 + m21 * m.m11 + m22 * m.m21 + m23 * m.m31;
double m22Tmp = m20 * m.m02 + m21 * m.m12 + m22 * m.m22 + m23 * m.m32;
double m23Tmp = m20 * m.m03 + m21 * m.m13 + m22 * m.m23 + m23 * m.m33;
double m30Tmp = m30 * m.m00 + m31 * m.m10 + m32 * m.m20 + m33 * m.m30;
double m31Tmp = m30 * m.m01 + m31 * m.m11 + m32 * m.m21 + m33 * m.m31;
double m32Tmp = m30 * m.m02 + m31 * m.m12 + m32 * m.m22 + m33 * m.m32;
double m33Tmp = m30 * m.m03 + m31 * m.m13 + m32 * m.m23 + m33 * m.m33;
m00 = m00Tmp;
m01 = m01Tmp;
m02 = m02Tmp;
m03 = m03Tmp;
m10 = m10Tmp;
m11 = m11Tmp;
m12 = m12Tmp;
m13 = m13Tmp;
m20 = m20Tmp;
m21 = m21Tmp;
m22 = m22Tmp;
m23 = m23Tmp;
m30 = m30Tmp;
m31 = m31Tmp;
m32 = m32Tmp;
m33 = m33Tmp;
}
public void transpose() {
double tmp = m10;
m10 = m01;
m01 = tmp;
tmp = m20;
m20 = m02;
m02 = tmp;
tmp = m30;
m30 = m03;
m03 = tmp;
tmp = m21;
m21 = m12;
m12 = tmp;
tmp = m31;
m31 = m13;
m13 = tmp;
tmp = m32;
m32 = m23;
m23 = tmp;
}
public void invert() {
double det = determinant();
if (det == 0) {
throw new RuntimeException("Cannot invert matrix with a determinat of 0.");
}
double detInv = 1f / det;
// first row
double t00 = determinant3x3(m11, m21, m31, m12, m22, m32, m13, m23, m33);
double t01 = -determinant3x3(m01, m21, m31, m02, m22, m32, m03, m23, m33);
double t02 = determinant3x3(m01, m11, m31, m02, m12, m32, m03, m13, m33);
double t03 = -determinant3x3(m01, m11, m21, m02, m12, m22, m03, m13, m23);
// second row
double t10 = -determinant3x3(m10, m20, m30, m12, m22, m32, m13, m23, m33);
double t11 = determinant3x3(m00, m20, m30, m02, m22, m32, m03, m23, m33);
double t12 = -determinant3x3(m00, m10, m30, m02, m12, m32, m03, m13, m33);
double t13 = determinant3x3(m00, m10, m20, m02, m12, m22, m03, m13, m23);
// third row
double t20 = determinant3x3(m10, m20, m30, m11, m21, m31, m13, m23, m33);
double t21 = -determinant3x3(m00, m20, m30, m01, m21, m31, m03, m23, m33);
double t22 = determinant3x3(m00, m10, m30, m01, m11, m31, m03, m13, m33);
double t23 = -determinant3x3(m00, m10, m20, m01, m11, m21, m03, m13, m23);
// fourth row
double t30 = -determinant3x3(m10, m20, m30, m11, m21, m31, m12, m22, m32);
double t31 = determinant3x3(m00, m20, m30, m01, m21, m31, m02, m22, m32);
double t32 = -determinant3x3(m00, m10, m30, m01, m11, m31, m02, m12, m32);
double t33 = determinant3x3(m00, m10, m20, m01, m11, m21, m02, m12, m22);
m00 = t00 * detInv;
m11 = t11 * detInv;
m22 = t22 * detInv;
m33 = t33 * detInv;
m10 = t10 * detInv;
m01 = t01 * detInv;
m02 = t02 * detInv;
m20 = t20 * detInv;
m21 = t21 * detInv;
m12 = t12 * detInv;
m30 = t30 * detInv;
m03 = t03 * detInv;
m31 = t31 * detInv;
m13 = t13 * detInv;
m23 = t23 * detInv;
m32 = t32 * detInv;
}
public double determinant() {
double result =
m00
* ((m11 * m22 * m33 + m21 * m32 * m13 + m31 * m12 * m23)
- m31 * m22 * m13
- m11 * m32 * m23
- m21 * m12 * m33);
result -= m10
* ((m01 * m22 * m33 + m21 * m32 * m03 + m31 * m02 * m23)
- m31 * m22 * m03
- m01 * m32 * m23
- m21 * m02 * m33);
result += m20
* ((m01 * m12 * m33 + m11 * m32 * m03 + m31 * m02 * m13)
- m31 * m12 * m03
- m01 * m32 * m13
- m11 * m02 * m33);
result -= m30
* ((m01 * m12 * m23 + m11 * m22 * m03 + m21 * m02 * m13)
- m21 * m12 * m03
- m01 * m22 * m13
- m11 * m02 * m23);
return result;
}
private double determinant3x3(double e00, double e01, double e02, double e10, double e11, double e12, double e20, double e21, double e22) {
return e00 * (e11 * e22 - e12 * e21) + e01 * (e12 * e20 - e10 * e22) + e02 * (e10 * e21 - e11 * e20);
}
@Override
public String toString() {
return "Matrix4d(\n" +
" " + m00 + ", " + m01 + ", " + m02 + ", " + m03 + "\n" +
" " + m10 + ", " + m11 + ", " + m12 + ", " + m13 + "\n" +
" " + m20 + ", " + m21 + ", " + m22 + ", " + m23 + "\n" +
" " + m30 + ", " + m31 + ", " + m32 + ", " + m33 + "\n" +
")\n";
}
}