pythagoras.d.Matrix4 Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of pythagoras Show documentation
Show all versions of pythagoras Show documentation
Portable geometry routines, adapted from Apache Harmony geometry classes.
The newest version!
//
// Pythagoras - a collection of geometry classes
// http://github.com/samskivert/pythagoras
package pythagoras.d;
import java.io.Serializable;
import java.nio.DoubleBuffer;
import pythagoras.util.Platform;
import pythagoras.util.SingularMatrixException;
/**
* A 4x4 column-major matrix.
*/
public final class Matrix4 implements IMatrix4, Serializable
{
private static final long serialVersionUID = 2376107607832772408L;
/** The identity matrix. */
public static final IMatrix4 IDENTITY = new Matrix4();
/** An empty matrix array. */
public static final Matrix4[] EMPTY_ARRAY = new Matrix4[0];
/** The values of the matrix. The names take the form {@code mCOLROW}. */
public double m00, m10, m20, m30;
public double m01, m11, m21, m31;
public double m02, m12, m22, m32;
public double m03, m13, m23, m33;
/**
* Creates a matrix from its components.
*/
public Matrix4 (
double m00, double m10, double m20, double m30,
double m01, double m11, double m21, double m31,
double m02, double m12, double m22, double m32,
double m03, double m13, double m23, double m33) {
set(m00, m10, m20, m30,
m01, m11, m21, m31,
m02, m12, m22, m32,
m03, m13, m23, m33);
}
/**
* Creates a matrix from an array of values.
*/
public Matrix4 (double[] values) {
set(values);
}
/**
* Creates a matrix from a double buffer.
*/
public Matrix4 (DoubleBuffer buf) {
set(buf);
}
/**
* Copy constructor.
*/
public Matrix4 (IMatrix4 other) {
set(other);
}
/**
* Creates an identity matrix.
*/
public Matrix4 () {
setToIdentity();
}
/**
* Sets this matrix to the identity matrix.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToIdentity () {
return set(1f, 0f, 0f, 0f,
0f, 1f, 0f, 0f,
0f, 0f, 1f, 0f,
0f, 0f, 0f, 1f);
}
/**
* Sets this matrix to all zeroes.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToZero () {
return set(0f, 0f, 0f, 0f,
0f, 0f, 0f, 0f,
0f, 0f, 0f, 0f,
0f, 0f, 0f, 0f);
}
/**
* Sets this to a matrix that first rotates, then translates.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToTransform (IVector3 translation, IQuaternion rotation) {
return setToRotation(rotation).setTranslation(translation);
}
/**
* Sets this to a matrix that first scales, then rotates, then translates.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToTransform (IVector3 translation, IQuaternion rotation, double scale) {
return setToRotation(rotation).set(
m00 * scale, m10 * scale, m20 * scale, translation.x(),
m01 * scale, m11 * scale, m21 * scale, translation.y(),
m02 * scale, m12 * scale, m22 * scale, translation.z(),
0f, 0f, 0f, 1f);
}
/**
* Sets this to a matrix that first scales, then rotates, then translates.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToTransform (IVector3 translation, IQuaternion rotation, IVector3 scale) {
double sx = scale.x(), sy = scale.y(), sz = scale.z();
return setToRotation(rotation).set(
m00 * sx, m10 * sy, m20 * sz, translation.x(),
m01 * sx, m11 * sy, m21 * sz, translation.y(),
m02 * sx, m12 * sy, m22 * sz, translation.z(),
0f, 0f, 0f, 1f);
}
/**
* Sets this to a translation matrix.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToTranslation (IVector3 translation) {
return setToTranslation(translation.x(), translation.y(), translation.z());
}
/**
* Sets this to a translation matrix.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToTranslation (double x, double y, double z) {
return set(1f, 0f, 0f, x,
0f, 1f, 0f, y,
0f, 0f, 1f, z,
0f, 0f, 0f, 1f);
}
/**
* Sets the translation component of this matrix.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setTranslation (IVector3 translation) {
return setTranslation(translation.x(), translation.y(), translation.z());
}
/**
* Sets the translation component of this matrix.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setTranslation (double x, double y, double z) {
m30 = x;
m31 = y;
m32 = z;
return this;
}
/**
* Sets this to a rotation matrix that rotates one vector onto another.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToRotation (IVector3 from, IVector3 to) {
double angle = from.angle(to);
if (angle < MathUtil.EPSILON) {
return setToIdentity();
}
if (angle <= Math.PI - MathUtil.EPSILON) {
return setToRotation(angle, from.cross(to).normalizeLocal());
}
// it's a 180 degree rotation; any axis orthogonal to the from vector will do
Vector3 axis = new Vector3(0f, from.z(), -from.y());
double length = axis.length();
return setToRotation(Math.PI, length < MathUtil.EPSILON ?
axis.set(-from.z(), 0f, from.x()).normalizeLocal() :
axis.multLocal(1f / length));
}
/**
* Sets this to a rotation matrix.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToRotation (double angle, IVector3 axis) {
return setToRotation(angle, axis.x(), axis.y(), axis.z());
}
/**
* Sets this to a rotation matrix. The formula comes from the OpenGL documentation for the
* glRotatef function.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToRotation (double angle, double x, double y, double z) {
double c = Math.cos(angle), s = Math.sin(angle), omc = 1f - c;
double xs = x*s, ys = y*s, zs = z*s, xy = x*y, xz = x*z, yz = y*z;
return set(x*x*omc + c, xy*omc - zs, xz*omc + ys, 0f,
xy*omc + zs, y*y*omc + c, yz*omc - xs, 0f,
xz*omc - ys, yz*omc + xs, z*z*omc + c, 0f,
0f, 0f, 0f, 1f);
}
/**
* Sets this to a rotation matrix. The formula comes from the
* Matrix and Quaternion FAQ.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToRotation (IQuaternion quat) {
double x = quat.x(), y = quat.y(), z = quat.z(), w = quat.w();
double xx = x*x, yy = y*y, zz = z*z;
double xy = x*y, xz = x*z, xw = x*w;
double yz = y*z, yw = y*w, zw = z*w;
return set(1f - 2f*(yy + zz), 2f*(xy - zw), 2f*(xz + yw), 0f,
2f*(xy + zw), 1f - 2f*(xx + zz), 2f*(yz - xw), 0f,
2f*(xz - yw), 2f*(yz + xw), 1f - 2f*(xx + yy), 0f,
0f, 0f, 0f, 1f);
}
/**
* Sets this to a rotation plus scale matrix.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToRotationScale (IMatrix3 rotScale) {
return set(rotScale.m00(), rotScale.m01(), rotScale.m02(), 0f,
rotScale.m10(), rotScale.m11(), rotScale.m12(), 0f,
rotScale.m20(), rotScale.m21(), rotScale.m22(), 0f,
0, 0, 0, 1);
}
/**
* Sets this to a scale matrix.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToScale (IVector3 scale) {
return setToScale(scale.x(), scale.y(), scale.z());
}
/**
* Sets this to a uniform scale matrix.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToScale (double s) {
return setToScale(s, s, s);
}
/**
* Sets this to a scale matrix.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToScale (double x, double y, double z) {
return set(x, 0f, 0f, 0f,
0f, y, 0f, 0f,
0f, 0f, z, 0f,
0f, 0f, 0f, 1f);
}
/**
* Sets this to a reflection across a plane intersecting the origin with the supplied normal.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToReflection (IVector3 normal) {
return setToReflection(normal.x(), normal.y(), normal.z());
}
/**
* Sets this to a reflection across a plane intersecting the origin with the supplied normal.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToReflection (double x, double y, double z) {
double x2 = -2f*x, y2 = -2f*y, z2 = -2f*z;
double xy2 = x2*y, xz2 = x2*z, yz2 = y2*z;
return set(1f + x2*x, xy2, xz2, 0f,
xy2, 1f + y2*y, yz2, 0f,
xz2, yz2, 1f + z2*z, 0f,
0f, 0f, 0f, 1f);
}
/**
* Sets this to a reflection across the specified plane.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToReflection (IPlane plane) {
return setToReflection(plane.normal(), plane.constant());
}
/**
* Sets this to a reflection across the specified plane.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToReflection (IVector3 normal, double constant) {
return setToReflection(normal.x(), normal.y(), normal.z(), constant);
}
/**
* Sets this to a reflection across the specified plane.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToReflection (double x, double y, double z, double w) {
double x2 = -2f*x, y2 = -2f*y, z2 = -2f*z;
double xy2 = x2*y, xz2 = x2*z, yz2 = y2*z;
double x2y2z2 = x*x + y*y + z*z;
return set(1f + x2*x, xy2, xz2, x2*w*x2y2z2,
xy2, 1f + y2*y, yz2, y2*w*x2y2z2,
xz2, yz2, 1f + z2*z, z2*w*x2y2z2,
0f, 0f, 0f, 1f);
}
/**
* Sets this to a skew by the specified amount relative to the given plane.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToSkew (IPlane plane, IVector3 amount) {
return setToSkew(plane.normal(), plane.constant(), amount);
}
/**
* Sets this to a skew by the specified amount relative to the given plane.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToSkew (IVector3 normal, double constant, IVector3 amount) {
return setToSkew(normal.x(), normal.y(), normal.z(), constant,
amount.x(), amount.y(), amount.z());
}
/**
* Sets this to a skew by the specified amount relative to the given plane.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToSkew (double a, double b, double c, double d, double x, double y, double z) {
return set(1f + a*x, b*x, c*x, d*x,
a*y, 1f + b*y, c*y, d*y,
a*z, b*z, 1f + c*z, d*z,
0f, 0f, 0f, 1f);
}
/**
* Sets this to a perspective projection matrix. The formula comes from the OpenGL
* documentation for the gluPerspective function.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToPerspective (double fovy, double aspect, double near, double far) {
double f = 1f / Math.tan(fovy / 2f), dscale = 1f / (near - far);
return set(f/aspect, 0f, 0f, 0f,
0f, f, 0f, 0f,
0f, 0f, (far+near) * dscale, 2f * far * near * dscale,
0f, 0f, -1f, 0f);
}
/**
* Sets this to a perspective projection matrix. The formula comes from the OpenGL
* documentation for the glFrustum function.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToFrustum (
double left, double right, double bottom, double top, double near, double far) {
return setToFrustum(left, right, bottom, top, near, far, Vector3.UNIT_Z);
}
/**
* Sets this to a perspective projection matrix.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToFrustum (
double left, double right, double bottom, double top,
double near, double far, IVector3 nearFarNormal) {
double rrl = 1f / (right - left);
double rtb = 1f / (top - bottom);
double rnf = 1f / (near - far);
double n2 = 2f * near;
double s = (far + near) / (near*nearFarNormal.z() - far*nearFarNormal.z());
return set(n2 * rrl, 0f, (right + left) * rrl, 0f,
0f, n2 * rtb, (top + bottom) * rtb, 0f,
s * nearFarNormal.x(), s * nearFarNormal.y(), (far + near) * rnf, n2 * far * rnf,
0f, 0f, -1f, 0f);
}
/**
* Sets this to an orthographic projection matrix. The formula comes from the OpenGL
* documentation for the glOrtho function.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToOrtho (
double left, double right, double bottom, double top, double near, double far) {
return setToOrtho(left, right, bottom, top, near, far, Vector3.UNIT_Z);
}
/**
* Sets this to an orthographic projection matrix.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 setToOrtho (
double left, double right, double bottom, double top,
double near, double far, IVector3 nearFarNormal) {
double rlr = 1f / (left - right);
double rbt = 1f / (bottom - top);
double rnf = 1f / (near - far);
double s = 2f / (near*nearFarNormal.z() - far*nearFarNormal.z());
return set(-2f * rlr, 0f, 0f, (right + left) * rlr,
0f, -2f * rbt, 0f, (top + bottom) * rbt,
s * nearFarNormal.x(), s * nearFarNormal.y(), 2f * rnf, (far + near) * rnf,
0f, 0f, 0f, 1f);
}
/**
* Copies the contents of another matrix.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 set (IMatrix4 other) {
return set(other.m00(), other.m10(), other.m20(), other.m30(),
other.m01(), other.m11(), other.m21(), other.m31(),
other.m02(), other.m12(), other.m22(), other.m32(),
other.m03(), other.m13(), other.m23(), other.m33());
}
/**
* Copies the elements of a row-major array.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 set (double[] values) {
return set(values[0], values[1], values[2], values[3],
values[4], values[5], values[6], values[7],
values[8], values[9], values[10], values[11],
values[12], values[13], values[14], values[15]);
}
/**
* Sets the contents of this matrix from the supplied (column-major) buffer.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 set (DoubleBuffer buf) {
m00 = buf.get(); m01 = buf.get(); m02 = buf.get(); m03 = buf.get();
m10 = buf.get(); m11 = buf.get(); m12 = buf.get(); m13 = buf.get();
m20 = buf.get(); m21 = buf.get(); m22 = buf.get(); m23 = buf.get();
m30 = buf.get(); m31 = buf.get(); m32 = buf.get(); m33 = buf.get();
return this;
}
/**
* Sets all of the matrix's components at once.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 set (
double m00, double m10, double m20, double m30,
double m01, double m11, double m21, double m31,
double m02, double m12, double m22, double m32,
double m03, double m13, double m23, 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;
return this;
}
/**
* Transposes this matrix in-place.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 transposeLocal () {
return transpose(this);
}
/**
* Multiplies this matrix in-place by another.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 multLocal (IMatrix4 other) {
return mult(other, this);
}
/**
* Multiplies this matrix in-place by another, treating the matricees as affine.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 multAffineLocal (IMatrix4 other) {
return multAffine(other, this);
}
/**
* Inverts this matrix in-place.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 invertLocal () {
return invert(this);
}
/**
* Inverts this matrix in-place as an affine matrix.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 invertAffineLocal () {
return invertAffine(this);
}
/**
* Linearly interpolates between the this and the specified other matrix, placing the result in
* this matrix.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 lerpLocal (IMatrix4 other, double t) {
return lerp(other, t, this);
}
/**
* Linearly interpolates between this and the specified other matrix (treating the matrices as
* affine), placing the result in this matrix.
*
* @return a reference to this matrix, for chaining.
*/
public Matrix4 lerpAffineLocal (IMatrix4 other, double t) {
return lerpAffine(other, t, this);
}
@Override // from IMatrix4
public double m00 () {
return m00;
}
@Override // from IMatrix4
public double m10 () {
return m10;
}
@Override // from IMatrix4
public double m20 () {
return m20;
}
@Override // from IMatrix4
public double m30 () {
return m30;
}
@Override // from IMatrix4
public double m01 () {
return m01;
}
@Override // from IMatrix4
public double m11 () {
return m11;
}
@Override // from IMatrix4
public double m21 () {
return m21;
}
@Override // from IMatrix4
public double m31 () {
return m31;
}
@Override // from IMatrix4
public double m02 () {
return m02;
}
@Override // from IMatrix4
public double m12 () {
return m12;
}
@Override // from IMatrix4
public double m22 () {
return m22;
}
@Override // from IMatrix4
public double m32 () {
return m32;
}
@Override // from IMatrix4
public double m03 () {
return m03;
}
@Override // from IMatrix4
public double m13 () {
return m13;
}
@Override // from IMatrix4
public double m23 () {
return m23;
}
@Override // from IMatrix4
public double m33 () {
return m33;
}
@Override // from IMatrix4
public Matrix4 transpose () {
return transpose(new Matrix4());
}
@Override // from IMatrix4
public Matrix4 transpose (Matrix4 result) {
return result.set(m00, m01, m02, m03,
m10, m11, m12, m13,
m20, m21, m22, m23,
m30, m31, m32, m33);
}
@Override // from IMatrix4
public Matrix4 mult (IMatrix4 other) {
return mult(other, new Matrix4());
}
@Override // from IMatrix4
public Matrix4 mult (IMatrix4 other, Matrix4 result) {
double m00 = this.m00, m10 = this.m10, m20 = this.m20, m30 = this.m30;
double m01 = this.m01, m11 = this.m11, m21 = this.m21, m31 = this.m31;
double m02 = this.m02, m12 = this.m12, m22 = this.m22, m32 = this.m32;
double m03 = this.m03, m13 = this.m13, m23 = this.m23, m33 = this.m33;
double om00 = other.m00(), om10 = other.m10(), om20 = other.m20(), om30 = other.m30();
double om01 = other.m01(), om11 = other.m11(), om21 = other.m21(), om31 = other.m31();
double om02 = other.m02(), om12 = other.m12(), om22 = other.m22(), om32 = other.m32();
double om03 = other.m03(), om13 = other.m13(), om23 = other.m23(), om33 = other.m33();
return result.set(m00*om00 + m10*om01 + m20*om02 + m30*om03,
m00*om10 + m10*om11 + m20*om12 + m30*om13,
m00*om20 + m10*om21 + m20*om22 + m30*om23,
m00*om30 + m10*om31 + m20*om32 + m30*om33,
m01*om00 + m11*om01 + m21*om02 + m31*om03,
m01*om10 + m11*om11 + m21*om12 + m31*om13,
m01*om20 + m11*om21 + m21*om22 + m31*om23,
m01*om30 + m11*om31 + m21*om32 + m31*om33,
m02*om00 + m12*om01 + m22*om02 + m32*om03,
m02*om10 + m12*om11 + m22*om12 + m32*om13,
m02*om20 + m12*om21 + m22*om22 + m32*om23,
m02*om30 + m12*om31 + m22*om32 + m32*om33,
m03*om00 + m13*om01 + m23*om02 + m33*om03,
m03*om10 + m13*om11 + m23*om12 + m33*om13,
m03*om20 + m13*om21 + m23*om22 + m33*om23,
m03*om30 + m13*om31 + m23*om32 + m33*om33);
}
@Override // from IMatrix4
public boolean isAffine () {
return (m03 == 0f && m13 == 0f && m23 == 0f && m33 == 1f);
}
@Override // from IMatrix4
public boolean isMirrored () {
return m00*(m11*m22 - m12*m21) + m01*(m12*m20 - m10*m22) + m02*(m10*m21 - m11*m20) < 0f;
}
@Override // from IMatrix4
public Matrix4 multAffine (IMatrix4 other) {
return multAffine(other, new Matrix4());
}
@Override // from IMatrix4
public Matrix4 multAffine (IMatrix4 other, Matrix4 result) {
double m00 = this.m00, m10 = this.m10, m20 = this.m20, m30 = this.m30;
double m01 = this.m01, m11 = this.m11, m21 = this.m21, m31 = this.m31;
double m02 = this.m02, m12 = this.m12, m22 = this.m22, m32 = this.m32;
double om00 = other.m00(), om10 = other.m10(), om20 = other.m20(), om30 = other.m30();
double om01 = other.m01(), om11 = other.m11(), om21 = other.m21(), om31 = other.m31();
double om02 = other.m02(), om12 = other.m12(), om22 = other.m22(), om32 = other.m32();
return result.set(m00*om00 + m10*om01 + m20*om02,
m00*om10 + m10*om11 + m20*om12,
m00*om20 + m10*om21 + m20*om22,
m00*om30 + m10*om31 + m20*om32 + m30,
m01*om00 + m11*om01 + m21*om02,
m01*om10 + m11*om11 + m21*om12,
m01*om20 + m11*om21 + m21*om22,
m01*om30 + m11*om31 + m21*om32 + m31,
m02*om00 + m12*om01 + m22*om02,
m02*om10 + m12*om11 + m22*om12,
m02*om20 + m12*om21 + m22*om22,
m02*om30 + m12*om31 + m22*om32 + m32,
0f, 0f, 0f, 1f);
}
@Override // from IMatrix4
public Matrix4 invert () {
return invert(new Matrix4());
}
/**
* {@inheritDoc} This code is based on the examples in the
* Matrix and Quaternion FAQ.
*/
@Override // from IMatrix4
public Matrix4 invert (Matrix4 result) throws SingularMatrixException {
double m00 = this.m00, m10 = this.m10, m20 = this.m20, m30 = this.m30;
double m01 = this.m01, m11 = this.m11, m21 = this.m21, m31 = this.m31;
double m02 = this.m02, m12 = this.m12, m22 = this.m22, m32 = this.m32;
double m03 = this.m03, m13 = this.m13, m23 = this.m23, m33 = this.m33;
// compute the determinant, storing the subdeterminants for later use
double sd00 = m11*(m22*m33 - m23*m32) + m21*(m13*m32 - m12*m33) + m31*(m12*m23 - m13*m22);
double sd10 = m01*(m22*m33 - m23*m32) + m21*(m03*m32 - m02*m33) + m31*(m02*m23 - m03*m22);
double sd20 = m01*(m12*m33 - m13*m32) + m11*(m03*m32 - m02*m33) + m31*(m02*m13 - m03*m12);
double sd30 = m01*(m12*m23 - m13*m22) + m11*(m03*m22 - m02*m23) + m21*(m02*m13 - m03*m12);
double det = m00*sd00 + m20*sd20 - m10*sd10 - m30*sd30;
if (Math.abs(det) == 0f) {
// determinant is zero; matrix is not invertible
throw new SingularMatrixException(this.toString());
}
double rdet = 1f / det;
return result.set(
+sd00 * rdet,
-(m10*(m22*m33 - m23*m32) + m20*(m13*m32 - m12*m33) + m30*(m12*m23 - m13*m22)) * rdet,
+(m10*(m21*m33 - m23*m31) + m20*(m13*m31 - m11*m33) + m30*(m11*m23 - m13*m21)) * rdet,
-(m10*(m21*m32 - m22*m31) + m20*(m12*m31 - m11*m32) + m30*(m11*m22 - m12*m21)) * rdet,
-sd10 * rdet,
+(m00*(m22*m33 - m23*m32) + m20*(m03*m32 - m02*m33) + m30*(m02*m23 - m03*m22)) * rdet,
-(m00*(m21*m33 - m23*m31) + m20*(m03*m31 - m01*m33) + m30*(m01*m23 - m03*m21)) * rdet,
+(m00*(m21*m32 - m22*m31) + m20*(m02*m31 - m01*m32) + m30*(m01*m22 - m02*m21)) * rdet,
+sd20 * rdet,
-(m00*(m12*m33 - m13*m32) + m10*(m03*m32 - m02*m33) + m30*(m02*m13 - m03*m12)) * rdet,
+(m00*(m11*m33 - m13*m31) + m10*(m03*m31 - m01*m33) + m30*(m01*m13 - m03*m11)) * rdet,
-(m00*(m11*m32 - m12*m31) + m10*(m02*m31 - m01*m32) + m30*(m01*m12 - m02*m11)) * rdet,
-sd30 * rdet,
+(m00*(m12*m23 - m13*m22) + m10*(m03*m22 - m02*m23) + m20*(m02*m13 - m03*m12)) * rdet,
-(m00*(m11*m23 - m13*m21) + m10*(m03*m21 - m01*m23) + m20*(m01*m13 - m03*m11)) * rdet,
+(m00*(m11*m22 - m12*m21) + m10*(m02*m21 - m01*m22) + m20*(m01*m12 - m02*m11)) * rdet);
}
@Override // from IMatrix4
public Matrix4 invertAffine () {
return invertAffine(new Matrix4());
}
@Override // from IMatrix4
public Matrix4 invertAffine (Matrix4 result) throws SingularMatrixException {
double m00 = this.m00, m10 = this.m10, m20 = this.m20, m30 = this.m30;
double m01 = this.m01, m11 = this.m11, m21 = this.m21, m31 = this.m31;
double m02 = this.m02, m12 = this.m12, m22 = this.m22, m32 = this.m32;
// compute the determinant, storing the subdeterminants for later use
double sd00 = m11*m22 - m21*m12;
double sd10 = m01*m22 - m21*m02;
double sd20 = m01*m12 - m11*m02;
double det = m00*sd00 + m20*sd20 - m10*sd10;
if (Math.abs(det) == 0f) {
// determinant is zero; matrix is not invertible
throw new SingularMatrixException(this.toString());
}
double rdet = 1f / det;
return result.set(
+sd00 * rdet,
-(m10*m22 - m20*m12) * rdet,
+(m10*m21 - m20*m11) * rdet,
-(m10*(m21*m32 - m22*m31) + m20*(m12*m31 - m11*m32) + m30*sd00) * rdet,
-sd10 * rdet,
+(m00*m22 - m20*m02) * rdet,
-(m00*m21 - m20*m01) * rdet,
+(m00*(m21*m32 - m22*m31) + m20*(m02*m31 - m01*m32) + m30*sd10) * rdet,
+sd20 * rdet,
-(m00*m12 - m10*m02) * rdet,
+(m00*m11 - m10*m01) * rdet,
-(m00*(m11*m32 - m12*m31) + m10*(m02*m31 - m01*m32) + m30*sd20) * rdet,
0f, 0f, 0f, 1f);
}
@Override // from IMatrix4
public Matrix4 lerp (IMatrix4 other, double t) {
return lerp(other, t, new Matrix4());
}
@Override // from IMatrix4
public Matrix4 lerp (IMatrix4 other, double t, Matrix4 result) {
double m00 = this.m00, m10 = this.m10, m20 = this.m20, m30 = this.m30;
double m01 = this.m01, m11 = this.m11, m21 = this.m21, m31 = this.m31;
double m02 = this.m02, m12 = this.m12, m22 = this.m22, m32 = this.m32;
double m03 = this.m03, m13 = this.m13, m23 = this.m23, m33 = this.m33;
return result.set(m00 + t*(other.m00() - m00),
m10 + t*(other.m10() - m10),
m20 + t*(other.m20() - m20),
m30 + t*(other.m30() - m30),
m01 + t*(other.m01() - m01),
m11 + t*(other.m11() - m11),
m21 + t*(other.m21() - m21),
m31 + t*(other.m31() - m31),
m02 + t*(other.m02() - m02),
m12 + t*(other.m12() - m12),
m22 + t*(other.m22() - m22),
m32 + t*(other.m32() - m32),
m03 + t*(other.m03() - m03),
m13 + t*(other.m13() - m13),
m23 + t*(other.m23() - m23),
m33 + t*(other.m33() - m33));
}
@Override // from IMatrix4
public Matrix4 lerpAffine (IMatrix4 other, double t) {
return lerpAffine(other, t, new Matrix4());
}
@Override // from IMatrix4
public Matrix4 lerpAffine (IMatrix4 other, double t, Matrix4 result) {
double m00 = this.m00, m10 = this.m10, m20 = this.m20, m30 = this.m30;
double m01 = this.m01, m11 = this.m11, m21 = this.m21, m31 = this.m31;
double m02 = this.m02, m12 = this.m12, m22 = this.m22, m32 = this.m32;
return result.set(m00 + t*(other.m00() - m00),
m10 + t*(other.m10() - m10),
m20 + t*(other.m20() - m20),
m30 + t*(other.m30() - m30),
m01 + t*(other.m01() - m01),
m11 + t*(other.m11() - m11),
m21 + t*(other.m21() - m21),
m31 + t*(other.m31() - m31),
m02 + t*(other.m02() - m02),
m12 + t*(other.m12() - m12),
m22 + t*(other.m22() - m22),
m32 + t*(other.m32() - m32),
0f, 0f, 0f, 1f);
}
@Override // from IMatrix4
public DoubleBuffer get (DoubleBuffer buf) {
buf.put(m00).put(m01).put(m02).put(m03);
buf.put(m10).put(m11).put(m12).put(m13);
buf.put(m20).put(m21).put(m22).put(m23);
buf.put(m30).put(m31).put(m32).put(m33);
return buf;
}
@Override // from IMatrix4
public Vector3 projectPointLocal (Vector3 point) {
return projectPoint(point, point);
}
@Override // from IMatrix4
public Vector3 projectPoint (IVector3 point) {
return projectPoint(point, new Vector3());
}
@Override // from IMatrix4
public Vector3 projectPoint (IVector3 point, Vector3 result) {
double px = point.x(), py = point.y(), pz = point.z();
double rw = 1f / (m03*px + m13*py + m23*pz + m33);
return result.set((m00*px + m10*py + m20*pz + m30) * rw,
(m01*px + m11*py + m21*pz + m31) * rw,
(m02*px + m12*py + m22*pz + m32) * rw);
}
@Override // from IMatrix4
public Vector3 transformPointLocal (Vector3 point) {
return transformPoint(point, point);
}
@Override // from IMatrix4
public Vector3 transformPoint (IVector3 point) {
return transformPoint(point, new Vector3());
}
@Override // from IMatrix4
public Vector3 transformPoint (IVector3 point, Vector3 result) {
double px = point.x(), py = point.y(), pz = point.z();
return result.set(m00*px + m10*py + m20*pz + m30,
m01*px + m11*py + m21*pz + m31,
m02*px + m12*py + m22*pz + m32);
}
@Override // from IMatrix4
public double transformPointZ (IVector3 point) {
return m02*point.x() + m12*point.y() + m22*point.z() + m32;
}
@Override // from IMatrix4
public Vector3 transformVectorLocal (Vector3 vector) {
return transformVector(vector, vector);
}
@Override // from IMatrix4
public Vector3 transformVector (IVector3 vector) {
return transformVector(vector, new Vector3());
}
@Override // from IMatrix4
public Vector3 transformVector (IVector3 vector, Vector3 result) {
double vx = vector.x(), vy = vector.y(), vz = vector.z();
return result.set(m00*vx + m10*vy + m20*vz,
m01*vx + m11*vy + m21*vz,
m02*vx + m12*vy + m22*vz);
}
@Override // from IMatrix4
public Vector4 transform (IVector4 vector) {
return transform(vector, new Vector4());
}
@Override // from IMatrix4
public Vector4 transform (IVector4 vector, Vector4 result) {
double vx = vector.x(), vy = vector.y(), vz = vector.z(), vw = vector.w();
return result.set(m00*vx + m10*vy + m20*vz + m30*vw,
m01*vx + m11*vy + m21*vz + m31*vw,
m02*vx + m12*vy + m22*vz + m32*vw,
m03*vx + m13*vy + m23*vz + m33*vw);
}
@Override // from IMatrix4
public Quaternion extractRotation () {
return extractRotation(new Quaternion());
}
/**
* {@inheritDoc} This uses the iterative polar decomposition algorithm described by
* Ken
* Shoemake.
*/
@Override // from IMatrix4
public Quaternion extractRotation (Quaternion result) throws SingularMatrixException {
// start with the contents of the upper 3x3 portion of the matrix
double n00 = this.m00, n10 = this.m10, n20 = this.m20;
double n01 = this.m01, n11 = this.m11, n21 = this.m21;
double n02 = this.m02, n12 = this.m12, n22 = this.m22;
for (int ii = 0; ii < 10; ii++) {
// store the results of the previous iteration
double o00 = n00, o10 = n10, o20 = n20;
double o01 = n01, o11 = n11, o21 = n21;
double o02 = n02, o12 = n12, o22 = n22;
// compute average of the matrix with its inverse transpose
double sd00 = o11*o22 - o21*o12;
double sd10 = o01*o22 - o21*o02;
double sd20 = o01*o12 - o11*o02;
double det = o00*sd00 + o20*sd20 - o10*sd10;
if (Math.abs(det) == 0f) {
// determinant is zero; matrix is not invertible
throw new SingularMatrixException(this.toString());
}
double hrdet = 0.5f / det;
n00 = +sd00 * hrdet + o00*0.5f;
n10 = -sd10 * hrdet + o10*0.5f;
n20 = +sd20 * hrdet + o20*0.5f;
n01 = -(o10*o22 - o20*o12) * hrdet + o01*0.5f;
n11 = +(o00*o22 - o20*o02) * hrdet + o11*0.5f;
n21 = -(o00*o12 - o10*o02) * hrdet + o21*0.5f;
n02 = +(o10*o21 - o20*o11) * hrdet + o02*0.5f;
n12 = -(o00*o21 - o20*o01) * hrdet + o12*0.5f;
n22 = +(o00*o11 - o10*o01) * hrdet + o22*0.5f;
// compute the difference; if it's small enough, we're done
double d00 = n00 - o00, d10 = n10 - o10, d20 = n20 - o20;
double d01 = n01 - o01, d11 = n11 - o11, d21 = n21 - o21;
double d02 = n02 - o02, d12 = n12 - o12, d22 = n22 - o22;
if (d00*d00 + d10*d10 + d20*d20 + d01*d01 + d11*d11 + d21*d21 +
d02*d02 + d12*d12 + d22*d22 < MathUtil.EPSILON) {
break;
}
}
// now that we have a nice orthogonal matrix, we can extract the rotation quaternion
// using the method described in http://en.wikipedia.org/wiki/Rotation_matrix#Conversions
double x2 = Math.abs(1f + n00 - n11 - n22);
double y2 = Math.abs(1f - n00 + n11 - n22);
double z2 = Math.abs(1f - n00 - n11 + n22);
double w2 = Math.abs(1f + n00 + n11 + n22);
result.set(
0.5f * Math.sqrt(x2) * (n12 >= n21 ? +1f : -1f),
0.5f * Math.sqrt(y2) * (n20 >= n02 ? +1f : -1f),
0.5f * Math.sqrt(z2) * (n01 >= n10 ? +1f : -1f),
0.5f * Math.sqrt(w2));
return result;
}
@Override // from IMatrix4
public Matrix3 extractRotationScale (Matrix3 result) {
return result.set(m00, m01, m02,
m10, m11, m12,
m20, m21, m22);
}
@Override // from IMatrix4
public Vector3 extractScale () {
return extractScale(new Vector3());
}
@Override // from IMatrix4
public Vector3 extractScale (Vector3 result) {
return result.set(Math.sqrt(m00*m00 + m01*m01 + m02*m02),
Math.sqrt(m10*m10 + m11*m11 + m12*m12),
Math.sqrt(m20*m20 + m21*m21 + m22*m22));
}
@Override // from IMatrix4
public double approximateUniformScale () {
return Math.cbrt(m00*(m11*m22 - m12*m21) +
m01*(m12*m20 - m10*m22) +
m02*(m10*m21 - m11*m20));
}
@Override // from IMatrix4
public boolean epsilonEquals (IMatrix4 other, double epsilon) {
return (Math.abs(m00 - other.m00()) < epsilon &&
Math.abs(m10 - other.m10()) < epsilon &&
Math.abs(m20 - other.m20()) < epsilon &&
Math.abs(m30 - other.m30()) < epsilon &&
Math.abs(m01 - other.m01()) < epsilon &&
Math.abs(m11 - other.m11()) < epsilon &&
Math.abs(m21 - other.m21()) < epsilon &&
Math.abs(m31 - other.m31()) < epsilon &&
Math.abs(m02 - other.m02()) < epsilon &&
Math.abs(m12 - other.m12()) < epsilon &&
Math.abs(m22 - other.m22()) < epsilon &&
Math.abs(m32 - other.m32()) < epsilon &&
Math.abs(m03 - other.m03()) < epsilon &&
Math.abs(m13 - other.m13()) < epsilon &&
Math.abs(m23 - other.m23()) < epsilon &&
Math.abs(m33 - other.m33()) < epsilon);
}
@Override
public String toString () {
return ("[[" + m00 + ", " + m10 + ", " + m20 + ", " + m30 + "], " +
"[" + m01 + ", " + m11 + ", " + m21 + ", " + m31 + "], " +
"[" + m02 + ", " + m12 + ", " + m22 + ", " + m32 + "], " +
"[" + m03 + ", " + m13 + ", " + m23 + ", " + m33 + "]]");
}
@Override
public int hashCode () {
return Platform.hashCode(m00) ^ Platform.hashCode(m10) ^
Platform.hashCode(m20) ^ Platform.hashCode(m30) ^
Platform.hashCode(m01) ^ Platform.hashCode(m11) ^
Platform.hashCode(m21) ^ Platform.hashCode(m31) ^
Platform.hashCode(m02) ^ Platform.hashCode(m12) ^
Platform.hashCode(m22) ^ Platform.hashCode(m32) ^
Platform.hashCode(m03) ^ Platform.hashCode(m13) ^
Platform.hashCode(m23) ^ Platform.hashCode(m33);
}
@Override
public boolean equals (Object other) {
if (!(other instanceof Matrix4)) {
return false;
}
Matrix4 omat = (Matrix4)other;
return (m00 == omat.m00 && m10 == omat.m10 && m20 == omat.m20 && m30 == omat.m30 &&
m01 == omat.m01 && m11 == omat.m11 && m21 == omat.m21 && m31 == omat.m31 &&
m02 == omat.m02 && m12 == omat.m12 && m22 == omat.m22 && m32 == omat.m32 &&
m03 == omat.m03 && m13 == omat.m13 && m23 == omat.m23 && m33 == omat.m33);
}
}