|
/*
* (C) 2004 - Geotechnical Software Services
*
* This code is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This code 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 Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this program; if not, write to the Free
* Software Foundation, Inc., 59 Temple Place - Suite 330, Boston,
* MA 02111-1307, USA.
*/
//package no.geosoft.cc.geometry;
/**
* Implementation of a 4x4 matrix suited for use in a 2D and 3D
* graphics rendering engine.
*
* @author <a href="mailto:jacob.dreyer@geosoft.no">Jacob Dreyer</a>
*/
public class Matrix4x4
{
private double[] m_; // of 16
/**
* Construct a 4x4 identity matrix.
*/
public Matrix4x4()
{
initialize();
setIdentity();
}
/**
* Construct a 4x4 matrix with the specified element values.
*
* @param m Array of 16 matrix elements, m00, m01, etc.
*/
public Matrix4x4 (double[] m)
{
initialize();
set (m);
}
/**
* Constrauct a 4x4 matrix as a copy of the specified matrix.
*
* @param matrix Matrix to copy.
*/
public Matrix4x4 (Matrix4x4 matrix)
{
initialize();
set (matrix);
}
/**
* Construct a 4x4 matrix with the specified values.
*
* @param m00 Value of element m[0,0].
* @param m01 Value of element m[0,1].
* @param m02 Value of element m[0,2].
* @param m03 Value of element m[0,3].
* @param m10 Value of element m[1,0].
* @param m11 Value of element m[1,1].
* @param m12 Value of element m[1,2].
* @param m13 Value of element m[1,3].
* @param m20 Value of element m[2,0].
* @param m21 Value of element m[2,1].
* @param m22 Value of element m[2,2].
* @param m23 Value of element m[2,3].
* @param m30 Value of element m[3,0].
* @param m31 Value of element m[3,1].
* @param m32 Value of element m[3,2].
* @param m33 Value of element m[3,3].
*/
public Matrix4x4 (double m00, double m01, double m02, double m03,
double m10, double m11, double m12, double m13,
double m20, double m21, double m22, double m23,
double m30, double m31, double m32, double m33)
{
initialize();
set (m00, m01, m02, m03,
m10, m11, m12, m13,
m20, m21, m22, m23,
m30, m31, m32, m33);
}
/**
* Initialize the matrix.
*/
private void initialize()
{
m_ = new double[16];
}
/**
* Make an identity matrix out of this 4x4 matrix.
*/
public void setIdentity()
{
for (int i=0; i<4; i++)
for (int j=0; j<4; j++)
m_[i*4 + j] = i == j ? 1.0 : 0.0;
}
/**
* Set the value of this 4x4matrix according to the specified
* matrix
*
* @param matrix Matrix to copy.
*/
public void set (Matrix4x4 matrix)
{
for (int i=0; i<16; i++)
m_[i] = matrix.m_[i];
}
/**
* Set the values of this 4x4 matrix.
*
* @param m Array of 16 matrix elements, m00, m01, etc.
*/
public void set (double[] m)
{
for (int i=0; i<16; i++)
m_[i] = m[i];
}
/**
* Set the values of this 4x4 matrix.
*
* @param m00 Value of element m[0,0].
* @param m01 Value of element m[0,1].
* @param m02 Value of element m[0,2].
* @param m03 Value of element m[0,3].
* @param m10 Value of element m[1,0].
* @param m11 Value of element m[1,1].
* @param m12 Value of element m[1,2].
* @param m13 Value of element m[1,3].
* @param m20 Value of element m[2,0].
* @param m21 Value of element m[2,1].
* @param m22 Value of element m[2,2].
* @param m23 Value of element m[2,3].
* @param m30 Value of element m[3,0].
* @param m31 Value of element m[3,1].
* @param m32 Value of element m[3,2].
* @param m33 Value of element m[3,3].
*/
public void set (double m00, double m01, double m02, double m03,
double m10, double m11, double m12, double m13,
double m20, double m21, double m22, double m23,
double m30, double m31, double m32, double m33)
{
m_[0] = m00;
m_[1] = m01;
m_[2] = m02;
m_[3] = m03;
m_[4] = m10;
m_[5] = m11;
m_[6] = m12;
m_[7] = m13;
m_[8] = m20;
m_[9] = m21;
m_[10] = m22;
m_[11] = m23;
m_[12] = m30;
m_[13] = m31;
m_[14] = m32;
m_[15] = m33;
}
/**
* Return the values of this 4x4 matrix.
*
* @return Array ov values: m00, m01, etc.
*/
public double[] get()
{
return m_;
}
/**
* Check if this 4x4 matrix equals the specified object.
*
* @param object Object to check.
* @return True if the two are equal, false otherwise.
* @throws ClassCastException if object is not of type Matrix4x4.
*/
public boolean equals (Object object)
{
Matrix4x4 matrix = (Matrix4x4) object;
for (int i=0; i<16; i++)
if (m_[i] != matrix.m_[i]) return false;
return true;
}
/**
* Return matrix element [i,j].
*
* @param i Row of element to get (first row is 0).
* @param j Column of element to get (first column is 0).
* @return Element at specified position.
* @throws ArrayOutOfBoundsException
*/
public double getElement (int i, int j)
{
return m_[i*4 + j];
}
/**
* Set specified matrix element.
*
* @param i Row of element to set (first row is 0).
* @param j Column of element to set (first column is 0).
* @param value New element value.
* @throws ArrayOutOfBoundsException
*/
public void setElement (int i, int j, double value)
{
m_[i*4 + j] = value;
}
/**
* Add the specified 4x4 matrix to this matrix.
*
* @param matrix Matrix to add.
*/
public void add (Matrix4x4 matrix)
{
for (int i=0; i<4; i++)
for (int j=0; j<4; j++)
m_[i*4 + j] += matrix.m_[i*4 + j];
}
/**
* Add two matrices and return the result matrix.
*
* @param m1 First matrix to add.
* @param m2 Second matrix to add.
* @return Sum m1 + m2.
*/
public static Matrix4x4 add (Matrix4x4 m1, Matrix4x4 m2)
{
Matrix4x4 m = new Matrix4x4 (m1);
m.add (m2);
return m;
}
/**
* Multiply this 4x4 matrix with the specified matrix and
* store the result in this 4x4 matrix.
*
* @param matrix Matrix to multiply with.
*/
public void multiply (Matrix4x4 matrix)
{
Matrix4x4 product = new Matrix4x4();
for (int i = 0; i < 16; i += 4) {
for (int j = 0; j < 4; j++) {
product.m_[i + j] = 0.0;
for (int k = 0; k < 4; k++)
product.m_[i + j] += m_[i + k] * matrix.m_[k*4 + j];
}
}
set (product);
}
/**
* Multiply two matrices and return the result matrix.
*
* @param m1 First matrix to multiply.
* @param m2 Second matrix to multiply.
* @return Product m1 * m2.
*/
public static Matrix4x4 multiply (Matrix4x4 m1, Matrix4x4 m2)
{
Matrix4x4 m = new Matrix4x4 (m1);
m.multiply (m2);
return m;
}
/**
* Multiply this 4x4 matrix with the specified vector.
*
* @param vector4 Vector to multiply with.
* @return Result of operation.
*/
public Vector4 multiply (Vector4 vector4)
{
Vector4 product = new Vector4();
for (int i = 0; i < 4; i++) {
double value = 0.0;
for (int j = 0; j < 4; j++)
value += getElement(i, j) * vector4.getElement (j);
product.setElement (i, value);
}
return product;
}
/**
* Transform one coordinate using this 4x4 matrix.
*
* @param point [x0,y0,z0]
* @return Result of operation: [x0',y0',z0']
*/
public double[] transformPoint (double[] point)
{
double[] result = new double[3];
result[0] = point[0] * m_[0] +
point[1] * m_[4] +
point[2] * m_[8] + m_[12];
result[1] = point[0] * m_[1] +
point[1] * m_[5] +
point[2] * m_[9] + m_[13];
result[2] = point[0] * m_[2] +
point[1] * m_[6] +
point[2] * m_[10] + m_[14];
return result;
}
/**
* Transform a set of 3D coordinates using this 4x4 matrix.
* The result of the operation is put back in the original array.
*
* @param point Points to transform [x0,y0,z0,x1,y1,z1,...]
*/
public void transformPoints (double[] points)
{
for (int i = 0; i < points.length; i += 3) {
double x = points[i + 0] * m_[0] +
points[i + 1] * m_[4] +
points[i + 2] * m_[8] + m_[12];
double y = points[i + 0] * m_[1] +
points[i + 1] * m_[5] +
points[i + 2] * m_[9] + m_[13];
double z = points[i + 0] * m_[2] +
points[i + 1] * m_[6] +
points[i + 2] * m_[10] + m_[14];
points[i + 0] = x;
points[i + 1] = y;
points[i + 2] = z;
}
}
/**
* Transform a set of 2D (x,y) coordinates using this 4x4 matrix.
* The result of the operation is put back in the original array
* rounded to the nearest integer.
*
* @param points Points to transform [x0,y0,x1,y1,...].
*/
public void transformXyPoints (double[] points)
{
for (int i = 0; i < points.length; i += 2) {
double x = points[i + 0] * m_[0] +
points[i + 1] * m_[4] + m_[12];
double y = points[i + 0] * m_[1] +
points[i + 1] * m_[5] + m_[13];
points[i + 0] = x;
points[i + 1] = y;
}
}
/**
* Transform a set of 3D coordinates using this 4x4 matrix.
* The result of the operation is put back in the original array.
*
* @param points Points to transform [x0,y0,z0,x1,y1,z1,...].
*/
public void transformPoints (int[] points)
{
for (int i = 0; i < points.length; i += 3) {
double x = points[i + 0] * m_[0] +
points[i + 1] * m_[4] +
points[i + 2] * m_[8] + m_[12];
double y = points[i + 0] * m_[1] +
points[i + 1] * m_[5] +
points[i + 2] * m_[9] + m_[13];
double z = points[i + 0] * m_[2] +
points[i + 1] * m_[6] +
points[i + 2] * m_[10] + m_[14];
points[i + 0] = (int) Math.round (x);
points[i + 1] = (int) Math.round (y);
points[i + 2] = (int) Math.round (z);
}
}
/**
* Transform a set of 2D (x,y) coordinates using this 4x4 matrix.
* The result of the operation is put back in the original array
* rounded to the nearest integer.
*
* @param points Points to transform [x0,y0,x1,y1,...].
*/
public void transformXyPoints (int[] points)
{
for (int i = 0; i < points.length; i += 2) {
double x = points[i + 0] * m_[0] +
points[i + 1] * m_[4] + m_[12];
double y = points[i + 0] * m_[1] +
points[i + 1] * m_[5] + m_[13];
points[i + 0] = (int) Math.round (x);
points[i + 1] = (int) Math.round (y);
}
}
/**
* Apply specified translation to this 4x4 matrix.
*
* @param dx x translation.
* @param dy y translation.
* @param dz z translation.
*/
public void translate (double dx, double dy, double dz)
{
Matrix4x4 translationMatrix = new Matrix4x4();
translationMatrix.setElement (3, 0, dx);
translationMatrix.setElement (3, 1, dy);
translationMatrix.setElement (3, 2, dz);
multiply (translationMatrix);
}
/**
* Apply specified XY translation to this 4x4 matrix.
*
* @param dx x translation.
* @param dy y translation.
*/
public void translate (double dx, double dy)
{
translate (dx, dy, 0.0);
}
/**
* Apply rotation around X axis to this matrix.
*
* @param angle Angle to rotate [radians].
*/
public void rotateX (double angle)
{
Matrix4x4 rotationMatrix = new Matrix4x4();
double cosAngle = Math.cos (angle);
double sinAngle = Math.sin (angle);
rotationMatrix.setElement (1, 1, cosAngle);
rotationMatrix.setElement (1, 2, sinAngle);
rotationMatrix.setElement (2, 1, -sinAngle);
rotationMatrix.setElement (2, 2, cosAngle);
multiply (rotationMatrix);
}
/**
* Apply rotation around Y axis to this matrix.
*
* @param angle Angle to rotate [radians].
*/
public void rotateY (double angle)
{
Matrix4x4 rotationMatrix = new Matrix4x4();
double cosAngle = Math.cos (angle);
double sinAngle = Math.sin (angle);
rotationMatrix.setElement (0, 0, cosAngle);
rotationMatrix.setElement (0, 2, -sinAngle);
rotationMatrix.setElement (2, 0, sinAngle);
rotationMatrix.setElement (2, 2, cosAngle);
multiply (rotationMatrix);
}
/**
* Apply rotation around z axis to this matrix.
*
* @param angle Angle to rotate [radians].
*/
public void rotateZ (double angle)
{
Matrix4x4 rotationMatrix = new Matrix4x4();
double cosAngle = Math.cos (angle);
double sinAngle = Math.sin (angle);
rotationMatrix.setElement (0, 0, cosAngle);
rotationMatrix.setElement (0, 1, sinAngle);
rotationMatrix.setElement (1, 0, -sinAngle);
rotationMatrix.setElement (1, 1, cosAngle);
multiply (rotationMatrix);
}
/**
* Apply rotation around an arbitrary axis.
*
* Ref: http://www.swin.edu.au/astronomy/pbourke/geometry/rotate/
* (but be aware of errors, corrected here)
*
* @param angle Angle to rotate [radians]
* @param p0 First point defining the axis (x,y,z)
* @param p1 Second point defining the axis (x,y,z)
*/
public void rotate (double angle, double[] p0, double[] p1)
{
// Represent axis of rotation by a unit vector [a,b,c]
double a = p1[0] - p0[0];
double b = p1[1] - p0[1];
double c = p1[2] - p0[2];
double length = Math.sqrt (a*a + b*b + c*c);
a /= length;
b /= length;
c /= length;
double d = Math.sqrt (b*b + c*c);
// Coefficients used for step 2 matrix
double e = d == 0.0 ? 1.0 : c / d;
double f = d == 0.0 ? 0.0 : b / d;
// Coefficients used for the step 3 matrix
double k = d;
double l = a;
// Coefficients for the step 5 matrix (inverse of step 3)
double m = d / (a*a + d*d);
double n = a / (a*a + d*d);
// Coefficients for the step 4 matrix
double cosAngle = Math.cos (angle);
double sinAngle = Math.sin (angle);
//
// Step 1
//
Matrix4x4 step1 = new Matrix4x4();
step1.setElement (3, 0, -p0[0]);
step1.setElement (3,
|
