/* * $RCSfile$ * * Copyright (c) 2007 Sun Microsystems, Inc. All rights reserved. * * Use is subject to license terms. * * $Revision$ * $Date$ * $State$ */ package javax.vecmath; import java.lang.Math; /** * A single precision floating point 3 by 3 matrix. * Primarily to support 3D rotations. * */ public class Matrix3f implements java.io.Serializable, Cloneable { // Compatible with 1.1 static final long serialVersionUID = 329697160112089834L; /** * The first matrix element in the first row. */ public float m00; /** * The second matrix element in the first row. */ public float m01; /** * The third matrix element in the first row. */ public float m02; /** * The first matrix element in the second row. */ public float m10; /** * The second matrix element in the second row. */ public float m11; /** * The third matrix element in the second row. */ public float m12; /** * The first matrix element in the third row. */ public float m20; /** * The second matrix element in the third row. */ public float m21; /** * The third matrix element in the third row. */ public float m22; /* double[] tmp = new double[9]; // scratch matrix double[] tmp_rot = new double[9]; // scratch matrix double[] tmp_scale = new double[3]; // scratch matrix */ private static final double EPS = 1.0E-8; /** * Constructs and initializes a Matrix3f from the specified nine values. * @param m00 the [0][0] element * @param m01 the [0][1] element * @param m02 the [0][2] element * @param m10 the [1][0] element * @param m11 the [1][1] element * @param m12 the [1][2] element * @param m20 the [2][0] element * @param m21 the [2][1] element * @param m22 the [2][2] element */ public Matrix3f(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22) { this.m00 = m00; this.m01 = m01; this.m02 = m02; this.m10 = m10; this.m11 = m11; this.m12 = m12; this.m20 = m20; this.m21 = m21; this.m22 = m22; } /** * Constructs and initializes a Matrix3f from the specified * nine-element array. this.m00 =v[0], this.m01=v[1], etc. * @param v the array of length 9 containing in order */ public Matrix3f(float[] v) { this.m00 = v[ 0]; this.m01 = v[ 1]; this.m02 = v[ 2]; this.m10 = v[ 3]; this.m11 = v[ 4]; this.m12 = v[ 5]; this.m20 = v[ 6]; this.m21 = v[ 7]; this.m22 = v[ 8]; } /** * Constructs a new matrix with the same values as the * Matrix3d parameter. * @param m1 the source matrix */ public Matrix3f(Matrix3d m1) { this.m00 = (float)m1.m00; this.m01 = (float)m1.m01; this.m02 = (float)m1.m02; this.m10 = (float)m1.m10; this.m11 = (float)m1.m11; this.m12 = (float)m1.m12; this.m20 = (float)m1.m20; this.m21 = (float)m1.m21; this.m22 = (float)m1.m22; } /** * Constructs a new matrix with the same values as the * Matrix3f parameter. * @param m1 the source matrix */ public Matrix3f(Matrix3f m1) { this.m00 = m1.m00; this.m01 = m1.m01; this.m02 = m1.m02; this.m10 = m1.m10; this.m11 = m1.m11; this.m12 = m1.m12; this.m20 = m1.m20; this.m21 = m1.m21; this.m22 = m1.m22; } /** * Constructs and initializes a Matrix3f to all zeros. */ public Matrix3f() { this.m00 = (float) 0.0; this.m01 = (float) 0.0; this.m02 = (float) 0.0; this.m10 = (float) 0.0; this.m11 = (float) 0.0; this.m12 = (float) 0.0; this.m20 = (float) 0.0; this.m21 = (float) 0.0; this.m22 = (float) 0.0; } /** * Returns a string that contains the values of this Matrix3f. * @return the String representation */ public String toString() { return this.m00 + ", " + this.m01 + ", " + this.m02 + "\n" + this.m10 + ", " + this.m11 + ", " + this.m12 + "\n" + this.m20 + ", " + this.m21 + ", " + this.m22 + "\n"; } /** * Sets this Matrix3f to identity. */ public final void setIdentity() { this.m00 = (float) 1.0; this.m01 = (float) 0.0; this.m02 = (float) 0.0; this.m10 = (float) 0.0; this.m11 = (float) 1.0; this.m12 = (float) 0.0; this.m20 = (float) 0.0; this.m21 = (float) 0.0; this.m22 = (float) 1.0; } /** * Sets the scale component of the current matrix by factoring * out the current scale (by doing an SVD) and multiplying by * the new scale. * @param scale the new scale amount */ public final void setScale(float scale) { double[] tmp_rot = new double[9]; // scratch matrix double[] tmp_scale = new double[3]; // scratch matrix getScaleRotate( tmp_scale, tmp_rot ); this.m00 = (float)(tmp_rot[0] * scale); this.m01 = (float)(tmp_rot[1] * scale); this.m02 = (float)(tmp_rot[2] * scale); this.m10 = (float)(tmp_rot[3] * scale); this.m11 = (float)(tmp_rot[4] * scale); this.m12 = (float)(tmp_rot[5] * scale); this.m20 = (float)(tmp_rot[6] * scale); this.m21 = (float)(tmp_rot[7] * scale); this.m22 = (float)(tmp_rot[8] * scale); } /** * Sets the specified element of this matrix3f to the value provided. * @param row the row number to be modified (zero indexed) * @param column the column number to be modified (zero indexed) * @param value the new value */ public final void setElement(int row, int column, float value) { switch (row) { case 0: switch(column) { case 0: this.m00 = value; break; case 1: this.m01 = value; break; case 2: this.m02 = value; break; default: throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3f0")); } break; case 1: switch(column) { case 0: this.m10 = value; break; case 1: this.m11 = value; break; case 2: this.m12 = value; break; default: throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3f0")); } break; case 2: switch(column) { case 0: this.m20 = value; break; case 1: this.m21 = value; break; case 2: this.m22 = value; break; default: throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3f0")); } break; default: throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3f0")); } } /** * Copies the matrix values in the specified row into the vector parameter. * @param row the matrix row * @param v the vector into which the matrix row values will be copied */ public final void getRow(int row, Vector3f v) { if( row == 0 ) { v.x = m00; v.y = m01; v.z = m02; } else if(row == 1) { v.x = m10; v.y = m11; v.z = m12; } else if(row == 2) { v.x = m20; v.y = m21; v.z = m22; } else { throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3f1")); } } /** * Copies the matrix values in the specified row into the array parameter. * @param row the matrix row * @param v the array into which the matrix row values will be copied */ public final void getRow(int row, float v[]) { if( row == 0 ) { v[0] = m00; v[1] = m01; v[2] = m02; } else if(row == 1) { v[0] = m10; v[1] = m11; v[2] = m12; } else if(row == 2) { v[0] = m20; v[1] = m21; v[2] = m22; } else { throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3f1")); } } /** * Copies the matrix values in the specified column into the vector * parameter. * @param column the matrix column * @param v the vector into which the matrix row values will be copied */ public final void getColumn(int column, Vector3f v) { if( column == 0 ) { v.x = m00; v.y = m10; v.z = m20; } else if(column == 1) { v.x = m01; v.y = m11; v.z = m21; }else if(column == 2){ v.x = m02; v.y = m12; v.z = m22; } else { throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3f3")); } } /** * Copies the matrix values in the specified column into the array * parameter. * @param column the matrix column * @param v the array into which the matrix row values will be copied */ public final void getColumn(int column, float v[]) { if( column == 0 ) { v[0] = m00; v[1] = m10; v[2] = m20; } else if(column == 1) { v[0] = m01; v[1] = m11; v[2] = m21; }else if(column == 2) { v[0] = m02; v[1] = m12; v[2] = m22; }else { throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3f3")); } } /** * Retrieves the value at the specified row and column of this * matrix. * @param row the row number to be retrieved (zero indexed) * @param column the column number to be retrieved (zero indexed) * @return the value at the indexed element. */ public final float getElement(int row, int column) { switch (row) { case 0: switch(column) { case 0: return(this.m00); case 1: return(this.m01); case 2: return(this.m02); default: break; } break; case 1: switch(column) { case 0: return(this.m10); case 1: return(this.m11); case 2: return(this.m12); default: break; } break; case 2: switch(column) { case 0: return(this.m20); case 1: return(this.m21); case 2: return(this.m22); default: break; } break; default: break; } throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3f5")); } /** * Sets the specified row of this matrix3f to the three values provided. * @param row the row number to be modified (zero indexed) * @param x the first column element * @param y the second column element * @param z the third column element */ public final void setRow(int row, float x, float y, float z) { switch (row) { case 0: this.m00 = x; this.m01 = y; this.m02 = z; break; case 1: this.m10 = x; this.m11 = y; this.m12 = z; break; case 2: this.m20 = x; this.m21 = y; this.m22 = z; break; default: throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3f6")); } } /** * Sets the specified row of this matrix3f to the Vector provided. * @param row the row number to be modified (zero indexed) * @param v the replacement row */ public final void setRow(int row, Vector3f v) { switch (row) { case 0: this.m00 = v.x; this.m01 = v.y; this.m02 = v.z; break; case 1: this.m10 = v.x; this.m11 = v.y; this.m12 = v.z; break; case 2: this.m20 = v.x; this.m21 = v.y; this.m22 = v.z; break; default: throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3f6")); } } /** * Sets the specified row of this matrix3f to the three values provided. * @param row the row number to be modified (zero indexed) * @param v the replacement row */ public final void setRow(int row, float v[]) { switch (row) { case 0: this.m00 = v[0]; this.m01 = v[1]; this.m02 = v[2]; break; case 1: this.m10 = v[0]; this.m11 = v[1]; this.m12 = v[2]; break; case 2: this.m20 = v[0]; this.m21 = v[1]; this.m22 = v[2]; break; default: throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3f6")); } } /** * Sets the specified column of this matrix3f to the three values provided. * @param column the column number to be modified (zero indexed) * @param x the first row element * @param y the second row element * @param z the third row element */ public final void setColumn(int column, float x, float y, float z) { switch (column) { case 0: this.m00 = x; this.m10 = y; this.m20 = z; break; case 1: this.m01 = x; this.m11 = y; this.m21 = z; break; case 2: this.m02 = x; this.m12 = y; this.m22 = z; break; default: throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3f9")); } } /** * Sets the specified column of this matrix3f to the vector provided. * @param column the column number to be modified (zero indexed) * @param v the replacement column */ public final void setColumn(int column, Vector3f v) { switch (column) { case 0: this.m00 = v.x; this.m10 = v.y; this.m20 = v.z; break; case 1: this.m01 = v.x; this.m11 = v.y; this.m21 = v.z; break; case 2: this.m02 = v.x; this.m12 = v.y; this.m22 = v.z; break; default: throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3f9")); } } /** * Sets the specified column of this matrix3f to the three values provided. * @param column the column number to be modified (zero indexed) * @param v the replacement column */ public final void setColumn(int column, float v[]) { switch (column) { case 0: this.m00 = v[0]; this.m10 = v[1]; this.m20 = v[2]; break; case 1: this.m01 = v[0]; this.m11 = v[1]; this.m21 = v[2]; break; case 2: this.m02 = v[0]; this.m12 = v[1]; this.m22 = v[2]; break; default: throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3f9")); } } /** * Performs an SVD normalization of this matrix to calculate * and return the uniform scale factor. If the matrix has non-uniform * scale factors, the largest of the x, y, and z scale factors will * be returned. This matrix is not modified. * @return the scale factor of this matrix */ public final float getScale() { double[] tmp_rot = new double[9]; // scratch matrix double[] tmp_scale = new double[3]; // scratch matrix getScaleRotate(tmp_scale, tmp_rot); return( (float)Matrix3d.max3(tmp_scale )); } /** * Adds a scalar to each component of this matrix. * @param scalar the scalar adder */ public final void add(float scalar) { m00 += scalar; m01 += scalar; m02 += scalar; m10 += scalar; m11 += scalar; m12 += scalar; m20 += scalar; m21 += scalar; m22 += scalar; } /** * Adds a scalar to each component of the matrix m1 and places * the result into this. Matrix m1 is not modified. * @param scalar the scalar adder. * @param m1 the original matrix values */ public final void add(float scalar, Matrix3f m1) { this.m00 = m1.m00 + scalar; this.m01 = m1.m01 + scalar; this.m02 = m1.m02 + scalar; this.m10 = m1.m10 + scalar; this.m11 = m1.m11 + scalar; this.m12 = m1.m12 + scalar; this.m20 = m1.m20 + scalar; this.m21 = m1.m21 + scalar; this.m22 = m1.m22 + scalar; } /** * Sets the value of this matrix to the matrix sum of matrices m1 and m2. * @param m1 the first matrix * @param m2 the second matrix */ public final void add(Matrix3f m1, Matrix3f m2) { this.m00 = m1.m00 + m2.m00; this.m01 = m1.m01 + m2.m01; this.m02 = m1.m02 + m2.m02; this.m10 = m1.m10 + m2.m10; this.m11 = m1.m11 + m2.m11; this.m12 = m1.m12 + m2.m12; this.m20 = m1.m20 + m2.m20; this.m21 = m1.m21 + m2.m21; this.m22 = m1.m22 + m2.m22; } /** * Sets the value of this matrix to the matrix sum of itself and * matrix m1. * @param m1 the other matrix */ public final void add(Matrix3f m1) { this.m00 += m1.m00; this.m01 += m1.m01; this.m02 += m1.m02; this.m10 += m1.m10; this.m11 += m1.m11; this.m12 += m1.m12; this.m20 += m1.m20; this.m21 += m1.m21; this.m22 += m1.m22; } /** * Sets the value of this matrix to the matrix difference * of matrices m1 and m2. * @param m1 the first matrix * @param m2 the second matrix */ public final void sub(Matrix3f m1, Matrix3f m2) { this.m00 = m1.m00 - m2.m00; this.m01 = m1.m01 - m2.m01; this.m02 = m1.m02 - m2.m02; this.m10 = m1.m10 - m2.m10; this.m11 = m1.m11 - m2.m11; this.m12 = m1.m12 - m2.m12; this.m20 = m1.m20 - m2.m20; this.m21 = m1.m21 - m2.m21; this.m22 = m1.m22 - m2.m22; } /** * Sets the value of this matrix to the matrix difference * of itself and matrix m1 (this = this - m1). * @param m1 the other matrix */ public final void sub(Matrix3f m1) { this.m00 -= m1.m00; this.m01 -= m1.m01; this.m02 -= m1.m02; this.m10 -= m1.m10; this.m11 -= m1.m11; this.m12 -= m1.m12; this.m20 -= m1.m20; this.m21 -= m1.m21; this.m22 -= m1.m22; } /** * Sets the value of this matrix to its transpose. */ public final void transpose() { float temp; temp = this.m10; this.m10 = this.m01; this.m01 = temp; temp = this.m20; this.m20 = this.m02; this.m02 = temp; temp = this.m21; this.m21 = this.m12; this.m12 = temp; } /** * Sets the value of this matrix to the transpose of the argument matrix. * @param m1 the matrix to be transposed */ public final void transpose(Matrix3f m1) { if (this != m1) { this.m00 = m1.m00; this.m01 = m1.m10; this.m02 = m1.m20; this.m10 = m1.m01; this.m11 = m1.m11; this.m12 = m1.m21; this.m20 = m1.m02; this.m21 = m1.m12; this.m22 = m1.m22; } else this.transpose(); } /** * Sets the value of this matrix to the matrix conversion of the * (single precision) quaternion argument. * @param q1 the quaternion to be converted */ public final void set(Quat4f q1) { this.m00 = 1.0f - 2.0f*q1.y*q1.y - 2.0f*q1.z*q1.z; this.m10 = 2.0f*(q1.x*q1.y + q1.w*q1.z); this.m20 = 2.0f*(q1.x*q1.z - q1.w*q1.y); this.m01 = 2.0f*(q1.x*q1.y - q1.w*q1.z); this.m11 = 1.0f - 2.0f*q1.x*q1.x - 2.0f*q1.z*q1.z; this.m21 = 2.0f*(q1.y*q1.z + q1.w*q1.x); this.m02 = 2.0f*(q1.x*q1.z + q1.w*q1.y); this.m12 = 2.0f*(q1.y*q1.z - q1.w*q1.x); this.m22 = 1.0f - 2.0f*q1.x*q1.x - 2.0f*q1.y*q1.y; } /** * Sets the value of this matrix to the matrix conversion of the * (single precision) axis and angle argument. * @param a1 the axis and angle to be converted */ public final void set(AxisAngle4f a1) { float mag = (float)Math.sqrt( a1.x*a1.x + a1.y*a1.y + a1.z*a1.z); if( mag < EPS ) { m00 = 1.0f; m01 = 0.0f; m02 = 0.0f; m10 = 0.0f; m11 = 1.0f; m12 = 0.0f; m20 = 0.0f; m21 = 0.0f; m22 = 1.0f; } else { mag = 1.0f/mag; float ax = a1.x*mag; float ay = a1.y*mag; float az = a1.z*mag; float sinTheta = (float)Math.sin((float)a1.angle); float cosTheta = (float)Math.cos((float)a1.angle); float t = (float)1.0 - cosTheta; float xz = ax * az; float xy = ax * ay; float yz = ay * az; m00 = t * ax * ax + cosTheta; m01 = t * xy - sinTheta * az; m02 = t * xz + sinTheta * ay; m10 = t * xy + sinTheta * az; m11 = t * ay * ay + cosTheta; m12 = t * yz - sinTheta * ax; m20 = t * xz - sinTheta * ay; m21 = t * yz + sinTheta * ax; m22 = t * az * az + cosTheta; } } /** * Sets the value of this matrix to the matrix conversion of the * (double precision) axis and angle argument. * @param a1 the axis and angle to be converted */ public final void set(AxisAngle4d a1) { double mag = Math.sqrt( a1.x*a1.x + a1.y*a1.y + a1.z*a1.z); if( mag < EPS ) { m00 = 1.0f; m01 = 0.0f; m02 = 0.0f; m10 = 0.0f; m11 = 1.0f; m12 = 0.0f; m20 = 0.0f; m21 = 0.0f; m22 = 1.0f; } else { mag = 1.0/mag; double ax = a1.x*mag; double ay = a1.y*mag; double az = a1.z*mag; double sinTheta = Math.sin(a1.angle); double cosTheta = Math.cos(a1.angle); double t = 1.0 - cosTheta; double xz = ax * az; double xy = ax * ay; double yz = ay * az; m00 = (float)(t * ax * ax + cosTheta); m01 = (float)(t * xy - sinTheta * az); m02 = (float)(t * xz + sinTheta * ay); m10 = (float)(t * xy + sinTheta * az); m11 = (float)(t * ay * ay + cosTheta); m12 = (float)(t * yz - sinTheta * ax); m20 = (float)(t * xz - sinTheta * ay); m21 = (float)(t * yz + sinTheta * ax); m22 = (float)(t * az * az + cosTheta); } } /** * Sets the value of this matrix to the matrix conversion of the * (single precision) quaternion argument. * @param q1 the quaternion to be converted */ public final void set(Quat4d q1) { this.m00 = (float) (1.0 - 2.0*q1.y*q1.y - 2.0*q1.z*q1.z); this.m10 = (float) (2.0*(q1.x*q1.y + q1.w*q1.z)); this.m20 = (float) (2.0*(q1.x*q1.z - q1.w*q1.y)); this.m01 = (float) (2.0*(q1.x*q1.y - q1.w*q1.z)); this.m11 = (float) (1.0 - 2.0*q1.x*q1.x - 2.0*q1.z*q1.z); this.m21 = (float) (2.0*(q1.y*q1.z + q1.w*q1.x)); this.m02 = (float) (2.0*(q1.x*q1.z + q1.w*q1.y)); this.m12 = (float) (2.0*(q1.y*q1.z - q1.w*q1.x)); this.m22 = (float) (1.0 - 2.0*q1.x*q1.x - 2.0*q1.y*q1.y); } /** * Sets the values in this Matrix3f equal to the row-major * array parameter (ie, the first three elements of the * array will be copied into the first row of this matrix, etc.). * @param m the single precision array of length 9 */ public final void set(float[] m) { m00 = m[0]; m01 = m[1]; m02 = m[2]; m10 = m[3]; m11 = m[4]; m12 = m[5]; m20 = m[6]; m21 = m[7]; m22 = m[8]; } /** * Sets the value of this matrix to the value of the Matrix3f * argument. * @param m1 the source matrix3f */ public final void set(Matrix3f m1) { this.m00 = m1.m00; this.m01 = m1.m01; this.m02 = m1.m02; this.m10 = m1.m10; this.m11 = m1.m11; this.m12 = m1.m12; this.m20 = m1.m20; this.m21 = m1.m21; this.m22 = m1.m22; } /** * Sets the value of this matrix to the float value of the Matrix3d * argument. * @param m1 the source matrix3d */ public final void set(Matrix3d m1) { this.m00 = (float)m1.m00; this.m01 = (float)m1.m01; this.m02 = (float)m1.m02; this.m10 = (float)m1.m10; this.m11 = (float)m1.m11; this.m12 = (float)m1.m12; this.m20 = (float)m1.m20; this.m21 = (float)m1.m21; this.m22 = (float)m1.m22; } /** * Sets the value of this matrix to the matrix inverse * of the passed matrix m1. * @param m1 the matrix to be inverted */ public final void invert(Matrix3f m1) { invertGeneral( m1); } /** * Inverts this matrix in place. */ public final void invert() { invertGeneral( this ); } /** * General invert routine. Inverts m1 and places the result in "this". * Note that this routine handles both the "this" version and the * non-"this" version. * * Also note that since this routine is slow anyway, we won't worry * about allocating a little bit of garbage. */ private final void invertGeneral(Matrix3f m1) { double temp[] = new double[9]; double result[] = new double[9]; int row_perm[] = new int[3]; int i, r, c; // Use LU decomposition and backsubstitution code specifically // for floating-point 3x3 matrices. // Copy source matrix to t1tmp temp[0] = (double)m1.m00; temp[1] = (double)m1.m01; temp[2] = (double)m1.m02; temp[3] = (double)m1.m10; temp[4] = (double)m1.m11; temp[5] = (double)m1.m12; temp[6] = (double)m1.m20; temp[7] = (double)m1.m21; temp[8] = (double)m1.m22; // Calculate LU decomposition: Is the matrix singular? if (!luDecomposition(temp, row_perm)) { // Matrix has no inverse throw new SingularMatrixException(VecMathI18N.getString("Matrix3f12")); } // Perform back substitution on the identity matrix for(i=0;i<9;i++) result[i] = 0.0; result[0] = 1.0; result[4] = 1.0; result[8] = 1.0; luBacksubstitution(temp, row_perm, result); this.m00 = (float)result[0]; this.m01 = (float)result[1]; this.m02 = (float)result[2]; this.m10 = (float)result[3]; this.m11 = (float)result[4]; this.m12 = (float)result[5]; this.m20 = (float)result[6]; this.m21 = (float)result[7]; this.m22 = (float)result[8]; } /** * Given a 3x3 array "matrix0", this function replaces it with the * LU decomposition of a row-wise permutation of itself. The input * parameters are "matrix0" and "dimen". The array "matrix0" is also * an output parameter. The vector "row_perm[3]" is an output * parameter that contains the row permutations resulting from partial * pivoting. The output parameter "even_row_xchg" is 1 when the * number of row exchanges is even, or -1 otherwise. Assumes data * type is always double. * * This function is similar to luDecomposition, except that it * is tuned specifically for 3x3 matrices. * * @return true if the matrix is nonsingular, or false otherwise. */ // // Reference: Press, Flannery, Teukolsky, Vetterling, // _Numerical_Recipes_in_C_, Cambridge University Press, // 1988, pp 40-45. // static boolean luDecomposition(double[] matrix0, int[] row_perm) { double row_scale[] = new double[3]; // Determine implicit scaling information by looping over rows { int i, j; int ptr, rs; double big, temp; ptr = 0; rs = 0; // For each row ... i = 3; while (i-- != 0) { big = 0.0; // For each column, find the largest element in the row j = 3; while (j-- != 0) { temp = matrix0[ptr++]; temp = Math.abs(temp); if (temp > big) { big = temp; } } // Is the matrix singular? if (big == 0.0) { return false; } row_scale[rs++] = 1.0 / big; } } { int j; int mtx; mtx = 0; // For all columns, execute Crout's method for (j = 0; j < 3; j++) { int i, imax, k; int target, p1, p2; double sum, big, temp; // Determine elements of upper diagonal matrix U for (i = 0; i < j; i++) { target = mtx + (3*i) + j; sum = matrix0[target]; k = i; p1 = mtx + (3*i); p2 = mtx + j; while (k-- != 0) { sum -= matrix0[p1] * matrix0[p2]; p1++; p2 += 3; } matrix0[target] = sum; } // Search for largest pivot element and calculate // intermediate elements of lower diagonal matrix L. big = 0.0; imax = -1; for (i = j; i < 3; i++) { target = mtx + (3*i) + j; sum = matrix0[target]; k = j; p1 = mtx + (3*i); p2 = mtx + j; while (k-- != 0) { sum -= matrix0[p1] * matrix0[p2]; p1++; p2 += 3; } matrix0[target] = sum; // Is this the best pivot so far? if ((temp = row_scale[i] * Math.abs(sum)) >= big) { big = temp; imax = i; } } if (imax < 0) { throw new RuntimeException(VecMathI18N.getString("Matrix3f13")); } // Is a row exchange necessary? if (j != imax) { // Yes: exchange rows k = 3; p1 = mtx + (3*imax); p2 = mtx + (3*j); while (k-- != 0) { temp = matrix0[p1]; matrix0[p1++] = matrix0[p2]; matrix0[p2++] = temp; } // Record change in scale factor row_scale[imax] = row_scale[j]; } // Record row permutation row_perm[j] = imax; // Is the matrix singular if (matrix0[(mtx + (3*j) + j)] == 0.0) { return false; } // Divide elements of lower diagonal matrix L by pivot if (j != (3-1)) { temp = 1.0 / (matrix0[(mtx + (3*j) + j)]); target = mtx + (3*(j+1)) + j; i = 2 - j; while (i-- != 0) { matrix0[target] *= temp; target += 3; } } } } return true; } /** * Solves a set of linear equations. The input parameters "matrix1", * and "row_perm" come from luDecompostionD3x3 and do not change * here. The parameter "matrix2" is a set of column vectors assembled * into a 3x3 matrix of floating-point values. The procedure takes each * column of "matrix2" in turn and treats it as the right-hand side of the * matrix equation Ax = LUx = b. The solution vector replaces the * original column of the matrix. * * If "matrix2" is the identity matrix, the procedure replaces its contents * with the inverse of the matrix from which "matrix1" was originally * derived. */ // // Reference: Press, Flannery, Teukolsky, Vetterling, // _Numerical_Recipes_in_C_, Cambridge University Press, // 1988, pp 44-45. // static void luBacksubstitution(double[] matrix1, int[] row_perm, double[] matrix2) { int i, ii, ip, j, k; int rp; int cv, rv; // rp = row_perm; rp = 0; // For each column vector of matrix2 ... for (k = 0; k < 3; k++) { // cv = &(matrix2[0][k]); cv = k; ii = -1; // Forward substitution for (i = 0; i < 3; i++) { double sum; ip = row_perm[rp+i]; sum = matrix2[cv+3*ip]; matrix2[cv+3*ip] = matrix2[cv+3*i]; if (ii >= 0) { // rv = &(matrix1[i][0]); rv = i*3; for (j = ii; j <= i-1; j++) { sum -= matrix1[rv+j] * matrix2[cv+3*j]; } } else if (sum != 0.0) { ii = i; } matrix2[cv+3*i] = sum; } // Backsubstitution // rv = &(matrix1[3][0]); rv = 2*3; matrix2[cv+3*2] /= matrix1[rv+2]; rv -= 3; matrix2[cv+3*1] = (matrix2[cv+3*1] - matrix1[rv+2] * matrix2[cv+3*2]) / matrix1[rv+1]; rv -= 3; matrix2[cv+4*0] = (matrix2[cv+3*0] - matrix1[rv+1] * matrix2[cv+3*1] - matrix1[rv+2] * matrix2[cv+3*2]) / matrix1[rv+0]; } } /** * Computes the determinant of this matrix. * @return the determinant of this matrix */ public final float determinant() { float total; total = this.m00*(this.m11*this.m22 - this.m12*this.m21) + this.m01*(this.m12*this.m20 - this.m10*this.m22) + this.m02*(this.m10*this.m21 - this.m11*this.m20); return total; } /** * Sets the value of this matrix to a scale matrix with * the passed scale amount. * @param scale the scale factor for the matrix */ public final void set(float scale) { this.m00 = scale; this.m01 = (float) 0.0; this.m02 = (float) 0.0; this.m10 = (float) 0.0; this.m11 = scale; this.m12 = (float) 0.0; this.m20 = (float) 0.0; this.m21 = (float) 0.0; this.m22 = scale; } /** * Sets the value of this matrix to a counter clockwise rotation * about the x axis. * @param angle the angle to rotate about the X axis in radians */ public final void rotX(float angle) { float sinAngle, cosAngle; sinAngle = (float) Math.sin((double) angle); cosAngle = (float) Math.cos((double) angle); this.m00 = (float) 1.0; this.m01 = (float) 0.0; this.m02 = (float) 0.0; this.m10 = (float) 0.0; this.m11 = cosAngle; this.m12 = -sinAngle; this.m20 = (float) 0.0; this.m21 = sinAngle; this.m22 = cosAngle; } /** * Sets the value of this matrix to a counter clockwise rotation * about the y axis. * @param angle the angle to rotate about the Y axis in radians */ public final void rotY(float angle) { float sinAngle, cosAngle; sinAngle = (float) Math.sin((double) angle); cosAngle = (float) Math.cos((double) angle); this.m00 = cosAngle; this.m01 = (float) 0.0; this.m02 = sinAngle; this.m10 = (float) 0.0; this.m11 = (float) 1.0; this.m12 = (float) 0.0; this.m20 = -sinAngle; this.m21 = (float) 0.0; this.m22 = cosAngle; } /** * Sets the value of this matrix to a counter clockwise rotation * about the z axis. * @param angle the angle to rotate about the Z axis in radians */ public final void rotZ(float angle) { float sinAngle, cosAngle; sinAngle = (float) Math.sin((double) angle); cosAngle = (float) Math.cos((double) angle); this.m00 = cosAngle; this.m01 = -sinAngle; this.m02 = (float) 0.0; this.m10 = sinAngle; this.m11 = cosAngle; this.m12 = (float) 0.0; this.m20 = (float) 0.0; this.m21 = (float) 0.0; this.m22 = (float) 1.0; } /** * Multiplies each element of this matrix by a scalar. * @param scalar the scalar multiplier */ public final void mul(float scalar) { m00 *= scalar; m01 *= scalar; m02 *= scalar; m10 *= scalar; m11 *= scalar; m12 *= scalar; m20 *= scalar; m21 *= scalar; m22 *= scalar; } /** * Multiplies each element of matrix m1 by a scalar and places * the result into this. Matrix m1 is not modified. * @param scalar the scalar multiplier * @param m1 the original matrix */ public final void mul(float scalar, Matrix3f m1) { this.m00 = scalar * m1.m00; this.m01 = scalar * m1.m01; this.m02 = scalar * m1.m02; this.m10 = scalar * m1.m10; this.m11 = scalar * m1.m11; this.m12 = scalar * m1.m12; this.m20 = scalar * m1.m20; this.m21 = scalar * m1.m21; this.m22 = scalar * m1.m22; } /** * Sets the value of this matrix to the result of multiplying itself * with matrix m1. * @param m1 the other matrix */ public final void mul(Matrix3f m1) { float m00, m01, m02, m10, m11, m12, m20, m21, m22; m00 = this.m00*m1.m00 + this.m01*m1.m10 + this.m02*m1.m20; m01 = this.m00*m1.m01 + this.m01*m1.m11 + this.m02*m1.m21; m02 = this.m00*m1.m02 + this.m01*m1.m12 + this.m02*m1.m22; m10 = this.m10*m1.m00 + this.m11*m1.m10 + this.m12*m1.m20; m11 = this.m10*m1.m01 + this.m11*m1.m11 + this.m12*m1.m21; m12 = this.m10*m1.m02 + this.m11*m1.m12 + this.m12*m1.m22; m20 = this.m20*m1.m00 + this.m21*m1.m10 + this.m22*m1.m20; m21 = this.m20*m1.m01 + this.m21*m1.m11 + this.m22*m1.m21; m22 = this.m20*m1.m02 + this.m21*m1.m12 + this.m22*m1.m22; this.m00 = m00; this.m01 = m01; this.m02 = m02; this.m10 = m10; this.m11 = m11; this.m12 = m12; this.m20 = m20; this.m21 = m21; this.m22 = m22; } /** * Sets the value of this matrix to the result of multiplying * the two argument matrices together. * @param m1 the first matrix * @param m2 the second matrix */ public final void mul(Matrix3f m1, Matrix3f m2) { if (this != m1 && this != m2) { this.m00 = m1.m00*m2.m00 + m1.m01*m2.m10 + m1.m02*m2.m20; this.m01 = m1.m00*m2.m01 + m1.m01*m2.m11 + m1.m02*m2.m21; this.m02 = m1.m00*m2.m02 + m1.m01*m2.m12 + m1.m02*m2.m22; this.m10 = m1.m10*m2.m00 + m1.m11*m2.m10 + m1.m12*m2.m20; this.m11 = m1.m10*m2.m01 + m1.m11*m2.m11 + m1.m12*m2.m21; this.m12 = m1.m10*m2.m02 + m1.m11*m2.m12 + m1.m12*m2.m22; this.m20 = m1.m20*m2.m00 + m1.m21*m2.m10 + m1.m22*m2.m20; this.m21 = m1.m20*m2.m01 + m1.m21*m2.m11 + m1.m22*m2.m21; this.m22 = m1.m20*m2.m02 + m1.m21*m2.m12 + m1.m22*m2.m22; } else { float m00, m01, m02, m10, m11, m12, m20, m21, m22; m00 = m1.m00*m2.m00 + m1.m01*m2.m10 + m1.m02*m2.m20; m01 = m1.m00*m2.m01 + m1.m01*m2.m11 + m1.m02*m2.m21; m02 = m1.m00*m2.m02 + m1.m01*m2.m12 + m1.m02*m2.m22; m10 = m1.m10*m2.m00 + m1.m11*m2.m10 + m1.m12*m2.m20; m11 = m1.m10*m2.m01 + m1.m11*m2.m11 + m1.m12*m2.m21; m12 = m1.m10*m2.m02 + m1.m11*m2.m12 + m1.m12*m2.m22; m20 = m1.m20*m2.m00 + m1.m21*m2.m10 + m1.m22*m2.m20; m21 = m1.m20*m2.m01 + m1.m21*m2.m11 + m1.m22*m2.m21; m22 = m1.m20*m2.m02 + m1.m21*m2.m12 + m1.m22*m2.m22; this.m00 = m00; this.m01 = m01; this.m02 = m02; this.m10 = m10; this.m11 = m11; this.m12 = m12; this.m20 = m20; this.m21 = m21; this.m22 = m22; } } /** * Multiplies this matrix by matrix m1, does an SVD normalization * of the result, and places the result back into this matrix. * this = SVDnorm(this*m1). * @param m1 the matrix on the right hand side of the multiplication */ public final void mulNormalize(Matrix3f m1){ double[] tmp = new double[9]; // scratch matrix double[] tmp_rot = new double[9]; // scratch matrix double[] tmp_scale = new double[3]; // scratch matrix tmp[0] = this.m00*m1.m00 + this.m01*m1.m10 + this.m02*m1.m20; tmp[1] = this.m00*m1.m01 + this.m01*m1.m11 + this.m02*m1.m21; tmp[2] = this.m00*m1.m02 + this.m01*m1.m12 + this.m02*m1.m22; tmp[3] = this.m10*m1.m00 + this.m11*m1.m10 + this.m12*m1.m20; tmp[4] = this.m10*m1.m01 + this.m11*m1.m11 + this.m12*m1.m21; tmp[5] = this.m10*m1.m02 + this.m11*m1.m12 + this.m12*m1.m22; tmp[6] = this.m20*m1.m00 + this.m21*m1.m10 + this.m22*m1.m20; tmp[7] = this.m20*m1.m01 + this.m21*m1.m11 + this.m22*m1.m21; tmp[8] = this.m20*m1.m02 + this.m21*m1.m12 + this.m22*m1.m22; Matrix3d.compute_svd( tmp, tmp_scale, tmp_rot); this.m00 = (float)(tmp_rot[0]); this.m01 = (float)(tmp_rot[1]); this.m02 = (float)(tmp_rot[2]); this.m10 = (float)(tmp_rot[3]); this.m11 = (float)(tmp_rot[4]); this.m12 = (float)(tmp_rot[5]); this.m20 = (float)(tmp_rot[6]); this.m21 = (float)(tmp_rot[7]); this.m22 = (float)(tmp_rot[8]); } /** * Multiplies matrix m1 by matrix m2, does an SVD normalization * of the result, and places the result into this matrix. * this = SVDnorm(m1*m2). * @param m1 the matrix on the left hand side of the multiplication * @param m2 the matrix on the right hand side of the multiplication */ public final void mulNormalize(Matrix3f m1, Matrix3f m2){ double[] tmp = new double[9]; // scratch matrix double[] tmp_rot = new double[9]; // scratch matrix double[] tmp_scale = new double[3]; // scratch matrix tmp[0] = m1.m00*m2.m00 + m1.m01*m2.m10 + m1.m02*m2.m20; tmp[1] = m1.m00*m2.m01 + m1.m01*m2.m11 + m1.m02*m2.m21; tmp[2] = m1.m00*m2.m02 + m1.m01*m2.m12 + m1.m02*m2.m22; tmp[3] = m1.m10*m2.m00 + m1.m11*m2.m10 + m1.m12*m2.m20; tmp[4] = m1.m10*m2.m01 + m1.m11*m2.m11 + m1.m12*m2.m21; tmp[5] = m1.m10*m2.m02 + m1.m11*m2.m12 + m1.m12*m2.m22; tmp[6] = m1.m20*m2.m00 + m1.m21*m2.m10 + m1.m22*m2.m20; tmp[7] = m1.m20*m2.m01 + m1.m21*m2.m11 + m1.m22*m2.m21; tmp[8] = m1.m20*m2.m02 + m1.m21*m2.m12 + m1.m22*m2.m22; Matrix3d.compute_svd( tmp, tmp_scale, tmp_rot); this.m00 = (float)(tmp_rot[0]); this.m01 = (float)(tmp_rot[1]); this.m02 = (float)(tmp_rot[2]); this.m10 = (float)(tmp_rot[3]); this.m11 = (float)(tmp_rot[4]); this.m12 = (float)(tmp_rot[5]); this.m20 = (float)(tmp_rot[6]); this.m21 = (float)(tmp_rot[7]); this.m22 = (float)(tmp_rot[8]); } /** * Multiplies the transpose of matrix m1 times the transpose of matrix * m2, and places the result into this. * @param m1 the matrix on the left hand side of the multiplication * @param m2 the matrix on the right hand side of the multiplication */ public final void mulTransposeBoth(Matrix3f m1, Matrix3f m2) { if (this != m1 && this != m2) { this.m00 = m1.m00*m2.m00 + m1.m10*m2.m01 + m1.m20*m2.m02; this.m01 = m1.m00*m2.m10 + m1.m10*m2.m11 + m1.m20*m2.m12; this.m02 = m1.m00*m2.m20 + m1.m10*m2.m21 + m1.m20*m2.m22; this.m10 = m1.m01*m2.m00 + m1.m11*m2.m01 + m1.m21*m2.m02; this.m11 = m1.m01*m2.m10 + m1.m11*m2.m11 + m1.m21*m2.m12; this.m12 = m1.m01*m2.m20 + m1.m11*m2.m21 + m1.m21*m2.m22; this.m20 = m1.m02*m2.m00 + m1.m12*m2.m01 + m1.m22*m2.m02; this.m21 = m1.m02*m2.m10 + m1.m12*m2.m11 + m1.m22*m2.m12; this.m22 = m1.m02*m2.m20 + m1.m12*m2.m21 + m1.m22*m2.m22; } else { float m00, m01, m02, m10, m11, m12, m20, m21, m22; // vars for temp result matrix m00 = m1.m00*m2.m00 + m1.m10*m2.m01 + m1.m20*m2.m02; m01 = m1.m00*m2.m10 + m1.m10*m2.m11 + m1.m20*m2.m12; m02 = m1.m00*m2.m20 + m1.m10*m2.m21 + m1.m20*m2.m22; m10 = m1.m01*m2.m00 + m1.m11*m2.m01 + m1.m21*m2.m02; m11 = m1.m01*m2.m10 + m1.m11*m2.m11 + m1.m21*m2.m12; m12 = m1.m01*m2.m20 + m1.m11*m2.m21 + m1.m21*m2.m22; m20 = m1.m02*m2.m00 + m1.m12*m2.m01 + m1.m22*m2.m02; m21 = m1.m02*m2.m10 + m1.m12*m2.m11 + m1.m22*m2.m12; m22 = m1.m02*m2.m20 + m1.m12*m2.m21 + m1.m22*m2.m22; this.m00 = m00; this.m01 = m01; this.m02 = m02; this.m10 = m10; this.m11 = m11; this.m12 = m12; this.m20 = m20; this.m21 = m21; this.m22 = m22; } } /** * Multiplies matrix m1 times the transpose of matrix m2, and * places the result into this. * @param m1 the matrix on the left hand side of the multiplication * @param m2 the matrix on the right hand side of the multiplication */ public final void mulTransposeRight(Matrix3f m1, Matrix3f m2) { if (this != m1 && this != m2) { this.m00 = m1.m00*m2.m00 + m1.m01*m2.m01 + m1.m02*m2.m02; this.m01 = m1.m00*m2.m10 + m1.m01*m2.m11 + m1.m02*m2.m12; this.m02 = m1.m00*m2.m20 + m1.m01*m2.m21 + m1.m02*m2.m22; this.m10 = m1.m10*m2.m00 + m1.m11*m2.m01 + m1.m12*m2.m02; this.m11 = m1.m10*m2.m10 + m1.m11*m2.m11 + m1.m12*m2.m12; this.m12 = m1.m10*m2.m20 + m1.m11*m2.m21 + m1.m12*m2.m22; this.m20 = m1.m20*m2.m00 + m1.m21*m2.m01 + m1.m22*m2.m02; this.m21 = m1.m20*m2.m10 + m1.m21*m2.m11 + m1.m22*m2.m12; this.m22 = m1.m20*m2.m20 + m1.m21*m2.m21 + m1.m22*m2.m22; } else { float m00, m01, m02, m10, m11, m12, m20, m21, m22; // vars for temp result matrix m00 = m1.m00*m2.m00 + m1.m01*m2.m01 + m1.m02*m2.m02; m01 = m1.m00*m2.m10 + m1.m01*m2.m11 + m1.m02*m2.m12; m02 = m1.m00*m2.m20 + m1.m01*m2.m21 + m1.m02*m2.m22; m10 = m1.m10*m2.m00 + m1.m11*m2.m01 + m1.m12*m2.m02; m11 = m1.m10*m2.m10 + m1.m11*m2.m11 + m1.m12*m2.m12; m12 = m1.m10*m2.m20 + m1.m11*m2.m21 + m1.m12*m2.m22; m20 = m1.m20*m2.m00 + m1.m21*m2.m01 + m1.m22*m2.m02; m21 = m1.m20*m2.m10 + m1.m21*m2.m11 + m1.m22*m2.m12; m22 = m1.m20*m2.m20 + m1.m21*m2.m21 + m1.m22*m2.m22; this.m00 = m00; this.m01 = m01; this.m02 = m02; this.m10 = m10; this.m11 = m11; this.m12 = m12; this.m20 = m20; this.m21 = m21; this.m22 = m22; } } /** * Multiplies the transpose of matrix m1 times matrix m2, and * places the result into this. * @param m1 the matrix on the left hand side of the multiplication * @param m2 the matrix on the right hand side of the multiplication */ public final void mulTransposeLeft(Matrix3f m1, Matrix3f m2) { if (this != m1 && this != m2) { this.m00 = m1.m00*m2.m00 + m1.m10*m2.m10 + m1.m20*m2.m20; this.m01 = m1.m00*m2.m01 + m1.m10*m2.m11 + m1.m20*m2.m21; this.m02 = m1.m00*m2.m02 + m1.m10*m2.m12 + m1.m20*m2.m22; this.m10 = m1.m01*m2.m00 + m1.m11*m2.m10 + m1.m21*m2.m20; this.m11 = m1.m01*m2.m01 + m1.m11*m2.m11 + m1.m21*m2.m21; this.m12 = m1.m01*m2.m02 + m1.m11*m2.m12 + m1.m21*m2.m22; this.m20 = m1.m02*m2.m00 + m1.m12*m2.m10 + m1.m22*m2.m20; this.m21 = m1.m02*m2.m01 + m1.m12*m2.m11 + m1.m22*m2.m21; this.m22 = m1.m02*m2.m02 + m1.m12*m2.m12 + m1.m22*m2.m22; } else { float m00, m01, m02, m10, m11, m12, m20, m21, m22; // vars for temp result matrix m00 = m1.m00*m2.m00 + m1.m10*m2.m10 + m1.m20*m2.m20; m01 = m1.m00*m2.m01 + m1.m10*m2.m11 + m1.m20*m2.m21; m02 = m1.m00*m2.m02 + m1.m10*m2.m12 + m1.m20*m2.m22; m10 = m1.m01*m2.m00 + m1.m11*m2.m10 + m1.m21*m2.m20; m11 = m1.m01*m2.m01 + m1.m11*m2.m11 + m1.m21*m2.m21; m12 = m1.m01*m2.m02 + m1.m11*m2.m12 + m1.m21*m2.m22; m20 = m1.m02*m2.m00 + m1.m12*m2.m10 + m1.m22*m2.m20; m21 = m1.m02*m2.m01 + m1.m12*m2.m11 + m1.m22*m2.m21; m22 = m1.m02*m2.m02 + m1.m12*m2.m12 + m1.m22*m2.m22; this.m00 = m00; this.m01 = m01; this.m02 = m02; this.m10 = m10; this.m11 = m11; this.m12 = m12; this.m20 = m20; this.m21 = m21; this.m22 = m22; } } /** * Performs singular value decomposition normalization of this matrix. */ public final void normalize(){ double[] tmp_rot = new double[9]; // scratch matrix double[] tmp_scale = new double[3]; // scratch matrix getScaleRotate( tmp_scale, tmp_rot ); this.m00 = (float)tmp_rot[0]; this.m01 = (float)tmp_rot[1]; this.m02 = (float)tmp_rot[2]; this.m10 = (float)tmp_rot[3]; this.m11 = (float)tmp_rot[4]; this.m12 = (float)tmp_rot[5]; this.m20 = (float)tmp_rot[6]; this.m21 = (float)tmp_rot[7]; this.m22 = (float)tmp_rot[8]; } /** * Perform singular value decomposition normalization of matrix m1 * and place the normalized values into this. * @param m1 the matrix values to be normalized */ public final void normalize(Matrix3f m1){ double[] tmp = new double[9]; // scratch matrix double[] tmp_rot = new double[9]; // scratch matrix double[] tmp_scale = new double[3]; // scratch matrix tmp[0] = m1.m00; tmp[1] = m1.m01; tmp[2] = m1.m02; tmp[3] = m1.m10; tmp[4] = m1.m11; tmp[5] = m1.m12; tmp[6] = m1.m20; tmp[7] = m1.m21; tmp[8] = m1.m22; Matrix3d.compute_svd( tmp, tmp_scale, tmp_rot ); this.m00 = (float)(tmp_rot[0]); this.m01 = (float)(tmp_rot[1]); this.m02 = (float)(tmp_rot[2]); this.m10 = (float)(tmp_rot[3]); this.m11 = (float)(tmp_rot[4]); this.m12 = (float)(tmp_rot[5]); this.m20 = (float)(tmp_rot[6]); this.m21 = (float)(tmp_rot[7]); this.m22 = (float)(tmp_rot[8]); } /** * Perform cross product normalization of this matrix. */ public final void normalizeCP() { float mag = 1.0f/(float)Math.sqrt(m00*m00 + m10*m10 + m20*m20); m00 = m00*mag; m10 = m10*mag; m20 = m20*mag; mag = 1.0f/(float)Math.sqrt(m01*m01 + m11*m11 + m21*m21); m01 = m01*mag; m11 = m11*mag; m21 = m21*mag; m02 = m10*m21 - m11*m20; m12 = m01*m20 - m00*m21; m22 = m00*m11 - m01*m10; } /** * Perform cross product normalization of matrix m1 and place the * normalized values into this. * @param m1 Provides the matrix values to be normalized */ public final void normalizeCP(Matrix3f m1) { float mag = 1.0f/(float)Math.sqrt(m1.m00*m1.m00 + m1.m10*m1.m10 + m1.m20*m1.m20); m00 = m1.m00*mag; m10 = m1.m10*mag; m20 = m1.m20*mag; mag = 1.0f/(float)Math.sqrt(m1.m01*m1.m01 + m1.m11*m1.m11 + m1.m21*m1.m21); m01 = m1.m01*mag; m11 = m1.m11*mag; m21 = m1.m21*mag; m02 = m10*m21 - m11*m20; m12 = m01*m20 - m00*m21; m22 = m00*m11 - m01*m10; } /** * Returns true if all of the data members of Matrix3f m1 are * equal to the corresponding data members in this Matrix3f. * @param m1 the matrix with which the comparison is made * @return true or false */ public boolean equals(Matrix3f m1) { try { return(this.m00 == m1.m00 && this.m01 == m1.m01 && this.m02 == m1.m02 && this.m10 == m1.m10 && this.m11 == m1.m11 && this.m12 == m1.m12 && this.m20 == m1.m20 && this.m21 == m1.m21 && this.m22 == m1.m22); } catch (NullPointerException e2) { return false; } } /** * Returns true if the Object o1 is of type Matrix3f and all of the * data members of o1 are equal to the corresponding data members in * this Matrix3f. * @param o1 the object with which the comparison is made * @return true or false */ public boolean equals(Object o1) { try { Matrix3f m2 = (Matrix3f) o1; return(this.m00 == m2.m00 && this.m01 == m2.m01 && this.m02 == m2.m02 && this.m10 == m2.m10 && this.m11 == m2.m11 && this.m12 == m2.m12 && this.m20 == m2.m20 && this.m21 == m2.m21 && this.m22 == m2.m22); } catch (ClassCastException e1) { return false; } catch (NullPointerException e2) { return false; } } /** * Returns true if the L-infinite distance between this matrix * and matrix m1 is less than or equal to the epsilon parameter, * otherwise returns false. The L-infinite * distance is equal to * MAX[i=0,1,2 ; j=0,1,2 ; abs(this.m(i,j) - m1.m(i,j)] * @param m1 the matrix to be compared to this matrix * @param epsilon the threshold value */ public boolean epsilonEquals(Matrix3f m1, float epsilon) { boolean status = true; if( Math.abs( this.m00 - m1.m00) > epsilon) status = false; if( Math.abs( this.m01 - m1.m01) > epsilon) status = false; if( Math.abs( this.m02 - m1.m02) > epsilon) status = false; if( Math.abs( this.m10 - m1.m10) > epsilon) status = false; if( Math.abs( this.m11 - m1.m11) > epsilon) status = false; if( Math.abs( this.m12 - m1.m12) > epsilon) status = false; if( Math.abs( this.m20 - m1.m20) > epsilon) status = false; if( Math.abs( this.m21 - m1.m21) > epsilon) status = false; if( Math.abs( this.m22 - m1.m22) > epsilon) status = false; return( status ); } /** * Returns a hash code value based on the data values in this * object. Two different Matrix3f objects with identical data values * (i.e., Matrix3f.equals returns true) will return the same hash * code value. Two objects with different data members may return the * same hash value, although this is not likely. * @return the integer hash code value */ public int hashCode() { long bits = 1L; bits = 31L * bits + (long)VecMathUtil.floatToIntBits(m00); bits = 31L * bits + (long)VecMathUtil.floatToIntBits(m01); bits = 31L * bits + (long)VecMathUtil.floatToIntBits(m02); bits = 31L * bits + (long)VecMathUtil.floatToIntBits(m10); bits = 31L * bits + (long)VecMathUtil.floatToIntBits(m11); bits = 31L * bits + (long)VecMathUtil.floatToIntBits(m12); bits = 31L * bits + (long)VecMathUtil.floatToIntBits(m20); bits = 31L * bits + (long)VecMathUtil.floatToIntBits(m21); bits = 31L * bits + (long)VecMathUtil.floatToIntBits(m22); return (int) (bits ^ (bits >> 32)); } /** * Sets this matrix to all zeros. */ public final void setZero() { m00 = 0.0f; m01 = 0.0f; m02 = 0.0f; m10 = 0.0f; m11 = 0.0f; m12 = 0.0f; m20 = 0.0f; m21 = 0.0f; m22 = 0.0f; } /** * Negates the value of this matrix: this = -this. */ public final void negate() { this.m00 = -this.m00; this.m01 = -this.m01; this.m02 = -this.m02; this.m10 = -this.m10; this.m11 = -this.m11; this.m12 = -this.m12; this.m20 = -this.m20; this.m21 = -this.m21; this.m22 = -this.m22; } /** * Sets the value of this matrix equal to the negation of * of the Matrix3f parameter. * @param m1 the source matrix */ public final void negate(Matrix3f m1) { this.m00 = -m1.m00; this.m01 = -m1.m01; this.m02 = -m1.m02; this.m10 = -m1.m10; this.m11 = -m1.m11; this.m12 = -m1.m12; this.m20 = -m1.m20; this.m21 = -m1.m21; this.m22 = -m1.m22; } /** * Multiply this matrix by the tuple t and place the result * back into the tuple (t = this*t). * @param t the tuple to be multiplied by this matrix and then replaced */ public final void transform(Tuple3f t) { float x,y,z; x = m00* t.x + m01*t.y + m02*t.z; y = m10* t.x + m11*t.y + m12*t.z; z = m20* t.x + m21*t.y + m22*t.z; t.set(x,y,z); } /** * Multiply this matrix by the tuple t and and place the result * into the tuple "result" (result = this*t). * @param t the tuple to be multiplied by this matrix * @param result the tuple into which the product is placed */ public final void transform(Tuple3f t, Tuple3f result) { float x,y,z; x = m00* t.x + m01*t.y + m02*t.z; y = m10* t.x + m11*t.y + m12*t.z; result.z = m20* t.x + m21*t.y + m22*t.z; result.x = x; result.y = y; } /** * perform SVD (if necessary to get rotational component */ void getScaleRotate( double[] scales, double[] rot ) { double[] tmp = new double[9]; // scratch matrix tmp[0] = m00; tmp[1] = m01; tmp[2] = m02; tmp[3] = m10; tmp[4] = m11; tmp[5] = m12; tmp[6] = m20; tmp[7] = m21; tmp[8] = m22; Matrix3d.compute_svd(tmp, scales, rot); return; } /** * Creates a new object of the same class as this object. * * @return a clone of this instance. * @exception OutOfMemoryError if there is not enough memory. * @see java.lang.Cloneable * @since vecmath 1.3 */ public Object clone() { Matrix3f m1 = null; try { m1 = (Matrix3f)super.clone(); } catch (CloneNotSupportedException e) { // this shouldn't happen, since we are Cloneable throw new InternalError(); } return m1; } /** * Get the first matrix element in the first row. * * @return Returns the m00. * * @since vecmath 1.5 */ public final float getM00() { return m00; } /** * Set the first matrix element in the first row. * * @param m00 The m00 to set. * * @since vecmath 1.5 */ public final void setM00(float m00) { this.m00 = m00; } /** * Get the second matrix element in the first row. * * @return Returns the m01. * * * @since vecmath 1.5 */ public final float getM01() { return m01; } /** * Set the second matrix element in the first row. * * @param m01 The m01 to set. * * @since vecmath 1.5 */ public final void setM01(float m01) { this.m01 = m01; } /** * Get the third matrix element in the first row. * * @return Returns the m02. * * @since vecmath 1.5 */ public final float getM02() { return m02; } /** * Set the third matrix element in the first row. * * @param m02 The m02 to set. * * @since vecmath 1.5 */ public final void setM02(float m02) { this.m02 = m02; } /** * Get first matrix element in the second row. * * @return Returns the m10. * * @since vecmath 1.5 */ public final float getM10() { return m10; } /** * Set first matrix element in the second row. * * @param m10 The m10 to set. * * @since vecmath 1.5 */ public final void setM10(float m10) { this.m10 = m10; } /** * Get second matrix element in the second row. * * @return Returns the m11. * * @since vecmath 1.5 */ public final float getM11() { return m11; } /** * Set the second matrix element in the second row. * * @param m11 The m11 to set. * * @since vecmath 1.5 */ public final void setM11(float m11) { this.m11 = m11; } /** * Get the third matrix element in the second row. * * @return Returns the m12. * * @since vecmath 1.5 */ public final float getM12() { return m12; } /** * Set the third matrix element in the second row. * @param m12 The m12 to set. * @since vecmath 1.5 */ public final void setM12(float m12) { this.m12 = m12; } /** * Get the first matrix element in the third row. * * @return Returns the m20. * * @since vecmath 1.5 */ public final float getM20() { return m20; } /** * Set the first matrix element in the third row. * * @param m20 The m20 to set. * * @since vecmath 1.5 */ public final void setM20(float m20) { this.m20 = m20; } /** * Get the second matrix element in the third row. * * @return Returns the m21. * * @since vecmath 1.5 */ public final float getM21() { return m21; } /** * Set the second matrix element in the third row. * * @param m21 The m21 to set. * * @since vecmath 1.5 */ public final void setM21(float m21) { this.m21 = m21; } /** * Get the third matrix element in the third row . * * @return Returns the m22. * * @since vecmath 1.5 */ public final float getM22() { return m22; } /** * Set the third matrix element in the third row. * * @param m22 The m22 to set. * * @since vecmath 1.5 */ public final void setM22(float m22) { this.m22 = m22; } }