/*
* Open Source Physics software is free software as described near the bottom of this code file.
*
* For additional information and documentation on Open Source Physics please see:
* <http://www.opensourcephysics.org/>
*/
package org.opensourcephysics.numerics;
import org.opensourcephysics.controls.XML;
import org.opensourcephysics.controls.XMLControl;
import org.opensourcephysics.controls.XMLLoader;
/**
* Matrix3DTransformation implements 3D affine transformations using a matrix representation.
*/
public class Matrix3DTransformation implements MatrixTransformation {
protected double[] origin = new double[3]; // origin for this rotation
protected double[][] matrix = new double[3][3]; // the transformation matrix
protected double[][] inverseMatrix = null; // the inverse transformation matrix if it exists
/**
* Constructs a 3D transformation using the given matrix.
*
* Affine transformations can be applied to 3D coordinates.
* A 3 by 3 matrix sets the rotation and shear.
* A null matrix sets the transformation to the identity transformation.
*
* @param matrix double[][]
*/
public Matrix3DTransformation(double[][] matrix) {
if(matrix==null) { // identiy matrix
this.matrix[0][0] = this.matrix[1][1] = this.matrix[2][2] = 1;
return;
}
for(int i = 0; i<matrix.length; i++) { // loop over the rows
System.arraycopy(matrix[i], 0, this.matrix[i], 0, matrix[i].length);
}
}
/**
* Creates a 3D transforamtion representing a rotation about the x axis by the given angle.
*
* @param theta double
*
* @return Affine3DTransformation
*/
public static Matrix3DTransformation rotationX(double theta) {
// same as Rotation(theta, new double[]{0, 0, 1});
Matrix3DTransformation at = new Matrix3DTransformation(null); // creates matrix
double[][] mat = at.matrix;
double sin = Math.sin(theta);
double cos = Math.cos(theta);
// matrix elements not listed are zero
mat[0][0] = 1;
mat[1][1] = cos;
mat[1][2] = -sin;
mat[2][1] = sin;
mat[2][2] = cos;
// inverse matrix is null but we know what it is so create it
double[][] inv = new double[3][3];
at.inverseMatrix = inv;
inv[0][0] = 1;
inv[1][1] = cos;
inv[1][2] = sin;
inv[2][1] = -sin;
inv[2][2] = cos;
return at;
}
/**
* Creates a 3D transforamtion representing a rotation about the y axis by the given angle.
*
* @param theta double
*
* @return Affine3DTransformation
*/
public static Matrix3DTransformation rotationY(double theta) {
// same as Rotation(theta, new double[]{0, 0, 1});
Matrix3DTransformation at = new Matrix3DTransformation(null); // creates matrix
double[][] mat = at.matrix;
double sin = Math.sin(theta);
double cos = Math.cos(theta);
// matrix elements not listed are zero
mat[1][1] = 1;
mat[0][0] = cos;
mat[0][2] = sin;
mat[2][0] = -sin;
mat[2][2] = cos;
// inverse matrix is null but we know what it is so create it
double[][] inv = new double[3][3];
at.inverseMatrix = inv;
// matrix elements not listed are zero
inv[1][1] = 1;
inv[0][0] = cos;
inv[0][2] = -sin;
inv[2][0] = sin;
inv[2][2] = cos;
return at;
}
/**
* Creates a 3D transforamtion representing a rotation about the z axis by the given angle.
*
* @param theta double
*
* @return Affine3DTransformation
*/
public static Matrix3DTransformation rotationZ(double theta) {
// same as Rotation(theta, new double[]{0, 0, 1});
Matrix3DTransformation at = new Matrix3DTransformation(null); // creates matrix
double[][] mat = at.matrix;
double sin = Math.sin(theta);
double cos = Math.cos(theta);
// matrix elements not listed are zero
mat[0][0] = cos;
mat[0][1] = -sin;
mat[1][0] = sin;
mat[1][1] = cos;
mat[2][2] = 1;
// inverse matrix is null but we know what it is so create it
double[][] inv = new double[3][3];
at.inverseMatrix = inv;
inv[0][0] = cos;
inv[0][1] = sin;
inv[1][0] = -sin;
inv[1][1] = cos;
inv[2][2] = 1;
return at;
}
/**
* Creates a 3D transforamtion representing a rotation about the origin by the given angle around
* the given axis.
*
* @param theta double
* @param axis double[]
* @return Affine3DTransformation
*/
public static Matrix3DTransformation rotation(double theta, double[] axis) {
Matrix3DTransformation at = new Matrix3DTransformation(null); // creates unit matrix
double[][] mat = at.matrix;
double x = axis[0], y = axis[1], z = axis[2];
double norm = x*x+y*y+z*z;
if(norm!=1) { // this usually doesn't happen because of roundoff but is worth a try
norm = 1/Math.sqrt(norm);
x *= norm;
y *= norm;
z *= norm;
}
double c = Math.cos(theta), s = Math.sin(theta);
double t = 1-c;
// matrix elements not listed are zero
mat[0][0] = t*x*x+c;
mat[0][1] = t*x*y-s*z;
mat[0][2] = t*x*z+s*y;
mat[1][0] = t*x*y+s*z;
mat[1][1] = t*y*y+c;
mat[1][2] = t*y*z-s*x;
mat[2][0] = t*x*z-s*y;
mat[2][1] = t*y*z+s*x;
mat[2][2] = t*z*z+c;
// inverse matrix is null but we know what it is so create it
double[][] inv = new double[3][3];
at.inverseMatrix = inv;
inv[0][0] = mat[0][0];
inv[1][0] = mat[0][1];
inv[2][0] = mat[0][2];
inv[0][1] = mat[1][0];
inv[1][1] = mat[1][1];
inv[2][1] = mat[1][2];
inv[0][2] = mat[2][0];
inv[1][2] = mat[2][1];
inv[2][2] = mat[2][2];
return at;
}
/**
* Creates an transformation representing a rotation about the origin by the given quaternion.
*
* @param quaternion double[]
* @return Affine3DTransformation
*/
public static Matrix3DTransformation Quaternion(double[] quaternion) {
return Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3]);
}
/**
* Creates an AffineMatrix representing a rotation about the origin by the given quaternion components.
*
* @param q0
* @param q1
* @param q2
* @param q3
* @return Affine3DTransformation
*/
public static Matrix3DTransformation Quaternion(double q0, double q1, double q2, double q3) {
Matrix3DTransformation at = new Matrix3DTransformation(null);
double[][] atMatrix = at.matrix;
double norm = q0*q0+q1*q1+q2*q2+q3*q3;
if(norm!=1) { // this usually doesn't happen because of roundoff but is worth a try
norm = 1/Math.sqrt(norm); // computationaly expensive
q0 *= norm;
q1 *= norm;
q2 *= norm;
q3 *= norm;
}
double q11 = 2*q1*q1, q22 = 2*q2*q2, q33 = 2*q3*q3;
double q12 = 2*q1*q2, q13 = 2*q1*q3, q23 = 2*q2*q3;
double q01 = 2*q0*q1, q02 = 2*q0*q2, q03 = 2*q0*q3;
// matrix elements not listed are zero
atMatrix[0][0] = 1-q22-q33;
atMatrix[0][1] = q12-q03;
atMatrix[0][2] = q13+q02;
atMatrix[1][0] = q12+q03;
atMatrix[1][1] = 1-q11-q33;
atMatrix[1][2] = q23-q01;
atMatrix[2][0] = q13-q02;
atMatrix[2][1] = q23+q01;
atMatrix[2][2] = 1-q11-q22;
// inverse matrix is null but we know what it is so create it
double[][] inv = new double[3][3];
at.inverseMatrix = inv;
inv[0][0] = atMatrix[0][0];
inv[1][0] = atMatrix[0][1];
inv[2][0] = atMatrix[0][2];
inv[0][1] = atMatrix[1][0];
inv[1][1] = atMatrix[1][1];
inv[2][1] = atMatrix[1][2];
inv[0][2] = atMatrix[2][0];
inv[1][2] = atMatrix[2][1];
inv[2][2] = atMatrix[2][2];
return at;
}
/**
* Sets the double[][] matrix array of the rotation
* @param newMatrix
* @returns true if there was a real change
*/
public boolean setMatrix(double[][] newMatrix) {
if (matrix.equals(newMatrix)) return false;
for (int i = 0; i<3; i++) { // loop over the rows
System.arraycopy(newMatrix[i], 0, this.matrix[i], 0, 3);
}
this.inverseMatrix = null;
return true;
}
/**
* Sets the matrix array of the rotation
* @param matrix a double[9] array with the matrix components
* in this order {m[0][0],m[0][1],m[0][2],m[1][0],...}
* @returns true if there was a real change
*/
public boolean setMatrix(double[] newMatrix) {
boolean changed = false;
for (int i = 0,j=0; i<3; i++) { // loop over the rows
if (matrix[i][0]!=newMatrix[j++]) { matrix[i][0] = newMatrix[j]; changed = true; }
if (matrix[i][1]!=newMatrix[j++]) { matrix[i][1] = newMatrix[j]; changed = true; }
if (matrix[i][2]!=newMatrix[j++]) { matrix[i][2] = newMatrix[j]; changed = true; }
}
if (changed) this.inverseMatrix = null;
return changed;
}
/**
* Provides a copy of this transformation.
*/
public Object clone() {
Matrix3DTransformation m = new Matrix3DTransformation(matrix);
m.origin = origin.clone();
if(inverseMatrix==null) {
return m;
}
m.inverseMatrix = new double[3][3]; // inverse exists so clone it too
for(int i = 0; i<inverseMatrix.length; i++) { // loop over the rows
System.arraycopy(inverseMatrix[i], 0, m.inverseMatrix[i], 0, inverseMatrix[i].length);
}
return m;
}
/**
* Gets the direct homogeneous affine transformation flattened into a 1-d array.
*
* If the mat parameter is null a double[16] array is created;
* otherwise the given array is used.
*
* @param mat double[] optional matrix
* @return double[] the matrix
*/
public final double[] getFlatMatrix(double[] mat) {
if(mat==null) {
mat = new double[16];
}
mat[0] = matrix[0][0];
mat[4] = matrix[0][1];
mat[8] = matrix[0][2];
mat[1] = matrix[1][0];
mat[5] = matrix[1][1];
mat[9] = matrix[1][2];
mat[2] = matrix[2][0];
mat[3] = 0;
mat[6] = matrix[2][1];
mat[7] = 0;
mat[10] = matrix[2][2];
mat[11] = 0;
mat[12] = origin[0];
mat[13] = origin[1];
mat[14] = origin[2];
mat[15] = 1;
return mat;
}
/**
* Gets the direct homogeneous affine transformation flattened into a 1-d array,
* ordered left to right, top to bottom
*
* If the mat parameter is null a double[16] array is created;
* otherwise the given array is used.
*
* @param mat double[] optional matrix
* @return double[] the matrix
*/
public final double[] getTransposedFlatMatrix(double[] mat) {
if(mat==null) {
mat = new double[16];
}
mat[0] = matrix[0][0];
mat[1] = matrix[0][1];
mat[2] = matrix[0][2];
mat[3] = 0;
mat[4] = matrix[1][0];
mat[5] = matrix[1][1];
mat[6] = matrix[1][2];
mat[7] = 0;
mat[8] = matrix[2][0];
mat[9] = matrix[2][1];
mat[10] = matrix[2][2];
mat[11] = 0;
mat[12] = origin[0];
mat[13] = origin[1];
mat[14] = origin[2];
mat[15] = 1;
return mat;
}
public final double[] toQuaternion(double[] q) {
double kx, ky, kz;
double kx1, ky1, kz1;
boolean add;
double strace = Math.sqrt(matrix[0][0]+matrix[1][1]+matrix[2][2]+1)/2.0;
kx = matrix[2][1]-matrix[1][2];
ky = matrix[0][2]-matrix[2][0];
kz = matrix[1][0]-matrix[0][1];
if((matrix[0][0]>=matrix[1][1])&&(matrix[0][0]>=matrix[2][2])) {
kx1 = matrix[0][0]-matrix[1][1]-matrix[2][2]+1;
ky1 = matrix[1][0]+matrix[0][1];
kz1 = matrix[2][0]+matrix[0][2];
add = (kx>=0);
} else if(matrix[1][1]>=matrix[2][2]) {
kx1 = matrix[1][0]+matrix[0][1];
ky1 = matrix[1][1]-matrix[0][0]-matrix[2][2]+1;
kz1 = matrix[2][1]+matrix[1][2];
add = (ky>=0);
} else {
kx1 = matrix[2][0]+matrix[0][2];
ky1 = matrix[2][1]+matrix[1][2];
kz1 = matrix[2][2]-matrix[0][0]-matrix[1][1]+1;
add = (kz>=0);
}
if(add) {
kx = kx+kx1;
ky = ky+ky1;
kz = kz+kz1;
} else {
kx = kx-kx1;
ky = ky-ky1;
kz = kz-kz1;
}
double nm = Math.sqrt(kx*kx+ky*ky+kz*kz);
if(nm==0) {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
} else {
double s = Math.sqrt(1-strace*strace)/nm;
q[0] = strace;
q[1] = s*kx;
q[2] = s*ky;
q[3] = s*kz;
}
return q;
}
/**
* Instantiates a rotation that aligns the first vector with the second vector.
*
* @param v1 double[]
* @param v2 double[]
* @return Quaternion
*/
public static Matrix3DTransformation createAlignmentTransformation(double[] v1, double v2[]) {
v1 = VectorMath.normalize(v1.clone());
v2 = VectorMath.normalize(v2.clone());
double theta = Math.acos(VectorMath.dot(v1, v2));
double[] axis = VectorMath.cross3D(v1, v2);
return Matrix3DTransformation.rotation(theta, axis);
}
/**
* Sets the origin for this rotation.
*
* @param ox double
* @param oy double
* @param oz double
*/
public void setOrigin(double ox, double oy, double oz) {
origin[0] = ox;
origin[1] = oy;
origin[2] = oz;
}
/**
* Multiplies (concatenates) this transformation matrix with the given transformation.
*
* @param trans Matrix3DTransformation
*/
public final void multiply(Matrix3DTransformation trans) {
multiply(trans.matrix);
}
/**
* Multiplies this rotation matrix by the given matrix from the right.
*
* @param mat double[][]
*/
public final void multiply(double[][] mat) {
for(int i = 0, n = matrix.length; i<n; i++) {
double[] row = matrix[i].clone();
for(int j = 0, m = matrix[0].length; j<m; j++) {
matrix[i][j] = 0;
for(int k = 0; k<m; k++) {
matrix[i][j] += row[k]*mat[k][j];
}
}
}
inverseMatrix = null;
}
/**
* Sets the origin for this rotation.
*
* @param origin double[] the new origin
* @return double[]
*/
public double[] setOrigin(double[] origin) {
this.origin[0] = origin[0];
this.origin[1] = origin[1];
this.origin[2] = origin[2];
return origin;
}
/**
* Transforms the given point+.
*
* @param point the coordinates to be transformed
*/
public double[] direct(double[] point) {
point[0] -= origin[0];
point[1] -= origin[1];
point[2] -= origin[2];
double[] tempPoint = point.clone();
for(int i = 0, n = point.length; i<n; i++) {
point[i] = origin[i];
for(int j = 0; j<n; j++) {
point[i] += matrix[i][j]*tempPoint[j];
}
}
return point;
}
/**
* Transforms the given matrix into the transformation's coordinate system.
*
* @param matrix to be transformed
*/
public double[][] direct(double[][] mat) {
if(inverseMatrix==null) {
calcInverse(); // computes the inverse using LU decompostion
if(inverseMatrix==null) { // inverse does not exist
throw new UnsupportedOperationException("The inverse matrix does not exist."); //$NON-NLS-1$
}
}
// multiple by inverse matrix from right
for(int i = 0; i<3; i++) { // row index
double[] row = mat[i].clone();
for(int j = 0; j<3; j++) { // col index
mat[i][j] = 0;
for(int k = 0; k<3; k++) { // sum index
mat[i][j] += row[k]*inverseMatrix[k][j];
}
}
}
// multiple by direct matrix from left
for(int j = 0; j<3; j++) { // col index
double[] col = new double[] {mat[0][j], mat[1][j], mat[2][j]};
for(int i = 0; i<3; i++) { // row index
mat[i][j] = 0;
for(int k = 0; k<3; k++) { // sum index
mat[i][j] += matrix[i][k]*col[k];
}
}
}
return mat;
}
/**
* Transforms the given point using the inverse transformation (if it exists).
*
* If the transformation is not invertible, then a call to this
* method must throw a UnsupportedOperationException exception.
*
* @param point the coordinates to be transformed
*/
public double[] inverse(double[] point) throws UnsupportedOperationException {
if(inverseMatrix==null) {
calcInverse(); // computes the inverse using LU decompostion
if(inverseMatrix==null) { // inverse does not exist
throw new UnsupportedOperationException("The inverse matrix does not exist."); //$NON-NLS-1$
}
}
point[0] -= origin[0];
point[1] -= origin[1];
point[2] -= origin[2];
double[] tempPoint = point.clone();
for(int i = 0, n = point.length; i<n; i++) {
point[i] = origin[i];
for(int j = 0; j<n; j++) {
point[i] += inverseMatrix[i][j]*tempPoint[j];
}
}
return point;
}
/**
* Transforms the given matrix from the transformation's coordinate system.
*
* @param matrix to be transformed
*/
public double[][] inverse(double[][] mat) {
if(inverseMatrix==null) {
calcInverse(); // computes the inverse using LU decompostion
if(inverseMatrix==null) { // inverse does not exist
throw new UnsupportedOperationException("The inverse matrix does not exist."); //$NON-NLS-1$
}
}
// multiple by direct matrix from right
for(int i = 0; i<3; i++) { // row index
double[] row = mat[i].clone();
for(int j = 0; j<3; j++) { // col index
mat[i][j] = 0;
for(int k = 0; k<3; k++) { // sum index
mat[i][j] += row[k]*matrix[k][j];
}
}
}
// multiple by inverse matrix from left
for(int j = 0; j<3; j++) { // col index
double[] col = new double[] {mat[0][j], mat[1][j], mat[2][j]};
for(int i = 0; i<3; i++) { // row index
mat[i][j] = 0;
for(int k = 0; k<3; k++) { // sum index
mat[i][j] += inverseMatrix[i][k]*col[k];
}
}
}
return mat;
}
public double[] getOrigin(){return this.origin;}
private void calcInverse() {
LUPDecomposition lupd = new LUPDecomposition(matrix);
inverseMatrix = lupd.inverseMatrixComponents();
}
public static XML.ObjectLoader getLoader() {
return new Affine3DTransformationLoader();
}
protected static class Affine3DTransformationLoader extends XMLLoader {
public void saveObject(XMLControl control, Object obj) {
Matrix3DTransformation transf = (Matrix3DTransformation) obj;
control.setValue("matrix", transf.matrix); //$NON-NLS-1$
if(transf.inverseMatrix!=null) {
control.setValue("inverse", transf.inverseMatrix); //$NON-NLS-1$
}
control.setValue("origin", transf.origin); //$NON-NLS-1$
}
public Object createObject(XMLControl control) {
return new Matrix3DTransformation(null);
}
public Object loadObject(XMLControl control, Object obj) {
Matrix3DTransformation transf = (Matrix3DTransformation) obj;
transf.matrix = (double[][]) control.getObject("matrix"); //$NON-NLS-1$
transf.inverseMatrix = (double[][]) control.getObject("inverse"); //$NON-NLS-1$
transf.origin = (double[]) control.getObject("origin"); //$NON-NLS-1$
return obj;
}
}
}
/*
* Open Source Physics software is free software; you can redistribute
* it and/or modify it under the terms of the GNU General Public License (GPL) as
* published by the Free Software Foundation; either version 2 of the License,
* or(at your option) any later version.
* Code that uses any portion of the code in the org.opensourcephysics package
* or any subpackage (subdirectory) of this package must must also be be released
* under the GNU GPL license.
*
* This software is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston MA 02111-1307 USA
* or view the license online at http://www.gnu.org/copyleft/gpl.html
*
* Copyright (c) 2007 The Open Source Physics project
* http://www.opensourcephysics.org
*/