package java3d;
public class Matrix3d {
public double m00;
public double m01;
public double m02;
public double m10;
public double m11;
public double m12;
public double m20;
public double m21;
public double m22;
// コンストラクタ
public Matrix3d() {
m00 = 0.0;
m01 = 0.0;
m02 = 0.0;
m10 = 0.0;
m11 = 0.0;
m12 = 0.0;
m20 = 0.0;
m21 = 0.0;
m22 = 0.0;
}
public Matrix3d(double[] v) {
m00 = v[0];
m01 = v[1];
m02 = v[2];
m10 = v[3];
m11 = v[4];
m12 = v[5];
m20 = v[6];
m21 = v[7];
m22 = v[8];
}
public Matrix3d(double m00, double m01, double m02, double m10, double m11,
double m12, double m20, double m21, double 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;
}
public Matrix3d(Matrix3d 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;
}
public Matrix3d clone() {
return new Matrix3d(m00, m01, m02, m10, m11, m12, m20, m21, m22);
}
/** 各要素に足す */
public void add(Matrix3d 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;
}
/** 二つの行列の和を格納する */
public void add(Matrix3d m1, Matrix3d 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;
}
/** x軸周りにangle回転 */
public void rotX(double angle) {
double d1 = Math.sin(angle);
double d2 = Math.cos(angle);
m00 = 1.0D;
m01 = 0.0D;
m02 = 0.0D;
m10 = 0.0D;
m11 = d2;
m12 = -d1;
m20 = 0.0D;
m21 = d1;
m22 = d2;
}
/** y軸周りにangle回転 */
public void rotY(double angle) {
double d1 = Math.sin(angle);
double d2 = Math.cos(angle);
m00 = d2;
m01 = 0.0D;
m02 = d1;
m10 = 0.0D;
m11 = 1.0D;
m12 = 0.0D;
m20 = -d1;
m21 = 0.0D;
m22 = d2;
}
/** z軸周りにangle1回転 */
public void rotZ(double angle) {
double d1 = Math.sin(angle);
double d2 = Math.cos(angle);
m00 = d2;
m01 = -d1;
m02 = 0.0D;
m10 = d1;
m11 = d2;
m12 = 0.0D;
m20 = 0.0D;
m21 = 0.0D;
m22 = 1.0D;
}
/** 自分に引数の行列を掛けたものを格納する */
public void mul(Matrix3d mat) {
double t00 = m00 * mat.m00 + m01 * mat.m10 + m02 * mat.m20;
double t01 = m00 * mat.m01 + m01 * mat.m11 + m02 * mat.m21;
double t02 = m00 * mat.m02 + m01 * mat.m12 + m02 * mat.m22;
double t10 = m10 * mat.m10 + m11 * mat.m10 + m12 * mat.m20;
double t11 = m10 * mat.m11 + m11 * mat.m11 + m12 * mat.m21;
double t12 = m10 * mat.m12 + m11 * mat.m12 + m12 * mat.m22;
double t20 = m20 * mat.m20 + m21 * mat.m10 + m22 * mat.m20;
double t21 = m20 * mat.m21 + m21 * mat.m11 + m22 * mat.m21;
double t22 = m20 * mat.m22 + m21 * mat.m12 + m22 * mat.m22;
set(t00, t01, t02, t10, t11, t12, t20, t21, t22);
}
public void mul(Matrix3d mat1, Matrix3d mat2) {
this.set(mat1.m00, mat1.m01, mat1.m02, mat1.m10, mat1.m11, mat1.m12, mat1.m20, mat1.m21, mat1.m22);
mul(mat2);
}
public void set(double t00, double t01, double t02, double t10, double t11,
double t12, double t20, double t21, double t22) {
m00 = t00;
m01 = t01;
m02 = t02;
m10 = t10;
m11 = t11;
m12 = t12;
m20 = t20;
m21 = t21;
m22 = t22;
}
/**
* 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(Matrix3d 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(Matrix3d m1) {
double result[] = new double[9];
int row_perm[] = new int[3];
int i, r, c;
double[] tmp = new double[9]; // scratch matrix
// Use LU decomposition and backsubstitution code specifically
// for floating-point 3x3 matrices.
// Copy source matrix to t1tmp
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;
// Calculate LU decomposition: Is the matrix singular?
if (!luDecomposition(tmp, row_perm)) {
// Matrix has no inverse
// throw new
// SingularMatrixException(VecMathI18N.getString("Matrix3d12"));
}
// 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(tmp, row_perm, result);
this.m00 = result[0];
this.m01 = result[1];
this.m02 = result[2];
this.m10 = result[3];
this.m11 = result[4];
this.m12 = result[5];
this.m20 = result[6];
this.m21 = result[7];
this.m22 = 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("Matrix3d13"));
}
// 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];
}
}
/**
* 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(Tuple3d t) {
double 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);
}
}
