899 lines
29 KiB
Java
899 lines
29 KiB
Java
package speiger.src.coreengine.math.vector.matrix;
|
|
|
|
import java.nio.ByteBuffer;
|
|
import java.nio.FloatBuffer;
|
|
import java.util.Arrays;
|
|
|
|
import speiger.src.coreengine.math.MathUtils;
|
|
import speiger.src.coreengine.math.vector.VectorUtil;
|
|
import speiger.src.coreengine.math.vector.floats.Vec2f;
|
|
import speiger.src.coreengine.math.vector.floats.Vec3f;
|
|
import speiger.src.coreengine.math.vector.floats.Vec4f;
|
|
import speiger.src.coreengine.math.vector.quaternion.Quaternion;
|
|
|
|
public class Matrix4f
|
|
{
|
|
static final Vec3f X_ROTATION = Vec3f.of(1F, 0F, 0F);
|
|
static final Vec3f Y_ROTATION = Vec3f.of(0F, 1F, 0F);
|
|
static final Vec3f Z_ROTATION = Vec3f.of(0F, 0F, 1F);
|
|
|
|
float[] data = new float[16];
|
|
|
|
public Matrix4f()
|
|
{
|
|
setIdentity();
|
|
}
|
|
|
|
public Matrix4f(float[] data)
|
|
{
|
|
System.arraycopy(data, 0, this.data, 0, 16);
|
|
}
|
|
|
|
public Matrix4f(Matrix4f other)
|
|
{
|
|
this(other.data);
|
|
}
|
|
|
|
public Matrix4f store(FloatBuffer buffer)
|
|
{
|
|
buffer.put(data);
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f store(ByteBuffer buffer)
|
|
{
|
|
for(int i = 0;i < 16;i++)
|
|
{
|
|
buffer.putFloat(data[i]);
|
|
}
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f store(Matrix4f other)
|
|
{
|
|
System.arraycopy(data, 0, other.data, 0, 16);
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f load(FloatBuffer buffer)
|
|
{
|
|
buffer.get(data);
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f load(ByteBuffer buffer)
|
|
{
|
|
for(int i = 0;i < 16;i++)
|
|
{
|
|
data[i] = buffer.getFloat();
|
|
}
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f load(Matrix4f other)
|
|
{
|
|
System.arraycopy(other.data, 0, data, 0, 16);
|
|
return this;
|
|
}
|
|
|
|
@Override
|
|
public String toString()
|
|
{
|
|
StringBuilder buf = new StringBuilder().append("\n");
|
|
buf.append("x0=").append(data[0]).append(' ').append("y0=").append(data[4]).append(' ').append("z0=").append(data[8]).append(' ').append("w0=").append(data[12]).append('\n');
|
|
buf.append("x1=").append(data[1]).append(' ').append("y1=").append(data[5]).append(' ').append("z1=").append(data[9]).append(' ').append("w1=").append(data[13]).append('\n');
|
|
buf.append("x2=").append(data[2]).append(' ').append("y2=").append(data[6]).append(' ').append("z2=").append(data[10]).append(' ').append("w2=").append(data[14]).append('\n');
|
|
buf.append("x3=").append(data[3]).append(' ').append("y3=").append(data[7]).append(' ').append("z3=").append(data[11]).append(' ').append("w3=").append(data[15]).append('\n');
|
|
return buf.toString();
|
|
}
|
|
|
|
@Override
|
|
public boolean equals(Object obj)
|
|
{
|
|
if(obj instanceof Matrix4f)
|
|
{
|
|
return Arrays.equals(data, ((Matrix4f)obj).data);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
public float[] getData()
|
|
{
|
|
return data;
|
|
}
|
|
|
|
public float get(int index)
|
|
{
|
|
return data[index];
|
|
}
|
|
|
|
public float get(int column, int row)
|
|
{
|
|
return data[(column * 4) + row];
|
|
}
|
|
|
|
public Matrix4f set(int column, int row, float value)
|
|
{
|
|
data[(column * 4) + row] = value;
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f set(int index, float value)
|
|
{
|
|
data[index] = value;
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f setIdentity()
|
|
{
|
|
Arrays.fill(data, 0F);
|
|
data[0] = 1F;
|
|
data[5] = 1F;
|
|
data[10] = 1F;
|
|
data[15] = 1F;
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f invert()
|
|
{
|
|
float determinant = determinant();
|
|
if(determinant != 0F)
|
|
{
|
|
float determinant_inv = 1F / determinant;
|
|
float t00 = determinant3x3(data[5], data[6], data[7], data[9], data[10], data[11], data[13], data[14], data[15]);
|
|
float t01 = -determinant3x3(data[4], data[6], data[7], data[8], data[10], data[11], data[12], data[14], data[15]);
|
|
float t02 = determinant3x3(data[4], data[5], data[7], data[8], data[9], data[11], data[12], data[13], data[15]);
|
|
float t03 = -determinant3x3(data[4], data[5], data[6], data[8], data[9], data[10], data[12], data[13], data[14]);
|
|
float t10 = -determinant3x3(data[1], data[2], data[3], data[9], data[10], data[11], data[13], data[14], data[15]);
|
|
float t11 = determinant3x3(data[0], data[2], data[3], data[8], data[10], data[11], data[12], data[14], data[15]);
|
|
float t12 = -determinant3x3(data[0], data[1], data[3], data[8], data[9], data[11], data[12], data[13], data[15]);
|
|
float t13 = determinant3x3(data[0], data[1], data[2], data[8], data[9], data[10], data[12], data[13], data[14]);
|
|
float t20 = determinant3x3(data[1], data[2], data[3], data[5], data[6], data[7], data[13], data[14], data[15]);
|
|
float t21 = -determinant3x3(data[0], data[2], data[3], data[4], data[6], data[7], data[12], data[14], data[15]);
|
|
float t22 = determinant3x3(data[0], data[1], data[3], data[4], data[5], data[7], data[12], data[13], data[15]);
|
|
float t23 = -determinant3x3(data[0], data[1], data[2], data[4], data[5], data[6], data[12], data[13], data[14]);
|
|
float t30 = -determinant3x3(data[1], data[2], data[3], data[5], data[6], data[7], data[9], data[10], data[11]);
|
|
float t31 = determinant3x3(data[0], data[2], data[3], data[4], data[6], data[7], data[8], data[10], data[11]);
|
|
float t32 = -determinant3x3(data[0], data[1], data[3], data[4], data[5], data[7], data[8], data[9], data[11]);
|
|
float t33 = determinant3x3(data[0], data[1], data[2], data[4], data[5], data[6], data[8], data[9], data[10]);
|
|
|
|
data[0] = t00 * determinant_inv;
|
|
data[5] = t11 * determinant_inv;
|
|
data[10] = t22 * determinant_inv;
|
|
data[15] = t33 * determinant_inv;
|
|
data[1] = t10 * determinant_inv;
|
|
data[4] = t01 * determinant_inv;
|
|
data[8] = t02 * determinant_inv;
|
|
data[2] = t20 * determinant_inv;
|
|
data[6] = t21 * determinant_inv;
|
|
data[9] = t12 * determinant_inv;
|
|
data[3] = t30 * determinant_inv;
|
|
data[12] = t03 * determinant_inv;
|
|
data[7] = t31 * determinant_inv;
|
|
data[13] = t13 * determinant_inv;
|
|
data[14] = t23 * determinant_inv;
|
|
data[11] = t32 * determinant_inv;
|
|
}
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f flip()
|
|
{
|
|
data[0] = -data[0];
|
|
data[1] = -data[1];
|
|
data[2] = -data[2];
|
|
data[3] = -data[3];
|
|
data[4] = -data[4];
|
|
data[5] = -data[5];
|
|
data[6] = -data[6];
|
|
data[7] = -data[7];
|
|
data[8] = -data[8];
|
|
data[9] = -data[9];
|
|
data[10] = -data[10];
|
|
data[11] = -data[11];
|
|
data[12] = -data[12];
|
|
data[13] = -data[13];
|
|
data[14] = -data[14];
|
|
data[15] = -data[15];
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f negate()
|
|
{
|
|
Arrays.fill(data, 0F);
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f add(Matrix4f other)
|
|
{
|
|
data[0] += other.data[0];
|
|
data[1] += other.data[1];
|
|
data[2] += other.data[2];
|
|
data[3] += other.data[3];
|
|
data[4] += other.data[4];
|
|
data[5] += other.data[5];
|
|
data[6] += other.data[6];
|
|
data[7] += other.data[7];
|
|
data[8] += other.data[8];
|
|
data[9] += other.data[9];
|
|
data[10] += other.data[10];
|
|
data[11] += other.data[11];
|
|
data[12] += other.data[12];
|
|
data[13] += other.data[13];
|
|
data[14] += other.data[14];
|
|
data[15] += other.data[15];
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f sub(Matrix4f other)
|
|
{
|
|
data[0] -= other.data[0];
|
|
data[1] -= other.data[1];
|
|
data[2] -= other.data[2];
|
|
data[3] -= other.data[3];
|
|
data[4] -= other.data[4];
|
|
data[5] -= other.data[5];
|
|
data[6] -= other.data[6];
|
|
data[7] -= other.data[7];
|
|
data[8] -= other.data[8];
|
|
data[9] -= other.data[9];
|
|
data[10] -= other.data[10];
|
|
data[11] -= other.data[11];
|
|
data[12] -= other.data[12];
|
|
data[13] -= other.data[13];
|
|
data[14] -= other.data[14];
|
|
data[15] -= other.data[15];
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f multiply(Matrix4f other)
|
|
{
|
|
float data0 = data[0] * other.data[0] + data[4] * other.data[1] + data[8] * other.data[2] + data[12] * other.data[3];
|
|
float data1 = data[1] * other.data[0] + data[5] * other.data[1] + data[9] * other.data[2] + data[13] * other.data[3];
|
|
float data2 = data[2] * other.data[0] + data[6] * other.data[1] + data[10] * other.data[2] + data[14] * other.data[3];
|
|
float data3 = data[3] * other.data[0] + data[7] * other.data[1] + data[11] * other.data[2] + data[15] * other.data[3];
|
|
float data4 = data[0] * other.data[4] + data[4] * other.data[5] + data[8] * other.data[6] + data[12] * other.data[7];
|
|
float data5 = data[1] * other.data[4] + data[5] * other.data[5] + data[9] * other.data[6] + data[13] * other.data[7];
|
|
float data6 = data[2] * other.data[4] + data[6] * other.data[5] + data[10] * other.data[6] + data[14] * other.data[7];
|
|
float data7 = data[3] * other.data[4] + data[7] * other.data[5] + data[11] * other.data[6] + data[15] * other.data[7];
|
|
float data8 = data[0] * other.data[8] + data[4] * other.data[9] + data[8] * other.data[10] + data[12] * other.data[11];
|
|
float data9 = data[1] * other.data[8] + data[5] * other.data[9] + data[9] * other.data[10] + data[13] * other.data[11];
|
|
float data10 = data[2] * other.data[8] + data[6] * other.data[9] + data[10] * other.data[10] + data[14] * other.data[11];
|
|
float data11 = data[3] * other.data[8] + data[7] * other.data[9] + data[11] * other.data[10] + data[15] * other.data[11];
|
|
float data12 = data[0] * other.data[12] + data[4] * other.data[13] + data[8] * other.data[14] + data[12] * other.data[15];
|
|
float data13 = data[1] * other.data[12] + data[5] * other.data[13] + data[9] * other.data[14] + data[13] * other.data[15];
|
|
float data14 = data[2] * other.data[12] + data[6] * other.data[13] + data[10] * other.data[14] + data[14] * other.data[15];
|
|
float data15 = data[3] * other.data[12] + data[7] * other.data[13] + data[11] * other.data[14] + data[15] * other.data[15];
|
|
data[0] = data0;
|
|
data[1] = data1;
|
|
data[2] = data2;
|
|
data[3] = data3;
|
|
data[4] = data4;
|
|
data[5] = data5;
|
|
data[6] = data6;
|
|
data[7] = data7;
|
|
data[8] = data8;
|
|
data[9] = data9;
|
|
data[10] = data10;
|
|
data[11] = data11;
|
|
data[12] = data12;
|
|
data[13] = data13;
|
|
data[14] = data14;
|
|
data[15] = data15;
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f transpose()
|
|
{
|
|
float data0 = data[0];
|
|
float data1 = data[4];
|
|
float data2 = data[8];
|
|
float data3 = data[12];
|
|
float data4 = data[1];
|
|
float data5 = data[5];
|
|
float data6 = data[9];
|
|
float data7 = data[13];
|
|
float data8 = data[2];
|
|
float data9 = data[6];
|
|
float data10 = data[10];
|
|
float data11 = data[14];
|
|
float data12 = data[3];
|
|
float data13 = data[7];
|
|
float data14 = data[11];
|
|
float data15 = data[15];
|
|
|
|
data[0] = data0;
|
|
data[1] = data1;
|
|
data[2] = data2;
|
|
data[3] = data3;
|
|
data[4] = data4;
|
|
data[5] = data5;
|
|
data[6] = data6;
|
|
data[7] = data7;
|
|
data[8] = data8;
|
|
data[9] = data9;
|
|
data[10] = data10;
|
|
data[11] = data11;
|
|
data[12] = data12;
|
|
data[13] = data13;
|
|
data[14] = data14;
|
|
data[15] = data15;
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f transpose3x3()
|
|
{
|
|
float nm00 = data[0];
|
|
float nm01 = data[4];
|
|
float nm02 = data[8];
|
|
float nm10 = data[1];
|
|
float nm11 = data[5];
|
|
float nm12 = data[9];
|
|
float nm20 = data[2];
|
|
float nm21 = data[6];
|
|
float nm22 = data[10];
|
|
data[0] = nm00;
|
|
data[1] = nm01;
|
|
data[2] = nm02;
|
|
data[4] = nm10;
|
|
data[5] = nm11;
|
|
data[6] = nm12;
|
|
data[8] = nm20;
|
|
data[9] = nm21;
|
|
data[10] = nm22;
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f decompose(Vec3f position, Quaternion rotation, Vec3f scale)
|
|
{
|
|
position.set(get(3, 0), get(3, 1), get(3, 2));
|
|
rotation.set(this);
|
|
scale.set(get(0, 0), get(1, 1), get(2, 2));
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f setTranslation(Vec3f vec)
|
|
{
|
|
data[12] = vec.getX();
|
|
data[13] = vec.getY();
|
|
data[14] = vec.getZ();
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f translate(Vec2f vec)
|
|
{
|
|
return translate(vec.getX(), vec.getY());
|
|
}
|
|
|
|
public Matrix4f translate(float x, float y)
|
|
{
|
|
data[12] += data[0] * x + data[4] * y;
|
|
data[13] += data[1] * x + data[5] * y;
|
|
data[14] += data[2] * x + data[6] * y;
|
|
data[15] += data[3] * x + data[7] * y;
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f translate(Vec3f vec)
|
|
{
|
|
data[12] += data[0] * vec.getX() + data[4] * vec.getY() + data[8] * vec.getZ();
|
|
data[13] += data[1] * vec.getX() + data[5] * vec.getY() + data[9] * vec.getZ();
|
|
data[14] += data[2] * vec.getX() + data[6] * vec.getY() + data[10] * vec.getZ();
|
|
data[15] += data[3] * vec.getX() + data[7] * vec.getY() + data[11] * vec.getZ();
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f translate(float x, float y, float z)
|
|
{
|
|
data[12] += data[0] * x + data[4] * y + data[8] * z;
|
|
data[13] += data[1] * x + data[5] * y + data[9] * z;
|
|
data[14] += data[2] * x + data[6] * y + data[10] * z;
|
|
data[15] += data[3] * x + data[7] * y + data[11] * z;
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f rotateX(float angle)
|
|
{
|
|
return rotate((float)Math.toRadians(angle), X_ROTATION);
|
|
}
|
|
|
|
public Matrix4f rotateY(float angle)
|
|
{
|
|
return rotate((float)Math.toRadians(angle), Y_ROTATION);
|
|
}
|
|
|
|
public Matrix4f rotateZ(float angle)
|
|
{
|
|
return rotate((float)Math.toRadians(angle), Z_ROTATION);
|
|
}
|
|
|
|
public Matrix4f rotateRadX(float angle)
|
|
{
|
|
return rotate(angle, X_ROTATION);
|
|
}
|
|
|
|
public Matrix4f rotateRadY(float angle)
|
|
{
|
|
return rotate(angle, Y_ROTATION);
|
|
}
|
|
|
|
public Matrix4f rotateRadZ(float angle)
|
|
{
|
|
return rotate(angle, Z_ROTATION);
|
|
}
|
|
|
|
public Matrix4f rotate(float angle, Vec3f axis)
|
|
{
|
|
float c = MathUtils.cos(angle);
|
|
float s = MathUtils.sin(angle);
|
|
float oneminusc = 1.0f - c;
|
|
float xy = axis.getX() * axis.getY();
|
|
float yz = axis.getY() * axis.getZ();
|
|
float xz = axis.getX() * axis.getZ();
|
|
float xs = axis.getX() * s;
|
|
float ys = axis.getY() * s;
|
|
float zs = axis.getZ() * s;
|
|
|
|
float f00 = axis.getX() * axis.getX() * oneminusc + c;
|
|
float f01 = xy * oneminusc + zs;
|
|
float f02 = xz * oneminusc - ys;
|
|
|
|
float f10 = xy * oneminusc - zs;
|
|
float f11 = axis.getY() * axis.getY() * oneminusc + c;
|
|
float f12 = yz * oneminusc + xs;
|
|
|
|
float f20 = xz * oneminusc + ys;
|
|
float f21 = yz * oneminusc - xs;
|
|
float f22 = axis.getZ() * axis.getZ() * oneminusc + c;
|
|
|
|
float t00 = data[0] * f00 + data[4] * f01 + data[8] * f02;
|
|
float t01 = data[1] * f00 + data[5] * f01 + data[9] * f02;
|
|
float t02 = data[2] * f00 + data[6] * f01 + data[10] * f02;
|
|
float t03 = data[3] * f00 + data[7] * f01 + data[11] * f02;
|
|
float t10 = data[0] * f10 + data[4] * f11 + data[8] * f12;
|
|
float t11 = data[1] * f10 + data[5] * f11 + data[9] * f12;
|
|
float t12 = data[2] * f10 + data[6] * f11 + data[10] * f12;
|
|
float t13 = data[3] * f10 + data[7] * f11 + data[11] * f12;
|
|
data[8] = data[0] * f20 + data[4] * f21 + data[8] * f22;
|
|
data[9] = data[1] * f20 + data[5] * f21 + data[9] * f22;
|
|
data[10] = data[2] * f20 + data[6] * f21 + data[10] * f22;
|
|
data[11] = data[3] * f20 + data[7] * f21 + data[11] * f22;
|
|
data[0] = t00;
|
|
data[1] = t01;
|
|
data[2] = t02;
|
|
data[3] = t03;
|
|
data[4] = t10;
|
|
data[5] = t11;
|
|
data[6] = t12;
|
|
data[7] = t13;
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f rotate(Quaternion rotation)
|
|
{
|
|
float w2 = rotation.getW() * rotation.getW();
|
|
float x2 = rotation.getX() * rotation.getX();
|
|
float y2 = rotation.getY() * rotation.getY();
|
|
float z2 = rotation.getZ() * rotation.getZ();
|
|
float zw = rotation.getZ() * rotation.getW();
|
|
float dzw = zw + zw;
|
|
float xy = rotation.getX() * rotation.getY();
|
|
float dxy = xy + xy;
|
|
float xz = rotation.getX() * rotation.getZ();
|
|
float dxz = xz + xz;
|
|
float yw = rotation.getY() * rotation.getW();
|
|
float dyw = yw + yw;
|
|
float yz = rotation.getY() * rotation.getZ();
|
|
float dyz = yz + yz;
|
|
float xw = rotation.getX() * rotation.getW();
|
|
float dxw = xw + xw;
|
|
float rm00 = w2 + x2 - z2 - y2;
|
|
float rm01 = dxy + dzw;
|
|
float rm02 = dxz - dyw;
|
|
float rm10 = -dzw + dxy;
|
|
float rm11 = y2 - z2 + w2 - x2;
|
|
float rm12 = dyz + dxw;
|
|
float rm20 = dyw + dxz;
|
|
float rm21 = dyz - dxw;
|
|
float rm22 = z2 - y2 - x2 + w2;
|
|
float nm00 = data[0] * rm00 + data[4] * rm01 + data[8] * rm02;
|
|
float nm01 = data[1] * rm00 + data[5] * rm01 + data[9] * rm02;
|
|
float nm02 = data[2] * rm00 + data[6] * rm01 + data[10] * rm02;
|
|
float nm03 = data[3] * rm00 + data[7] * rm01 + data[11] * rm02;
|
|
float nm10 = data[0] * rm10 + data[4] * rm11 + data[8] * rm12;
|
|
float nm11 = data[1] * rm10 + data[5] * rm11 + data[9] * rm12;
|
|
float nm12 = data[2] * rm10 + data[6] * rm11 + data[10] * rm12;
|
|
float nm13 = data[3] * rm10 + data[7] * rm11 + data[11] * rm12;
|
|
data[8] = data[0] * rm20 + data[4] * rm21 + data[8] * rm22;
|
|
data[9] = data[1] * rm20 + data[5] * rm21 + data[9] * rm22;
|
|
data[10] = data[2] * rm20 + data[6] * rm21 + data[10] * rm22;
|
|
data[11] = data[3] * rm20 + data[7] * rm21 + data[11] * rm22;
|
|
data[0] = nm00;
|
|
data[1] = nm01;
|
|
data[2] = nm02;
|
|
data[3] = nm03;
|
|
data[4] = nm10;
|
|
data[5] = nm11;
|
|
data[6] = nm12;
|
|
data[7] = nm13;
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f setRotation(Matrix4f source)
|
|
{
|
|
data[0] = source.data[0];
|
|
data[1] = source.data[1];
|
|
data[2] = source.data[2];
|
|
data[4] = source.data[4];
|
|
data[5] = source.data[5];
|
|
data[6] = source.data[6];
|
|
data[8] = source.data[8];
|
|
data[9] = source.data[9];
|
|
data[10] = source.data[10];
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f setBillboard(Matrix4f source)
|
|
{
|
|
data[0] = source.data[0];
|
|
data[1] = source.data[4];
|
|
data[2] = source.data[8];
|
|
data[4] = source.data[1];
|
|
data[5] = source.data[5];
|
|
data[6] = source.data[9];
|
|
data[8] = source.data[2];
|
|
data[9] = source.data[6];
|
|
data[10] = source.data[10];
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f scale(Vec3f vec)
|
|
{
|
|
return scale(vec.getX(), vec.getY(), vec.getZ());
|
|
}
|
|
|
|
public Matrix4f scale(float value)
|
|
{
|
|
return scale(value, value, value);
|
|
}
|
|
|
|
public Matrix4f scale(float x, float y, float z)
|
|
{
|
|
data[0] *= x;
|
|
data[1] *= x;
|
|
data[2] *= x;
|
|
data[3] *= x;
|
|
data[4] *= y;
|
|
data[5] *= y;
|
|
data[6] *= y;
|
|
data[7] *= y;
|
|
data[8] *= z;
|
|
data[9] *= z;
|
|
data[10] *= z;
|
|
data[11] *= z;
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f unscale(Vec3f vec)
|
|
{
|
|
return unscale(vec.getX(), vec.getY(), vec.getZ());
|
|
}
|
|
|
|
public Matrix4f unscale(float scale)
|
|
{
|
|
return unscale(scale, scale, scale);
|
|
}
|
|
|
|
public Matrix4f unscale(float x, float y, float z)
|
|
{
|
|
data[0] /= x;
|
|
data[1] /= x;
|
|
data[2] /= x;
|
|
data[3] /= x;
|
|
data[4] /= y;
|
|
data[5] /= y;
|
|
data[6] /= y;
|
|
data[7] /= y;
|
|
data[8] /= z;
|
|
data[9] /= z;
|
|
data[10] /= z;
|
|
data[11] /= z;
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f setPerspective(float fovy, float aspect, float zNear, float zFar)
|
|
{
|
|
float h = (float)Math.tan(fovy * 0.5f);
|
|
set(0, 0, 1F / (h * aspect)).set(1, 1, 1F / h);
|
|
boolean farInf = zFar > 0 && Float.isInfinite(zFar);
|
|
boolean nearInf = zNear > 0 && Float.isInfinite(zNear);
|
|
if(farInf)
|
|
{
|
|
float e = 1E-6f;
|
|
set(2, 2, e - 1F).set(3, 2, (e - 2F) * zNear);
|
|
}
|
|
else if(nearInf)
|
|
{
|
|
float e = 1E-6f;
|
|
set(2, 2, 1F - e).set(3, 2, (2F - e) * zFar);
|
|
}
|
|
else
|
|
{
|
|
set(2, 2, (zFar + zNear) / (zNear - zFar)).set(3, 2, (zFar + zFar) * zNear / (zNear - zFar));
|
|
}
|
|
return set(2, 3, -1.0f);
|
|
}
|
|
|
|
public Matrix4f getTranslation(Vec3f vec)
|
|
{
|
|
vec.set(data[12], data[13], data[14]);
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f getScale(Vec3f vec)
|
|
{
|
|
vec.setX((float)Math.sqrt(data[0] * data[0] + data[1] * data[1] + data[2] * data[2]));
|
|
vec.setY((float)Math.sqrt(data[4] * data[4] + data[5] * data[5] + data[6] * data[6]));
|
|
vec.setZ((float)Math.sqrt(data[8] * data[8] + data[9] * data[9] + data[10] * data[10]));
|
|
return this;
|
|
}
|
|
|
|
public Vec4f transform(Vec4f input)
|
|
{
|
|
return transform(input, input);
|
|
}
|
|
|
|
public Vec4f transform(Vec4f input, Vec4f output)
|
|
{
|
|
float x = data[0] * input.getX() + data[4] * input.getY() + data[8] * input.getZ() + data[12] * input.getW();
|
|
float y = data[1] * input.getX() + data[5] * input.getY() + data[9] * input.getZ() + data[13] * input.getW();
|
|
float z = data[2] * input.getX() + data[6] * input.getY() + data[10] * input.getZ() + data[14] * input.getW();
|
|
float w = data[3] * input.getX() + data[7] * input.getY() + data[11] * input.getZ() + data[15] * input.getW();
|
|
return output.set(x, y, z, w);
|
|
}
|
|
|
|
public void transform(Vec4f input, FloatBuffer buffer)
|
|
{
|
|
buffer.put(data[0] * input.getX() + data[4] * input.getY() + data[8] * input.getZ() + data[12] * input.getW());
|
|
buffer.put(data[1] * input.getX() + data[5] * input.getY() + data[9] * input.getZ() + data[13] * input.getW());
|
|
buffer.put(data[2] * input.getX() + data[6] * input.getY() + data[10] * input.getZ() + data[14] * input.getW());
|
|
buffer.put(data[3] * input.getX() + data[7] * input.getY() + data[11] * input.getZ() + data[15] * input.getW());
|
|
}
|
|
|
|
public Vec3f transform(Vec3f input, boolean position)
|
|
{
|
|
return transform(input, input, position);
|
|
}
|
|
|
|
public Vec3f transform(Vec3f input, Vec3f output, boolean position)
|
|
{
|
|
float pos = position ? 1F : 0F;
|
|
return output.set(data[0] * input.getX() + data[4] * input.getY() + data[8] * input.getZ() + data[12] * pos, data[1] * input.getX() + data[5] * input.getY() + data[9] * input.getZ() + data[13] * pos, data[2] * input.getX() + data[6] * input.getY() + data[10] * input.getZ() + data[14] * pos);
|
|
}
|
|
|
|
public void transform(Vec3f input, FloatBuffer buffer, boolean position)
|
|
{
|
|
float pos = position ? 1F : 0F;
|
|
buffer.put(data[0] * input.getX() + data[4] * input.getY() + data[8] * input.getZ() + data[12] * pos);
|
|
buffer.put(data[1] * input.getX() + data[5] * input.getY() + data[9] * input.getZ() + data[13] * pos);
|
|
buffer.put(data[2] * input.getX() + data[6] * input.getY() + data[10] * input.getZ() + data[14] * pos);
|
|
}
|
|
|
|
public float determinant()
|
|
{
|
|
float f = data[0] * ((data[5] * data[10] * data[15] + data[6] * data[11] * data[13] + data[7] * data[9] * data[14]) - data[7] * data[10] * data[13] - data[5] * data[11] * data[14] - data[6] * data[9] * data[15]);
|
|
f -= data[1] * ((data[4] * data[10] * data[15] + data[6] * data[11] * data[12] + data[7] * data[8] * data[14]) - data[7] * data[10] * data[12] - data[4] * data[11] * data[14] - data[6] * data[8] * data[15]);
|
|
f += data[2] * ((data[4] * data[9] * data[15] + data[5] * data[11] * data[12] + data[7] * data[8] * data[13]) - data[7] * data[9] * data[12] - data[4] * data[11] * data[13] - data[5] * data[8] * data[15]);
|
|
f -= data[3] * ((data[4] * data[9] * data[14] + data[5] * data[10] * data[12] + data[6] * data[8] * data[13]) - data[6] * data[9] * data[12] - data[4] * data[10] * data[13] - data[5] * data[8] * data[14]);
|
|
return f;
|
|
}
|
|
|
|
public float determinant3x3(float t00, float t01, float t02, float t10, float t11, float t12, float t20, float t21, float t22)
|
|
{
|
|
return t00 * (t11 * t22 - t12 * t21) + t01 * (t12 * t20 - t10 * t22) + t02 * (t10 * t21 - t11 * t20);
|
|
}
|
|
|
|
public Matrix4f setTransform(Vec3f position, Vec3f rotation, float scale)
|
|
{
|
|
setIdentity();
|
|
translate(position);
|
|
rotate((float)Math.toRadians(rotation.getX()), X_ROTATION);
|
|
rotate((float)Math.toRadians(rotation.getY()), Y_ROTATION);
|
|
rotate((float)Math.toRadians(rotation.getZ()), Z_ROTATION);
|
|
scale(scale, scale, scale);
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f setTransform(Vec3f position, Vec3f rotation, Vec3f scale)
|
|
{
|
|
setIdentity();
|
|
translate(position);
|
|
rotate((float)Math.toRadians(rotation.getX()), X_ROTATION);
|
|
rotate((float)Math.toRadians(rotation.getY()), Y_ROTATION);
|
|
rotate((float)Math.toRadians(rotation.getZ()), Z_ROTATION);
|
|
scale(scale);
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f setTransform(Vec3f position, Vec3f offset, Vec3f rotation, float scale)
|
|
{
|
|
setIdentity();
|
|
translate(position);
|
|
translate(offset);
|
|
rotate((float)Math.toRadians(rotation.getX()), X_ROTATION);
|
|
rotate((float)Math.toRadians(rotation.getY()), Y_ROTATION);
|
|
rotate((float)Math.toRadians(rotation.getZ()), Z_ROTATION);
|
|
scale(scale, scale, scale);
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f setTransform(Vec3f position, Vec3f offset, Vec3f rotation, Vec3f scale)
|
|
{
|
|
setIdentity();
|
|
translate(position);
|
|
translate(offset);
|
|
rotate((float)Math.toRadians(rotation.getX()), X_ROTATION);
|
|
rotate((float)Math.toRadians(rotation.getY()), Y_ROTATION);
|
|
rotate((float)Math.toRadians(rotation.getZ()), Z_ROTATION);
|
|
scale(scale);
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f setTransform(Vec3f position, Quaternion rotation, float scale)
|
|
{
|
|
setIdentity();
|
|
translate(position);
|
|
rotate(rotation);
|
|
scale(scale, scale, scale);
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f setTransform(Vec3f position, Quaternion rotation, Vec3f scale)
|
|
{
|
|
setIdentity();
|
|
translate(position);
|
|
rotate(rotation);
|
|
scale(scale);
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f setTransform(Vec3f position, Vec3f offset, Quaternion rotation, float scale)
|
|
{
|
|
setIdentity();
|
|
translate(position);
|
|
translate(offset);
|
|
rotate(rotation);
|
|
scale(scale, scale, scale);
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f setTransform(Vec3f position, Vec3f offset, Quaternion rotation, Vec3f scale)
|
|
{
|
|
setIdentity();
|
|
translate(position);
|
|
translate(offset);
|
|
rotate(rotation);
|
|
scale(scale);
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f setTransform(Vec3f position, Matrix4f billRotation, float scale)
|
|
{
|
|
setIdentity();
|
|
translate(position);
|
|
setBillboard(billRotation);
|
|
scale(scale, scale, scale);
|
|
return this;
|
|
}
|
|
|
|
public Matrix4f setTransform(Vec3f position, Matrix4f billRotation, Vec3f scale)
|
|
{
|
|
setIdentity();
|
|
translate(position);
|
|
setBillboard(billRotation);
|
|
transpose3x3();
|
|
scale(scale);
|
|
return this;
|
|
}
|
|
|
|
public Vec3f project(float x, float y, float z, int[] viewport, Vec3f winCoordsDest)
|
|
{
|
|
float invW = 1F / VectorUtil.fma(data[3], x, VectorUtil.fma(data[7], y, VectorUtil.fma(data[11], z, data[15])));
|
|
float nx = VectorUtil.fma(data[0], x, VectorUtil.fma(data[4], y, VectorUtil.fma(data[8], z, data[12]))) * invW;
|
|
float ny = VectorUtil.fma(data[1], x, VectorUtil.fma(data[5], y, VectorUtil.fma(data[9], z, data[13]))) * invW;
|
|
float nz = VectorUtil.fma(data[2], x, VectorUtil.fma(data[6], y, VectorUtil.fma(data[10], z, data[14]))) * invW;
|
|
return winCoordsDest.set(VectorUtil.fma(VectorUtil.fma(nx, 0.5F, 0.5F), viewport[2], viewport[0]), VectorUtil.fma(VectorUtil.fma(ny, 0.5F, 0.5F), viewport[3], viewport[1]), VectorUtil.fma(0.5F, nz, 0.5F));
|
|
}
|
|
|
|
public Vec3f unproject(float winX, float winY, float winZ, int[] viewport, Vec3f dest) {
|
|
float a = data[0] * data[5] - data[1] * data[4];
|
|
float b = data[0] * data[6] - data[2] * data[4];
|
|
float c = data[0] * data[7] - data[3] * data[4];
|
|
float d = data[1] * data[6] - data[2] * data[5];
|
|
float e = data[1] * data[7] - data[3] * data[5];
|
|
float f = data[2] * data[7] - data[3] * data[6];
|
|
float g = data[8] * data[13] - data[9] * data[12];
|
|
float h = data[8] * data[14] - data[10] * data[12];
|
|
float i = data[8] * data[15] - data[11] * data[12];
|
|
float j = data[9] * data[14] - data[10] * data[13];
|
|
float k = data[9] * data[15] - data[11] * data[13];
|
|
float l = data[10] * data[15] - data[11] * data[14];
|
|
float det = a * l - b * k + c * j + d * i - e * h + f * g;
|
|
det = 1.0f / det;
|
|
float im00 = ( data[5] * l - data[6] * k + data[7] * j) * det;
|
|
float im01 = (-data[1] * l + data[2] * k - data[3] * j) * det;
|
|
float im02 = ( data[13] * f - data[14] * e + data[15] * d) * det;
|
|
float im03 = (-data[9] * f + data[10] * e - data[11] * d) * det;
|
|
float im10 = (-data[4] * l + data[6] * i - data[7] * h) * det;
|
|
float im11 = ( data[0] * l - data[2] * i + data[3] * h) * det;
|
|
float im12 = (-data[12] * f + data[14] * c - data[15] * b) * det;
|
|
float im13 = ( data[8] * f - data[10] * c + data[11] * b) * det;
|
|
float im20 = ( data[4] * k - data[5] * i + data[7] * g) * det;
|
|
float im21 = (-data[0] * k + data[1] * i - data[3] * g) * det;
|
|
float im22 = ( data[12] * e - data[13] * c + data[15] * a) * det;
|
|
float im23 = (-data[8] * e + data[9] * c - data[11] * a) * det;
|
|
float im30 = (-data[4] * j + data[5] * h - data[6] * g) * det;
|
|
float im31 = ( data[0] * j - data[1] * h + data[2] * g) * det;
|
|
float im32 = (-data[12] * d + data[13] * b - data[14] * a) * det;
|
|
float im33 = ( data[8] * d - data[9] * b + data[10] * a) * det;
|
|
float ndcX = (winX-viewport[0])/viewport[2]*2.0f-1.0f;
|
|
float ndcY = (winY-viewport[1])/viewport[3]*2.0f-1.0f;
|
|
float ndcZ = winZ+winZ-1.0f;
|
|
float invW = 1.0f / (im03 * ndcX + im13 * ndcY + im23 * ndcZ + im33);
|
|
return dest.set((im00 * ndcX + im10 * ndcY + im20 * ndcZ + im30) * invW, (im01 * ndcX + im11 * ndcY + im21 * ndcZ + im31) * invW, (im02 * ndcX + im12 * ndcY + im22 * ndcZ + im32) * invW);
|
|
}
|
|
|
|
public Matrix4f ortho(float x, float y, float width, float height, float zNear, float zFar)
|
|
{
|
|
return ortho(x, x+width, y+height, y, zNear, zFar, false);
|
|
}
|
|
|
|
public Matrix4f ortho(float left, float right, float bottom, float top, float zNear, float zFar, boolean zZeroToOne)
|
|
{
|
|
float rm00 = 2F / (right - left);
|
|
float rm11 = 2F / (top - bottom);
|
|
float rm22 = (zZeroToOne ? 1F : 2F) / (zFar - zNear);
|
|
float rm30 = (left + right) / (left - right);
|
|
float rm31 = (top + bottom) / (bottom - top);
|
|
float rm32 = (zZeroToOne ? zNear : (zFar + zNear)) / (zNear - zFar);
|
|
|
|
data[12] = data[0] * rm30 + data[4] * rm31 + data[8] * rm32 + data[12];
|
|
data[13] = data[1] * rm30 + data[5] * rm31 + data[9] * rm32 + data[13];
|
|
data[14] = data[2] * rm30 + data[6] * rm31 + data[10] * rm32 + data[14];
|
|
data[15] = data[3] * rm30 + data[7] * rm31 + data[11] * rm32 + data[15];
|
|
data[0] = data[0] * rm00;
|
|
data[1] = data[1] * rm00;
|
|
data[2] = data[2] * rm00;
|
|
data[3] = data[3] * rm00;
|
|
data[4] = data[4] * rm11;
|
|
data[5] = data[5] * rm11;
|
|
data[6] = data[6] * rm11;
|
|
data[7] = data[7] * rm11;
|
|
data[8] = data[8] * rm22;
|
|
data[9] = data[9] * rm22;
|
|
data[10] = data[10] * rm22;
|
|
data[11] = data[11] * rm22;
|
|
return this;
|
|
}
|
|
|
|
public Vec4f storeFrustrumPlane(int plane, Vec4f toStore)
|
|
{
|
|
switch(plane)
|
|
{
|
|
case 0:
|
|
return toStore.set(data[3] + data[0], data[7] + data[4], data[11] + data[8], data[15] + data[12]).normalize3D();
|
|
case 1:
|
|
return toStore.set(data[3] - data[0], data[7] - data[4], data[11] - data[8], data[15] - data[12]).normalize3D();
|
|
case 2:
|
|
return toStore.set(data[3] + data[1], data[7] + data[5], data[11] + data[9], data[15] + data[13]).normalize3D();
|
|
case 3:
|
|
return toStore.set(data[3] - data[1], data[7] - data[5], data[11] - data[9], data[15] - data[13]).normalize3D();
|
|
case 4:
|
|
return toStore.set(data[3] + data[2], data[7] + data[6], data[11] + data[10], data[15] + data[14]).normalize3D();
|
|
case 5:
|
|
return toStore.set(data[3] - data[2], data[7] - data[6], data[11] - data[10], data[15] - data[14]).normalize3D();
|
|
}
|
|
return toStore;
|
|
}
|
|
}
|