/*
* Copyright 1996-2008 Sun Microsystems, Inc. All Rights Reserved.
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
*
* This code is free software; you can redistribute it and/or modify it
* under the terms of the GNU General Public License version 2 only, as
* published by the Free Software Foundation. Sun designates this
* particular file as subject to the "Classpath" exception as provided
* by Sun in the LICENSE file that accompanied this code.
*
* 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 General Public License
* version 2 for more details (a copy is included in the LICENSE file that
* accompanied this code).
*
* You should have received a copy of the GNU General Public License version
* 2 along with this work; if not, write to the Free Software Foundation,
* Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
*
* Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
* CA 95054 USA or visit www.sun.com if you need additional information or
* have any questions.
*
*/
package javax.media.j3d;
import javax.vecmath.AxisAngle4d;
import javax.vecmath.AxisAngle4f;
import javax.vecmath.GMatrix;
import javax.vecmath.Matrix3d;
import javax.vecmath.Matrix3f;
import javax.vecmath.Matrix4d;
import javax.vecmath.Matrix4f;
import javax.vecmath.Point3d;
import javax.vecmath.Point3f;
import javax.vecmath.Point4d;
import javax.vecmath.Quat4d;
import javax.vecmath.Quat4f;
import javax.vecmath.SingularMatrixException;
import javax.vecmath.Vector3d;
import javax.vecmath.Vector3f;
import javax.vecmath.Vector4d;
import javax.vecmath.Vector4f;
/**
* A generalized transform object represented internally as a 4x4
* double-precision floating point matrix. The mathematical
* representation is
* row major, as in traditional matrix mathematics.
* A Transform3D is used to perform translations, rotations, and
* scaling and shear effects.<P>
*
* A transform has an associated type, and
* all type classification is left to the Transform3D object.
* A transform will typically have multiple types, unless it is a
* general, unclassifiable matrix, in which case it won't be assigned
* a type. <P>
*
* The Transform3D type is internally computed when the transform
* object is constructed and updated any time it is modified. A
* matrix will typically have multiple types. For example, the type
* associated with an identity matrix is the result of ORing all of
* the types, except for ZERO and NEGATIVE_DETERMINANT, together.
* There are public methods available to get the ORed type of the
* transformation, the sign of the determinant, and the least
* general matrix type. The matrix type flags are defined as
* follows:<P>
* <UL>
* <LI>ZERO - zero matrix. All of the elements in the matrix
* have the value 0.</LI><P>
* <LI>IDENTITY - identity matrix. A matrix with ones on its
* main diagonal and zeros every where else.</LI><P>
* <LI>SCALE - the matrix is a uniform scale matrix - there are
* no rotational or translation components.</LI><P>
* <LI>ORTHOGONAL - the four row vectors that make up an orthogonal
* matrix form a basis, meaning that they are mutually orthogonal.
* The scale is unity and there are no translation components.</LI><P>
* <LI>RIGID - the upper 3 X 3 of the matrix is orthogonal, and
* there is a translation component-the scale is unity.</LI><P>
* <LI>CONGRUENT - this is an angle- and length-preserving matrix,
* meaning that it can translate, rotate, and reflect about an axis,
* and scale by an amount that is uniform in all directions. These
* operations preserve the distance between any two points, and the
* angle between any two intersecting lines.</LI><P>
* <LI>AFFINE - an affine matrix can translate, rotate, reflect,
* scale anisotropically, and shear. Lines remain straight, and parallel
* lines remain parallel, but the angle between intersecting lines can
* change.</LI><P>
* </UL>
* A matrix is also classified by the sign of its determinant:<P>
* <UL>
* NEGATIVE_DETERMINANT - this matrix has a negative determinant.
* An orthogonal matrix with a positive determinant is a rotation
* matrix. An orthogonal matrix with a negative determinant is a
* reflection and rotation matrix.<P></UL>
* The Java 3D model for 4 X 4 transformations is:<P>
* <UL><pre>
* [ m00 m01 m02 m03 ] [ x ] [ x' ]
* [ m10 m11 m12 m13 ] . [ y ] = [ y' ]
* [ m20 m21 m22 m23 ] [ z ] [ z' ]
* [ m30 m31 m32 m33 ] [ w ] [ w' ]
*
* x' = m00 . x+m01 . y+m02 . z+m03 . w
* y' = m10 . x+m11 . y+m12 . z+m13 . w
* z' = m20 . x+m21 . y+m22 . z+m23 . w
* w' = m30 . x+m31 . y+m32 . z+m33 . w
* </pre></ul><P>
* Note: When transforming a Point3f or a Point3d, the input w is set to
* 1. When transforming a Vector3f or Vector3d, the input w is set to 0.
*/
public class Transform3D {
double[] mat = new double[16];
//double[] rot = new double[9];
//double[] scales = new double[3];
// allocate the memory only when it is needed. Following three places will allocate the memory,
// void setScaleTranslation(), void computeScales() and void computeScaleRotation()
double[] rot = null;
double[] scales = null;
// Unknown until lazy classification is done
private int type = 0;
// Dirty bit for classification, this is used
// for classify()
private static final int AFFINE_BIT = 0x01;
private static final int ORTHO_BIT = 0x02;
private static final int CONGRUENT_BIT = 0x04;
private static final int RIGID_BIT = 0x08;
private static final int CLASSIFY_BIT = 0x10;
// this is used for scales[], rot[]
private static final int SCALE_BIT = 0x20;
private static final int ROTATION_BIT = 0x40;
// set when SVD renormalization is necessary
private static final int SVD_BIT = 0x80;
private static final int CLASSIFY_ALL_DIRTY = AFFINE_BIT |
ORTHO_BIT |
CONGRUENT_BIT |
RIGID_BIT |
CLASSIFY_BIT;
private static final int ROTSCALESVD_DIRTY = SCALE_BIT |
ROTATION_BIT |
SVD_BIT;
private static final int ALL_DIRTY = CLASSIFY_ALL_DIRTY | ROTSCALESVD_DIRTY;
private int dirtyBits;
boolean autoNormalize = false; // Don't auto normalize by default
/*
// reused temporaries for compute_svd
private boolean svdAllocd =false;
private double[] u1 = null;
private double[] v1 = null;
private double[] t1 = null; // used by both compute_svd and compute_qr
private double[] t2 = null; // used by both compute_svd and compute_qr
private double[] ts = null;
private double[] svdTmp = null;
private double[] svdRot = null;
private double[] single_values = null;
private double[] e = null;
private double[] svdScales = null;
// from svrReorder
private int[] svdOut = null;
private double[] svdMag = null;
// from compute_qr
private double[] cosl = null;
private double[] cosr = null;
private double[] sinl = null;
private double[] sinr = null;
private double[] qr_m = null;
*/
private static final double EPS = 1.110223024E-16;
static final double EPSILON = 1.0e-10;
static final double EPSILON_ABSOLUTE = 1.0e-5;
static final double EPSILON_RELATIVE = 1.0e-4;
/**
* A zero matrix.
*/
public static final int ZERO = 0x01;
/**
* An identity matrix.
*/
public static final int IDENTITY = 0x02;
/**
* A Uniform scale matrix with no translation or other
* off-diagonal components.
*/
public static final int SCALE = 0x04;
/**
* A translation-only matrix with ones on the diagonal.
*
*/
public static final int TRANSLATION = 0x08;
/**
* The four row vectors that make up an orthogonal matrix form a basis,
* meaning that they are mutually orthogonal; an orthogonal matrix with
* positive determinant is a pure rotation matrix; a negative
* determinant indicates a rotation and a reflection.
*/
public static final int ORTHOGONAL = 0x10;
/**
* This matrix is a rotation and a translation with unity scale;
* The upper 3x3 of the matrix is orthogonal, and there is a
* translation component.
*/
public static final int RIGID = 0x20;
/**
* This is an angle and length preserving matrix, meaning that it
* can translate, rotate, and reflect
* about an axis, and scale by an amount that is uniform in all directions.
* These operations preserve the distance between any two points and the
* angle between any two intersecting lines.
*/
public static final int CONGRUENT = 0x40;
/**
* An affine matrix can translate, rotate, reflect, scale anisotropically,
* and shear. Lines remain straight, and parallel lines remain parallel,
* but the angle between intersecting lines can change. In order for a
* transform to be classified as affine, the 4th row must be: [0, 0, 0, 1].
*/
public static final int AFFINE = 0x80;
/**
* This matrix has a negative determinant; an orthogonal matrix with
* a positive determinant is a rotation matrix; an orthogonal matrix
* with a negative determinant is a reflection and rotation matrix.
*/
public static final int NEGATIVE_DETERMINANT = 0x100;
/**
* The upper 3x3 column vectors that make up an orthogonal
* matrix form a basis meaning that they are mutually orthogonal.
* It can have non-uniform or zero x/y/z scale as long as
* the dot product of any two column is zero.
* This one is used by Java3D internal only and should not
* expose to the user.
*/
private static final int ORTHO = 0x40000000;
/**
* Constructs and initializes a transform from the 4 x 4 matrix. The
* type of the constructed transform will be classified automatically.
* @param m1 the 4 x 4 transformation matrix
*/
public Transform3D(Matrix4f m1) {
set(m1);
}
/**
* Constructs and initializes a transform from the 4 x 4 matrix. The
* type of the constructed transform will be classified automatically.
* @param m1 the 4 x 4 transformation matrix
*/
public Transform3D(Matrix4d m1) {
set(m1);
}
/**
* Constructs and initializes a transform from the Transform3D object.
* @param t1 the transformation object to be copied
*/
public Transform3D(Transform3D t1) {
set(t1);
}
/**
* Constructs and initializes a transform to the identity matrix.
*/
public Transform3D() {
setIdentity(); // this will also classify the matrix
}
/**
* Constructs and initializes a transform from the float array of
* length 16; the top row of the matrix is initialized to the first
* four elements of the array, and so on. The type of the transform
* object is classified internally.
* @param matrix a float array of 16
*/
public Transform3D(float[] matrix) {
set(matrix);
}
/**
* Constructs and initializes a transform from the double precision array
* of length 16; the top row of the matrix is initialized to the first
* four elements of the array, and so on. The type of the transform is
* classified internally.
* @param matrix a float array of 16
*/
public Transform3D(double[] matrix) {
set(matrix);
}
/**
* Constructs and initializes a transform from the quaternion,
* translation, and scale values. The scale is applied only to the
* rotational components of the matrix (upper 3 x 3) and not to the
* translational components of the matrix.
* @param q1 the quaternion value representing the rotational component
* @param t1 the translational component of the matrix
* @param s the scale value applied to the rotational components
*/
public Transform3D(Quat4d q1, Vector3d t1, double s) {
set(q1, t1, s);
}
/**
* Constructs and initializes a transform from the quaternion,
* translation, and scale values. The scale is applied only to the
* rotational components of the matrix (upper 3 x 3) and not to the
* translational components of the matrix.
* @param q1 the quaternion value representing the rotational component
* @param t1 the translational component of the matrix
* @param s the scale value applied to the rotational components
*/
public Transform3D(Quat4f q1, Vector3d t1, double s) {
set(q1, t1, s);
}
/**
* Constructs and initializes a transform from the quaternion,
* translation, and scale values. The scale is applied only to the
* rotational components of the matrix (upper 3 x 3) and not to the
* translational components of the matrix.
* @param q1 the quaternion value representing the rotational component
* @param t1 the translational component of the matrix
* @param s the scale value applied to the rotational components
*/
public Transform3D(Quat4f q1, Vector3f t1, float s) {
set(q1, t1, s);
}
/**
* Constructs a transform and initializes it to the upper 4 x 4
* of the GMatrix argument. If the parameter matrix is
* smaller than 4 x 4, the remaining elements in the transform matrix are
* assigned to zero.
* @param m1 the GMatrix
*/
public Transform3D(GMatrix m1) {
set(m1);
}
/**
* Constructs and initializes a transform from the rotation matrix,
* translation, and scale values. The scale is applied only to the
* rotational component of the matrix (upper 3x3) and not to the
* translational component of the matrix.
* @param m1 the rotation matrix representing the rotational component
* @param t1 the translational component of the matrix
* @param s the scale value applied to the rotational components
*/
public Transform3D(Matrix3f m1, Vector3d t1, double s) {
set(m1, t1, s);
}
/**
* Constructs and initializes a transform from the rotation matrix,
* translation, and scale values. The scale is applied only to the
* rotational components of the matrix (upper 3x3) and not to the
* translational components of the matrix.
* @param m1 the rotation matrix representing the rotational component
* @param t1 the translational component of the matrix
* @param s the scale value applied to the rotational components
*/
public Transform3D(Matrix3d m1, Vector3d t1, double s) {
set(m1, t1, s);
}
/**
* Constructs and initializes a transform from the rotation matrix,
* translation, and scale values. The scale is applied only to the
* rotational components of the matrix (upper 3x3) and not to the
* translational components of the matrix.
* @param m1 the rotation matrix representing the rotational component
* @param t1 the translational component of the matrix
* @param s the scale value applied to the rotational components
*/
public Transform3D(Matrix3f m1, Vector3f t1, float s) {
set(m1, t1, s);
}
/**
* Returns the type of this matrix as an or'ed bitmask of
* of all of the type classifications to which it belongs.
* @return or'ed bitmask of all of the type classifications
* of this transform
*/
public final int getType() {
if ((dirtyBits & CLASSIFY_BIT) != 0) {
classify();
}
// clear ORTHO bit which only use internally
return (type & ~ORTHO);
}
// True if type is ORTHO
// Since ORTHO didn't take into account the last row.
final boolean isOrtho() {
if ((dirtyBits & ORTHO_BIT) != 0) {
if ((almostZero(mat[0]*mat[2] + mat[4]*mat[6] +
mat[8]*mat[10]) &&
almostZero(mat[0]*mat[1] + mat[4]*mat[5] +
mat[8]*mat[9]) &&
almostZero(mat[1]*mat[2] + mat[5]*mat[6] +
mat[9]*mat[10]))) {
type |= ORTHO;
dirtyBits &= ~ORTHO_BIT;
return true;
} else {
type &= ~ORTHO;
dirtyBits &= ~ORTHO_BIT;
return false;
}
}
return ((type & ORTHO) != 0);
}
final boolean isCongruent() {
if ((dirtyBits & CONGRUENT_BIT) != 0) {
// This will also classify AFFINE
classifyRigid();
}
return ((type & CONGRUENT) != 0);
}
final boolean isAffine() {
if ((dirtyBits & AFFINE_BIT) != 0) {
classifyAffine();
}
return ((type & AFFINE) != 0);
}
final boolean isRigid() {
if ((dirtyBits & RIGID_BIT) != 0) {
// This will also classify AFFINE & CONGRUENT
if ((dirtyBits & CONGRUENT_BIT) != 0) {
classifyRigid();
} else {
if ((type & CONGRUENT) != 0) {
// Matrix is Congruent, need only
// to check scale
double s;
if ((dirtyBits & SCALE_BIT) != 0){
s = mat[0]*mat[0] + mat[4]*mat[4] +
mat[8]*mat[8];
// Note that
// scales[0] = sqrt(s);
// but since sqrt(1) = 1,
// we don't need to do s = sqrt(s) here.
} else {
if(scales == null)
scales = new double[3];
s = scales[0];
}
if (almostOne(s)) {
type |= RIGID;
} else {
type &= ~RIGID;
}
} else {
// Not even congruent, so isRigid must be false
type &= ~RIGID;
}
dirtyBits &= ~RIGID_BIT;
}
}
return ((type & RIGID) != 0);
}
/**
* Returns the least general type of this matrix; the order of
* generality from least to most is: ZERO, IDENTITY,
* SCALE/TRANSLATION, ORTHOGONAL, RIGID, CONGRUENT, AFFINE.
* If the matrix is ORTHOGONAL, calling the method
* getDeterminantSign() will yield more information.
* @return the least general matrix type
*/
public final int getBestType() {
getType(); // force classify if necessary
if ((type & ZERO) != 0 ) return ZERO;
if ((type & IDENTITY) != 0 ) return IDENTITY;
if ((type & SCALE) != 0 ) return SCALE;
if ((type & TRANSLATION) != 0 ) return TRANSLATION;
if ((type & ORTHOGONAL) != 0 ) return ORTHOGONAL;
if ((type & RIGID) != 0 ) return RIGID;
if ((type & CONGRUENT) != 0 ) return CONGRUENT;
if ((type & AFFINE) != 0 ) return AFFINE;
if ((type & NEGATIVE_DETERMINANT) != 0 ) return NEGATIVE_DETERMINANT;
return 0;
}
/*
private void print_type() {
if ((type & ZERO) > 0 ) System.err.print(" ZERO");
if ((type & IDENTITY) > 0 ) System.err.print(" IDENTITY");
if ((type & SCALE) > 0 ) System.err.print(" SCALE");
if ((type & TRANSLATION) > 0 ) System.err.print(" TRANSLATION");
if ((type & ORTHOGONAL) > 0 ) System.err.print(" ORTHOGONAL");
if ((type & RIGID) > 0 ) System.err.print(" RIGID");
if ((type & CONGRUENT) > 0 ) System.err.print(" CONGRUENT");
if ((type & AFFINE) > 0 ) System.err.print(" AFFINE");
if ((type & NEGATIVE_DETERMINANT) > 0 ) System.err.print(" NEGATIVE_DETERMINANT");
}
*/
/**
* Returns the sign of the determinant of this matrix; a return value
* of true indicates a non-negative determinant; a return value of false
* indicates a negative determinant. A value of true will be returned if
* the determinant is NaN. In general, an orthogonal matrix
* with a positive determinant is a pure rotation matrix; an orthogonal
* matrix with a negative determinant is a both a rotation and a
* reflection matrix.
* @return determinant sign : true means non-negative, false means negative
*/
public final boolean getDeterminantSign() {
double det = determinant();
if (Double.isNaN(det)) {
return true;
}
return det >= 0;
}
/**
* Sets a flag that enables or disables automatic SVD
* normalization. If this flag is enabled, an automatic SVD
* normalization of the rotational components (upper 3x3) of this
* matrix is done after every subsequent matrix operation that
* modifies this matrix. This is functionally equivalent to
* calling normalize() after every subsequent call, but may be
* less computationally expensive.
* The default value for this parameter is false.
* @param autoNormalize the boolean state of auto normalization
*/
public final void setAutoNormalize(boolean autoNormalize) {
this.autoNormalize = autoNormalize;
if (autoNormalize) {
normalize();
}
}
/**
* Returns the state of auto-normalization.
* @return boolean state of auto-normalization
*/
public final boolean getAutoNormalize() {
return this.autoNormalize;
}
/**
* Transforms the point parameter with this transform and
* places the result into pointOut. The fourth element of the
* point input paramter is assumed to be one.
* @param point the input point to be transformed
* @param pointOut the transformed point
*/
void transform(Point3d point, Point4d pointOut) {
pointOut.x = mat[0]*point.x + mat[1]*point.y +
mat[2]*point.z + mat[3];
pointOut.y = mat[4]*point.x + mat[5]*point.y +
mat[6]*point.z + mat[7];
pointOut.z = mat[8]*point.x + mat[9]*point.y +
mat[10]*point.z + mat[11];
pointOut.w = mat[12]*point.x + mat[13]*point.y +
mat[14]*point.z + mat[15];
}
private static final boolean almostZero(double a) {
return ((a < EPSILON_ABSOLUTE) && (a > -EPSILON_ABSOLUTE));
}
private static final boolean almostOne(double a) {
return ((a < 1+EPSILON_ABSOLUTE) && (a > 1-EPSILON_ABSOLUTE));
}
private static final boolean almostEqual(double a, double b) {
double diff = a-b;
if (diff >= 0) {
if (diff < EPSILON) {
return true;
}
// a > b
if ((b > 0) || (a > -b)) {
return (diff < EPSILON_RELATIVE*a);
} else {
return (diff < -EPSILON_RELATIVE*b);
}
} else {
if (diff > -EPSILON) {
return true;
}
// a < b
if ((b < 0) || (-a > b)) {
return (diff > EPSILON_RELATIVE*a);
} else {
return (diff > -EPSILON_RELATIVE*b);
}
}
}
private final void classifyAffine() {
if (!isInfOrNaN() &&
almostZero(mat[12]) &&
almostZero(mat[13]) &&
almostZero(mat[14]) &&
almostOne(mat[15])) {
type |= AFFINE;
} else {
type &= ~AFFINE;
}
dirtyBits &= ~AFFINE_BIT;
}
// same amount of work to classify rigid and congruent
private final void classifyRigid() {
if ((dirtyBits & AFFINE_BIT) != 0) {
// should not touch ORTHO bit
type &= ORTHO;
classifyAffine();
} else {
// keep the affine bit if there is one
// and clear the others (CONGRUENT/RIGID) bit
type &= (ORTHO | AFFINE);
}
if ((type & AFFINE) != 0) {
// checking orthogonal condition
if (isOrtho()) {
if ((dirtyBits & SCALE_BIT) != 0) {
double s0 = mat[0]*mat[0] + mat[4]*mat[4] +
mat[8]*mat[8];
double s1 = mat[1]*mat[1] + mat[5]*mat[5] +
mat[9]*mat[9];
if (almostEqual(s0, s1)) {
double s2 = mat[2]*mat[2] + mat[6]*mat[6] +
mat[10]*mat[10];
if (almostEqual(s2, s0)) {
type |= CONGRUENT;
// Note that scales[0] = sqrt(s0);
if (almostOne(s0)) {
type |= RIGID;
}
}
}
} else {
if(scales == null)
scales = new double[3];
double s = scales[0];
if (almostEqual(s, scales[1]) &&
almostEqual(s, scales[2])) {
type |= CONGRUENT;
if (almostOne(s)) {
type |= RIGID;
}
}
}
}
}
dirtyBits &= (~RIGID_BIT | ~CONGRUENT_BIT);
}
/**
* Classifies a matrix.
*/
private final void classify() {
if ((dirtyBits & (RIGID_BIT|AFFINE_BIT|CONGRUENT_BIT)) != 0) {
// Test for RIGID, CONGRUENT, AFFINE.
classifyRigid();
}
// Test for ZERO, IDENTITY, SCALE, TRANSLATION,
// ORTHOGONAL, NEGATIVE_DETERMINANT
if ((type & AFFINE) != 0) {
if ((type & CONGRUENT) != 0) {
if ((type & RIGID) != 0) {
if (zeroTranslation()) {
type |= ORTHOGONAL;
if (rotateZero()) {
// mat[0], mat[5], mat[10] can be only be
// 1 or -1 when reach here
if ((mat[0] > 0) &&
(mat[5] > 0) &&
(mat[10] > 0)) {
type |= IDENTITY|SCALE|TRANSLATION;
}
}
} else {
if (rotateZero()) {
type |= TRANSLATION;
}
}
} else {
// uniform scale
if (zeroTranslation() && rotateZero()) {
type |= SCALE;
}
}
}
} else {
// last row is not (0, 0, 0, 1)
if (almostZero(mat[12]) &&
almostZero(mat[13]) &&
almostZero(mat[14]) &&
almostZero(mat[15]) &&
zeroTranslation() &&
rotateZero() &&
almostZero(mat[0]) &&
almostZero(mat[5]) &&
almostZero(mat[10])) {
type |= ZERO;
}
}
if (!getDeterminantSign()) {
type |= NEGATIVE_DETERMINANT;
}
dirtyBits &= ~CLASSIFY_BIT;
}
final boolean zeroTranslation() {
return (almostZero(mat[3]) &&
almostZero(mat[7]) &&
almostZero(mat[11]));
}
final boolean rotateZero() {
return (almostZero(mat[1]) && almostZero(mat[2]) &&
almostZero(mat[4]) && almostZero(mat[6]) &&
almostZero(mat[8]) && almostZero(mat[9]));
}
/**
* Returns the matrix elements of this transform as a string.
* @return the matrix elements of this transform
*/
@Override
public String toString() {
// also, print classification?
return
mat[0] + ", " + mat[1] + ", " + mat[2] + ", " + mat[3] + "\n" +
mat[4] + ", " + mat[5] + ", " + mat[6] + ", " + mat[7] + "\n" +
mat[8] + ", " + mat[9] + ", " + mat[10] + ", " + mat[11] + "\n" +
mat[12] + ", " + mat[13] + ", " + mat[14] + ", " + mat[15]
+ "\n";
}
/**
* Sets this transform to the identity matrix.
*/
public final void setIdentity() {
mat[0] = 1.0; mat[1] = 0.0; mat[2] = 0.0; mat[3] = 0.0;
mat[4] = 0.0; mat[5] = 1.0; mat[6] = 0.0; mat[7] = 0.0;
mat[8] = 0.0; mat[9] = 0.0; mat[10] = 1.0; mat[11] = 0.0;
mat[12] = 0.0; mat[13] = 0.0; mat[14] = 0.0; mat[15] = 1.0;
type = IDENTITY | SCALE | ORTHOGONAL | RIGID | CONGRUENT |
AFFINE | TRANSLATION | ORTHO;
dirtyBits = SCALE_BIT | ROTATION_BIT;
// No need to set SVD_BIT
}
/**
* Sets this transform to all zeros.
*/
public final void setZero() {
mat[0] = 0.0; mat[1] = 0.0; mat[2] = 0.0; mat[3] = 0.0;
mat[4] = 0.0; mat[5] = 0.0; mat[6] = 0.0; mat[7] = 0.0;
mat[8] = 0.0; mat[9] = 0.0; mat[10] = 0.0; mat[11] = 0.0;
mat[12] = 0.0; mat[13] = 0.0; mat[14] = 0.0; mat[15] = 0.0;
type = ZERO | ORTHO;
dirtyBits = SCALE_BIT | ROTATION_BIT;
}
/**
* Adds this transform to transform t1 and places the result into
* this: this = this + t1.
* @param t1 the transform to be added to this transform
*/
public final void add(Transform3D t1) {
for (int i=0 ; i<16 ; i++) {
mat[i] += t1.mat[i];
}
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Adds transforms t1 and t2 and places the result into this transform.
* @param t1 the transform to be added
* @param t2 the transform to be added
*/
public final void add(Transform3D t1, Transform3D t2) {
for (int i=0 ; i<16 ; i++) {
mat[i] = t1.mat[i] + t2.mat[i];
}
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Subtracts transform t1 from this transform and places the result
* into this: this = this - t1.
* @param t1 the transform to be subtracted from this transform
*/
public final void sub(Transform3D t1) {
for (int i=0 ; i<16 ; i++) {
mat[i] -= t1.mat[i];
}
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Subtracts transform t2 from transform t1 and places the result into
* this: this = t1 - t2.
* @param t1 the left transform
* @param t2 the right transform
*/
public final void sub(Transform3D t1, Transform3D t2) {
for (int i=0 ; i<16 ; i++) {
mat[i] = t1.mat[i] - t2.mat[i];
}
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Transposes this matrix in place.
*/
public final void transpose() {
double temp;
temp = mat[4];
mat[4] = mat[1];
mat[1] = temp;
temp = mat[8];
mat[8] = mat[2];
mat[2] = temp;
temp = mat[12];
mat[12] = mat[3];
mat[3] = temp;
temp = mat[9];
mat[9] = mat[6];
mat[6] = temp;
temp = mat[13];
mat[13] = mat[7];
mat[7] = temp;
temp = mat[14];
mat[14] = mat[11];
mat[11] = temp;
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Transposes transform t1 and places the value into this transform.
* The transform t1 is not modified.
* @param t1 the transform whose transpose is placed into this transform
*/
public final void transpose(Transform3D t1) {
if (this != t1) {
mat[0] = t1.mat[0];
mat[1] = t1.mat[4];
mat[2] = t1.mat[8];
mat[3] = t1.mat[12];
mat[4] = t1.mat[1];
mat[5] = t1.mat[5];
mat[6] = t1.mat[9];
mat[7] = t1.mat[13];
mat[8] = t1.mat[2];
mat[9] = t1.mat[6];
mat[10] = t1.mat[10];
mat[11] = t1.mat[14];
mat[12] = t1.mat[3];
mat[13] = t1.mat[7];
mat[14] = t1.mat[11];
mat[15] = t1.mat[15];
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
} else {
this.transpose();
}
}
/**
* Sets the value of this transform to the matrix conversion of the
* single precision quaternion argument; the non-rotational
* components are set as if this were an identity matrix.
* @param q1 the quaternion to be converted
*/
public final void set(Quat4f q1) {
mat[0] = (1.0f - 2.0f*q1.y*q1.y - 2.0f*q1.z*q1.z);
mat[4] = (2.0f*(q1.x*q1.y + q1.w*q1.z));
mat[8] = (2.0f*(q1.x*q1.z - q1.w*q1.y));
mat[1] = (2.0f*(q1.x*q1.y - q1.w*q1.z));
mat[5] = (1.0f - 2.0f*q1.x*q1.x - 2.0f*q1.z*q1.z);
mat[9] = (2.0f*(q1.y*q1.z + q1.w*q1.x));
mat[2] = (2.0f*(q1.x*q1.z + q1.w*q1.y));
mat[6] = (2.0f*(q1.y*q1.z - q1.w*q1.x));
mat[10] = (1.0f - 2.0f*q1.x*q1.x - 2.0f*q1.y*q1.y);
mat[3] = 0.0;
mat[7] = 0.0;
mat[11] = 0.0;
mat[12] = 0.0;
mat[13] = 0.0;
mat[14] = 0.0;
mat[15] = 1.0;
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(q1)) {
dirtyBits = ALL_DIRTY;
return;
}
dirtyBits = CLASSIFY_BIT | SCALE_BIT | ROTATION_BIT;
type = RIGID | CONGRUENT | AFFINE | ORTHO;
}
/**
* Sets the value of this transform to the matrix conversion of the
* double precision quaternion argument; the non-rotational
* components are set as if this were an identity matrix.
* @param q1 the quaternion to be converted
*/
public final void set(Quat4d q1) {
mat[0] = (1.0 - 2.0*q1.y*q1.y - 2.0*q1.z*q1.z);
mat[4] = (2.0*(q1.x*q1.y + q1.w*q1.z));
mat[8] = (2.0*(q1.x*q1.z - q1.w*q1.y));
mat[1] = (2.0*(q1.x*q1.y - q1.w*q1.z));
mat[5] = (1.0 - 2.0*q1.x*q1.x - 2.0*q1.z*q1.z);
mat[9] = (2.0*(q1.y*q1.z + q1.w*q1.x));
mat[2] = (2.0*(q1.x*q1.z + q1.w*q1.y));
mat[6] = (2.0*(q1.y*q1.z - q1.w*q1.x));
mat[10] = (1.0 - 2.0*q1.x*q1.x - 2.0*q1.y*q1.y);
mat[3] = 0.0;
mat[7] = 0.0;
mat[11] = 0.0;
mat[12] = 0.0;
mat[13] = 0.0;
mat[14] = 0.0;
mat[15] = 1.0;
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(q1)) {
dirtyBits = ALL_DIRTY;
return;
}
dirtyBits = CLASSIFY_BIT | SCALE_BIT | ROTATION_BIT;
type = RIGID | CONGRUENT | AFFINE | ORTHO;
}
/**
* Sets the rotational component (upper 3x3) of this transform to the
* matrix values in the double precision Matrix3d argument; the other
* elements of this transform are unchanged; any pre-existing scale
* will be preserved; the argument matrix m1 will be checked for proper
* normalization when this transform is internally classified.
* @param m1 the double precision 3x3 matrix
*/
public final void setRotation(Matrix3d m1) {
if ((dirtyBits & SCALE_BIT)!= 0) {
computeScales(false);
}
mat[0] = m1.m00*scales[0];
mat[1] = m1.m01*scales[1];
mat[2] = m1.m02*scales[2];
mat[4] = m1.m10*scales[0];
mat[5] = m1.m11*scales[1];
mat[6] = m1.m12*scales[2];
mat[8] = m1.m20*scales[0];
mat[9] = m1.m21*scales[1];
mat[10]= m1.m22*scales[2];
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
// the matrix pass in may not normalize
normalize();
}
}
/**
* Sets the rotational component (upper 3x3) of this transform to the
* matrix values in the single precision Matrix3f argument; the other
* elements of this transform are unchanged; any pre-existing scale
* will be preserved; the argument matrix m1 will be checked for proper
* normalization when this transform is internally classified.
* @param m1 the single precision 3x3 matrix
*/
public final void setRotation(Matrix3f m1) {
if ((dirtyBits & SCALE_BIT)!= 0) {
computeScales(false);
}
mat[0] = m1.m00*scales[0];
mat[1] = m1.m01*scales[1];
mat[2] = m1.m02*scales[2];
mat[4] = m1.m10*scales[0];
mat[5] = m1.m11*scales[1];
mat[6] = m1.m12*scales[2];
mat[8] = m1.m20*scales[0];
mat[9] = m1.m21*scales[1];
mat[10]= m1.m22*scales[2];
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Sets the rotational component (upper 3x3) of this transform to the
* matrix equivalent values of the quaternion argument; the other
* elements of this transform are unchanged; any pre-existing scale
* in the transform is preserved.
* @param q1 the quaternion that specifies the rotation
*/
public final void setRotation(Quat4f q1) {
if ((dirtyBits & SCALE_BIT)!= 0) {
computeScales(false);
}
mat[0] = (1.0 - 2.0*q1.y*q1.y - 2.0*q1.z*q1.z)*scales[0];
mat[4] = (2.0*(q1.x*q1.y + q1.w*q1.z))*scales[0];
mat[8] = (2.0*(q1.x*q1.z - q1.w*q1.y))*scales[0];
mat[1] = (2.0*(q1.x*q1.y - q1.w*q1.z))*scales[1];
mat[5] = (1.0 - 2.0*q1.x*q1.x - 2.0*q1.z*q1.z)*scales[1];
mat[9] = (2.0*(q1.y * q1.z + q1.w * q1.x))*scales[1];
mat[2] = (2.0*(q1.x*q1.z + q1.w*q1.y))*scales[2];
mat[6] = (2.0*(q1.y*q1.z - q1.w*q1.x))*scales[2];
mat[10] = (1.0 - 2.0*q1.x*q1.x - 2.0*q1.y*q1.y)*scales[2];
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(q1)) {
dirtyBits = ALL_DIRTY;
return;
}
dirtyBits |= CLASSIFY_BIT | ROTATION_BIT;
dirtyBits &= ~ORTHO_BIT;
type |= ORTHO;
type &= ~(ORTHOGONAL|IDENTITY|SCALE|TRANSLATION|SCALE|ZERO);
}
/**
* Sets the rotational component (upper 3x3) of this transform to the
* matrix equivalent values of the quaternion argument; the other
* elements of this transform are unchanged; any pre-existing scale
* in the transform is preserved.
* @param q1 the quaternion that specifies the rotation
*/
public final void setRotation(Quat4d q1) {
if ((dirtyBits & SCALE_BIT)!= 0) {
computeScales(false);
}
mat[0] = (1.0 - 2.0*q1.y*q1.y - 2.0*q1.z*q1.z)*scales[0];
mat[4] = (2.0*(q1.x*q1.y + q1.w*q1.z))*scales[0];
mat[8] = (2.0*(q1.x*q1.z - q1.w*q1.y))*scales[0];
mat[1] = (2.0*(q1.x*q1.y - q1.w*q1.z))*scales[1];
mat[5] = (1.0 - 2.0*q1.x*q1.x - 2.0*q1.z*q1.z)*scales[1];
mat[9] = (2.0*(q1.y * q1.z + q1.w * q1.x))*scales[1];
mat[2] = (2.0*(q1.x*q1.z + q1.w*q1.y))*scales[2];
mat[6] = (2.0*(q1.y*q1.z - q1.w*q1.x))*scales[2];
mat[10] = (1.0 - 2.0*q1.x*q1.x - 2.0*q1.y*q1.y)*scales[2];
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(q1)) {
dirtyBits = ALL_DIRTY;
return;
}
dirtyBits |= CLASSIFY_BIT | ROTATION_BIT;
dirtyBits &= ~ORTHO_BIT;
type |= ORTHO;
type &= ~(ORTHOGONAL|IDENTITY|SCALE|TRANSLATION|SCALE|ZERO);
}
/**
* Sets the value of this transform to the matrix conversion
* of the single precision axis-angle argument; all of the matrix
* values are modified.
* @param a1 the axis-angle to be converted (x, y, z, angle)
*/
public final void set(AxisAngle4f a1) {
double mag = Math.sqrt( a1.x*a1.x + a1.y*a1.y + a1.z*a1.z);
if (almostZero(mag)) {
setIdentity();
} else {
mag = 1.0/mag;
double ax = a1.x*mag;
double ay = a1.y*mag;
double az = a1.z*mag;
double sinTheta = Math.sin((double)a1.angle);
double cosTheta = Math.cos((double)a1.angle);
double t = 1.0 - cosTheta;
double xz = ax * az;
double xy = ax * ay;
double yz = ay * az;
mat[0] = t * ax * ax + cosTheta;
mat[1] = t * xy - sinTheta * az;
mat[2] = t * xz + sinTheta * ay;
mat[3] = 0.0;
mat[4] = t * xy + sinTheta * az;
mat[5] = t * ay * ay + cosTheta;
mat[6] = t * yz - sinTheta * ax;
mat[7] = 0.0;
mat[8] = t * xz - sinTheta * ay;
mat[9] = t * yz + sinTheta * ax;
mat[10] = t * az * az + cosTheta;
mat[11] = 0.0;
mat[12] = 0.0;
mat[13] = 0.0;
mat[14] = 0.0;
mat[15] = 1.0;
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(a1)) {
dirtyBits = ALL_DIRTY;
return;
}
type = CONGRUENT | AFFINE | RIGID | ORTHO;
dirtyBits = CLASSIFY_BIT | ROTATION_BIT | SCALE_BIT;
}
}
/**
* Sets the value of this transform to the matrix conversion
* of the double precision axis-angle argument; all of the matrix
* values are modified.
* @param a1 the axis-angle to be converted (x, y, z, angle)
*/
public final void set(AxisAngle4d a1) {
double mag = Math.sqrt( a1.x*a1.x + a1.y*a1.y + a1.z*a1.z);
if (almostZero(mag)) {
setIdentity();
} 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;
mat[0] = t * ax * ax + cosTheta;
mat[1] = t * xy - sinTheta * az;
mat[2] = t * xz + sinTheta * ay;
mat[3] = 0.0;
mat[4] = t * xy + sinTheta * az;
mat[5] = t * ay * ay + cosTheta;
mat[6] = t * yz - sinTheta * ax;
mat[7] = 0.0;
mat[8] = t * xz - sinTheta * ay;
mat[9] = t * yz + sinTheta * ax;
mat[10] = t * az * az + cosTheta;
mat[11] = 0.0;
mat[12] = 0.0;
mat[13] = 0.0;
mat[14] = 0.0;
mat[15] = 1.0;
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(a1)) {
dirtyBits = ALL_DIRTY;
return;
}
type = CONGRUENT | AFFINE | RIGID | ORTHO;
dirtyBits = CLASSIFY_BIT | ROTATION_BIT | SCALE_BIT;
}
}
/**
* Sets the rotational component (upper 3x3) of this transform to the
* matrix equivalent values of the axis-angle argument; the other
* elements of this transform are unchanged; any pre-existing scale
* in the transform is preserved.
* @param a1 the axis-angle to be converted (x, y, z, angle)
*/
public final void setRotation(AxisAngle4d a1) {
if ((dirtyBits & SCALE_BIT)!= 0) {
computeScales(false);
}
double mag = Math.sqrt( a1.x*a1.x + a1.y*a1.y + a1.z*a1.z);
if (almostZero(mag)) {
mat[0] = scales[0];
mat[1] = 0.0;
mat[2] = 0.0;
mat[4] = 0.0;
mat[5] = scales[1];
mat[6] = 0.0;
mat[8] = 0.0;
mat[9] = 0.0;
mat[10] = scales[2];
} 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;
mat[0] = (t * ax * ax + cosTheta)*scales[0];
mat[1] = (t * xy - sinTheta * az)*scales[1];
mat[2] = (t * xz + sinTheta * ay)*scales[2];
mat[4] = (t * xy + sinTheta * az)*scales[0];
mat[5] = (t * ay * ay + cosTheta)*scales[1];
mat[6] = (t * yz - sinTheta * ax)*scales[2];
mat[8] = (t * xz - sinTheta * ay)*scales[0];
mat[9] = (t * yz + sinTheta * ax)*scales[1];
mat[10] = (t * az * az + cosTheta)*scales[2];
}
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(a1)) {
dirtyBits = ALL_DIRTY;
return;
}
// Rigid remain rigid, congruent remain congruent after
// set rotation
dirtyBits |= CLASSIFY_BIT | ROTATION_BIT;
dirtyBits &= ~ORTHO_BIT;
type |= ORTHO;
type &= ~(ORTHOGONAL|IDENTITY|SCALE|TRANSLATION|SCALE|ZERO);
}
/**
* Sets the rotational component (upper 3x3) of this transform to the
* matrix equivalent values of the axis-angle argument; the other
* elements of this transform are unchanged; any pre-existing scale
* in the transform is preserved.
* @param a1 the axis-angle to be converted (x, y, z, angle)
*/
public final void setRotation(AxisAngle4f a1) {
if ((dirtyBits & SCALE_BIT)!= 0) {
computeScales(false);
}
double mag = Math.sqrt( a1.x*a1.x + a1.y*a1.y + a1.z*a1.z);
if (almostZero(mag)) {
mat[0] = scales[0];
mat[1] = 0.0;
mat[2] = 0.0;
mat[4] = 0.0;
mat[5] = scales[1];
mat[6] = 0.0;
mat[8] = 0.0;
mat[9] = 0.0;
mat[10] = scales[2];
} 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;
mat[0] = (t * ax * ax + cosTheta)*scales[0];
mat[1] = (t * xy - sinTheta * az)*scales[1];
mat[2] = (t * xz + sinTheta * ay)*scales[2];
mat[4] = (t * xy + sinTheta * az)*scales[0];
mat[5] = (t * ay * ay + cosTheta)*scales[1];
mat[6] = (t * yz - sinTheta * ax)*scales[2];
mat[8] = (t * xz - sinTheta * ay)*scales[0];
mat[9] = (t * yz + sinTheta * ax)*scales[1];
mat[10] = (t * az * az + cosTheta)*scales[2];
}
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(a1)) {
dirtyBits = ALL_DIRTY;
return;
}
// Rigid remain rigid, congruent remain congruent after
// set rotation
dirtyBits |= CLASSIFY_BIT | ROTATION_BIT;
dirtyBits &= (~ORTHO_BIT | ~SVD_BIT);
type |= ORTHO;
type &= ~(ORTHOGONAL|IDENTITY|SCALE|TRANSLATION|SCALE|ZERO);
}
/**
* Sets the value of this transform to a counter clockwise rotation
* about the x axis. All of the non-rotational components are set as
* if this were an identity matrix.
* @param angle the angle to rotate about the X axis in radians
*/
public void rotX(double angle) {
double sinAngle = Math.sin(angle);
double cosAngle = Math.cos(angle);
mat[0] = 1.0;
mat[1] = 0.0;
mat[2] = 0.0;
mat[3] = 0.0;
mat[4] = 0.0;
mat[5] = cosAngle;
mat[6] = -sinAngle;
mat[7] = 0.0;
mat[8] = 0.0;
mat[9] = sinAngle;
mat[10] = cosAngle;
mat[11] = 0.0;
mat[12] = 0.0;
mat[13] = 0.0;
mat[14] = 0.0;
mat[15] = 1.0;
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(angle)) {
dirtyBits = ALL_DIRTY;
return;
}
type = CONGRUENT | AFFINE | RIGID | ORTHO;
dirtyBits = CLASSIFY_BIT | ROTATION_BIT | SCALE_BIT;
}
/**
* Sets the value of this transform to a counter clockwise rotation about
* the y axis. All of the non-rotational components are set as if this
* were an identity matrix.
* @param angle the angle to rotate about the Y axis in radians
*/
public void rotY(double angle) {
double sinAngle = Math.sin(angle);
double cosAngle = Math.cos(angle);
mat[0] = cosAngle;
mat[1] = 0.0;
mat[2] = sinAngle;
mat[3] = 0.0;
mat[4] = 0.0;
mat[5] = 1.0;
mat[6] = 0.0;
mat[7] = 0.0;
mat[8] = -sinAngle;
mat[9] = 0.0;
mat[10] = cosAngle;
mat[11] = 0.0;
mat[12] = 0.0;
mat[13] = 0.0;
mat[14] = 0.0;
mat[15] = 1.0;
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(angle)) {
dirtyBits = ALL_DIRTY;
return;
}
type = CONGRUENT | AFFINE | RIGID | ORTHO;
dirtyBits = CLASSIFY_BIT | ROTATION_BIT | SCALE_BIT;
}
/**
* Sets the value of this transform to a counter clockwise rotation
* about the z axis. All of the non-rotational components are set
* as if this were an identity matrix.
* @param angle the angle to rotate about the Z axis in radians
*/
public void rotZ(double angle) {
double sinAngle = Math.sin(angle);
double cosAngle = Math.cos(angle);
mat[0] = cosAngle;
mat[1] = -sinAngle;
mat[2] = 0.0;
mat[3] = 0.0;
mat[4] = sinAngle;
mat[5] = cosAngle;
mat[6] = 0.0;
mat[7] = 0.0;
mat[8] = 0.0;
mat[9] = 0.0;
mat[10] = 1.0;
mat[11] = 0.0;
mat[12] = 0.0;
mat[13] = 0.0;
mat[14] = 0.0;
mat[15] = 1.0;
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(angle)) {
dirtyBits = ALL_DIRTY;
return;
}
type = CONGRUENT | AFFINE | RIGID | ORTHO;
dirtyBits = CLASSIFY_BIT | ROTATION_BIT | SCALE_BIT;
}
/**
* Sets the translational value of this matrix to the Vector3f parameter
* values, and sets the other components of the matrix as if this
* transform were an identity matrix.
* @param trans the translational component
*/
public final void set(Vector3f trans) {
mat[0] = 1.0; mat[1] = 0.0; mat[2] = 0.0; mat[3] = trans.x;
mat[4] = 0.0; mat[5] = 1.0; mat[6] = 0.0; mat[7] = trans.y;
mat[8] = 0.0; mat[9] = 0.0; mat[10] = 1.0; mat[11] = trans.z;
mat[12] = 0.0; mat[13] = 0.0; mat[14] = 0.0; mat[15] = 1.0;
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(trans)) {
dirtyBits = ALL_DIRTY;
return;
}
type = CONGRUENT | AFFINE | RIGID | ORTHO;
dirtyBits = CLASSIFY_BIT | ROTATION_BIT | SCALE_BIT;
}
/**
* Sets the translational value of this matrix to the Vector3d paramter
* values, and sets the other components of the matrix as if this
* transform were an identity matrix.
* @param trans the translational component
*/
public final void set(Vector3d trans) {
mat[0] = 1.0; mat[1] = 0.0; mat[2] = 0.0; mat[3] = trans.x;
mat[4] = 0.0; mat[5] = 1.0; mat[6] = 0.0; mat[7] = trans.y;
mat[8] = 0.0; mat[9] = 0.0; mat[10] = 1.0; mat[11] = trans.z;
mat[12] = 0.0; mat[13] = 0.0; mat[14] = 0.0; mat[15] = 1.0;
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(trans)) {
dirtyBits = ALL_DIRTY;
return;
}
type = CONGRUENT | AFFINE | RIGID | ORTHO;
dirtyBits = CLASSIFY_BIT | ROTATION_BIT | SCALE_BIT;
}
/**
* Sets the scale component of the current transform; any existing
* scale is first factored out of the existing transform before
* the new scale is applied.
* @param scale the new scale amount
*/
public final void setScale(double scale) {
if ((dirtyBits & ROTATION_BIT)!= 0) {
computeScaleRotation(false);
}
scales[0] = scales[1] = scales[2] = scale;
mat[0] = rot[0]*scale;
mat[1] = rot[1]*scale;
mat[2] = rot[2]*scale;
mat[4] = rot[3]*scale;
mat[5] = rot[4]*scale;
mat[6] = rot[5]*scale;
mat[8] = rot[6]*scale;
mat[9] = rot[7]*scale;
mat[10] = rot[8]*scale;
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(scale)) {
dirtyBits = ALL_DIRTY;
return;
}
dirtyBits |= (CLASSIFY_BIT | RIGID_BIT | CONGRUENT_BIT | SVD_BIT);
dirtyBits &= ~SCALE_BIT;
}
/**
* Sets the possibly non-uniform scale component of the current
* transform; any existing scale is first factored out of the
* existing transform before the new scale is applied.
* @param scale the new x,y,z scale values
*/
public final void setScale(Vector3d scale) {
if ((dirtyBits & ROTATION_BIT)!= 0) {
computeScaleRotation(false);
}
scales[0] = scale.x;
scales[1] = scale.y;
scales[2] = scale.z;
mat[0] = rot[0]*scale.x;
mat[1] = rot[1]*scale.y;
mat[2] = rot[2]*scale.z;
mat[4] = rot[3]*scale.x;
mat[5] = rot[4]*scale.y;
mat[6] = rot[5]*scale.z;
mat[8] = rot[6]*scale.x;
mat[9] = rot[7]*scale.y;
mat[10] = rot[8]*scale.z;
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(scale)) {
dirtyBits = ALL_DIRTY;
return;
}
dirtyBits |= (CLASSIFY_BIT | RIGID_BIT | CONGRUENT_BIT | SVD_BIT);
dirtyBits &= ~SCALE_BIT;
}
/**
* Replaces the current transform with a non-uniform scale transform.
* All values of the existing transform are replaced.
* @param xScale the new X scale amount
* @param yScale the new Y scale amount
* @param zScale the new Z scale amount
* @deprecated Use setScale(Vector3d) instead of setNonUniformScale;
* note that the setScale only modifies the scale component
*/
public final void setNonUniformScale(double xScale,
double yScale,
double zScale) {
if(scales == null)
scales = new double[3];
scales[0] = xScale;
scales[1] = yScale;
scales[2] = zScale;
mat[0] = xScale;
mat[1] = 0.0;
mat[2] = 0.0;
mat[3] = 0.0;
mat[4] = 0.0;
mat[5] = yScale;
mat[6] = 0.0;
mat[7] = 0.0;
mat[8] = 0.0;
mat[9] = 0.0;
mat[10] = zScale;
mat[11] = 0.0;
mat[12] = 0.0;
mat[13] = 0.0;
mat[14] = 0.0;
mat[15] = 1.0;
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
}
/**
* Replaces the translational components of this transform to the values
* in the Vector3f argument; the other values of this transform are not
* modified.
* @param trans the translational component
*/
public final void setTranslation(Vector3f trans) {
mat[3] = trans.x;
mat[7] = trans.y;
mat[11] = trans.z;
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(trans)) {
dirtyBits = ALL_DIRTY;
return;
}
// Only preserve CONGRUENT, RIGID, ORTHO
type &= ~(ORTHOGONAL|IDENTITY|SCALE|TRANSLATION|SCALE|ZERO);
dirtyBits |= CLASSIFY_BIT;
}
/**
* Replaces the translational components of this transform to the values
* in the Vector3d argument; the other values of this transform are not
* modified.
* @param trans the translational component
*/
public final void setTranslation(Vector3d trans) {
mat[3] = trans.x;
mat[7] = trans.y;
mat[11] = trans.z;
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(trans)) {
dirtyBits = ALL_DIRTY;
return;
}
type &= ~(ORTHOGONAL|IDENTITY|SCALE|TRANSLATION|SCALE|ZERO);
dirtyBits |= CLASSIFY_BIT;
}
/**
* Sets the value of this matrix from the rotation expressed
* by the quaternion q1, the translation t1, and the scale s.
* @param q1 the rotation expressed as a quaternion
* @param t1 the translation
* @param s the scale value
*/
public final void set(Quat4d q1, Vector3d t1, double s) {
if(scales == null)
scales = new double[3];
scales[0] = scales[1] = scales[2] = s;
mat[0] = (1.0 - 2.0*q1.y*q1.y - 2.0*q1.z*q1.z)*s;
mat[4] = (2.0*(q1.x*q1.y + q1.w*q1.z))*s;
mat[8] = (2.0*(q1.x*q1.z - q1.w*q1.y))*s;
mat[1] = (2.0*(q1.x*q1.y - q1.w*q1.z))*s;
mat[5] = (1.0 - 2.0*q1.x*q1.x - 2.0*q1.z*q1.z)*s;
mat[9] = (2.0*(q1.y*q1.z + q1.w*q1.x))*s;
mat[2] = (2.0*(q1.x*q1.z + q1.w*q1.y))*s;
mat[6] = (2.0*(q1.y*q1.z - q1.w*q1.x))*s;
mat[10] = (1.0 - 2.0*q1.x*q1.x - 2.0*q1.y*q1.y)*s;
mat[3] = t1.x;
mat[7] = t1.y;
mat[11] = t1.z;
mat[12] = 0.0;
mat[13] = 0.0;
mat[14] = 0.0;
mat[15] = 1.0;
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
}
/**
* Sets the value of this matrix from the rotation expressed
* by the quaternion q1, the translation t1, and the scale s.
* @param q1 the rotation expressed as a quaternion
* @param t1 the translation
* @param s the scale value
*/
public final void set(Quat4f q1, Vector3d t1, double s) {
if(scales == null)
scales = new double[3];
scales[0] = scales[1] = scales[2] = s;
mat[0] = (1.0f - 2.0f*q1.y*q1.y - 2.0f*q1.z*q1.z)*s;
mat[4] = (2.0f*(q1.x*q1.y + q1.w*q1.z))*s;
mat[8] = (2.0f*(q1.x*q1.z - q1.w*q1.y))*s;
mat[1] = (2.0f*(q1.x*q1.y - q1.w*q1.z))*s;
mat[5] = (1.0f - 2.0f*q1.x*q1.x - 2.0f*q1.z*q1.z)*s;
mat[9] = (2.0f*(q1.y*q1.z + q1.w*q1.x))*s;
mat[2] = (2.0f*(q1.x*q1.z + q1.w*q1.y))*s;
mat[6] = (2.0f*(q1.y*q1.z - q1.w*q1.x))*s;
mat[10] = (1.0f - 2.0f*q1.x*q1.x - 2.0f*q1.y*q1.y)*s;
mat[3] = t1.x;
mat[7] = t1.y;
mat[11] = t1.z;
mat[12] = 0.0;
mat[13] = 0.0;
mat[14] = 0.0;
mat[15] = 1.0;
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
}
/**
* Sets the value of this matrix from the rotation expressed
* by the quaternion q1, the translation t1, and the scale s.
* @param q1 the rotation expressed as a quaternion
* @param t1 the translation
* @param s the scale value
*/
public final void set(Quat4f q1, Vector3f t1, float s) {
if(scales == null)
scales = new double[3];
scales[0] = scales[1] = scales[2] = s;
mat[0] = (1.0f - 2.0f*q1.y*q1.y - 2.0f*q1.z*q1.z)*s;
mat[4] = (2.0f*(q1.x*q1.y + q1.w*q1.z))*s;
mat[8] = (2.0f*(q1.x*q1.z - q1.w*q1.y))*s;
mat[1] = (2.0f*(q1.x*q1.y - q1.w*q1.z))*s;
mat[5] = (1.0f - 2.0f*q1.x*q1.x - 2.0f*q1.z*q1.z)*s;
mat[9] = (2.0f*(q1.y*q1.z + q1.w*q1.x))*s;
mat[2] = (2.0f*(q1.x*q1.z + q1.w*q1.y))*s;
mat[6] = (2.0f*(q1.y*q1.z - q1.w*q1.x))*s;
mat[10] = (1.0f - 2.0f*q1.x*q1.x - 2.0f*q1.y*q1.y)*s;
mat[3] = t1.x;
mat[7] = t1.y;
mat[11] = t1.z;
mat[12] = 0.0;
mat[13] = 0.0;
mat[14] = 0.0;
mat[15] = 1.0;
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
}
/**
* Sets the value of this matrix from the rotation expressed
* by the rotation matrix m1, the translation t1, and the scale s.
* The scale is only applied to the
* rotational component of the matrix (upper 3x3) and not to the
* translational component of the matrix.
* @param m1 the rotation matrix
* @param t1 the translation
* @param s the scale value
*/
public final void set(Matrix3f m1, Vector3f t1, float s) {
mat[0]=m1.m00*s;
mat[1]=m1.m01*s;
mat[2]=m1.m02*s;
mat[3]=t1.x;
mat[4]=m1.m10*s;
mat[5]=m1.m11*s;
mat[6]=m1.m12*s;
mat[7]=t1.y;
mat[8]=m1.m20*s;
mat[9]=m1.m21*s;
mat[10]=m1.m22*s;
mat[11]=t1.z;
mat[12]=0.0;
mat[13]=0.0;
mat[14]=0.0;
mat[15]=1.0;
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
// input matrix may not normalize
normalize();
}
}
/**
* Sets the value of this matrix from the rotation expressed
* by the rotation matrix m1, the translation t1, and the scale s.
* The scale is only applied to the
* rotational component of the matrix (upper 3x3) and not to the
* translational component of the matrix.
* @param m1 the rotation matrix
* @param t1 the translation
* @param s the scale value
*/
public final void set(Matrix3f m1, Vector3d t1, double s) {
mat[0]=m1.m00*s;
mat[1]=m1.m01*s;
mat[2]=m1.m02*s;
mat[3]=t1.x;
mat[4]=m1.m10*s;
mat[5]=m1.m11*s;
mat[6]=m1.m12*s;
mat[7]=t1.y;
mat[8]=m1.m20*s;
mat[9]=m1.m21*s;
mat[10]=m1.m22*s;
mat[11]=t1.z;
mat[12]=0.0;
mat[13]=0.0;
mat[14]=0.0;
mat[15]=1.0;
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Sets the value of this matrix from the rotation expressed
* by the rotation matrix m1, the translation t1, and the scale s.
* The scale is only applied to the
* rotational component of the matrix (upper 3x3) and not to the
* translational component of the matrix.
* @param m1 the rotation matrix
* @param t1 the translation
* @param s the scale value
*/
public final void set(Matrix3d m1, Vector3d t1, double s) {
mat[0]=m1.m00*s;
mat[1]=m1.m01*s;
mat[2]=m1.m02*s;
mat[3]=t1.x;
mat[4]=m1.m10*s;
mat[5]=m1.m11*s;
mat[6]=m1.m12*s;
mat[7]=t1.y;
mat[8]=m1.m20*s;
mat[9]=m1.m21*s;
mat[10]=m1.m22*s;
mat[11]=t1.z;
mat[12]=0.0;
mat[13]=0.0;
mat[14]=0.0;
mat[15]=1.0;
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Sets the matrix values of this transform to the matrix values in the
* upper 4x4 corner of the GMatrix parameter. If the parameter matrix is
* smaller than 4x4, the remaining elements in the transform matrix are
* assigned to zero. The transform matrix type is classified
* internally by the Transform3D class.
* @param matrix the general matrix from which the Transform3D matrix is derived
*/
public final void set(GMatrix matrix) {
int i,j, k;
int numRows = matrix.getNumRow();
int numCol = matrix.getNumCol();
for(i=0 ; i<4 ; i++) {
k = i*4;
for(j=0 ; j<4 ; j++) {
if(i>=numRows || j>=numCol)
mat[k+j] = 0.0;
else
mat[k+j] = matrix.getElement(i,j);
}
}
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Sets the matrix, type, and state of this transform to the matrix,
* type, and state of transform t1.
* @param t1 the transform to be copied
*/
public final void set(Transform3D t1){
mat[0] = t1.mat[0];
mat[1] = t1.mat[1];
mat[2] = t1.mat[2];
mat[3] = t1.mat[3];
mat[4] = t1.mat[4];
mat[5] = t1.mat[5];
mat[6] = t1.mat[6];
mat[7] = t1.mat[7];
mat[8] = t1.mat[8];
mat[9] = t1.mat[9];
mat[10] = t1.mat[10];
mat[11] = t1.mat[11];
mat[12] = t1.mat[12];
mat[13] = t1.mat[13];
mat[14] = t1.mat[14];
mat[15] = t1.mat[15];
type = t1.type;
// don't copy rot[] and scales[]
dirtyBits = t1.dirtyBits | ROTATION_BIT | SCALE_BIT;
autoNormalize = t1.autoNormalize;
}
// This version gets a lock before doing the set. It is used internally
synchronized void setWithLock(Transform3D t1) {
this.set(t1);
}
// This version gets a lock before doing the get. It is used internally
synchronized void getWithLock(Transform3D t1) {
t1.set(this);
}
/**
* Sets the matrix values of this transform to the matrix values in the
* double precision array parameter. The matrix type is classified
* internally by the Transform3D class.
* @param matrix the double precision array of length 16 in row major format
*/
public final void set(double[] matrix) {
mat[0] = matrix[0];
mat[1] = matrix[1];
mat[2] = matrix[2];
mat[3] = matrix[3];
mat[4] = matrix[4];
mat[5] = matrix[5];
mat[6] = matrix[6];
mat[7] = matrix[7];
mat[8] = matrix[8];
mat[9] = matrix[9];
mat[10] = matrix[10];
mat[11] = matrix[11];
mat[12] = matrix[12];
mat[13] = matrix[13];
mat[14] = matrix[14];
mat[15] = matrix[15];
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Sets the matrix values of this transform to the matrix values in the
* single precision array parameter. The matrix type is classified
* internally by the Transform3D class.
* @param matrix the single precision array of length 16 in row major format
*/
public final void set(float[] matrix) {
mat[0] = matrix[0];
mat[1] = matrix[1];
mat[2] = matrix[2];
mat[3] = matrix[3];
mat[4] = matrix[4];
mat[5] = matrix[5];
mat[6] = matrix[6];
mat[7] = matrix[7];
mat[8] = matrix[8];
mat[9] = matrix[9];
mat[10] = matrix[10];
mat[11] = matrix[11];
mat[12] = matrix[12];
mat[13] = matrix[13];
mat[14] = matrix[14];
mat[15] = matrix[15];
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Sets the matrix values of this transform to the matrix values in the
* double precision Matrix4d argument. The transform type is classified
* internally by the Transform3D class.
* @param m1 the double precision 4x4 matrix
*/
public final void set(Matrix4d m1) {
mat[0] = m1.m00;
mat[1] = m1.m01;
mat[2] = m1.m02;
mat[3] = m1.m03;
mat[4] = m1.m10;
mat[5] = m1.m11;
mat[6] = m1.m12;
mat[7] = m1.m13;
mat[8] = m1.m20;
mat[9] = m1.m21;
mat[10] = m1.m22;
mat[11] = m1.m23;
mat[12] = m1.m30;
mat[13] = m1.m31;
mat[14] = m1.m32;
mat[15] = m1.m33;
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Sets the matrix values of this transform to the matrix values in the
* single precision Matrix4f argument. The transform type is classified
* internally by the Transform3D class.
* @param m1 the single precision 4x4 matrix
*/
public final void set(Matrix4f m1) {
mat[0] = m1.m00;
mat[1] = m1.m01;
mat[2] = m1.m02;
mat[3] = m1.m03;
mat[4] = m1.m10;
mat[5] = m1.m11;
mat[6] = m1.m12;
mat[7] = m1.m13;
mat[8] = m1.m20;
mat[9] = m1.m21;
mat[10] = m1.m22;
mat[11] = m1.m23;
mat[12] = m1.m30;
mat[13] = m1.m31;
mat[14] = m1.m32;
mat[15] = m1.m33;
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Sets the rotational component (upper 3x3) of this transform to the
* matrix values in the single precision Matrix3f argument; the other
* elements of this transform are initialized as if this were an identity
* matrix (i.e., affine matrix with no translational component).
* @param m1 the single precision 3x3 matrix
*/
public final void set(Matrix3f m1) {
mat[0] = m1.m00;
mat[1] = m1.m01;
mat[2] = m1.m02;
mat[3] = 0.0;
mat[4] = m1.m10;
mat[5] = m1.m11;
mat[6] = m1.m12;
mat[7] = 0.0;
mat[8] = m1.m20;
mat[9] = m1.m21;
mat[10] = m1.m22;
mat[11] = 0.0;
mat[12] = 0.0;
mat[13] = 0.0;
mat[14] = 0.0;
mat[15] = 1.0;
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Sets the rotational component (upper 3x3) of this transform to the
* matrix values in the double precision Matrix3d argument; the other
* elements of this transform are initialized as if this were an identity
* matrix (ie, affine matrix with no translational component).
* @param m1 the double precision 3x3 matrix
*/
public final void set(Matrix3d m1) {
mat[0] = m1.m00;
mat[1] = m1.m01;
mat[2] = m1.m02;
mat[3] = 0.0;
mat[4] = m1.m10;
mat[5] = m1.m11;
mat[6] = m1.m12;
mat[7] = 0.0;
mat[8] = m1.m20;
mat[9] = m1.m21;
mat[10] = m1.m22;
mat[11] = 0.0;
mat[12] = 0.0;
mat[13] = 0.0;
mat[14] = 0.0;
mat[15] = 1.0;
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Sets the rotational component (upper 3x3) of this transform to the
* rotation matrix converted from the Euler angles provided; the other
* non-rotational elements are set as if this were an identity matrix.
* The euler parameter is a Vector3d consisting of three rotation angles
* applied first about the X, then Y then Z axis.
* These rotations are applied using a static frame of reference. In
* other words, the orientation of the Y rotation axis is not affected
* by the X rotation and the orientation of the Z rotation axis is not
* affected by the X or Y rotation.
* @param euler the Vector3d consisting of three rotation angles about X,Y,Z
*
*/
public final void setEuler(Vector3d euler) {
double sina, sinb, sinc;
double cosa, cosb, cosc;
sina = Math.sin(euler.x);
sinb = Math.sin(euler.y);
sinc = Math.sin(euler.z);
cosa = Math.cos(euler.x);
cosb = Math.cos(euler.y);
cosc = Math.cos(euler.z);
mat[0] = cosb * cosc;
mat[1] = -(cosa * sinc) + (sina * sinb * cosc);
mat[2] = (sina * sinc) + (cosa * sinb *cosc);
mat[3] = 0.0;
mat[4] = cosb * sinc;
mat[5] = (cosa * cosc) + (sina * sinb * sinc);
mat[6] = -(sina * cosc) + (cosa * sinb *sinc);
mat[7] = 0.0;
mat[8] = -sinb;
mat[9] = sina * cosb;
mat[10] = cosa * cosb;
mat[11] = 0.0;
mat[12] = 0.0;
mat[13] = 0.0;
mat[14] = 0.0;
mat[15] = 1.0;
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(euler)) {
dirtyBits = ALL_DIRTY;
return;
}
type = AFFINE | CONGRUENT | RIGID | ORTHO;
dirtyBits = CLASSIFY_BIT | SCALE_BIT | ROTATION_BIT;
}
/**
* Places the values of this transform into the double precision array
* of length 16. The first four elements of the array will contain
* the top row of the transform matrix, etc.
* @param matrix the double precision array of length 16
*/
public final void get(double[] matrix) {
matrix[0] = mat[0];
matrix[1] = mat[1];
matrix[2] = mat[2];
matrix[3] = mat[3];
matrix[4] = mat[4];
matrix[5] = mat[5];
matrix[6] = mat[6];
matrix[7] = mat[7];
matrix[8] = mat[8];
matrix[9] = mat[9];
matrix[10] = mat[10];
matrix[11] = mat[11];
matrix[12] = mat[12];
matrix[13] = mat[13];
matrix[14] = mat[14];
matrix[15] = mat[15];
}
/**
* Places the values of this transform into the single precision array
* of length 16. The first four elements of the array will contain
* the top row of the transform matrix, etc.
* @param matrix the single precision array of length 16
*/
public final void get(float[] matrix) {
matrix[0] = (float) mat[0];
matrix[1] = (float) mat[1];
matrix[2] = (float) mat[2];
matrix[3] = (float) mat[3];
matrix[4] = (float) mat[4];
matrix[5] = (float) mat[5];
matrix[6] = (float) mat[6];
matrix[7] = (float) mat[7];
matrix[8] = (float) mat[8];
matrix[9] = (float) mat[9];
matrix[10] = (float) mat[10];
matrix[11] = (float) mat[11];
matrix[12] = (float) mat[12];
matrix[13] = (float) mat[13];
matrix[14] = (float) mat[14];
matrix[15] = (float) mat[15];
}
/**
* Places the normalized rotational component of this transform
* into the 3x3 matrix argument.
* @param m1 the matrix into which the rotational component is placed
*/
public final void get(Matrix3d m1) {
if ((dirtyBits & ROTATION_BIT) != 0) {
computeScaleRotation(false);
}
m1.m00 = rot[0];
m1.m01 = rot[1];
m1.m02 = rot[2];
m1.m10 = rot[3];
m1.m11 = rot[4];
m1.m12 = rot[5];
m1.m20 = rot[6];
m1.m21 = rot[7];
m1.m22 = rot[8];
}
/**
* Places the normalized rotational component of this transform
* into the 3x3 matrix argument.
* @param m1 the matrix into which the rotational component is placed
*/
public final void get(Matrix3f m1) {
if ((dirtyBits & ROTATION_BIT) != 0) {
computeScaleRotation(false);
}
m1.m00 = (float)rot[0];
m1.m01 = (float)rot[1];
m1.m02 = (float)rot[2];
m1.m10 = (float)rot[3];
m1.m11 = (float)rot[4];
m1.m12 = (float)rot[5];
m1.m20 = (float)rot[6];
m1.m21 = (float)rot[7];
m1.m22 = (float)rot[8];
}
/**
* Places the quaternion equivalent of the normalized rotational
* component of this transform into the quaternion parameter.
* @param q1 the quaternion into which the rotation component is placed
*/
public final void get(Quat4f q1) {
if ((dirtyBits & ROTATION_BIT) != 0) {
computeScaleRotation(false);
}
double ww = 0.25*(1.0 + rot[0] + rot[4] + rot[8]);
if (!((ww < 0 ? -ww : ww) < 1.0e-10)) {
q1.w = (float)Math.sqrt(ww);
ww = 0.25/q1.w;
q1.x = (float)((rot[7] - rot[5])*ww);
q1.y = (float)((rot[2] - rot[6])*ww);
q1.z = (float)((rot[3] - rot[1])*ww);
return;
}
q1.w = 0.0f;
ww = -0.5*(rot[4] + rot[8]);
if (!((ww < 0 ? -ww : ww) < 1.0e-10)) {
q1.x = (float)Math.sqrt(ww);
ww = 0.5/q1.x;
q1.y = (float)(rot[3]*ww);
q1.z = (float)(rot[6]*ww);
return;
}
q1.x = 0.0f;
ww = 0.5*(1.0 - rot[8]);
if (!((ww < 0 ? -ww : ww) < 1.0e-10)) {
q1.y = (float)Math.sqrt(ww);
q1.z = (float)(rot[7]/(2.0*q1.y));
return;
}
q1.y = 0.0f;
q1.z = 1.0f;
}
/**
* Places the quaternion equivalent of the normalized rotational
* component of this transform into the quaternion parameter.
* @param q1 the quaternion into which the rotation component is placed
*/
public final void get(Quat4d q1) {
if ((dirtyBits & ROTATION_BIT) != 0) {
computeScaleRotation(false);
}
double ww = 0.25*(1.0 + rot[0] + rot[4] + rot[8]);
if (!((ww < 0 ? -ww : ww) < 1.0e-10)) {
q1.w = Math.sqrt(ww);
ww = 0.25/q1.w;
q1.x = (rot[7] - rot[5])*ww;
q1.y = (rot[2] - rot[6])*ww;
q1.z = (rot[3] - rot[1])*ww;
return;
}
q1.w = 0.0;
ww = -0.5*(rot[4] + rot[8]);
if (!((ww < 0 ? -ww : ww) < 1.0e-10)) {
q1.x = Math.sqrt(ww);
ww = 0.5/q1.x;
q1.y = rot[3]*ww;
q1.z = rot[6]*ww;
return;
}
q1.x = 0.0;
ww = 0.5*(1.0 - rot[8]);
if (!((ww < 0 ? -ww : ww) < 1.0e-10)) {
q1.y = Math.sqrt(ww);
q1.z = rot[7]/(2.0*q1.y);
return;
}
q1.y = 0.0;
q1.z = 1.0;
}
/**
* Places the values of this transform into the double precision
* matrix argument.
* @param matrix the double precision matrix
*/
public final void get(Matrix4d matrix) {
matrix.m00 = mat[0];
matrix.m01 = mat[1];
matrix.m02 = mat[2];
matrix.m03 = mat[3];
matrix.m10 = mat[4];
matrix.m11 = mat[5];
matrix.m12 = mat[6];
matrix.m13 = mat[7];
matrix.m20 = mat[8];
matrix.m21 = mat[9];
matrix.m22 = mat[10];
matrix.m23 = mat[11];
matrix.m30 = mat[12];
matrix.m31 = mat[13];
matrix.m32 = mat[14];
matrix.m33 = mat[15];
}
/**
* Places the values of this transform into the single precision matrix
* argument.
* @param matrix the single precision matrix
*/
public final void get(Matrix4f matrix) {
matrix.m00 = (float) mat[0];
matrix.m01 = (float) mat[1];
matrix.m02 = (float) mat[2];
matrix.m03 = (float) mat[3];
matrix.m10 = (float) mat[4];
matrix.m11 = (float) mat[5];
matrix.m12 = (float) mat[6];
matrix.m13 = (float) mat[7];
matrix.m20 = (float) mat[8];
matrix.m21 = (float) mat[9];
matrix.m22 = (float) mat[10];
matrix.m23 = (float) mat[11];
matrix.m30 = (float) mat[12];
matrix.m31 = (float) mat[13];
matrix.m32 = (float) mat[14];
matrix.m33 = (float) mat[15];
}
/**
* Places the quaternion equivalent of the normalized rotational
* component of this transform into the quaternion parameter;
* places the translational component into the Vector parameter.
* @param q1 the quaternion representing the rotation
* @param t1 the translation component
* @return the scale component of this transform
*/
public final double get(Quat4d q1, Vector3d t1) {
if ((dirtyBits & ROTATION_BIT) != 0) {
computeScaleRotation(false);
} else if ((dirtyBits & SCALE_BIT) != 0) {
computeScales(false);
}
t1.x = mat[3];
t1.y = mat[7];
t1.z = mat[11];
double maxScale = max3(scales);
double ww = 0.25*(1.0 + rot[0] + rot[4] + rot[8]);
if (!((ww < 0 ? -ww : ww) < EPSILON)) {
q1.w = Math.sqrt(ww);
ww = 0.25/q1.w;
q1.x = (rot[7] - rot[5])*ww;
q1.y = (rot[2] - rot[6])*ww;
q1.z = (rot[3] - rot[1])*ww;
return maxScale;
}
q1.w = 0.0;
ww = -0.5*(rot[4] + rot[8]);
if (!((ww < 0 ? -ww : ww) < EPSILON)) {
q1.x = Math.sqrt(ww);
ww = 0.5/q1.x;
q1.y = rot[3]*ww;
q1.z = rot[6]*ww;
return maxScale;
}
q1.x = 0.0;
ww = 0.5*(1.0 - rot[8]);
if (!((ww < 0 ? -ww : ww) < EPSILON)) {
q1.y = Math.sqrt(ww);
q1.z = rot[7]/(2.0*q1.y);
return maxScale;
}
q1.y = 0.0;
q1.z = 1.0;
return maxScale;
}
/**
* Places the quaternion equivalent of the normalized rotational
* component of this transform into the quaternion parameter;
* places the translational component into the Vector parameter.
* @param q1 the quaternion representing the rotation
* @param t1 the translation component
* @return the scale component of this transform
*/
public final float get(Quat4f q1, Vector3f t1) {
if ((dirtyBits & ROTATION_BIT) != 0) {
computeScaleRotation(false);
} else if ((dirtyBits & SCALE_BIT) != 0) {
computeScales(false);
}
double maxScale = max3(scales);
t1.x = (float)mat[3];
t1.y = (float)mat[7];
t1.z = (float)mat[11];
double ww = 0.25*(1.0 + rot[0] + rot[4] + rot[8]);
if (!((ww < 0 ? -ww : ww) < EPSILON)) {
q1.w = (float)Math.sqrt(ww);
ww = 0.25/q1.w;
q1.x = (float)((rot[7] - rot[5])*ww);
q1.y = (float)((rot[2] - rot[6])*ww);
q1.z = (float)((rot[3] - rot[1])*ww);
return (float) maxScale;
}
q1.w = 0.0f;
ww = -0.5*(rot[4] + rot[8]);
if (!((ww < 0 ? -ww : ww) < EPSILON)) {
q1.x = (float)Math.sqrt(ww);
ww = 0.5/q1.x;
q1.y = (float)(rot[3]*ww);
q1.z = (float)(rot[6]*ww);
return (float) maxScale;
}
q1.x = 0.0f;
ww = 0.5*(1.0 - rot[8]);
if (!((ww < 0? -ww : ww) < EPSILON)) {
q1.y = (float)Math.sqrt(ww);
q1.z = (float)(rot[7]/(2.0*q1.y));
return (float) maxScale;
}
q1.y = 0.0f;
q1.z = 1.0f;
return (float) maxScale;
}
/**
* Places the quaternion equivalent of the normalized rotational
* component of this transform into the quaternion parameter;
* places the translational component into the Vector parameter.
* @param q1 the quaternion representing the rotation
* @param t1 the translation component
* @return the scale component of this transform
*/
public final double get(Quat4f q1, Vector3d t1) {
if ((dirtyBits & ROTATION_BIT) != 0) {
computeScaleRotation(false);
} else if ((dirtyBits & SCALE_BIT) != 0) {
computeScales(false);
}
double maxScale = max3(scales);
t1.x = mat[3];
t1.y = mat[7];
t1.z = mat[11];
double ww = 0.25*(1.0 + rot[0] + rot[4] + rot[8]);
if (!(( ww < 0 ? -ww : ww) < EPSILON)) {
q1.w = (float)Math.sqrt(ww);
ww = 0.25/q1.w;
q1.x = (float)((rot[7] - rot[5])*ww);
q1.y = (float)((rot[2] - rot[6])*ww);
q1.z = (float)((rot[3] - rot[1])*ww);
return maxScale;
}
q1.w = 0.0f;
ww = -0.5*(rot[4] + rot[8]);
if (!(( ww < 0 ? -ww : ww) < EPSILON)) {
q1.x = (float)Math.sqrt(ww);
ww = 0.5/q1.x;
q1.y = (float)(rot[3]*ww);
q1.z = (float)(rot[6]*ww);
return maxScale;
}
q1.x = 0.0f;
ww = 0.5*(1.0 - rot[8]);
if (!(( ww < 0 ? -ww : ww) < EPSILON)) {
q1.y = (float)Math.sqrt(ww);
q1.z = (float)(rot[7]/(2.0*q1.y));
return maxScale;
}
q1.y = 0.0f;
q1.z = 1.0f;
return maxScale;
}
/**
* Places the normalized rotational component of this transform
* into the matrix parameter; place the translational component
* into the vector parameter.
* @param m1 the normalized matrix representing the rotation
* @param t1 the translation component
* @return the scale component of this transform
*/
public final double get(Matrix3d m1, Vector3d t1) {
if ((dirtyBits & ROTATION_BIT) != 0) {
computeScaleRotation(false);
} else if ((dirtyBits & SCALE_BIT) != 0) {
computeScales(false);
}
t1.x = mat[3];
t1.y = mat[7];
t1.z = mat[11];
m1.m00 = rot[0];
m1.m01 = rot[1];
m1.m02 = rot[2];
m1.m10 = rot[3];
m1.m11 = rot[4];
m1.m12 = rot[5];
m1.m20 = rot[6];
m1.m21 = rot[7];
m1.m22 = rot[8];
return max3(scales);
}
/**
* Places the normalized rotational component of this transform
* into the matrix parameter; place the translational component
* into the vector parameter.
* @param m1 the normalized matrix representing the rotation
* @param t1 the translation component
* @return the scale component of this transform
*/
public final float get(Matrix3f m1, Vector3f t1) {
if ((dirtyBits & ROTATION_BIT) != 0) {
computeScaleRotation(false);
} else if ((dirtyBits & SCALE_BIT) != 0) {
computeScales(false);
}
t1.x = (float)mat[3];
t1.y = (float)mat[7];
t1.z = (float)mat[11];
m1.m00 = (float)rot[0];
m1.m01 = (float)rot[1];
m1.m02 = (float)rot[2];
m1.m10 = (float)rot[3];
m1.m11 = (float)rot[4];
m1.m12 = (float)rot[5];
m1.m20 = (float)rot[6];
m1.m21 = (float)rot[7];
m1.m22 = (float)rot[8];
return (float) max3(scales);
}
/**
* Places the normalized rotational component of this transform
* into the matrix parameter; place the translational component
* into the vector parameter.
* @param m1 the normalized matrix representing the rotation
* @param t1 the translation component
* @return the scale component of this transform
*/
public final double get(Matrix3f m1, Vector3d t1) {
if ((dirtyBits & ROTATION_BIT) != 0) {
computeScaleRotation(false);
} else if ((dirtyBits & SCALE_BIT) != 0) {
computeScales(false);
}
t1.x = mat[3];
t1.y = mat[7];
t1.z = mat[11];
m1.m00 = (float)rot[0];
m1.m01 = (float)rot[1];
m1.m02 = (float)rot[2];
m1.m10 = (float)rot[3];
m1.m11 = (float)rot[4];
m1.m12 = (float)rot[5];
m1.m20 = (float)rot[6];
m1.m21 = (float)rot[7];
m1.m22 = (float)rot[8];
return max3(scales);
}
/**
* Returns the uniform scale factor of this matrix.
* If the matrix has non-uniform scale factors, the largest of the
* x, y, and z scale factors will be returned.
* @return the scale factor of this matrix
*/
public final double getScale() {
if ((dirtyBits & SCALE_BIT) != 0) {
computeScales(false);
}
return max3(scales);
}
/**
* Gets the possibly non-uniform scale components of the current
* transform and places them into the scale vector.
* @param scale the vector into which the x,y,z scale values will be placed
*/
public final void getScale(Vector3d scale) {
if ((dirtyBits & SCALE_BIT) != 0) {
computeScales(false);
}
scale.x = scales[0];
scale.y = scales[1];
scale.z = scales[2];
}
/**
* Retrieves the translational components of this transform.
* @param trans the vector that will receive the translational component
*/
public final void get(Vector3f trans) {
trans.x = (float)mat[3];
trans.y = (float)mat[7];
trans.z = (float)mat[11];
}
/**
* Retrieves the translational components of this transform.
* @param trans the vector that will receive the translational component
*/
public final void get(Vector3d trans) {
trans.x = mat[3];
trans.y = mat[7];
trans.z = mat[11];
}
/**
* Sets the value of this transform to the inverse of the passed
* Transform3D parameter. This method uses the transform type
* to determine the optimal algorithm for inverting transform t1.
* @param t1 the transform to be inverted
* @exception SingularMatrixException thrown if transform t1 is
* not invertible
*/
public final void invert(Transform3D t1) {
if (t1 == this) {
invert();
} else if (t1.isAffine()) {
// We can't use invertOrtho() because of numerical
// instability unless we set tolerance of ortho test to 0
invertAffine(t1);
} else {
invertGeneral(t1);
}
}
/**
* Inverts this transform in place. This method uses the transform
* type to determine the optimal algorithm for inverting this transform.
* @exception SingularMatrixException thrown if this transform is
* not invertible
*/
public final void invert() {
if (isAffine()) {
invertAffine();
} else {
invertGeneral(this);
}
}
/**
* Congruent invert routine.
*
* if uniform scale s
*
* [R | t] => [R^T/s*s | -R^T * t/s*s]
*
*/
/*
final void invertOrtho() {
double tmp, s1, s2, s3;
// do not force classifyRigid()
if (((dirtyBits & CONGRUENT_BIT) == 0) &&
((type & CONGRUENT) != 0)) {
s1 = mat[0]*mat[0] + mat[4]*mat[4] + mat[8]*mat[8];
if (s1 == 0) {
throw new SingularMatrixException(J3dI18N.getString("Transform3D1"));
}
s1 = s2 = s3 = 1/s1;
dirtyBits |= ROTSCALESVD_DIRTY;
} else {
// non-uniform scale matrix
s1 = mat[0]*mat[0] + mat[4]*mat[4] + mat[8]*mat[8];
s2 = mat[1]*mat[1] + mat[5]*mat[5] + mat[9]*mat[9];
s3 = mat[2]*mat[2] + mat[6]*mat[6] + mat[10]*mat[10];
if ((s1 == 0) || (s2 == 0) || (s3 == 0)) {
throw new SingularMatrixException(J3dI18N.getString("Transform3D1"));
}
s1 = 1/s1;
s2 = 1/s2;
s3 = 1/s3;
dirtyBits |= ROTSCALESVD_DIRTY | ORTHO_BIT | CONGRUENT_BIT
| RIGID_BIT | CLASSIFY_BIT;
}
// multiple by 1/s will cause loss in numerical value
tmp = mat[1];
mat[1] = mat[4]*s1;
mat[4] = tmp*s2;
tmp = mat[2];
mat[2] = mat[8]*s1;
mat[8] = tmp*s3;
tmp = mat[6];
mat[6] = mat[9]*s2;
mat[9] = tmp*s3;
mat[0] *= s1;
mat[5] *= s2;
mat[10] *= s3;
tmp = mat[3];
s1 = mat[7];
mat[3] = -(tmp * mat[0] + s1 * mat[1] + mat[11] * mat[2]);
mat[7] = -(tmp * mat[4] + s1 * mat[5] + mat[11] * mat[6]);
mat[11]= -(tmp * mat[8] + s1 * mat[9] + mat[11] * mat[10]);
mat[12] = mat[13] = mat[14] = 0.0;
mat[15] = 1.0;
}
*/
/**
* Orthogonal matrix invert routine.
* Inverts t1 and places the result in "this".
*/
/*
final void invertOrtho(Transform3D t1) {
double s1, s2, s3;
// do not force classifyRigid()
if (((t1.dirtyBits & CONGRUENT_BIT) == 0) &&
((t1.type & CONGRUENT) != 0)) {
s1 = t1.mat[0]*t1.mat[0] + t1.mat[4]*t1.mat[4] +
t1.mat[8]*t1.mat[8];
if (s1 == 0) {
throw new SingularMatrixException(J3dI18N.getString("Transform3D1"));
}
s1 = s2 = s3 = 1/s1;
dirtyBits = t1.dirtyBits | ROTSCALESVD_DIRTY;
} else {
// non-uniform scale matrix
s1 = t1.mat[0]*t1.mat[0] + t1.mat[4]*t1.mat[4] +
t1.mat[8]*t1.mat[8];
s2 = t1.mat[1]*t1.mat[1] + t1.mat[5]*t1.mat[5] +
t1.mat[9]*t1.mat[9];
s3 = t1.mat[2]*t1.mat[2] + t1.mat[6]*t1.mat[6] +
t1.mat[10]*t1.mat[10];
if ((s1 == 0) || (s2 == 0) || (s3 == 0)) {
throw new SingularMatrixException(J3dI18N.getString("Transform3D1"));
}
s1 = 1/s1;
s2 = 1/s2;
s3 = 1/s3;
dirtyBits = t1.dirtyBits | ROTSCALESVD_DIRTY | ORTHO_BIT |
CONGRUENT_BIT | RIGID_BIT;
}
mat[0] = t1.mat[0]*s1;
mat[1] = t1.mat[4]*s1;
mat[2] = t1.mat[8]*s1;
mat[4] = t1.mat[1]*s2;
mat[5] = t1.mat[5]*s2;
mat[6] = t1.mat[9]*s2;
mat[8] = t1.mat[2]*s3;
mat[9] = t1.mat[6]*s3;
mat[10] = t1.mat[10]*s3;
mat[3] = -(t1.mat[3] * mat[0] + t1.mat[7] * mat[1] +
t1.mat[11] * mat[2]);
mat[7] = -(t1.mat[3] * mat[4] + t1.mat[7] * mat[5] +
t1.mat[11] * mat[6]);
mat[11]= -(t1.mat[3] * mat[8] + t1.mat[7] * mat[9] +
t1.mat[11] * mat[10]);
mat[12] = mat[13] = mat[14] = 0.0;
mat[15] = 1.0;
type = t1.type;
}
*/
/**
* Affine invert routine. Inverts t1 and places the result in "this".
*/
final void invertAffine(Transform3D t1) {
double determinant = t1.affineDeterminant();
if (determinant == 0.0)
throw new SingularMatrixException(J3dI18N.getString("Transform3D1"));
double s = (t1.mat[0]*t1.mat[0] + t1.mat[1]*t1.mat[1] +
t1.mat[2]*t1.mat[2] + t1.mat[3]*t1.mat[3])*
(t1.mat[4]*t1.mat[4] + t1.mat[5]*t1.mat[5] +
t1.mat[6]*t1.mat[6] + t1.mat[7]*t1.mat[7])*
(t1.mat[8]*t1.mat[8] + t1.mat[9]*t1.mat[9] +
t1.mat[10]*t1.mat[10] + t1.mat[11]*t1.mat[11]);
if ((determinant*determinant) < (EPS * s)) {
// using invertGeneral is numerically more stable for
//this case see bug 4227733
invertGeneral(t1);
return;
}
s = 1.0 / determinant;
mat[0] = (t1.mat[5]*t1.mat[10] - t1.mat[9]*t1.mat[6]) * s;
mat[1] = -(t1.mat[1]*t1.mat[10] - t1.mat[9]*t1.mat[2]) * s;
mat[2] = (t1.mat[1]*t1.mat[6] - t1.mat[5]*t1.mat[2]) * s;
mat[4] = -(t1.mat[4]*t1.mat[10] - t1.mat[8]*t1.mat[6]) * s;
mat[5] = (t1.mat[0]*t1.mat[10] - t1.mat[8]*t1.mat[2]) * s;
mat[6] = -(t1.mat[0]*t1.mat[6] - t1.mat[4]*t1.mat[2]) * s;
mat[8] = (t1.mat[4]*t1.mat[9] - t1.mat[8]*t1.mat[5]) * s;
mat[9] = -(t1.mat[0]*t1.mat[9] - t1.mat[8]*t1.mat[1]) * s;
mat[10]= (t1.mat[0]*t1.mat[5] - t1.mat[4]*t1.mat[1]) * s;
mat[3] = -(t1.mat[3] * mat[0] + t1.mat[7] * mat[1] +
t1.mat[11] * mat[2]);
mat[7] = -(t1.mat[3] * mat[4] + t1.mat[7] * mat[5] +
t1.mat[11] * mat[6]);
mat[11]= -(t1.mat[3] * mat[8] + t1.mat[7] * mat[9] +
t1.mat[11] * mat[10]);
mat[12] = mat[13] = mat[14] = 0.0;
mat[15] = 1.0;
dirtyBits = t1.dirtyBits | ROTSCALESVD_DIRTY | CLASSIFY_BIT | ORTHO_BIT;
type = t1.type;
}
/**
* Affine invert routine. Inverts "this" matrix in place.
*/
final void invertAffine() {
double determinant = affineDeterminant();
if (determinant == 0.0)
throw new SingularMatrixException(J3dI18N.getString("Transform3D1"));
double s = (mat[0]*mat[0] + mat[1]*mat[1] +
mat[2]*mat[2] + mat[3]*mat[3])*
(mat[4]*mat[4] + mat[5]*mat[5] +
mat[6]*mat[6] + mat[7]*mat[7])*
(mat[8]*mat[8] + mat[9]*mat[9] +
mat[10]*mat[10] + mat[11]*mat[11]);
if ((determinant*determinant) < (EPS * s)) {
invertGeneral(this);
return;
}
s = 1.0 / determinant;
double tmp0 = (mat[5]*mat[10] - mat[9]*mat[6]) * s;
double tmp1 = -(mat[1]*mat[10] - mat[9]*mat[2]) * s;
double tmp2 = (mat[1]*mat[6] - mat[5]*mat[2]) * s;
double tmp4 = -(mat[4]*mat[10] - mat[8]*mat[6]) * s;
double tmp5 = (mat[0]*mat[10] - mat[8]*mat[2]) * s;
double tmp6 = -(mat[0]*mat[6] - mat[4]*mat[2]) * s;
double tmp8 = (mat[4]*mat[9] - mat[8]*mat[5]) * s;
double tmp9 = -(mat[0]*mat[9] - mat[8]*mat[1]) * s;
double tmp10= (mat[0]*mat[5] - mat[4]*mat[1]) * s;
double tmp3 = -(mat[3] * tmp0 + mat[7] * tmp1 + mat[11] * tmp2);
double tmp7 = -(mat[3] * tmp4 + mat[7] * tmp5 + mat[11] * tmp6);
mat[11]= -(mat[3] * tmp8 + mat[7] * tmp9 + mat[11] * tmp10);
mat[0]=tmp0; mat[1]=tmp1; mat[2]=tmp2; mat[3]=tmp3;
mat[4]=tmp4; mat[5]=tmp5; mat[6]=tmp6; mat[7]=tmp7;
mat[8]=tmp8; mat[9]=tmp9; mat[10]=tmp10;
mat[12] = mat[13] = mat[14] = 0.0;
mat[15] = 1.0;
dirtyBits |= ROTSCALESVD_DIRTY | CLASSIFY_BIT | ORTHO_BIT;
}
/**
* General invert routine. Inverts t1 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.
*/
final void invertGeneral(Transform3D t1) {
double tmp[] = new double[16];
int row_perm[] = new int[4];
int i, r, c;
// Use LU decomposition and backsubstitution code specifically
// for floating-point 4x4 matrices.
// Copy source matrix to tmp
System.arraycopy(t1.mat, 0, tmp, 0, tmp.length);
// Calculate LU decomposition: Is the matrix singular?
if (!luDecomposition(tmp, row_perm)) {
// Matrix has no inverse
throw new SingularMatrixException(J3dI18N.getString("Transform3D1"));
}
// Perform back substitution on the identity matrix
// luDecomposition will set rot[] & scales[] for use
// in luBacksubstituation
mat[0] = 1.0; mat[1] = 0.0; mat[2] = 0.0; mat[3] = 0.0;
mat[4] = 0.0; mat[5] = 1.0; mat[6] = 0.0; mat[7] = 0.0;
mat[8] = 0.0; mat[9] = 0.0; mat[10] = 1.0; mat[11] = 0.0;
mat[12] = 0.0; mat[13] = 0.0; mat[14] = 0.0; mat[15] = 1.0;
luBacksubstitution(tmp, row_perm, this.mat);
type = 0;
dirtyBits = ALL_DIRTY;
}
/**
* Given a 4x4 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[4]" 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 4x4 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) {
// Can't re-use this temporary since the method is static.
double row_scale[] = new double[4];
// 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 = 4;
while (i-- != 0) {
big = 0.0;
// For each column, find the largest element in the row
j = 4;
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 < 4; 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 + (4*i) + j;
sum = matrix0[target];
k = i;
p1 = mtx + (4*i);
p2 = mtx + j;
while (k-- != 0) {
sum -= matrix0[p1] * matrix0[p2];
p1++;
p2 += 4;
}
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 < 4; i++) {
target = mtx + (4*i) + j;
sum = matrix0[target];
k = j;
p1 = mtx + (4*i);
p2 = mtx + j;
while (k-- != 0) {
sum -= matrix0[p1] * matrix0[p2];
p1++;
p2 += 4;
}
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) {
return false;
}
// Is a row exchange necessary?
if (j != imax) {
// Yes: exchange rows
k = 4;
p1 = mtx + (4*imax);
p2 = mtx + (4*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 + (4*j) + j)] == 0.0) {
return false;
}
// Divide elements of lower diagonal matrix L by pivot
if (j != (4-1)) {
temp = 1.0 / (matrix0[(mtx + (4*j) + j)]);
target = mtx + (4*(j+1)) + j;
i = 3 - j;
while (i-- != 0) {
matrix0[target] *= temp;
target += 4;
}
}
}
}
return true;
}
/**
* Solves a set of linear equations. The input parameters "matrix1",
* and "row_perm" come from luDecompostionD4x4 and do not change
* here. The parameter "matrix2" is a set of column vectors assembled
* into a 4x4 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 < 4; k++) {
// cv = &(matrix2[0][k]);
cv = k;
ii = -1;
// Forward substitution
for (i = 0; i < 4; i++) {
double sum;
ip = row_perm[rp+i];
sum = matrix2[cv+4*ip];
matrix2[cv+4*ip] = matrix2[cv+4*i];
if (ii >= 0) {
// rv = &(matrix1[i][0]);
rv = i*4;
for (j = ii; j <= i-1; j++) {
sum -= matrix1[rv+j] * matrix2[cv+4*j];
}
}
else if (sum != 0.0) {
ii = i;
}
matrix2[cv+4*i] = sum;
}
// Backsubstitution
// rv = &(matrix1[3][0]);
rv = 3*4;
matrix2[cv+4*3] /= matrix1[rv+3];
rv -= 4;
matrix2[cv+4*2] = (matrix2[cv+4*2] -
matrix1[rv+3] * matrix2[cv+4*3]) / matrix1[rv+2];
rv -= 4;
matrix2[cv+4*1] = (matrix2[cv+4*1] -
matrix1[rv+2] * matrix2[cv+4*2] -
matrix1[rv+3] * matrix2[cv+4*3]) / matrix1[rv+1];
rv -= 4;
matrix2[cv+4*0] = (matrix2[cv+4*0] -
matrix1[rv+1] * matrix2[cv+4*1] -
matrix1[rv+2] * matrix2[cv+4*2] -
matrix1[rv+3] * matrix2[cv+4*3]) / matrix1[rv+0];
}
}
// given that this matrix is affine
final double affineDeterminant() {
return mat[0]*(mat[5]*mat[10] - mat[6]*mat[9]) -
mat[1]*(mat[4]*mat[10] - mat[6]*mat[8]) +
mat[2]*(mat[4]*mat[ 9] - mat[5]*mat[8]);
}
/**
* Calculates and returns the determinant of this transform.
* @return the double precision determinant
*/
public final double determinant() {
if (isAffine()) {
return mat[0]*(mat[5]*mat[10] - mat[6]*mat[9]) -
mat[1]*(mat[4]*mat[10] - mat[6]*mat[8]) +
mat[2]*(mat[4]*mat[ 9] - mat[5]*mat[8]);
}
// cofactor exapainsion along first row
return mat[0]*(mat[5]*(mat[10]*mat[15] - mat[11]*mat[14]) -
mat[6]*(mat[ 9]*mat[15] - mat[11]*mat[13]) +
mat[7]*(mat[ 9]*mat[14] - mat[10]*mat[13])) -
mat[1]*(mat[4]*(mat[10]*mat[15] - mat[11]*mat[14]) -
mat[6]*(mat[ 8]*mat[15] - mat[11]*mat[12]) +
mat[7]*(mat[ 8]*mat[14] - mat[10]*mat[12])) +
mat[2]*(mat[4]*(mat[ 9]*mat[15] - mat[11]*mat[13]) -
mat[5]*(mat[ 8]*mat[15] - mat[11]*mat[12]) +
mat[7]*(mat[ 8]*mat[13] - mat[ 9]*mat[12])) -
mat[3]*(mat[4]*(mat[ 9]*mat[14] - mat[10]*mat[13]) -
mat[5]*(mat[ 8]*mat[14] - mat[10]*mat[12]) +
mat[6]*(mat[ 8]*mat[13] - mat[ 9]*mat[12]));
}
/**
* Sets the value of this transform to a uniform scale; all of
* the matrix values are modified.
* @param scale the scale factor for the transform
*/
public final void set(double scale) {
setScaleTranslation(0, 0, 0, scale);
}
/**
* Sets the value of this transform to a scale and translation
* matrix; the scale is not applied to the translation and all
* of the matrix values are modified.
* @param scale the scale factor for the transform
* @param v1 the translation amount
*/
public final void set(double scale, Vector3d v1) {
setScaleTranslation(v1.x, v1.y, v1.z, scale);
}
/**
* Sets the value of this transform to a scale and translation
* matrix; the scale is not applied to the translation and all
* of the matrix values are modified.
* @param scale the scale factor for the transform
* @param v1 the translation amount
*/
public final void set(float scale, Vector3f v1) {
setScaleTranslation(v1.x, v1.y, v1.z, scale);
}
/**
* Sets the value of this transform to a scale and translation matrix;
* the translation is scaled by the scale factor and all of the
* matrix values are modified.
* @param v1 the translation amount
* @param scale the scale factor for the transform AND the translation
*/
public final void set(Vector3d v1, double scale) {
setScaleTranslation(v1.x*scale, v1.y*scale, v1.z*scale, scale);
}
/**
* Sets the value of this transform to a scale and translation matrix;
* the translation is scaled by the scale factor and all of the
* matrix values are modified.
* @param v1 the translation amount
* @param scale the scale factor for the transform AND the translation
*/
public final void set(Vector3f v1, float scale) {
setScaleTranslation(v1.x*scale, v1.y*scale, v1.z*scale, scale);
}
private final void setScaleTranslation(double x, double y,
double z, double scale) {
mat[0] = scale;
mat[1] = 0.0;
mat[2] = 0.0;
mat[3] = x;
mat[4] = 0.0;
mat[5] = scale;
mat[6] = 0.0;
mat[7] = y;
mat[8] = 0.0;
mat[9] = 0.0;
mat[10] = scale;
mat[11] = z;
mat[12] = 0.0;
mat[13] = 0.0;
mat[14] = 0.0;
mat[15] = 1.0;
if(scales == null)
scales = new double[3];
scales[0] = scales[1] = scales[2] = scale;
// Issue 253: set all dirty bits if input is infinity or NaN
if (isInfOrNaN(x) || isInfOrNaN(y) || isInfOrNaN(z) || isInfOrNaN(scale)) {
dirtyBits = ALL_DIRTY;
return;
}
type = AFFINE | CONGRUENT | ORTHO;
dirtyBits = CLASSIFY_BIT | ROTATION_BIT | RIGID_BIT;
}
/**
* Multiplies each element of this transform by a scalar.
* @param scalar the scalar multiplier
*/
public final void mul(double scalar) {
for (int i=0 ; i<16 ; i++) {
mat[i] *= scalar;
}
dirtyBits = ALL_DIRTY;
}
/**
* Multiplies each element of transform t1 by a scalar and places
* the result into this. Transform t1 is not modified.
* @param scalar the scalar multiplier
* @param t1 the original transform
*/
public final void mul(double scalar, Transform3D t1) {
for (int i=0 ; i<16 ; i++) {
mat[i] = t1.mat[i] * scalar;
}
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Sets the value of this transform to the result of multiplying itself
* with transform t1 (this = this * t1).
* @param t1 the other transform
*/
public final void mul(Transform3D t1) {
double tmp0, tmp1, tmp2, tmp3;
double tmp4, tmp5, tmp6, tmp7;
double tmp8, tmp9, tmp10, tmp11;
boolean aff = false;
if (t1.isAffine()) {
tmp0 = mat[0]*t1.mat[0] + mat[1]*t1.mat[4] + mat[2]*t1.mat[8];
tmp1 = mat[0]*t1.mat[1] + mat[1]*t1.mat[5] + mat[2]*t1.mat[9];
tmp2 = mat[0]*t1.mat[2] + mat[1]*t1.mat[6] + mat[2]*t1.mat[10];
tmp3 = mat[0]*t1.mat[3] + mat[1]*t1.mat[7] + mat[2]*t1.mat[11] + mat[3];
tmp4 = mat[4]*t1.mat[0] + mat[5]*t1.mat[4] + mat[6]*t1.mat[8];
tmp5 = mat[4]*t1.mat[1] + mat[5]*t1.mat[5] + mat[6]*t1.mat[9];
tmp6 = mat[4]*t1.mat[2] + mat[5]*t1.mat[6] + mat[6]*t1.mat[10];
tmp7 = mat[4]*t1.mat[3] + mat[5]*t1.mat[7] + mat[6]*t1.mat[11] + mat[7];
tmp8 = mat[8]*t1.mat[0] + mat[9]*t1.mat[4] + mat[10]*t1.mat[8];
tmp9 = mat[8]*t1.mat[1] + mat[9]*t1.mat[5] + mat[10]*t1.mat[9];
tmp10 = mat[8]*t1.mat[2] + mat[9]*t1.mat[6] + mat[10]*t1.mat[10];
tmp11 = mat[8]*t1.mat[3] + mat[9]*t1.mat[7] + mat[10]*t1.mat[11] + mat[11];
if (isAffine()) {
mat[12] = mat[13] = mat[14] = 0;
mat[15] = 1;
aff = true;
} else {
double tmp12 = mat[12]*t1.mat[0] + mat[13]*t1.mat[4] +
mat[14]*t1.mat[8];
double tmp13 = mat[12]*t1.mat[1] + mat[13]*t1.mat[5] +
mat[14]*t1.mat[9];
double tmp14 = mat[12]*t1.mat[2] + mat[13]*t1.mat[6] +
mat[14]*t1.mat[10];
double tmp15 = mat[12]*t1.mat[3] + mat[13]*t1.mat[7] +
mat[14]*t1.mat[11] + mat[15];
mat[12] = tmp12;
mat[13] = tmp13;
mat[14] = tmp14;
mat[15] = tmp15;
}
} else {
tmp0 = mat[0]*t1.mat[0] + mat[1]*t1.mat[4] + mat[2]*t1.mat[8] +
mat[3]*t1.mat[12];
tmp1 = mat[0]*t1.mat[1] + mat[1]*t1.mat[5] + mat[2]*t1.mat[9] +
mat[3]*t1.mat[13];
tmp2 = mat[0]*t1.mat[2] + mat[1]*t1.mat[6] + mat[2]*t1.mat[10] +
mat[3]*t1.mat[14];
tmp3 = mat[0]*t1.mat[3] + mat[1]*t1.mat[7] + mat[2]*t1.mat[11] +
mat[3]*t1.mat[15];
tmp4 = mat[4]*t1.mat[0] + mat[5]*t1.mat[4] + mat[6]*t1.mat[8] +
mat[7]*t1.mat[12];
tmp5 = mat[4]*t1.mat[1] + mat[5]*t1.mat[5] + mat[6]*t1.mat[9] +
mat[7]*t1.mat[13];
tmp6 = mat[4]*t1.mat[2] + mat[5]*t1.mat[6] + mat[6]*t1.mat[10] +
mat[7]*t1.mat[14];
tmp7 = mat[4]*t1.mat[3] + mat[5]*t1.mat[7] + mat[6]*t1.mat[11] +
mat[7]*t1.mat[15];
tmp8 = mat[8]*t1.mat[0] + mat[9]*t1.mat[4] + mat[10]*t1.mat[8] +
mat[11]*t1.mat[12];
tmp9 = mat[8]*t1.mat[1] + mat[9]*t1.mat[5] + mat[10]*t1.mat[9] +
mat[11]*t1.mat[13];
tmp10 = mat[8]*t1.mat[2] + mat[9]*t1.mat[6] +
mat[10]*t1.mat[10]+ mat[11]*t1.mat[14];
tmp11 = mat[8]*t1.mat[3] + mat[9]*t1.mat[7] +
mat[10]*t1.mat[11] + mat[11]*t1.mat[15];
if (isAffine()) {
mat[12] = t1.mat[12];
mat[13] = t1.mat[13];
mat[14] = t1.mat[14];
mat[15] = t1.mat[15];
} else {
double tmp12 = mat[12]*t1.mat[0] + mat[13]*t1.mat[4] +
mat[14]*t1.mat[8] + mat[15]*t1.mat[12];
double tmp13 = mat[12]*t1.mat[1] + mat[13]*t1.mat[5] +
mat[14]*t1.mat[9] + mat[15]*t1.mat[13];
double tmp14 = mat[12]*t1.mat[2] + mat[13]*t1.mat[6] +
mat[14]*t1.mat[10] + mat[15]*t1.mat[14];
double tmp15 = mat[12]*t1.mat[3] + mat[13]*t1.mat[7] +
mat[14]*t1.mat[11] + mat[15]*t1.mat[15];
mat[12] = tmp12;
mat[13] = tmp13;
mat[14] = tmp14;
mat[15] = tmp15;
}
}
mat[0] = tmp0;
mat[1] = tmp1;
mat[2] = tmp2;
mat[3] = tmp3;
mat[4] = tmp4;
mat[5] = tmp5;
mat[6] = tmp6;
mat[7] = tmp7;
mat[8] = tmp8;
mat[9] = tmp9;
mat[10] = tmp10;
mat[11] = tmp11;
if (((dirtyBits & CONGRUENT_BIT) == 0) &&
((type & CONGRUENT) != 0) &&
((t1.dirtyBits & CONGRUENT_BIT) == 0) &&
((t1.type & CONGRUENT) != 0)) {
type &= t1.type;
dirtyBits |= t1.dirtyBits | CLASSIFY_BIT |
ROTSCALESVD_DIRTY | RIGID_BIT;
} else {
if (aff) {
dirtyBits = ORTHO_BIT | CONGRUENT_BIT | RIGID_BIT |
CLASSIFY_BIT | ROTSCALESVD_DIRTY;
} else {
dirtyBits = ALL_DIRTY;
}
}
if (autoNormalize) {
normalize();
}
}
/**
* Sets the value of this transform to the result of multiplying transform
* t1 by transform t2 (this = t1*t2).
* @param t1 the left transform
* @param t2 the right transform
*/
public final void mul(Transform3D t1, Transform3D t2) {
boolean aff = false;
if ((this != t1) && (this != t2)) {
if (t2.isAffine()) {
mat[0] = t1.mat[0]*t2.mat[0] + t1.mat[1]*t2.mat[4] + t1.mat[2]*t2.mat[8];
mat[1] = t1.mat[0]*t2.mat[1] + t1.mat[1]*t2.mat[5] + t1.mat[2]*t2.mat[9];
mat[2] = t1.mat[0]*t2.mat[2] + t1.mat[1]*t2.mat[6] + t1.mat[2]*t2.mat[10];
mat[3] = t1.mat[0]*t2.mat[3] + t1.mat[1]*t2.mat[7] +
t1.mat[2]*t2.mat[11] + t1.mat[3];
mat[4] = t1.mat[4]*t2.mat[0] + t1.mat[5]*t2.mat[4] + t1.mat[6]*t2.mat[8];
mat[5] = t1.mat[4]*t2.mat[1] + t1.mat[5]*t2.mat[5] + t1.mat[6]*t2.mat[9];
mat[6] = t1.mat[4]*t2.mat[2] + t1.mat[5]*t2.mat[6] + t1.mat[6]*t2.mat[10];
mat[7] = t1.mat[4]*t2.mat[3] + t1.mat[5]*t2.mat[7] +
t1.mat[6]*t2.mat[11] + t1.mat[7];
mat[8] = t1.mat[8]*t2.mat[0] + t1.mat[9]*t2.mat[4] + t1.mat[10]*t2.mat[8];
mat[9] = t1.mat[8]*t2.mat[1] + t1.mat[9]*t2.mat[5] + t1.mat[10]*t2.mat[9];
mat[10] = t1.mat[8]*t2.mat[2] + t1.mat[9]*t2.mat[6] + t1.mat[10]*t2.mat[10];
mat[11] = t1.mat[8]*t2.mat[3] + t1.mat[9]*t2.mat[7] +
t1.mat[10]*t2.mat[11] + t1.mat[11];
if (t1.isAffine()) {
aff = true;
mat[12] = mat[13] = mat[14] = 0;
mat[15] = 1;
} else {
mat[12] = t1.mat[12]*t2.mat[0] + t1.mat[13]*t2.mat[4] +
t1.mat[14]*t2.mat[8];
mat[13] = t1.mat[12]*t2.mat[1] + t1.mat[13]*t2.mat[5] +
t1.mat[14]*t2.mat[9];
mat[14] = t1.mat[12]*t2.mat[2] + t1.mat[13]*t2.mat[6] +
t1.mat[14]*t2.mat[10];
mat[15] = t1.mat[12]*t2.mat[3] + t1.mat[13]*t2.mat[7] +
t1.mat[14]*t2.mat[11] + t1.mat[15];
}
} else {
mat[0] = t1.mat[0]*t2.mat[0] + t1.mat[1]*t2.mat[4] +
t1.mat[2]*t2.mat[8] + t1.mat[3]*t2.mat[12];
mat[1] = t1.mat[0]*t2.mat[1] + t1.mat[1]*t2.mat[5] +
t1.mat[2]*t2.mat[9] + t1.mat[3]*t2.mat[13];
mat[2] = t1.mat[0]*t2.mat[2] + t1.mat[1]*t2.mat[6] +
t1.mat[2]*t2.mat[10] + t1.mat[3]*t2.mat[14];
mat[3] = t1.mat[0]*t2.mat[3] + t1.mat[1]*t2.mat[7] +
t1.mat[2]*t2.mat[11] + t1.mat[3]*t2.mat[15];
mat[4] = t1.mat[4]*t2.mat[0] + t1.mat[5]*t2.mat[4] +
t1.mat[6]*t2.mat[8] + t1.mat[7]*t2.mat[12];
mat[5] = t1.mat[4]*t2.mat[1] + t1.mat[5]*t2.mat[5] +
t1.mat[6]*t2.mat[9] + t1.mat[7]*t2.mat[13];
mat[6] = t1.mat[4]*t2.mat[2] + t1.mat[5]*t2.mat[6] +
t1.mat[6]*t2.mat[10] + t1.mat[7]*t2.mat[14];
mat[7] = t1.mat[4]*t2.mat[3] + t1.mat[5]*t2.mat[7] +
t1.mat[6]*t2.mat[11] + t1.mat[7]*t2.mat[15];
mat[8] = t1.mat[8]*t2.mat[0] + t1.mat[9]*t2.mat[4] +
t1.mat[10]*t2.mat[8] + t1.mat[11]*t2.mat[12];
mat[9] = t1.mat[8]*t2.mat[1] + t1.mat[9]*t2.mat[5] +
t1.mat[10]*t2.mat[9] + t1.mat[11]*t2.mat[13];
mat[10] = t1.mat[8]*t2.mat[2] + t1.mat[9]*t2.mat[6] +
t1.mat[10]*t2.mat[10] + t1.mat[11]*t2.mat[14];
mat[11] = t1.mat[8]*t2.mat[3] + t1.mat[9]*t2.mat[7] +
t1.mat[10]*t2.mat[11] + t1.mat[11]*t2.mat[15];
if (t1.isAffine()) {
mat[12] = t2.mat[12];
mat[13] = t2.mat[13];
mat[14] = t2.mat[14];
mat[15] = t2.mat[15];
} else {
mat[12] = t1.mat[12]*t2.mat[0] + t1.mat[13]*t2.mat[4] +
t1.mat[14]*t2.mat[8] + t1.mat[15]*t2.mat[12];
mat[13] = t1.mat[12]*t2.mat[1] + t1.mat[13]*t2.mat[5] +
t1.mat[14]*t2.mat[9] + t1.mat[15]*t2.mat[13];
mat[14] = t1.mat[12]*t2.mat[2] + t1.mat[13]*t2.mat[6] +
t1.mat[14]*t2.mat[10] + t1.mat[15]*t2.mat[14];
mat[15] = t1.mat[12]*t2.mat[3] + t1.mat[13]*t2.mat[7] +
t1.mat[14]*t2.mat[11] + t1.mat[15]*t2.mat[15];
}
}
} else {
double tmp0, tmp1, tmp2, tmp3;
double tmp4, tmp5, tmp6, tmp7;
double tmp8, tmp9, tmp10, tmp11;
if (t2.isAffine()) {
tmp0 = t1.mat[0]*t2.mat[0] + t1.mat[1]*t2.mat[4] + t1.mat[2]*t2.mat[8];
tmp1 = t1.mat[0]*t2.mat[1] + t1.mat[1]*t2.mat[5] + t1.mat[2]*t2.mat[9];
tmp2 = t1.mat[0]*t2.mat[2] + t1.mat[1]*t2.mat[6] + t1.mat[2]*t2.mat[10];
tmp3 = t1.mat[0]*t2.mat[3] + t1.mat[1]*t2.mat[7] +
t1.mat[2]*t2.mat[11] + t1.mat[3];
tmp4 = t1.mat[4]*t2.mat[0] + t1.mat[5]*t2.mat[4] + t1.mat[6]*t2.mat[8];
tmp5 = t1.mat[4]*t2.mat[1] + t1.mat[5]*t2.mat[5] + t1.mat[6]*t2.mat[9];
tmp6 = t1.mat[4]*t2.mat[2] + t1.mat[5]*t2.mat[6] + t1.mat[6]*t2.mat[10];
tmp7 = t1.mat[4]*t2.mat[3] + t1.mat[5]*t2.mat[7] +
t1.mat[6]*t2.mat[11] + t1.mat[7];
tmp8 = t1.mat[8]*t2.mat[0] + t1.mat[9]*t2.mat[4] + t1.mat[10]*t2.mat[8];
tmp9 = t1.mat[8]*t2.mat[1] + t1.mat[9]*t2.mat[5] + t1.mat[10]*t2.mat[9];
tmp10 = t1.mat[8]*t2.mat[2] + t1.mat[9]*t2.mat[6] + t1.mat[10]*t2.mat[10];
tmp11 = t1.mat[8]*t2.mat[3] + t1.mat[9]*t2.mat[7] +
t1.mat[10]*t2.mat[11] + t1.mat[11];
if (t1.isAffine()) {
aff = true;
mat[12] = mat[13] = mat[14] = 0;
mat[15] = 1;
} else {
double tmp12 = t1.mat[12]*t2.mat[0] + t1.mat[13]*t2.mat[4] +
t1.mat[14]*t2.mat[8];
double tmp13 = t1.mat[12]*t2.mat[1] + t1.mat[13]*t2.mat[5] +
t1.mat[14]*t2.mat[9];
double tmp14 = t1.mat[12]*t2.mat[2] + t1.mat[13]*t2.mat[6] +
t1.mat[14]*t2.mat[10];
double tmp15 = t1.mat[12]*t2.mat[3] + t1.mat[13]*t2.mat[7] +
t1.mat[14]*t2.mat[11] + t1.mat[15];
mat[12] = tmp12;
mat[13] = tmp13;
mat[14] = tmp14;
mat[15] = tmp15;
}
} else {
tmp0 = t1.mat[0]*t2.mat[0] + t1.mat[1]*t2.mat[4] +
t1.mat[2]*t2.mat[8] + t1.mat[3]*t2.mat[12];
tmp1 = t1.mat[0]*t2.mat[1] + t1.mat[1]*t2.mat[5] +
t1.mat[2]*t2.mat[9] + t1.mat[3]*t2.mat[13];
tmp2 = t1.mat[0]*t2.mat[2] + t1.mat[1]*t2.mat[6] +
t1.mat[2]*t2.mat[10] + t1.mat[3]*t2.mat[14];
tmp3 = t1.mat[0]*t2.mat[3] + t1.mat[1]*t2.mat[7] +
t1.mat[2]*t2.mat[11] + t1.mat[3]*t2.mat[15];
tmp4 = t1.mat[4]*t2.mat[0] + t1.mat[5]*t2.mat[4] +
t1.mat[6]*t2.mat[8] + t1.mat[7]*t2.mat[12];
tmp5 = t1.mat[4]*t2.mat[1] + t1.mat[5]*t2.mat[5] +
t1.mat[6]*t2.mat[9] + t1.mat[7]*t2.mat[13];
tmp6 = t1.mat[4]*t2.mat[2] + t1.mat[5]*t2.mat[6] +
t1.mat[6]*t2.mat[10] + t1.mat[7]*t2.mat[14];
tmp7 = t1.mat[4]*t2.mat[3] + t1.mat[5]*t2.mat[7] +
t1.mat[6]*t2.mat[11] + t1.mat[7]*t2.mat[15];
tmp8 = t1.mat[8]*t2.mat[0] + t1.mat[9]*t2.mat[4] +
t1.mat[10]*t2.mat[8] + t1.mat[11]*t2.mat[12];
tmp9 = t1.mat[8]*t2.mat[1] + t1.mat[9]*t2.mat[5] +
t1.mat[10]*t2.mat[9] + t1.mat[11]*t2.mat[13];
tmp10 = t1.mat[8]*t2.mat[2] + t1.mat[9]*t2.mat[6] +
t1.mat[10]*t2.mat[10] + t1.mat[11]*t2.mat[14];
tmp11 = t1.mat[8]*t2.mat[3] + t1.mat[9]*t2.mat[7] +
t1.mat[10]*t2.mat[11] + t1.mat[11]*t2.mat[15];
if (t1.isAffine()) {
mat[12] = t2.mat[12];
mat[13] = t2.mat[13];
mat[14] = t2.mat[14];
mat[15] = t2.mat[15];
} else {
double tmp12 = t1.mat[12]*t2.mat[0] + t1.mat[13]*t2.mat[4] +
t1.mat[14]*t2.mat[8] + t1.mat[15]*t2.mat[12];
double tmp13 = t1.mat[12]*t2.mat[1] + t1.mat[13]*t2.mat[5] +
t1.mat[14]*t2.mat[9] + t1.mat[15]*t2.mat[13];
double tmp14 = t1.mat[12]*t2.mat[2] + t1.mat[13]*t2.mat[6] +
t1.mat[14]*t2.mat[10] + t1.mat[15]*t2.mat[14];
double tmp15 = t1.mat[12]*t2.mat[3] + t1.mat[13]*t2.mat[7] +
t1.mat[14]*t2.mat[11] + t1.mat[15]*t2.mat[15];
mat[12] = tmp12;
mat[13] = tmp13;
mat[14] = tmp14;
mat[15] = tmp15;
}
}
mat[0] = tmp0;
mat[1] = tmp1;
mat[2] = tmp2;
mat[3] = tmp3;
mat[4] = tmp4;
mat[5] = tmp5;
mat[6] = tmp6;
mat[7] = tmp7;
mat[8] = tmp8;
mat[9] = tmp9;
mat[10] = tmp10;
mat[11] = tmp11;
}
if (((t1.dirtyBits & CONGRUENT_BIT) == 0) &&
((t1.type & CONGRUENT) != 0) &&
((t2.dirtyBits & CONGRUENT_BIT) == 0) &&
((t2.type & CONGRUENT) != 0)) {
type = t1.type & t2.type;
dirtyBits = t1.dirtyBits | t2.dirtyBits | CLASSIFY_BIT |
ROTSCALESVD_DIRTY | RIGID_BIT;
} else {
if (aff) {
dirtyBits = ORTHO_BIT | CONGRUENT_BIT | RIGID_BIT |
CLASSIFY_BIT | ROTSCALESVD_DIRTY;
} else {
dirtyBits = ALL_DIRTY;
}
}
if (autoNormalize) {
normalize();
}
}
/**
* Multiplies this transform by the inverse of transform t1. The final
* value is placed into this matrix (this = this*t1^-1).
* @param t1 the matrix whose inverse is computed.
*/
public final void mulInverse(Transform3D t1) {
Transform3D t2 = new Transform3D();
t2.autoNormalize = false;
t2.invert(t1);
this.mul(t2);
}
/**
* Multiplies transform t1 by the inverse of transform t2. The final
* value is placed into this matrix (this = t1*t2^-1).
* @param t1 the left transform in the multiplication
* @param t2 the transform whose inverse is computed.
*/
public final void mulInverse(Transform3D t1, Transform3D t2) {
Transform3D t3 = new Transform3D();
t3.autoNormalize = false;
t3.invert(t2);
this.mul(t1,t3);
}
/**
* Multiplies transform t1 by the transpose of transform t2 and places
* the result into this transform (this = t1 * transpose(t2)).
* @param t1 the transform on the left hand side of the multiplication
* @param t2 the transform whose transpose is computed
*/
public final void mulTransposeRight(Transform3D t1, Transform3D t2) {
Transform3D t3 = new Transform3D();
t3.autoNormalize = false;
t3.transpose(t2);
mul(t1, t3);
}
/**
* Multiplies the transpose of transform t1 by transform t2 and places
* the result into this matrix (this = transpose(t1) * t2).
* @param t1 the transform whose transpose is computed
* @param t2 the transform on the right hand side of the multiplication
*/
public final void mulTransposeLeft(Transform3D t1, Transform3D t2){
Transform3D t3 = new Transform3D();
t3.autoNormalize = false;
t3.transpose(t1);
mul(t3, t2);
}
/**
* Multiplies the transpose of transform t1 by the transpose of
* transform t2 and places the result into this transform
* (this = transpose(t1) * transpose(t2)).
* @param t1 the transform on the left hand side of the multiplication
* @param t2 the transform on the right hand side of the multiplication
*/
public final void mulTransposeBoth(Transform3D t1, Transform3D t2) {
Transform3D t3 = new Transform3D();
Transform3D t4 = new Transform3D();
t3.autoNormalize = false;
t4.autoNormalize = false;
t3.transpose(t1);
t4.transpose(t2);
mul(t3, t4);
}
/**
* Normalizes the rotational components (upper 3x3) of this matrix
* in place using a Singular Value Decomposition (SVD).
* This operation ensures that the column vectors of this matrix
* are orthogonal to each other. The primary use of this method
* is to correct for floating point errors that accumulate over
* time when concatenating a large number of rotation matrices.
* Note that the scale of the matrix is not altered by this method.
*/
public final void normalize() {
// Issue 253: Unable to normalize matrices with infinity or NaN
if (!isAffine() && isInfOrNaN()) {
return;
}
if ((dirtyBits & (ROTATION_BIT|SVD_BIT)) != 0) {
computeScaleRotation(true);
} else if ((dirtyBits & (SCALE_BIT|SVD_BIT)) != 0) {
computeScales(true);
}
mat[0] = rot[0]*scales[0];
mat[1] = rot[1]*scales[1];
mat[2] = rot[2]*scales[2];
mat[4] = rot[3]*scales[0];
mat[5] = rot[4]*scales[1];
mat[6] = rot[5]*scales[2];
mat[8] = rot[6]*scales[0];
mat[9] = rot[7]*scales[1];
mat[10] = rot[8]*scales[2];
dirtyBits |= CLASSIFY_BIT;
dirtyBits &= ~ORTHO_BIT;
type |= ORTHO;
}
/**
* Normalizes the rotational components (upper 3x3) of transform t1
* using a Singular Value Decomposition (SVD), and places the result
* into this transform.
* This operation ensures that the column vectors of this matrix
* are orthogonal to each other. The primary use of this method
* is to correct for floating point errors that accumulate over
* time when concatenating a large number of rotation matrices.
* Note that the scale of the matrix is not altered by this method.
*
* @param t1 the source transform, which is not modified
*/
public final void normalize(Transform3D t1){
set(t1);
normalize();
}
/**
* Normalizes the rotational components (upper 3x3) of this transform
* in place using a Cross Product (CP) normalization.
* This operation ensures that the column vectors of this matrix
* are orthogonal to each other. The primary use of this method
* is to correct for floating point errors that accumulate over
* time when concatenating a large number of rotation matrices.
* Note that the scale of the matrix is not altered by this method.
*/
public final void normalizeCP() {
// Issue 253: Unable to normalize matrices with infinity or NaN
if (!isAffine() && isInfOrNaN()) {
return;
}
if ((dirtyBits & SCALE_BIT) != 0) {
computeScales(false);
}
double mag = mat[0]*mat[0] + mat[4]*mat[4] +
mat[8]*mat[8];
if (mag != 0) {
mag = 1.0/Math.sqrt(mag);
mat[0] = mat[0]*mag;
mat[4] = mat[4]*mag;
mat[8] = mat[8]*mag;
}
mag = mat[1]*mat[1] + mat[5]*mat[5] +
mat[9]*mat[9];
if (mag != 0) {
mag = 1.0/Math.sqrt(mag);
mat[1] = mat[1]*mag;
mat[5] = mat[5]*mag;
mat[9] = mat[9]*mag;
}
mat[2] = (mat[4]*mat[9] - mat[5]*mat[8])*scales[0];
mat[6] = (mat[1]*mat[8] - mat[0]*mat[9])*scales[1];
mat[10] = (mat[0]*mat[5] - mat[1]*mat[4])*scales[2];
mat[0] *= scales[0];
mat[1] *= scales[0];
mat[4] *= scales[1];
mat[5] *= scales[1];
mat[8] *= scales[2];
mat[9] *= scales[2];
// leave the AFFINE bit
dirtyBits |= CONGRUENT_BIT | RIGID_BIT | CLASSIFY_BIT | ROTATION_BIT | SVD_BIT;
dirtyBits &= ~ORTHO_BIT;
type |= ORTHO;
}
/**
* Normalizes the rotational components (upper 3x3) of transform t1
* using a Cross Product (CP) normalization, and
* places the result into this transform.
* This operation ensures that the column vectors of this matrix
* are orthogonal to each other. The primary use of this method
* is to correct for floating point errors that accumulate over
* time when concatenating a large number of rotation matrices.
* Note that the scale of the matrix is not altered by this method.
*
* @param t1 the transform to be normalized
*/
public final void normalizeCP(Transform3D t1) {
set(t1);
normalizeCP();
}
/**
* Returns true if all of the data members of transform t1 are
* equal to the corresponding data members in this Transform3D.
* @param t1 the transform with which the comparison is made
* @return true or false
*/
public boolean equals(Transform3D t1) {
return (t1 != null) &&
(mat[0] == t1.mat[0]) && (mat[1] == t1.mat[1]) &&
(mat[2] == t1.mat[2]) && (mat[3] == t1.mat[3]) &&
(mat[4] == t1.mat[4]) && (mat[5] == t1.mat[5]) &&
(mat[6] == t1.mat[6]) && (mat[7] == t1.mat[7]) &&
(mat[8] == t1.mat[8]) && (mat[9] == t1.mat[9]) &&
(mat[10] == t1.mat[10]) && (mat[11] == t1.mat[11]) &&
(mat[12] == t1.mat[12]) && (mat[13] == t1.mat[13]) &&
(mat[14] == t1.mat[14]) && ( mat[15] == t1.mat[15]);
}
/**
* Returns true if the Object o1 is of type Transform3D and all of the
* data members of o1 are equal to the corresponding data members in
* this Transform3D.
* @param o1 the object with which the comparison is made.
* @return true or false
*/
@Override
public boolean equals(Object o1) {
return (o1 instanceof Transform3D) && equals((Transform3D) o1);
}
/**
* 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,3 ; j=0,1,2,3 ; abs[(this.m(i,j) - m1.m(i,j)]
* @param t1 the transform to be compared to this transform
* @param epsilon the threshold value
*/
public boolean epsilonEquals(Transform3D t1, double epsilon) {
double diff;
for (int i=0 ; i<16 ; i++) {
diff = mat[i] - t1.mat[i];
if ((diff < 0 ? -diff : diff) > epsilon) {
return false;
}
}
return true;
}
/**
* Returns a hash code value based on the data values in this
* object. Two different Transform3D objects with identical data
* values (i.e., Transform3D.equals returns true) will return the
* same hash number. Two Transform3D objects with different data
* members may return the same hash value, although this is not
* likely.
* @return the integer hash code value
*/
@Override
public int hashCode() {
long bits = 1L;
for (int i = 0; i < 16; i++) {
bits = J3dHash.mixDoubleBits(bits, mat[i]);
}
return J3dHash.finish(bits);
}
/**
* Transform the vector vec using this transform and place the
* result into vecOut.
* @param vec the double precision vector to be transformed
* @param vecOut the vector into which the transformed values are placed
*/
public final void transform(Vector4d vec, Vector4d vecOut) {
if (vec != vecOut) {
vecOut.x = (mat[0]*vec.x + mat[1]*vec.y
+ mat[2]*vec.z + mat[3]*vec.w);
vecOut.y = (mat[4]*vec.x + mat[5]*vec.y
+ mat[6]*vec.z + mat[7]*vec.w);
vecOut.z = (mat[8]*vec.x + mat[9]*vec.y
+ mat[10]*vec.z + mat[11]*vec.w);
vecOut.w = (mat[12]*vec.x + mat[13]*vec.y
+ mat[14]*vec.z + mat[15]*vec.w);
} else {
transform(vec);
}
}
/**
* Transform the vector vec using this Transform and place the
* result back into vec.
* @param vec the double precision vector to be transformed
*/
public final void transform(Vector4d vec) {
double x = (mat[0]*vec.x + mat[1]*vec.y
+ mat[2]*vec.z + mat[3]*vec.w);
double y = (mat[4]*vec.x + mat[5]*vec.y
+ mat[6]*vec.z + mat[7]*vec.w);
double z = (mat[8]*vec.x + mat[9]*vec.y
+ mat[10]*vec.z + mat[11]*vec.w);
vec.w = (mat[12]*vec.x + mat[13]*vec.y
+ mat[14]*vec.z + mat[15]*vec.w);
vec.x = x;
vec.y = y;
vec.z = z;
}
/**
* Transform the vector vec using this Transform and place the
* result into vecOut.
* @param vec the single precision vector to be transformed
* @param vecOut the vector into which the transformed values are placed
*/
public final void transform(Vector4f vec, Vector4f vecOut) {
if (vecOut != vec) {
vecOut.x = (float) (mat[0]*vec.x + mat[1]*vec.y
+ mat[2]*vec.z + mat[3]*vec.w);
vecOut.y = (float) (mat[4]*vec.x + mat[5]*vec.y
+ mat[6]*vec.z + mat[7]*vec.w);
vecOut.z = (float) (mat[8]*vec.x + mat[9]*vec.y
+ mat[10]*vec.z + mat[11]*vec.w);
vecOut.w = (float) (mat[12]*vec.x + mat[13]*vec.y
+ mat[14]*vec.z + mat[15]*vec.w);
} else {
transform(vec);
}
}
/**
* Transform the vector vec using this Transform and place the
* result back into vec.
* @param vec the single precision vector to be transformed
*/
public final void transform(Vector4f vec) {
float x = (float) (mat[0]*vec.x + mat[1]*vec.y
+ mat[2]*vec.z + mat[3]*vec.w);
float y = (float) (mat[4]*vec.x + mat[5]*vec.y
+ mat[6]*vec.z + mat[7]*vec.w);
float z = (float) (mat[8]*vec.x + mat[9]*vec.y
+ mat[10]*vec.z + mat[11]*vec.w);
vec.w = (float) (mat[12]*vec.x + mat[13]*vec.y
+ mat[14]*vec.z + mat[15]*vec.w);
vec.x = x;
vec.y = y;
vec.z = z;
}
/**
* Transforms the point parameter with this transform and
* places the result into pointOut. The fourth element of the
* point input paramter is assumed to be one.
* @param point the input point to be transformed
* @param pointOut the transformed point
*/
public final void transform(Point3d point, Point3d pointOut) {
if (point != pointOut) {
pointOut.x = mat[0]*point.x + mat[1]*point.y +
mat[2]*point.z + mat[3];
pointOut.y = mat[4]*point.x + mat[5]*point.y +
mat[6]*point.z + mat[7];
pointOut.z = mat[8]*point.x + mat[9]*point.y +
mat[10]*point.z + mat[11];
} else {
transform(point);
}
}
/**
* Transforms the point parameter with this transform and
* places the result back into point. The fourth element of the
* point input paramter is assumed to be one.
* @param point the input point to be transformed
*/
public final void transform(Point3d point) {
double x = mat[0]*point.x + mat[1]*point.y + mat[2]*point.z + mat[3];
double y = mat[4]*point.x + mat[5]*point.y + mat[6]*point.z + mat[7];
point.z = mat[8]*point.x + mat[9]*point.y + mat[10]*point.z + mat[11];
point.x = x;
point.y = y;
}
/**
* Transforms the normal parameter by this transform and places the value
* into normalOut. The fourth element of the normal is assumed to be zero.
* @param normal the input normal to be transformed
* @param normalOut the transformed normal
*/
public final void transform(Vector3d normal, Vector3d normalOut) {
if (normalOut != normal) {
normalOut.x = mat[0]*normal.x + mat[1]*normal.y + mat[2]*normal.z;
normalOut.y = mat[4]*normal.x + mat[5]*normal.y + mat[6]*normal.z;
normalOut.z = mat[8]*normal.x + mat[9]*normal.y + mat[10]*normal.z;
} else {
transform(normal);
}
}
/**
* Transforms the normal parameter by this transform and places the value
* back into normal. The fourth element of the normal is assumed to be zero.
* @param normal the input normal to be transformed
*/
public final void transform(Vector3d normal) {
double x = mat[0]*normal.x + mat[1]*normal.y + mat[2]*normal.z;
double y = mat[4]*normal.x + mat[5]*normal.y + mat[6]*normal.z;
normal.z = mat[8]*normal.x + mat[9]*normal.y + mat[10]*normal.z;
normal.x = x;
normal.y = y;
}
/**
* Transforms the point parameter with this transform and
* places the result into pointOut. The fourth element of the
* point input paramter is assumed to be one.
* @param point the input point to be transformed
* @param pointOut the transformed point
*/
public final void transform(Point3f point, Point3f pointOut) {
if (point != pointOut) {
pointOut.x = (float)(mat[0]*point.x + mat[1]*point.y +
mat[2]*point.z + mat[3]);
pointOut.y = (float)(mat[4]*point.x + mat[5]*point.y +
mat[6]*point.z + mat[7]);
pointOut.z = (float)(mat[8]*point.x + mat[9]*point.y +
mat[10]*point.z + mat[11]);
} else {
transform(point);
}
}
/**
* Transforms the point parameter with this transform and
* places the result back into point. The fourth element of the
* point input paramter is assumed to be one.
* @param point the input point to be transformed
*/
public final void transform(Point3f point) {
float x = (float) (mat[0]*point.x + mat[1]*point.y +
mat[2]*point.z + mat[3]);
float y = (float) (mat[4]*point.x + mat[5]*point.y +
mat[6]*point.z + mat[7]);
point.z = (float) (mat[8]*point.x + mat[9]*point.y +
mat[10]*point.z + mat[11]);
point.x = x;
point.y = y;
}
/**
* Transforms the normal parameter by this transform and places the value
* into normalOut. The fourth element of the normal is assumed to be zero.
* Note: For correct lighting results, if a transform has uneven scaling
* surface normals should transformed by the inverse transpose of
* the transform. This the responsibility of the application and is not
* done automatically by this method.
* @param normal the input normal to be transformed
* @param normalOut the transformed normal
*/
public final void transform(Vector3f normal, Vector3f normalOut) {
if (normal != normalOut) {
normalOut.x = (float) (mat[0]*normal.x + mat[1]*normal.y +
mat[2]*normal.z);
normalOut.y = (float) (mat[4]*normal.x + mat[5]*normal.y +
mat[6]*normal.z);
normalOut.z = (float) (mat[8]*normal.x + mat[9]*normal.y +
mat[10]*normal.z);
} else {
transform(normal);
}
}
/**
* Transforms the normal parameter by this transform and places the value
* back into normal. The fourth element of the normal is assumed to be zero.
* Note: For correct lighting results, if a transform has uneven scaling
* surface normals should transformed by the inverse transpose of
* the transform. This the responsibility of the application and is not
* done automatically by this method.
* @param normal the input normal to be transformed
*/
public final void transform(Vector3f normal) {
float x = (float) (mat[0]*normal.x + mat[1]*normal.y +
mat[2]*normal.z);
float y = (float) (mat[4]*normal.x + mat[5]*normal.y +
mat[6]*normal.z);
normal.z = (float) (mat[8]*normal.x + mat[9]*normal.y +
mat[10]*normal.z);
normal.x = x;
normal.y = y;
}
/**
* Replaces the upper 3x3 matrix values of this transform with the
* values in the matrix m1.
* @param m1 the matrix that will be the new upper 3x3
*/
public final void setRotationScale(Matrix3f m1) {
mat[0] = m1.m00; mat[1] = m1.m01; mat[2] = m1.m02;
mat[4] = m1.m10; mat[5] = m1.m11; mat[6] = m1.m12;
mat[8] = m1.m20; mat[9] = m1.m21; mat[10] = m1.m22;
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Replaces the upper 3x3 matrix values of this transform with the
* values in the matrix m1.
* @param m1 the matrix that will be the new upper 3x3
*/
public final void setRotationScale(Matrix3d m1) {
mat[0] = m1.m00; mat[1] = m1.m01; mat[2] = m1.m02;
mat[4] = m1.m10; mat[5] = m1.m11; mat[6] = m1.m12;
mat[8] = m1.m20; mat[9] = m1.m21; mat[10] = m1.m22;
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Scales transform t1 by a Uniform scale matrix with scale
* factor s and then adds transform t2 (this = S*t1 + t2).
* @param s the scale factor
* @param t1 the transform to be scaled
* @param t2 the transform to be added
*/
public final void scaleAdd(double s, Transform3D t1, Transform3D t2) {
for (int i=0 ; i<16 ; i++) {
mat[i] = s*t1.mat[i] + t2.mat[i];
}
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Scales this transform by a Uniform scale matrix with scale factor
* s and then adds transform t1 (this = S*this + t1).
* @param s the scale factor
* @param t1 the transform to be added
*/
public final void scaleAdd(double s, Transform3D t1) {
for (int i=0 ; i<16 ; i++) {
mat[i] = s*mat[i] + t1.mat[i];
}
dirtyBits = ALL_DIRTY;
if (autoNormalize) {
normalize();
}
}
/**
* Gets the upper 3x3 values of this matrix and places them into
* the matrix m1.
* @param m1 the matrix that will hold the values
*/
public final void getRotationScale(Matrix3f m1) {
m1.m00 = (float) mat[0];
m1.m01 = (float) mat[1];
m1.m02 = (float) mat[2];
m1.m10 = (float) mat[4];
m1.m11 = (float) mat[5];
m1.m12 = (float) mat[6];
m1.m20 = (float) mat[8];
m1.m21 = (float) mat[9];
m1.m22 = (float) mat[10];
}
/**
* Gets the upper 3x3 values of this matrix and places them into
* the matrix m1.
* @param m1 the matrix that will hold the values
*/
public final void getRotationScale(Matrix3d m1) {
m1.m00 = mat[0];
m1.m01 = mat[1];
m1.m02 = mat[2];
m1.m10 = mat[4];
m1.m11 = mat[5];
m1.m12 = mat[6];
m1.m20 = mat[8];
m1.m21 = mat[9];
m1.m22 = mat[10];
}
/**
* Helping function that specifies the position and orientation of a
* view matrix. The inverse of this transform can be used to control
* the ViewPlatform object within the scene graph.
* @param eye the location of the eye
* @param center a point in the virtual world where the eye is looking
* @param up an up vector specifying the frustum's up direction
*/
public void lookAt(Point3d eye, Point3d center, Vector3d up) {
double forwardx,forwardy,forwardz,invMag;
double upx,upy,upz;
double sidex,sidey,sidez;
forwardx = eye.x - center.x;
forwardy = eye.y - center.y;
forwardz = eye.z - center.z;
invMag = 1.0/Math.sqrt( forwardx*forwardx + forwardy*forwardy + forwardz*forwardz);
forwardx = forwardx*invMag;
forwardy = forwardy*invMag;
forwardz = forwardz*invMag;
invMag = 1.0/Math.sqrt( up.x*up.x + up.y*up.y + up.z*up.z);
upx = up.x*invMag;
upy = up.y*invMag;
upz = up.z*invMag;
// side = Up cross forward
sidex = upy*forwardz-forwardy*upz;
sidey = upz*forwardx-upx*forwardz;
sidez = upx*forwardy-upy*forwardx;
invMag = 1.0/Math.sqrt( sidex*sidex + sidey*sidey + sidez*sidez);
sidex *= invMag;
sidey *= invMag;
sidez *= invMag;
// recompute up = forward cross side
upx = forwardy*sidez-sidey*forwardz;
upy = forwardz*sidex-forwardx*sidez;
upz = forwardx*sidey-forwardy*sidex;
// transpose because we calculated the inverse of what we want
mat[0] = sidex;
mat[1] = sidey;
mat[2] = sidez;
mat[4] = upx;
mat[5] = upy;
mat[6] = upz;
mat[8] = forwardx;
mat[9] = forwardy;
mat[10] = forwardz;
mat[3] = -eye.x*mat[0] + -eye.y*mat[1] + -eye.z*mat[2];
mat[7] = -eye.x*mat[4] + -eye.y*mat[5] + -eye.z*mat[6];
mat[11] = -eye.x*mat[8] + -eye.y*mat[9] + -eye.z*mat[10];
mat[12] = mat[13] = mat[14] = 0;
mat[15] = 1;
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
}
/**
* Creates a perspective projection transform that mimics a standard,
* camera-based,
* view-model. This transform maps coordinates from Eye Coordinates (EC)
* to Clipping Coordinates (CC). Note that unlike the similar function
* in OpenGL, the clipping coordinates generated by the resulting
* transform are in a right-handed coordinate system
* (as are all other coordinate systems in Java 3D).
* <p>
* The frustum function-call establishes a view model with the eye
* at the apex of a symmetric view frustum. The arguments
* define the frustum and its associated perspective projection:
* (left, bottom, -near) and (right, top, -near) specify the
* point on the near clipping plane that maps onto the
* lower-left and upper-right corners of the window respectively,
* assuming the eye is located at (0, 0, 0).
* @param left the vertical line on the left edge of the near
* clipping plane mapped to the left edge of the graphics window
* @param right the vertical line on the right edge of the near
* clipping plane mapped to the right edge of the graphics window
* @param bottom the horizontal line on the bottom edge of the near
* clipping plane mapped to the bottom edge of the graphics window
* @param top the horizontal line on the top edge of the near
* @param near the distance to the frustum's near clipping plane.
* This value must be positive, (the value -near is the location of the
* near clip plane).
* @param far the distance to the frustum's far clipping plane.
* This value must be positive, and must be greater than near.
*/
public void frustum(double left, double right,
double bottom, double top,
double near, double far) {
double dx = 1/(right - left);
double dy = 1/(top - bottom);
double dz = 1/(far - near);
mat[0] = (2.0*near)*dx;
mat[5] = (2.0*near)*dy;
mat[10] = (far+near)*dz;
mat[2] = (right+left)*dx;
mat[6] = (top+bottom)*dy;
mat[11] = (2.0*far*near)*dz;
mat[14] = -1.0;
mat[1] = mat[3] = mat[4] = mat[7] = mat[8] = mat[9] = mat[12]
= mat[13] = mat[15] = 0;
// Matrix is a projection transform
type = 0;
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
}
/**
* Creates a perspective projection transform that mimics a standard,
* camera-based,
* view-model. This transform maps coordinates from Eye Coordinates (EC)
* to Clipping Coordinates (CC). Note that unlike the similar function
* in OpenGL, the clipping coordinates generated by the resulting
* transform are in a right-handed coordinate system
* (as are all other coordinate systems in Java 3D). Also note that the
* field of view is specified in radians.
* @param fovx specifies the field of view in the x direction, in radians
* @param aspect specifies the aspect ratio and thus the field of
* view in the x direction. The aspect ratio is the ratio of x to y,
* or width to height.
* @param zNear the distance to the frustum's near clipping plane.
* This value must be positive, (the value -zNear is the location of the
* near clip plane).
* @param zFar the distance to the frustum's far clipping plane
*/
public void perspective(double fovx, double aspect,
double zNear, double zFar) {
double sine, cotangent, deltaZ;
double half_fov = fovx * 0.5;
double x, y;
Vector3d v1, v2, v3, v4;
Vector3d norm = new Vector3d();
deltaZ = zFar - zNear;
sine = Math.sin(half_fov);
// if ((deltaZ == 0.0) || (sine == 0.0) || (aspect == 0.0)) {
// return;
// }
cotangent = Math.cos(half_fov) / sine;
mat[0] = cotangent;
mat[5] = cotangent * aspect;
mat[10] = (zFar + zNear) / deltaZ;
mat[11] = 2.0 * zNear * zFar / deltaZ;
mat[14] = -1.0;
mat[1] = mat[2] = mat[3] = mat[4] = mat[6] = mat[7] = mat[8] =
mat[9] = mat[12] = mat[13] = mat[15] = 0;
// Matrix is a projection transform
type = 0;
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
}
/**
* Creates an orthographic projection transform that mimics a standard,
* camera-based,
* view-model. This transform maps coordinates from Eye Coordinates (EC)
* to Clipping Coordinates (CC). Note that unlike the similar function
* in OpenGL, the clipping coordinates generated by the resulting
* transform are in a right-handed coordinate system
* (as are all other coordinate systems in Java 3D).
* @param left the vertical line on the left edge of the near
* clipping plane mapped to the left edge of the graphics window
* @param right the vertical line on the right edge of the near
* clipping plane mapped to the right edge of the graphics window
* @param bottom the horizontal line on the bottom edge of the near
* clipping plane mapped to the bottom edge of the graphics window
* @param top the horizontal line on the top edge of the near
* clipping plane mapped to the top edge of the graphics window
* @param near the distance to the frustum's near clipping plane
* (the value -near is the location of the near clip plane)
* @param far the distance to the frustum's far clipping plane
*/
public void ortho(double left, double right, double bottom,
double top, double near, double far) {
double deltax = 1/(right - left);
double deltay = 1/(top - bottom);
double deltaz = 1/(far - near);
// if ((deltax == 0.0) || (deltay == 0.0) || (deltaz == 0.0)) {
// return;
// }
mat[0] = 2.0 * deltax;
mat[3] = -(right + left) * deltax;
mat[5] = 2.0 * deltay;
mat[7] = -(top + bottom) * deltay;
mat[10] = 2.0 * deltaz;
mat[11] = (far + near) * deltaz;
mat[1] = mat[2] = mat[4] = mat[6] = mat[8] =
mat[9] = mat[12] = mat[13] = mat[14] = 0;
mat[15] = 1;
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
}
/**
* get the scaling factor of matrix in this transform,
* use for distance scaling
*/
double getDistanceScale() {
// The caller know that this matrix is affine
// orthogonal before invoke this procedure
if ((dirtyBits & SCALE_BIT) != 0) {
double max = mat[0]*mat[0] + mat[4]*mat[4] +
mat[8]*mat[8];
if (((dirtyBits & CONGRUENT_BIT) == 0) &&
((type & CONGRUENT) != 0)) {
// in most case it is congruent
return Math.sqrt(max);
}
double tmp = mat[1]*mat[1] + mat[5]*mat[5] +
mat[9]*mat[9];
if (tmp > max) {
max = tmp;
}
tmp = mat[2]*mat[2] + mat[6]*mat[6] + mat[10]*mat[10];
return Math.sqrt((tmp > max) ? tmp : max);
}
return max3(scales);
}
static private void mat_mul(double[] m1, double[] m2, double[] m3) {
double[] result = m3;
if ((m1 == m3) || (m2 == m3)) {
result = new double[9];
}
result[0] = m1[0]*m2[0] + m1[1]*m2[3] + m1[2]*m2[6];
result[1] = m1[0]*m2[1] + m1[1]*m2[4] + m1[2]*m2[7];
result[2] = m1[0]*m2[2] + m1[1]*m2[5] + m1[2]*m2[8];
result[3] = m1[3]*m2[0] + m1[4]*m2[3] + m1[5]*m2[6];
result[4] = m1[3]*m2[1] + m1[4]*m2[4] + m1[5]*m2[7];
result[5] = m1[3]*m2[2] + m1[4]*m2[5] + m1[5]*m2[8];
result[6] = m1[6]*m2[0] + m1[7]*m2[3] + m1[8]*m2[6];
result[7] = m1[6]*m2[1] + m1[7]*m2[4] + m1[8]*m2[7];
result[8] = m1[6]*m2[2] + m1[7]*m2[5] + m1[8]*m2[8];
if (result != m3) {
for(int i=0;i<9;i++) {
m3[i] = result[i];
}
}
}
static private void transpose_mat(double[] in, double[] out) {
out[0] = in[0];
out[1] = in[3];
out[2] = in[6];
out[3] = in[1];
out[4] = in[4];
out[5] = in[7];
out[6] = in[2];
out[7] = in[5];
out[8] = in[8];
}
final static private void multipleScale(double m[] , double s[]) {
m[0] *= s[0];
m[1] *= s[0];
m[2] *= s[0];
m[4] *= s[1];
m[5] *= s[1];
m[6] *= s[1];
m[8] *= s[2];
m[9] *= s[2];
m[10] *= s[2];
}
private void compute_svd(Transform3D matrix, double[] outScale,
double[] outRot) {
int i,j;
double g,scale;
double m[] = new double[9];
// if (!svdAllocd) {
double[] u1 = new double[9];
double[] v1 = new double[9];
double[] t1 = new double[9];
double[] t2 = new double[9];
// double[] ts = new double[9];
// double[] svdTmp = new double[9]; It is replaced by t1
double[] svdRot = new double[9];
// double[] single_values = new double[3]; replaced by t2
double[] e = new double[3];
double[] svdScales = new double[3];
// XXXX: initialize to 0's if alread allocd? Should not have to, since
// no operations depend on these being init'd to zero.
int converged, negCnt=0;
double cs,sn;
double c1,c2,c3,c4;
double s1,s2,s3,s4;
double cl1,cl2,cl3;
svdRot[0] = m[0] = matrix.mat[0];
svdRot[1] = m[1] = matrix.mat[1];
svdRot[2] = m[2] = matrix.mat[2];
svdRot[3] = m[3] = matrix.mat[4];
svdRot[4] = m[4] = matrix.mat[5];
svdRot[5] = m[5] = matrix.mat[6];
svdRot[6] = m[6] = matrix.mat[8];
svdRot[7] = m[7] = matrix.mat[9];
svdRot[8] = m[8] = matrix.mat[10];
// u1
if( m[3]*m[3] < EPS ) {
u1[0] = 1.0; u1[1] = 0.0; u1[2] = 0.0;
u1[3] = 0.0; u1[4] = 1.0; u1[5] = 0.0;
u1[6] = 0.0; u1[7] = 0.0; u1[8] = 1.0;
} else if( m[0]*m[0] < EPS ) {
t1[0] = m[0];
t1[1] = m[1];
t1[2] = m[2];
m[0] = m[3];
m[1] = m[4];
m[2] = m[5];
m[3] = -t1[0]; // zero
m[4] = -t1[1];
m[5] = -t1[2];
u1[0] = 0.0; u1[1] = 1.0; u1[2] = 0.0;
u1[3] = -1.0; u1[4] = 0.0; u1[5] = 0.0;
u1[6] = 0.0; u1[7] = 0.0; u1[8] = 1.0;
} else {
g = 1.0/Math.sqrt(m[0]*m[0] + m[3]*m[3]);
c1 = m[0]*g;
s1 = m[3]*g;
t1[0] = c1*m[0] + s1*m[3];
t1[1] = c1*m[1] + s1*m[4];
t1[2] = c1*m[2] + s1*m[5];
m[3] = -s1*m[0] + c1*m[3]; // zero
m[4] = -s1*m[1] + c1*m[4];
m[5] = -s1*m[2] + c1*m[5];
m[0] = t1[0];
m[1] = t1[1];
m[2] = t1[2];
u1[0] = c1; u1[1] = s1; u1[2] = 0.0;
u1[3] = -s1; u1[4] = c1; u1[5] = 0.0;
u1[6] = 0.0; u1[7] = 0.0; u1[8] = 1.0;
}
// u2
if( m[6]*m[6] < EPS ) {
} else if( m[0]*m[0] < EPS ){
t1[0] = m[0];
t1[1] = m[1];
t1[2] = m[2];
m[0] = m[6];
m[1] = m[7];
m[2] = m[8];
m[6] = -t1[0]; // zero
m[7] = -t1[1];
m[8] = -t1[2];
t1[0] = u1[0];
t1[1] = u1[1];
t1[2] = u1[2];
u1[0] = u1[6];
u1[1] = u1[7];
u1[2] = u1[8];
u1[6] = -t1[0]; // zero
u1[7] = -t1[1];
u1[8] = -t1[2];
} else {
g = 1.0/Math.sqrt(m[0]*m[0] + m[6]*m[6]);
c2 = m[0]*g;
s2 = m[6]*g;
t1[0] = c2*m[0] + s2*m[6];
t1[1] = c2*m[1] + s2*m[7];
t1[2] = c2*m[2] + s2*m[8];
m[6] = -s2*m[0] + c2*m[6];
m[7] = -s2*m[1] + c2*m[7];
m[8] = -s2*m[2] + c2*m[8];
m[0] = t1[0];
m[1] = t1[1];
m[2] = t1[2];
t1[0] = c2*u1[0];
t1[1] = c2*u1[1];
u1[2] = s2;
t1[6] = -u1[0]*s2;
t1[7] = -u1[1]*s2;
u1[8] = c2;
u1[0] = t1[0];
u1[1] = t1[1];
u1[6] = t1[6];
u1[7] = t1[7];
}
// v1
if( m[2]*m[2] < EPS ) {
v1[0] = 1.0; v1[1] = 0.0; v1[2] = 0.0;
v1[3] = 0.0; v1[4] = 1.0; v1[5] = 0.0;
v1[6] = 0.0; v1[7] = 0.0; v1[8] = 1.0;
} else if( m[1]*m[1] < EPS ) {
t1[2] = m[2];
t1[5] = m[5];
t1[8] = m[8];
m[2] = -m[1];
m[5] = -m[4];
m[8] = -m[7];
m[1] = t1[2]; // zero
m[4] = t1[5];
m[7] = t1[8];
v1[0] = 1.0; v1[1] = 0.0; v1[2] = 0.0;
v1[3] = 0.0; v1[4] = 0.0; v1[5] =-1.0;
v1[6] = 0.0; v1[7] = 1.0; v1[8] = 0.0;
} else {
g = 1.0/Math.sqrt(m[1]*m[1] + m[2]*m[2]);
c3 = m[1]*g;
s3 = m[2]*g;
t1[1] = c3*m[1] + s3*m[2]; // can assign to m[1]?
m[2] =-s3*m[1] + c3*m[2]; // zero
m[1] = t1[1];
t1[4] = c3*m[4] + s3*m[5];
m[5] =-s3*m[4] + c3*m[5];
m[4] = t1[4];
t1[7] = c3*m[7] + s3*m[8];
m[8] =-s3*m[7] + c3*m[8];
m[7] = t1[7];
v1[0] = 1.0; v1[1] = 0.0; v1[2] = 0.0;
v1[3] = 0.0; v1[4] = c3; v1[5] = -s3;
v1[6] = 0.0; v1[7] = s3; v1[8] = c3;
}
// u3
if( m[7]*m[7] < EPS ) {
} else if( m[4]*m[4] < EPS ) {
t1[3] = m[3];
t1[4] = m[4];
t1[5] = m[5];
m[3] = m[6]; // zero
m[4] = m[7];
m[5] = m[8];
m[6] = -t1[3]; // zero
m[7] = -t1[4]; // zero
m[8] = -t1[5];
t1[3] = u1[3];
t1[4] = u1[4];
t1[5] = u1[5];
u1[3] = u1[6];
u1[4] = u1[7];
u1[5] = u1[8];
u1[6] = -t1[3]; // zero
u1[7] = -t1[4];
u1[8] = -t1[5];
} else {
g = 1.0/Math.sqrt(m[4]*m[4] + m[7]*m[7]);
c4 = m[4]*g;
s4 = m[7]*g;
t1[3] = c4*m[3] + s4*m[6];
m[6] =-s4*m[3] + c4*m[6]; // zero
m[3] = t1[3];
t1[4] = c4*m[4] + s4*m[7];
m[7] =-s4*m[4] + c4*m[7];
m[4] = t1[4];
t1[5] = c4*m[5] + s4*m[8];
m[8] =-s4*m[5] + c4*m[8];
m[5] = t1[5];
t1[3] = c4*u1[3] + s4*u1[6];
u1[6] =-s4*u1[3] + c4*u1[6];
u1[3] = t1[3];
t1[4] = c4*u1[4] + s4*u1[7];
u1[7] =-s4*u1[4] + c4*u1[7];
u1[4] = t1[4];
t1[5] = c4*u1[5] + s4*u1[8];
u1[8] =-s4*u1[5] + c4*u1[8];
u1[5] = t1[5];
}
t2[0] = m[0];
t2[1] = m[4];
t2[2] = m[8];
e[0] = m[1];
e[1] = m[5];
if( e[0]*e[0]>EPS || e[1]*e[1]>EPS ) {
compute_qr( t2, e, u1, v1);
}
svdScales[0] = t2[0];
svdScales[1] = t2[1];
svdScales[2] = t2[2];
// Do some optimization here. If scale is unity, simply return the rotation matric.
if(almostOne(Math.abs(svdScales[0])) &&
almostOne(Math.abs(svdScales[1])) &&
almostOne(Math.abs(svdScales[2]))) {
for(i=0;i<3;i++)
if(svdScales[i]<0.0)
negCnt++;
if((negCnt==0)||(negCnt==2)) {
//System.err.println("Optimize!!");
outScale[0] = outScale[1] = outScale[2] = 1.0;
for(i=0;i<9;i++)
outRot[i] = svdRot[i];
return;
}
}
// XXXX: could eliminate use of t1 and t1 by making a new method which
// transposes and multiplies two matricies
transpose_mat(u1, t1);
transpose_mat(v1, t2);
svdReorder( m, t1, t2, svdRot, svdScales, outRot, outScale);
}
private void svdReorder( double[] m, double[] t1, double[] t2, double[] rot,
double[] scales, double[] outRot, double[] outScale) {
int in0, in1, in2, index,i;
int[] svdOut = new int[3];
double[] svdMag = new double[3];
// check for rotation information in the scales
if(scales[0] < 0.0 ) { // move the rotation info to rotation matrix
scales[0] = -scales[0];
t2[0] = -t2[0];
t2[1] = -t2[1];
t2[2] = -t2[2];
}
if(scales[1] < 0.0 ) { // move the rotation info to rotation matrix
scales[1] = -scales[1];
t2[3] = -t2[3];
t2[4] = -t2[4];
t2[5] = -t2[5];
}
if(scales[2] < 0.0 ) { // move the rotation info to rotation matrix
scales[2] = -scales[2];
t2[6] = -t2[6];
t2[7] = -t2[7];
t2[8] = -t2[8];
}
mat_mul(t1,t2,rot);
// check for equal scales case and do not reorder
if(almostEqual(Math.abs(scales[0]), Math.abs(scales[1])) &&
almostEqual(Math.abs(scales[1]), Math.abs(scales[2])) ){
for(i=0;i<9;i++){
outRot[i] = rot[i];
}
for(i=0;i<3;i++){
outScale[i] = scales[i];
}
}else {
// sort the order of the results of SVD
if( scales[0] > scales[1]) {
if( scales[0] > scales[2] ) {
if( scales[2] > scales[1] ) {
svdOut[0] = 0; svdOut[1] = 2; svdOut[2] = 1; // xzy
} else {
svdOut[0] = 0; svdOut[1] = 1; svdOut[2] = 2; // xyz
}
} else {
svdOut[0] = 2; svdOut[1] = 0; svdOut[2] = 1; // zxy
}
} else { // y > x
if( scales[1] > scales[2] ) {
if( scales[2] > scales[0] ) {
svdOut[0] = 1; svdOut[1] = 2; svdOut[2] = 0; // yzx
} else {
svdOut[0] = 1; svdOut[1] = 0; svdOut[2] = 2; // yxz
}
} else {
svdOut[0] = 2; svdOut[1] = 1; svdOut[2] = 0; // zyx
}
}
// sort the order of the input matrix
svdMag[0] = (m[0]*m[0] + m[1]*m[1] + m[2]*m[2]);
svdMag[1] = (m[3]*m[3] + m[4]*m[4] + m[5]*m[5]);
svdMag[2] = (m[6]*m[6] + m[7]*m[7] + m[8]*m[8]);
if( svdMag[0] > svdMag[1]) {
if( svdMag[0] > svdMag[2] ) {
if( svdMag[2] > svdMag[1] ) {
// 0 - 2 - 1
in0 = 0; in2 = 1; in1 = 2;// xzy
} else {
// 0 - 1 - 2
in0 = 0; in1 = 1; in2 = 2; // xyz
}
} else {
// 2 - 0 - 1
in2 = 0; in0 = 1; in1 = 2; // zxy
}
} else { // y > x 1>0
if( svdMag[1] > svdMag[2] ) { // 1>2
if( svdMag[2] > svdMag[0] ) { // 2>0
// 1 - 2 - 0
in1 = 0; in2 = 1; in0 = 2; // yzx
} else {
// 1 - 0 - 2
in1 = 0; in0 = 1; in2 = 2; // yxz
}
} else {
// 2 - 1 - 0
in2 = 0; in1 = 1; in0 = 2; // zyx
}
}
index = svdOut[in0];
outScale[0] = scales[index];
index = svdOut[in1];
outScale[1] = scales[index];
index = svdOut[in2];
outScale[2] = scales[index];
index = svdOut[in0];
if (outRot == null) {
MasterControl.getCoreLogger().severe("outRot == null");
}
if (rot == null) {
MasterControl.getCoreLogger().severe("rot == null");
}
outRot[0] = rot[index];
index = svdOut[in0]+3;
outRot[0+3] = rot[index];
index = svdOut[in0]+6;
outRot[0+6] = rot[index];
index = svdOut[in1];
outRot[1] = rot[index];
index = svdOut[in1]+3;
outRot[1+3] = rot[index];
index = svdOut[in1]+6;
outRot[1+6] = rot[index];
index = svdOut[in2];
outRot[2] = rot[index];
index = svdOut[in2]+3;
outRot[2+3] = rot[index];
index = svdOut[in2]+6;
outRot[2+6] = rot[index];
}
}
private int compute_qr( double[] s, double[] e, double[] u, double[] v) {
int i,j,k;
boolean converged;
double shift,ssmin,ssmax,r;
double utemp,vtemp;
double f,g;
final int MAX_INTERATIONS = 10;
final double CONVERGE_TOL = 4.89E-15;
double[] cosl = new double[2];
double[] cosr = new double[2];
double[] sinl = new double[2];
double[] sinr = new double[2];
double[] qr_m = new double[9];
double c_b48 = 1.;
double c_b71 = -1.;
int first;
converged = false;
first = 1;
if( Math.abs(e[1]) < CONVERGE_TOL || Math.abs(e[0]) < CONVERGE_TOL) converged = true;
for(k=0;k<MAX_INTERATIONS && !converged;k++) {
shift = compute_shift( s[1], e[1], s[2]);
f = (Math.abs(s[0]) - shift) * (d_sign(c_b48, s[0]) + shift/s[0]);
g = e[0];
r = compute_rot(f, g, sinr, cosr, 0, first);
f = cosr[0] * s[0] + sinr[0] * e[0];
e[0] = cosr[0] * e[0] - sinr[0] * s[0];
g = sinr[0] * s[1];
s[1] = cosr[0] * s[1];
r = compute_rot(f, g, sinl, cosl, 0, first);
first = 0;
s[0] = r;
f = cosl[0] * e[0] + sinl[0] * s[1];
s[1] = cosl[0] * s[1] - sinl[0] * e[0];
g = sinl[0] * e[1];
e[1] = cosl[0] * e[1];
r = compute_rot(f, g, sinr, cosr, 1, first);
e[0] = r;
f = cosr[1] * s[1] + sinr[1] * e[1];
e[1] = cosr[1] * e[1] - sinr[1] * s[1];
g = sinr[1] * s[2];
s[2] = cosr[1] * s[2];
r = compute_rot(f, g, sinl, cosl, 1, first);
s[1] = r;
f = cosl[1] * e[1] + sinl[1] * s[2];
s[2] = cosl[1] * s[2] - sinl[1] * e[1];
e[1] = f;
// update u matrices
utemp = u[0];
u[0] = cosl[0]*utemp + sinl[0]*u[3];
u[3] = -sinl[0]*utemp + cosl[0]*u[3];
utemp = u[1];
u[1] = cosl[0]*utemp + sinl[0]*u[4];
u[4] = -sinl[0]*utemp + cosl[0]*u[4];
utemp = u[2];
u[2] = cosl[0]*utemp + sinl[0]*u[5];
u[5] = -sinl[0]*utemp + cosl[0]*u[5];
utemp = u[3];
u[3] = cosl[1]*utemp + sinl[1]*u[6];
u[6] = -sinl[1]*utemp + cosl[1]*u[6];
utemp = u[4];
u[4] = cosl[1]*utemp + sinl[1]*u[7];
u[7] = -sinl[1]*utemp + cosl[1]*u[7];
utemp = u[5];
u[5] = cosl[1]*utemp + sinl[1]*u[8];
u[8] = -sinl[1]*utemp + cosl[1]*u[8];
// update v matrices
vtemp = v[0];
v[0] = cosr[0]*vtemp + sinr[0]*v[1];
v[1] = -sinr[0]*vtemp + cosr[0]*v[1];
vtemp = v[3];
v[3] = cosr[0]*vtemp + sinr[0]*v[4];
v[4] = -sinr[0]*vtemp + cosr[0]*v[4];
vtemp = v[6];
v[6] = cosr[0]*vtemp + sinr[0]*v[7];
v[7] = -sinr[0]*vtemp + cosr[0]*v[7];
vtemp = v[1];
v[1] = cosr[1]*vtemp + sinr[1]*v[2];
v[2] = -sinr[1]*vtemp + cosr[1]*v[2];
vtemp = v[4];
v[4] = cosr[1]*vtemp + sinr[1]*v[5];
v[5] = -sinr[1]*vtemp + cosr[1]*v[5];
vtemp = v[7];
v[7] = cosr[1]*vtemp + sinr[1]*v[8];
v[8] = -sinr[1]*vtemp + cosr[1]*v[8];
// if(debug)System.err.println("\n*********************** iteration #"+k+" ***********************\n");
qr_m[0] = s[0]; qr_m[1] = e[0]; qr_m[2] = 0.0;
qr_m[3] = 0.0; qr_m[4] = s[1]; qr_m[5] =e[1];
qr_m[6] = 0.0; qr_m[7] = 0.0; qr_m[8] =s[2];
if( Math.abs(e[1]) < CONVERGE_TOL || Math.abs(e[0]) < CONVERGE_TOL) converged = true;
}
if( Math.abs(e[1]) < CONVERGE_TOL ) {
compute_2X2( s[0],e[0],s[1],s,sinl,cosl,sinr,cosr, 0);
utemp = u[0];
u[0] = cosl[0]*utemp + sinl[0]*u[3];
u[3] = -sinl[0]*utemp + cosl[0]*u[3];
utemp = u[1];
u[1] = cosl[0]*utemp + sinl[0]*u[4];
u[4] = -sinl[0]*utemp + cosl[0]*u[4];
utemp = u[2];
u[2] = cosl[0]*utemp + sinl[0]*u[5];
u[5] = -sinl[0]*utemp + cosl[0]*u[5];
// update v matrices
vtemp = v[0];
v[0] = cosr[0]*vtemp + sinr[0]*v[1];
v[1] = -sinr[0]*vtemp + cosr[0]*v[1];
vtemp = v[3];
v[3] = cosr[0]*vtemp + sinr[0]*v[4];
v[4] = -sinr[0]*vtemp + cosr[0]*v[4];
vtemp = v[6];
v[6] = cosr[0]*vtemp + sinr[0]*v[7];
v[7] = -sinr[0]*vtemp + cosr[0]*v[7];
} else {
compute_2X2( s[1],e[1],s[2],s,sinl,cosl,sinr,cosr,1);
utemp = u[3];
u[3] = cosl[0]*utemp + sinl[0]*u[6];
u[6] = -sinl[0]*utemp + cosl[0]*u[6];
utemp = u[4];
u[4] = cosl[0]*utemp + sinl[0]*u[7];
u[7] = -sinl[0]*utemp + cosl[0]*u[7];
utemp = u[5];
u[5] = cosl[0]*utemp + sinl[0]*u[8];
u[8] = -sinl[0]*utemp + cosl[0]*u[8];
// update v matrices
vtemp = v[1];
v[1] = cosr[0]*vtemp + sinr[0]*v[2];
v[2] = -sinr[0]*vtemp + cosr[0]*v[2];
vtemp = v[4];
v[4] = cosr[0]*vtemp + sinr[0]*v[5];
v[5] = -sinr[0]*vtemp + cosr[0]*v[5];
vtemp = v[7];
v[7] = cosr[0]*vtemp + sinr[0]*v[8];
v[8] = -sinr[0]*vtemp + cosr[0]*v[8];
}
return(0);
}
static final double max( double a, double b) {
return ( a > b ? a : b);
}
static final double min( double a, double b) {
return ( a < b ? a : b);
}
static final double d_sign(double a, double b) {
double x = (a >= 0 ? a : - a);
return( b >= 0 ? x : -x);
}
static final double compute_shift( double f, double g, double h) {
double d__1, d__2;
double fhmn, fhmx, c, fa, ga, ha, as, at, au;
double ssmin;
fa = Math.abs(f);
ga = Math.abs(g);
ha = Math.abs(h);
fhmn = min(fa,ha);
fhmx = max(fa,ha);
if (fhmn == 0.) {
ssmin = 0.;
if (fhmx == 0.) {
} else {
d__1 = min(fhmx,ga) / max(fhmx,ga);
}
} else {
if (ga < fhmx) {
as = fhmn / fhmx + 1.;
at = (fhmx - fhmn) / fhmx;
d__1 = ga / fhmx;
au = d__1 * d__1;
c = 2. / (Math.sqrt(as * as + au) + Math.sqrt(at * at + au));
ssmin = fhmn * c;
} else {
au = fhmx / ga;
if (au == 0.) {
ssmin = fhmn * fhmx / ga;
} else {
as = fhmn / fhmx + 1.;
at = (fhmx - fhmn) / fhmx;
d__1 = as * au;
d__2 = at * au;
c = 1. / (Math.sqrt(d__1 * d__1 + 1.) + Math.sqrt(d__2 * d__2 + 1.));
ssmin = fhmn * c * au;
ssmin += ssmin;
}
}
}
return(ssmin);
}
static int compute_2X2( double f, double g, double h, double[] single_values,
double[] snl, double[] csl, double[] snr, double[] csr, int index) {
double c_b3 = 2.;
double c_b4 = 1.;
double d__1;
int pmax;
double temp;
boolean swap;
double a, d, l, m, r, s, t, tsign, fa, ga, ha;
double ft, gt, ht, mm;
boolean gasmal;
double tt, clt, crt, slt, srt;
double ssmin,ssmax;
ssmax = single_values[0];
ssmin = single_values[1];
clt = 0.0;
crt = 0.0;
slt = 0.0;
srt = 0.0;
tsign = 0.0;
ft = f;
fa = Math.abs(ft);
ht = h;
ha = Math.abs(h);
pmax = 1;
if( ha > fa)
swap = true;
else
swap = false;
if (swap) {
pmax = 3;
temp = ft;
ft = ht;
ht = temp;
temp = fa;
fa = ha;
ha = temp;
}
gt = g;
ga = Math.abs(gt);
if (ga == 0.) {
single_values[1] = ha;
single_values[0] = fa;
clt = 1.;
crt = 1.;
slt = 0.;
srt = 0.;
} else {
gasmal = true;
if (ga > fa) {
pmax = 2;
if (fa / ga < EPS) {
gasmal = false;
ssmax = ga;
if (ha > 1.) {
ssmin = fa / (ga / ha);
} else {
ssmin = fa / ga * ha;
}
clt = 1.;
slt = ht / gt;
srt = 1.;
crt = ft / gt;
}
}
if (gasmal) {
d = fa - ha;
if (d == fa) {
l = 1.;
} else {
l = d / fa;
}
m = gt / ft;
t = 2. - l;
mm = m * m;
tt = t * t;
s = Math.sqrt(tt + mm);
if (l == 0.) {
r = Math.abs(m);
} else {
r = Math.sqrt(l * l + mm);
}
a = (s + r) * .5;
if (ga > fa) {
pmax = 2;
if (fa / ga < EPS) {
gasmal = false;
ssmax = ga;
if (ha > 1.) {
ssmin = fa / (ga / ha);
} else {
ssmin = fa / ga * ha;
}
clt = 1.;
slt = ht / gt;
srt = 1.;
crt = ft / gt;
}
}
if (gasmal) {
d = fa - ha;
if (d == fa) {
l = 1.;
} else {
l = d / fa;
}
m = gt / ft;
t = 2. - l;
mm = m * m;
tt = t * t;
s = Math.sqrt(tt + mm);
if (l == 0.) {
r = Math.abs(m);
} else {
r = Math.sqrt(l * l + mm);
}
a = (s + r) * .5;
ssmin = ha / a;
ssmax = fa * a;
if (mm == 0.) {
if (l == 0.) {
t = d_sign(c_b3, ft) * d_sign(c_b4, gt);
} else {
t = gt / d_sign(d, ft) + m / t;
}
} else {
t = (m / (s + t) + m / (r + l)) * (a + 1.);
}
l = Math.sqrt(t * t + 4.);
crt = 2. / l;
srt = t / l;
clt = (crt + srt * m) / a;
slt = ht / ft * srt / a;
}
}
if (swap) {
csl[0] = srt;
snl[0] = crt;
csr[0] = slt;
snr[0] = clt;
} else {
csl[0] = clt;
snl[0] = slt;
csr[0] = crt;
snr[0] = srt;
}
if (pmax == 1) {
tsign = d_sign(c_b4, csr[0]) * d_sign(c_b4, csl[0]) * d_sign(c_b4, f);
}
if (pmax == 2) {
tsign = d_sign(c_b4, snr[0]) * d_sign(c_b4, csl[0]) * d_sign(c_b4, g);
}
if (pmax == 3) {
tsign = d_sign(c_b4, snr[0]) * d_sign(c_b4, snl[0]) * d_sign(c_b4, h);
}
single_values[index] = d_sign(ssmax, tsign);
d__1 = tsign * d_sign(c_b4, f) * d_sign(c_b4, h);
single_values[index+1] = d_sign(ssmin, d__1);
}
return 0;
}
static double compute_rot( double f, double g, double[] sin, double[] cos, int index, int first) {
int i__1;
double d__1, d__2;
double cs,sn;
int i;
double scale;
int count;
double f1, g1;
double r;
final double safmn2 = 2.002083095183101E-146;
final double safmx2 = 4.994797680505588E+145;
if (g == 0.) {
cs = 1.;
sn = 0.;
r = f;
} else if (f == 0.) {
cs = 0.;
sn = 1.;
r = g;
} else {
f1 = f;
g1 = g;
scale = max(Math.abs(f1),Math.abs(g1));
if (scale >= safmx2) {
count = 0;
while(scale >= safmx2) {
++count;
f1 *= safmn2;
g1 *= safmn2;
scale = max(Math.abs(f1),Math.abs(g1));
}
r = Math.sqrt(f1*f1 + g1*g1);
cs = f1 / r;
sn = g1 / r;
i__1 = count;
for (i = 1; i <= count; ++i) {
r *= safmx2;
}
} else if (scale <= safmn2) {
count = 0;
while(scale <= safmn2) {
++count;
f1 *= safmx2;
g1 *= safmx2;
scale = max(Math.abs(f1),Math.abs(g1));
}
r = Math.sqrt(f1*f1 + g1*g1);
cs = f1 / r;
sn = g1 / r;
i__1 = count;
for (i = 1; i <= count; ++i) {
r *= safmn2;
}
} else {
r = Math.sqrt(f1*f1 + g1*g1);
cs = f1 / r;
sn = g1 / r;
}
if (Math.abs(f) > Math.abs(g) && cs < 0.) {
cs = -cs;
sn = -sn;
r = -r;
}
}
sin[index] = sn;
cos[index] = cs;
return r;
}
static final private double max3( double[] values) {
if( values[0] > values[1] ) {
if( values[0] > values[2] )
return(values[0]);
else
return(values[2]);
} else {
if( values[1] > values[2] )
return(values[1]);
else
return(values[2]);
}
}
final private void computeScales(boolean forceSVD) {
if(scales == null)
scales = new double[3];
if ((!forceSVD || ((dirtyBits & SVD_BIT) == 0)) && isAffine()) {
if (isCongruent()) {
if (((dirtyBits & RIGID_BIT) == 0) &&
((type & RIGID) != 0)) {
scales[0] = scales[1] = scales[2] = 1;
dirtyBits &= ~SCALE_BIT;
return;
}
scales[0] = scales[1] = scales[2] =
Math.sqrt(mat[0]*mat[0] + mat[4]*mat[4] +
mat[8]*mat[8]);
dirtyBits &= ~SCALE_BIT;
return;
}
if (isOrtho()) {
scales[0] = Math.sqrt(mat[0]*mat[0] + mat[4]*mat[4] +
mat[8]*mat[8]);
scales[1] = Math.sqrt(mat[1]*mat[1] + mat[5]*mat[5] +
mat[9]*mat[9]);
scales[2] = Math.sqrt(mat[2]*mat[2] + mat[6]*mat[6] +
mat[10]*mat[10]);
dirtyBits &= ~SCALE_BIT;
return;
}
}
// fall back to use SVD decomposition
if (rot == null)
rot = new double[9];
compute_svd(this, scales, rot);
dirtyBits &= ~ROTSCALESVD_DIRTY;
}
final private void computeScaleRotation(boolean forceSVD) {
if(rot == null)
rot = new double[9];
if(scales == null)
scales = new double[3];
if ((!forceSVD || ((dirtyBits & SVD_BIT) == 0)) && isAffine()) {
if (isCongruent()) {
if (((dirtyBits & RIGID_BIT) == 0) &&
((type & RIGID) != 0)) {
rot[0] = mat[0];
rot[1] = mat[1];
rot[2] = mat[2];
rot[3] = mat[4];
rot[4] = mat[5];
rot[5] = mat[6];
rot[6] = mat[8];
rot[7] = mat[9];
rot[8] = mat[10];
scales[0] = scales[1] = scales[2] = 1;
dirtyBits &= (~ROTATION_BIT | ~SCALE_BIT);
return;
}
double s = Math.sqrt(mat[0]*mat[0] + mat[4]*mat[4] + mat[8]*mat[8]);
if (s == 0) {
compute_svd(this, scales, rot);
return;
}
scales[0] = scales[1] = scales[2] = s;
s = 1/s;
rot[0] = mat[0]*s;
rot[1] = mat[1]*s;
rot[2] = mat[2]*s;
rot[3] = mat[4]*s;
rot[4] = mat[5]*s;
rot[5] = mat[6]*s;
rot[6] = mat[8]*s;
rot[7] = mat[9]*s;
rot[8] = mat[10]*s;
dirtyBits &= (~ROTATION_BIT | ~SCALE_BIT);
return;
}
if (isOrtho()) {
double s;
scales[0] = Math.sqrt(mat[0]*mat[0] + mat[4]*mat[4] + mat[8]*mat[8]);
scales[1] = Math.sqrt(mat[1]*mat[1] + mat[5]*mat[5] + mat[9]*mat[9]);
scales[2] = Math.sqrt(mat[2]*mat[2] + mat[6]*mat[6] + mat[10]*mat[10]);
if ((scales[0] == 0) || (scales[1] == 0) || (scales[2] == 0)) {
compute_svd(this, scales, rot);
return;
}
s = 1/scales[0];
rot[0] = mat[0]*s;
rot[3] = mat[4]*s;
rot[6] = mat[8]*s;
s = 1/scales[1];
rot[1] = mat[1]*s;
rot[4] = mat[5]*s;
rot[7] = mat[9]*s;
s = 1/scales[2];
rot[2] = mat[2]*s;
rot[5] = mat[6]*s;
rot[8] = mat[10]*s;
dirtyBits &= (~ROTATION_BIT | ~SCALE_BIT);
return;
}
}
// fall back to use SVD decomposition
compute_svd(this, scales, rot);
dirtyBits &= ~ROTSCALESVD_DIRTY;
}
final void getRotation(Transform3D t) {
if ((dirtyBits & ROTATION_BIT)!= 0) {
computeScaleRotation(false);
}
t.mat[3] = t.mat[7] = t.mat[11] = t.mat[12] = t.mat[13] =
t.mat[14] = 0;
t.mat[15] = 1;
t.mat[0] = rot[0];
t.mat[1] = rot[1];
t.mat[2] = rot[2];
t.mat[4] = rot[3];
t.mat[5] = rot[4];
t.mat[6] = rot[5];
t.mat[8] = rot[6];
t.mat[9] = rot[7];
t.mat[10] = rot[8];
// Issue 253: set all dirty bits
t.dirtyBits = ALL_DIRTY;
}
// somehow CanvasViewCache will directly modify mat[]
// instead of calling ortho(). So we need to reset dirty bit
final void setOrthoDirtyBit() {
// Issue 253: set all dirty bits
dirtyBits = ALL_DIRTY;
type = 0;
}
// Fix for Issue 167 -- don't classify matrices with Infinity or NaN values
// as affine
private final boolean isInfOrNaN() {
// The following is a faster version of the check.
// Instead of 3 tests per array element (Double.isInfinite is 2 tests),
// for a total of 48 tests, we will do 16 multiplies and 1 test.
double d = 0.0;
for (int i = 0; i < 16; i++) {
d *= mat[i];
}
return d != 0.0;
}
// Fix for Issue 253
// Methods to check input parameters for Infinity or NaN values
private final boolean isInfOrNaN(Quat4f q) {
return (Float.isNaN(q.x) || Float.isInfinite(q.x) ||
Float.isNaN(q.y) || Float.isInfinite(q.y) ||
Float.isNaN(q.z) || Float.isInfinite(q.z) ||
Float.isNaN(q.w) || Float.isInfinite(q.w));
}
private boolean isInfOrNaN(Quat4d q) {
return (Double.isNaN(q.x) || Double.isInfinite(q.x) ||
Double.isNaN(q.y) || Double.isInfinite(q.y) ||
Double.isNaN(q.z) || Double.isInfinite(q.z) ||
Double.isNaN(q.w) || Double.isInfinite(q.w));
}
private boolean isInfOrNaN(AxisAngle4f a) {
return (Float.isNaN(a.x) || Float.isInfinite(a.x) ||
Float.isNaN(a.y) || Float.isInfinite(a.y) ||
Float.isNaN(a.z) || Float.isInfinite(a.z) ||
Float.isNaN(a.angle) || Float.isInfinite(a.angle));
}
private boolean isInfOrNaN(AxisAngle4d a) {
return (Double.isNaN(a.x) || Double.isInfinite(a.x) ||
Double.isNaN(a.y) || Double.isInfinite(a.y) ||
Double.isNaN(a.z) || Double.isInfinite(a.z) ||
Double.isNaN(a.angle) || Double.isInfinite(a.angle));
}
private boolean isInfOrNaN(double val) {
return Double.isNaN(val) || Double.isInfinite(val);
}
private boolean isInfOrNaN(Vector3f v) {
return (Float.isNaN(v.x) || Float.isInfinite(v.x) ||
Float.isNaN(v.y) || Float.isInfinite(v.y) ||
Float.isNaN(v.z) || Float.isInfinite(v.z));
}
private boolean isInfOrNaN(Vector3d v) {
return (Double.isNaN(v.x) || Double.isInfinite(v.x) ||
Double.isNaN(v.y) || Double.isInfinite(v.y) ||
Double.isNaN(v.z) || Double.isInfinite(v.z));
}
}