0001: /*
0002: * $RCSfile: Transform3D.java,v $
0003: *
0004: * Copyright 1996-2008 Sun Microsystems, Inc. All Rights Reserved.
0005: * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
0006: *
0007: * This code is free software; you can redistribute it and/or modify it
0008: * under the terms of the GNU General Public License version 2 only, as
0009: * published by the Free Software Foundation. Sun designates this
0010: * particular file as subject to the "Classpath" exception as provided
0011: * by Sun in the LICENSE file that accompanied this code.
0012: *
0013: * This code is distributed in the hope that it will be useful, but WITHOUT
0014: * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
0015: * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
0016: * version 2 for more details (a copy is included in the LICENSE file that
0017: * accompanied this code).
0018: *
0019: * You should have received a copy of the GNU General Public License version
0020: * 2 along with this work; if not, write to the Free Software Foundation,
0021: * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
0022: *
0023: * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
0024: * CA 95054 USA or visit www.sun.com if you need additional information or
0025: * have any questions.
0026: *
0027: * $Revision: 1.11 $
0028: * $Date: 2008/02/28 20:17:32 $
0029: * $State: Exp $
0030: */
0031:
0032: package javax.media.j3d;
0033:
0034: import javax.vecmath.*;
0035: import com.sun.j3d.internal.HashCodeUtil;
0036:
0037: /**
0038: * A generalized transform object represented internally as a 4x4
0039: * double-precision floating point matrix. The mathematical
0040: * representation is
0041: * row major, as in traditional matrix mathematics.
0042: * A Transform3D is used to perform translations, rotations, and
0043: * scaling and shear effects.<P>
0044: *
0045: * A transform has an associated type, and
0046: * all type classification is left to the Transform3D object.
0047: * A transform will typically have multiple types, unless it is a
0048: * general, unclassifiable matrix, in which case it won't be assigned
0049: * a type. <P>
0050: *
0051: * The Transform3D type is internally computed when the transform
0052: * object is constructed and updated any time it is modified. A
0053: * matrix will typically have multiple types. For example, the type
0054: * associated with an identity matrix is the result of ORing all of
0055: * the types, except for ZERO and NEGATIVE_DETERMINANT, together.
0056: * There are public methods available to get the ORed type of the
0057: * transformation, the sign of the determinant, and the least
0058: * general matrix type. The matrix type flags are defined as
0059: * follows:<P>
0060: * <UL>
0061: * <LI>ZERO - zero matrix. All of the elements in the matrix
0062: * have the value 0.</LI><P>
0063: * <LI>IDENTITY - identity matrix. A matrix with ones on its
0064: * main diagonal and zeros every where else.</LI><P>
0065: * <LI>SCALE - the matrix is a uniform scale matrix - there are
0066: * no rotational or translation components.</LI><P>
0067: * <LI>ORTHOGONAL - the four row vectors that make up an orthogonal
0068: * matrix form a basis, meaning that they are mutually orthogonal.
0069: * The scale is unity and there are no translation components.</LI><P>
0070: * <LI>RIGID - the upper 3 X 3 of the matrix is orthogonal, and
0071: * there is a translation component-the scale is unity.</LI><P>
0072: * <LI>CONGRUENT - this is an angle- and length-preserving matrix,
0073: * meaning that it can translate, rotate, and reflect about an axis,
0074: * and scale by an amount that is uniform in all directions. These
0075: * operations preserve the distance between any two points, and the
0076: * angle between any two intersecting lines.</LI><P>
0077: * <LI>AFFINE - an affine matrix can translate, rotate, reflect,
0078: * scale anisotropically, and shear. Lines remain straight, and parallel
0079: * lines remain parallel, but the angle between intersecting lines can
0080: * change.</LI><P>
0081: * </UL>
0082: * A matrix is also classified by the sign of its determinant:<P>
0083: * <UL>
0084: * NEGATIVE_DETERMINANT - this matrix has a negative determinant.
0085: * An orthogonal matrix with a positive determinant is a rotation
0086: * matrix. An orthogonal matrix with a negative determinant is a
0087: * reflection and rotation matrix.<P></UL>
0088: * The Java 3D model for 4 X 4 transformations is:<P>
0089: * <UL><pre>
0090: * [ m00 m01 m02 m03 ] [ x ] [ x' ]
0091: * [ m10 m11 m12 m13 ] . [ y ] = [ y' ]
0092: * [ m20 m21 m22 m23 ] [ z ] [ z' ]
0093: * [ m30 m31 m32 m33 ] [ w ] [ w' ]
0094: *
0095: * x' = m00 . x+m01 . y+m02 . z+m03 . w
0096: * y' = m10 . x+m11 . y+m12 . z+m13 . w
0097: * z' = m20 . x+m21 . y+m22 . z+m23 . w
0098: * w' = m30 . x+m31 . y+m32 . z+m33 . w
0099: * </pre></ul><P>
0100: * Note: When transforming a Point3f or a Point3d, the input w is set to
0101: * 1. When transforming a Vector3f or Vector3d, the input w is set to 0.
0102: */
0103:
0104: public class Transform3D {
0105:
0106: double[] mat = new double[16];
0107: //double[] rot = new double[9];
0108: //double[] scales = new double[3];
0109: // allocate the memory only when it is needed. Following three places will allocate the memory,
0110: // void setScaleTranslation(), void computeScales() and void computeScaleRotation()
0111: double[] rot = null;
0112: double[] scales = null;
0113:
0114: // Unknown until lazy classification is done
0115: private int type = 0;
0116:
0117: // Dirty bit for classification, this is used
0118: // for classify()
0119: private static final int AFFINE_BIT = 0x01;
0120: private static final int ORTHO_BIT = 0x02;
0121: private static final int CONGRUENT_BIT = 0x04;
0122: private static final int RIGID_BIT = 0x08;
0123: private static final int CLASSIFY_BIT = 0x10;
0124:
0125: // this is used for scales[], rot[]
0126: private static final int SCALE_BIT = 0x20;
0127: private static final int ROTATION_BIT = 0x40;
0128: // set when SVD renormalization is necessary
0129: private static final int SVD_BIT = 0x80;
0130:
0131: private static final int CLASSIFY_ALL_DIRTY = AFFINE_BIT
0132: | ORTHO_BIT | CONGRUENT_BIT | RIGID_BIT | CLASSIFY_BIT;
0133: private static final int ROTSCALESVD_DIRTY = SCALE_BIT
0134: | ROTATION_BIT | SVD_BIT;
0135: private static final int ALL_DIRTY = CLASSIFY_ALL_DIRTY
0136: | ROTSCALESVD_DIRTY;
0137:
0138: private int dirtyBits;
0139:
0140: boolean autoNormalize = false; // Don't auto normalize by default
0141: /*
0142: // reused temporaries for compute_svd
0143: private boolean svdAllocd =false;
0144: private double[] u1 = null;
0145: private double[] v1 = null;
0146: private double[] t1 = null; // used by both compute_svd and compute_qr
0147: private double[] t2 = null; // used by both compute_svd and compute_qr
0148: private double[] ts = null;
0149: private double[] svdTmp = null;
0150: private double[] svdRot = null;
0151: private double[] single_values = null;
0152: private double[] e = null;
0153: private double[] svdScales = null;
0154: // from svrReorder
0155: private int[] svdOut = null;
0156: private double[] svdMag = null;
0157:
0158: // from compute_qr
0159: private double[] cosl = null;
0160: private double[] cosr = null;
0161: private double[] sinl = null;
0162: private double[] sinr = null;
0163: private double[] qr_m = null;
0164: */
0165:
0166: private static final double EPS = 1.110223024E-16;
0167:
0168: static final double EPSILON = 1.0e-10;
0169: static final double EPSILON_ABSOLUTE = 1.0e-5;
0170: static final double EPSILON_RELATIVE = 1.0e-4;
0171: /**
0172: * A zero matrix.
0173: */
0174: public static final int ZERO = 0x01;
0175:
0176: /**
0177: * An identity matrix.
0178: */
0179: public static final int IDENTITY = 0x02;
0180:
0181: /**
0182: * A Uniform scale matrix with no translation or other
0183: * off-diagonal components.
0184: */
0185: public static final int SCALE = 0x04;
0186:
0187: /**
0188: * A translation-only matrix with ones on the diagonal.
0189: *
0190: */
0191: public static final int TRANSLATION = 0x08;
0192:
0193: /**
0194: * The four row vectors that make up an orthogonal matrix form a basis,
0195: * meaning that they are mutually orthogonal; an orthogonal matrix with
0196: * positive determinant is a pure rotation matrix; a negative
0197: * determinant indicates a rotation and a reflection.
0198: */
0199: public static final int ORTHOGONAL = 0x10;
0200:
0201: /**
0202: * This matrix is a rotation and a translation with unity scale;
0203: * The upper 3x3 of the matrix is orthogonal, and there is a
0204: * translation component.
0205: */
0206: public static final int RIGID = 0x20;
0207:
0208: /**
0209: * This is an angle and length preserving matrix, meaning that it
0210: * can translate, rotate, and reflect
0211: * about an axis, and scale by an amount that is uniform in all directions.
0212: * These operations preserve the distance between any two points and the
0213: * angle between any two intersecting lines.
0214: */
0215: public static final int CONGRUENT = 0x40;
0216:
0217: /**
0218: * An affine matrix can translate, rotate, reflect, scale anisotropically,
0219: * and shear. Lines remain straight, and parallel lines remain parallel,
0220: * but the angle between intersecting lines can change. In order for a
0221: * transform to be classified as affine, the 4th row must be: [0, 0, 0, 1].
0222: */
0223: public static final int AFFINE = 0x80;
0224:
0225: /**
0226: * This matrix has a negative determinant; an orthogonal matrix with
0227: * a positive determinant is a rotation matrix; an orthogonal matrix
0228: * with a negative determinant is a reflection and rotation matrix.
0229: */
0230: public static final int NEGATIVE_DETERMINANT = 0x100;
0231:
0232: /**
0233: * The upper 3x3 column vectors that make up an orthogonal
0234: * matrix form a basis meaning that they are mutually orthogonal.
0235: * It can have non-uniform or zero x/y/z scale as long as
0236: * the dot product of any two column is zero.
0237: * This one is used by Java3D internal only and should not
0238: * expose to the user.
0239: */
0240: private static final int ORTHO = 0x40000000;
0241:
0242: /**
0243: * Constructs and initializes a transform from the 4 x 4 matrix. The
0244: * type of the constructed transform will be classified automatically.
0245: * @param m1 the 4 x 4 transformation matrix
0246: */
0247: public Transform3D(Matrix4f m1) {
0248: set(m1);
0249: }
0250:
0251: /**
0252: * Constructs and initializes a transform from the 4 x 4 matrix. The
0253: * type of the constructed transform will be classified automatically.
0254: * @param m1 the 4 x 4 transformation matrix
0255: */
0256: public Transform3D(Matrix4d m1) {
0257: set(m1);
0258: }
0259:
0260: /**
0261: * Constructs and initializes a transform from the Transform3D object.
0262: * @param t1 the transformation object to be copied
0263: */
0264: public Transform3D(Transform3D t1) {
0265: set(t1);
0266: }
0267:
0268: /**
0269: * Constructs and initializes a transform to the identity matrix.
0270: */
0271: public Transform3D() {
0272: setIdentity(); // this will also classify the matrix
0273: }
0274:
0275: /**
0276: * Constructs and initializes a transform from the float array of
0277: * length 16; the top row of the matrix is initialized to the first
0278: * four elements of the array, and so on. The type of the transform
0279: * object is classified internally.
0280: * @param matrix a float array of 16
0281: */
0282: public Transform3D(float[] matrix) {
0283: set(matrix);
0284: }
0285:
0286: /**
0287: * Constructs and initializes a transform from the double precision array
0288: * of length 16; the top row of the matrix is initialized to the first
0289: * four elements of the array, and so on. The type of the transform is
0290: * classified internally.
0291: * @param matrix a float array of 16
0292: */
0293: public Transform3D(double[] matrix) {
0294: set(matrix);
0295: }
0296:
0297: /**
0298: * Constructs and initializes a transform from the quaternion,
0299: * translation, and scale values. The scale is applied only to the
0300: * rotational components of the matrix (upper 3 x 3) and not to the
0301: * translational components of the matrix.
0302: * @param q1 the quaternion value representing the rotational component
0303: * @param t1 the translational component of the matrix
0304: * @param s the scale value applied to the rotational components
0305: */
0306: public Transform3D(Quat4d q1, Vector3d t1, double s) {
0307: set(q1, t1, s);
0308: }
0309:
0310: /**
0311: * Constructs and initializes a transform from the quaternion,
0312: * translation, and scale values. The scale is applied only to the
0313: * rotational components of the matrix (upper 3 x 3) and not to the
0314: * translational components of the matrix.
0315: * @param q1 the quaternion value representing the rotational component
0316: * @param t1 the translational component of the matrix
0317: * @param s the scale value applied to the rotational components
0318: */
0319: public Transform3D(Quat4f q1, Vector3d t1, double s) {
0320: set(q1, t1, s);
0321: }
0322:
0323: /**
0324: * Constructs and initializes a transform from the quaternion,
0325: * translation, and scale values. The scale is applied only to the
0326: * rotational components of the matrix (upper 3 x 3) and not to the
0327: * translational components of the matrix.
0328: * @param q1 the quaternion value representing the rotational component
0329: * @param t1 the translational component of the matrix
0330: * @param s the scale value applied to the rotational components
0331: */
0332: public Transform3D(Quat4f q1, Vector3f t1, float s) {
0333: set(q1, t1, s);
0334: }
0335:
0336: /**
0337: * Constructs a transform and initializes it to the upper 4 x 4
0338: * of the GMatrix argument. If the parameter matrix is
0339: * smaller than 4 x 4, the remaining elements in the transform matrix are
0340: * assigned to zero.
0341: * @param m1 the GMatrix
0342: */
0343: public Transform3D(GMatrix m1) {
0344: set(m1);
0345: }
0346:
0347: /**
0348: * Constructs and initializes a transform from the rotation matrix,
0349: * translation, and scale values. The scale is applied only to the
0350: * rotational component of the matrix (upper 3x3) and not to the
0351: * translational component of the matrix.
0352: * @param m1 the rotation matrix representing the rotational component
0353: * @param t1 the translational component of the matrix
0354: * @param s the scale value applied to the rotational components
0355: */
0356: public Transform3D(Matrix3f m1, Vector3d t1, double s) {
0357: set(m1, t1, s);
0358: }
0359:
0360: /**
0361: * Constructs and initializes a transform from the rotation matrix,
0362: * translation, and scale values. The scale is applied only to the
0363: * rotational components of the matrix (upper 3x3) and not to the
0364: * translational components of the matrix.
0365: * @param m1 the rotation matrix representing the rotational component
0366: * @param t1 the translational component of the matrix
0367: * @param s the scale value applied to the rotational components
0368: */
0369: public Transform3D(Matrix3d m1, Vector3d t1, double s) {
0370: set(m1, t1, s);
0371: }
0372:
0373: /**
0374: * Constructs and initializes a transform from the rotation matrix,
0375: * translation, and scale values. The scale is applied only to the
0376: * rotational components of the matrix (upper 3x3) and not to the
0377: * translational components of the matrix.
0378: * @param m1 the rotation matrix representing the rotational component
0379: * @param t1 the translational component of the matrix
0380: * @param s the scale value applied to the rotational components
0381: */
0382: public Transform3D(Matrix3f m1, Vector3f t1, float s) {
0383: set(m1, t1, s);
0384: }
0385:
0386: /**
0387: * Returns the type of this matrix as an or'ed bitmask of
0388: * of all of the type classifications to which it belongs.
0389: * @return or'ed bitmask of all of the type classifications
0390: * of this transform
0391: */
0392: public final int getType() {
0393: if ((dirtyBits & CLASSIFY_BIT) != 0) {
0394: classify();
0395: }
0396: // clear ORTHO bit which only use internally
0397: return (type & ~ORTHO);
0398: }
0399:
0400: // True if type is ORTHO
0401: // Since ORTHO didn't take into account the last row.
0402: final boolean isOrtho() {
0403: if ((dirtyBits & ORTHO_BIT) != 0) {
0404: if ((almostZero(mat[0] * mat[2] + mat[4] * mat[6] + mat[8]
0405: * mat[10])
0406: && almostZero(mat[0] * mat[1] + mat[4] * mat[5]
0407: + mat[8] * mat[9]) && almostZero(mat[1]
0408: * mat[2] + mat[5] * mat[6] + mat[9] * mat[10]))) {
0409: type |= ORTHO;
0410: dirtyBits &= ~ORTHO_BIT;
0411: return true;
0412: } else {
0413: type &= ~ORTHO;
0414: dirtyBits &= ~ORTHO_BIT;
0415: return false;
0416: }
0417: }
0418: return ((type & ORTHO) != 0);
0419: }
0420:
0421: final boolean isCongruent() {
0422: if ((dirtyBits & CONGRUENT_BIT) != 0) {
0423: // This will also classify AFFINE
0424: classifyRigid();
0425: }
0426: return ((type & CONGRUENT) != 0);
0427: }
0428:
0429: final boolean isAffine() {
0430: if ((dirtyBits & AFFINE_BIT) != 0) {
0431: classifyAffine();
0432: }
0433: return ((type & AFFINE) != 0);
0434: }
0435:
0436: final boolean isRigid() {
0437: if ((dirtyBits & RIGID_BIT) != 0) {
0438:
0439: // This will also classify AFFINE & CONGRUENT
0440: if ((dirtyBits & CONGRUENT_BIT) != 0) {
0441: classifyRigid();
0442: } else {
0443:
0444: if ((type & CONGRUENT) != 0) {
0445: // Matrix is Congruent, need only
0446: // to check scale
0447: double s;
0448: if ((dirtyBits & SCALE_BIT) != 0) {
0449: s = mat[0] * mat[0] + mat[4] * mat[4] + mat[8]
0450: * mat[8];
0451: // Note that
0452: // scales[0] = sqrt(s);
0453: // but since sqrt(1) = 1,
0454: // we don't need to do s = sqrt(s) here.
0455: } else {
0456: if (scales == null)
0457: scales = new double[3];
0458: s = scales[0];
0459: }
0460: if (almostOne(s)) {
0461: type |= RIGID;
0462: } else {
0463: type &= ~RIGID;
0464: }
0465: } else {
0466: // Not even congruent, so isRigid must be false
0467: type &= ~RIGID;
0468: }
0469: dirtyBits &= ~RIGID_BIT;
0470: }
0471: }
0472: return ((type & RIGID) != 0);
0473: }
0474:
0475: /**
0476: * Returns the least general type of this matrix; the order of
0477: * generality from least to most is: ZERO, IDENTITY,
0478: * SCALE/TRANSLATION, ORTHOGONAL, RIGID, CONGRUENT, AFFINE.
0479: * If the matrix is ORTHOGONAL, calling the method
0480: * getDeterminantSign() will yield more information.
0481: * @return the least general matrix type
0482: */
0483: public final int getBestType() {
0484: getType(); // force classify if necessary
0485:
0486: if ((type & ZERO) != 0)
0487: return ZERO;
0488: if ((type & IDENTITY) != 0)
0489: return IDENTITY;
0490: if ((type & SCALE) != 0)
0491: return SCALE;
0492: if ((type & TRANSLATION) != 0)
0493: return TRANSLATION;
0494: if ((type & ORTHOGONAL) != 0)
0495: return ORTHOGONAL;
0496: if ((type & RIGID) != 0)
0497: return RIGID;
0498: if ((type & CONGRUENT) != 0)
0499: return CONGRUENT;
0500: if ((type & AFFINE) != 0)
0501: return AFFINE;
0502: if ((type & NEGATIVE_DETERMINANT) != 0)
0503: return NEGATIVE_DETERMINANT;
0504: return 0;
0505: }
0506:
0507: /*
0508: private void print_type() {
0509: if ((type & ZERO) > 0 ) System.err.print(" ZERO");
0510: if ((type & IDENTITY) > 0 ) System.err.print(" IDENTITY");
0511: if ((type & SCALE) > 0 ) System.err.print(" SCALE");
0512: if ((type & TRANSLATION) > 0 ) System.err.print(" TRANSLATION");
0513: if ((type & ORTHOGONAL) > 0 ) System.err.print(" ORTHOGONAL");
0514: if ((type & RIGID) > 0 ) System.err.print(" RIGID");
0515: if ((type & CONGRUENT) > 0 ) System.err.print(" CONGRUENT");
0516: if ((type & AFFINE) > 0 ) System.err.print(" AFFINE");
0517: if ((type & NEGATIVE_DETERMINANT) > 0 ) System.err.print(" NEGATIVE_DETERMINANT");
0518: }
0519: */
0520:
0521: /**
0522: * Returns the sign of the determinant of this matrix; a return value
0523: * of true indicates a non-negative determinant; a return value of false
0524: * indicates a negative determinant. A value of true will be returned if
0525: * the determinant is NaN. In general, an orthogonal matrix
0526: * with a positive determinant is a pure rotation matrix; an orthogonal
0527: * matrix with a negative determinant is a both a rotation and a
0528: * reflection matrix.
0529: * @return determinant sign : true means non-negative, false means negative
0530: */
0531: public final boolean getDeterminantSign() {
0532: double det = determinant();
0533: if (Double.isNaN(det)) {
0534: return true;
0535: }
0536: return det >= 0;
0537: }
0538:
0539: /**
0540: * Sets a flag that enables or disables automatic SVD
0541: * normalization. If this flag is enabled, an automatic SVD
0542: * normalization of the rotational components (upper 3x3) of this
0543: * matrix is done after every subsequent matrix operation that
0544: * modifies this matrix. This is functionally equivalent to
0545: * calling normalize() after every subsequent call, but may be
0546: * less computationally expensive.
0547: * The default value for this parameter is false.
0548: * @param autoNormalize the boolean state of auto normalization
0549: */
0550: public final void setAutoNormalize(boolean autoNormalize) {
0551: this .autoNormalize = autoNormalize;
0552:
0553: if (autoNormalize) {
0554: normalize();
0555: }
0556: }
0557:
0558: /**
0559: * Returns the state of auto-normalization.
0560: * @return boolean state of auto-normalization
0561: */
0562: public final boolean getAutoNormalize() {
0563: return this .autoNormalize;
0564: }
0565:
0566: /**
0567: * Transforms the point parameter with this transform and
0568: * places the result into pointOut. The fourth element of the
0569: * point input paramter is assumed to be one.
0570: * @param point the input point to be transformed
0571: * @param pointOut the transformed point
0572: */
0573: void transform(Point3d point, Point4d pointOut) {
0574:
0575: pointOut.x = mat[0] * point.x + mat[1] * point.y + mat[2]
0576: * point.z + mat[3];
0577: pointOut.y = mat[4] * point.x + mat[5] * point.y + mat[6]
0578: * point.z + mat[7];
0579: pointOut.z = mat[8] * point.x + mat[9] * point.y + mat[10]
0580: * point.z + mat[11];
0581: pointOut.w = mat[12] * point.x + mat[13] * point.y + mat[14]
0582: * point.z + mat[15];
0583: }
0584:
0585: private static final boolean almostZero(double a) {
0586: return ((a < EPSILON_ABSOLUTE) && (a > -EPSILON_ABSOLUTE));
0587: }
0588:
0589: private static final boolean almostOne(double a) {
0590: return ((a < 1 + EPSILON_ABSOLUTE) && (a > 1 - EPSILON_ABSOLUTE));
0591: }
0592:
0593: private static final boolean almostEqual(double a, double b) {
0594: double diff = a - b;
0595:
0596: if (diff >= 0) {
0597: if (diff < EPSILON) {
0598: return true;
0599: }
0600: // a > b
0601: if ((b > 0) || (a > -b)) {
0602: return (diff < EPSILON_RELATIVE * a);
0603: } else {
0604: return (diff < -EPSILON_RELATIVE * b);
0605: }
0606:
0607: } else {
0608: if (diff > -EPSILON) {
0609: return true;
0610: }
0611: // a < b
0612: if ((b < 0) || (-a > b)) {
0613: return (diff > EPSILON_RELATIVE * a);
0614: } else {
0615: return (diff > -EPSILON_RELATIVE * b);
0616: }
0617: }
0618: }
0619:
0620: private final void classifyAffine() {
0621: if (!isInfOrNaN() && almostZero(mat[12]) && almostZero(mat[13])
0622: && almostZero(mat[14]) && almostOne(mat[15])) {
0623: type |= AFFINE;
0624: } else {
0625: type &= ~AFFINE;
0626: }
0627: dirtyBits &= ~AFFINE_BIT;
0628: }
0629:
0630: // same amount of work to classify rigid and congruent
0631: private final void classifyRigid() {
0632:
0633: if ((dirtyBits & AFFINE_BIT) != 0) {
0634: // should not touch ORTHO bit
0635: type &= ORTHO;
0636: classifyAffine();
0637: } else {
0638: // keep the affine bit if there is one
0639: // and clear the others (CONGRUENT/RIGID) bit
0640: type &= (ORTHO | AFFINE);
0641: }
0642:
0643: if ((type & AFFINE) != 0) {
0644: // checking orthogonal condition
0645: if (isOrtho()) {
0646: if ((dirtyBits & SCALE_BIT) != 0) {
0647: double s0 = mat[0] * mat[0] + mat[4] * mat[4]
0648: + mat[8] * mat[8];
0649: double s1 = mat[1] * mat[1] + mat[5] * mat[5]
0650: + mat[9] * mat[9];
0651: if (almostEqual(s0, s1)) {
0652: double s2 = mat[2] * mat[2] + mat[6] * mat[6]
0653: + mat[10] * mat[10];
0654: if (almostEqual(s2, s0)) {
0655: type |= CONGRUENT;
0656: // Note that scales[0] = sqrt(s0);
0657: if (almostOne(s0)) {
0658: type |= RIGID;
0659: }
0660: }
0661: }
0662: } else {
0663: if (scales == null)
0664: scales = new double[3];
0665:
0666: double s = scales[0];
0667: if (almostEqual(s, scales[1])
0668: && almostEqual(s, scales[2])) {
0669: type |= CONGRUENT;
0670: if (almostOne(s)) {
0671: type |= RIGID;
0672: }
0673: }
0674: }
0675: }
0676: }
0677: dirtyBits &= (~RIGID_BIT | ~CONGRUENT_BIT);
0678: }
0679:
0680: /**
0681: * Classifies a matrix.
0682: */
0683: private final void classify() {
0684:
0685: if ((dirtyBits & (RIGID_BIT | AFFINE_BIT | CONGRUENT_BIT)) != 0) {
0686: // Test for RIGID, CONGRUENT, AFFINE.
0687: classifyRigid();
0688: }
0689:
0690: // Test for ZERO, IDENTITY, SCALE, TRANSLATION,
0691: // ORTHOGONAL, NEGATIVE_DETERMINANT
0692: if ((type & AFFINE) != 0) {
0693: if ((type & CONGRUENT) != 0) {
0694: if ((type & RIGID) != 0) {
0695: if (zeroTranslation()) {
0696: type |= ORTHOGONAL;
0697: if (rotateZero()) {
0698: // mat[0], mat[5], mat[10] can be only be
0699: // 1 or -1 when reach here
0700: if ((mat[0] > 0) && (mat[5] > 0)
0701: && (mat[10] > 0)) {
0702: type |= IDENTITY | SCALE | TRANSLATION;
0703: }
0704: }
0705: } else {
0706: if (rotateZero()) {
0707: type |= TRANSLATION;
0708: }
0709: }
0710: } else {
0711: // uniform scale
0712: if (zeroTranslation() && rotateZero()) {
0713: type |= SCALE;
0714: }
0715: }
0716:
0717: }
0718: } else {
0719: // last row is not (0, 0, 0, 1)
0720: if (almostZero(mat[12]) && almostZero(mat[13])
0721: && almostZero(mat[14]) && almostZero(mat[15])
0722: && zeroTranslation() && rotateZero()
0723: && almostZero(mat[0]) && almostZero(mat[5])
0724: && almostZero(mat[10])) {
0725: type |= ZERO;
0726: }
0727: }
0728:
0729: if (!getDeterminantSign()) {
0730: type |= NEGATIVE_DETERMINANT;
0731: }
0732: dirtyBits &= ~CLASSIFY_BIT;
0733: }
0734:
0735: final boolean zeroTranslation() {
0736: return (almostZero(mat[3]) && almostZero(mat[7]) && almostZero(mat[11]));
0737: }
0738:
0739: final boolean rotateZero() {
0740: return (almostZero(mat[1]) && almostZero(mat[2])
0741: && almostZero(mat[4]) && almostZero(mat[6])
0742: && almostZero(mat[8]) && almostZero(mat[9]));
0743: }
0744:
0745: /**
0746: * Returns the matrix elements of this transform as a string.
0747: * @return the matrix elements of this transform
0748: */
0749: public String toString() {
0750: // also, print classification?
0751: return mat[0] + ", " + mat[1] + ", " + mat[2] + ", " + mat[3]
0752: + "\n" + mat[4] + ", " + mat[5] + ", " + mat[6] + ", "
0753: + mat[7] + "\n" + mat[8] + ", " + mat[9] + ", "
0754: + mat[10] + ", " + mat[11] + "\n" + mat[12] + ", "
0755: + mat[13] + ", " + mat[14] + ", " + mat[15] + "\n";
0756: }
0757:
0758: /**
0759: * Sets this transform to the identity matrix.
0760: */
0761: public final void setIdentity() {
0762: mat[0] = 1.0;
0763: mat[1] = 0.0;
0764: mat[2] = 0.0;
0765: mat[3] = 0.0;
0766: mat[4] = 0.0;
0767: mat[5] = 1.0;
0768: mat[6] = 0.0;
0769: mat[7] = 0.0;
0770: mat[8] = 0.0;
0771: mat[9] = 0.0;
0772: mat[10] = 1.0;
0773: mat[11] = 0.0;
0774: mat[12] = 0.0;
0775: mat[13] = 0.0;
0776: mat[14] = 0.0;
0777: mat[15] = 1.0;
0778: type = IDENTITY | SCALE | ORTHOGONAL | RIGID | CONGRUENT
0779: | AFFINE | TRANSLATION | ORTHO;
0780: dirtyBits = SCALE_BIT | ROTATION_BIT;
0781: // No need to set SVD_BIT
0782: }
0783:
0784: /**
0785: * Sets this transform to all zeros.
0786: */
0787: public final void setZero() {
0788: mat[0] = 0.0;
0789: mat[1] = 0.0;
0790: mat[2] = 0.0;
0791: mat[3] = 0.0;
0792: mat[4] = 0.0;
0793: mat[5] = 0.0;
0794: mat[6] = 0.0;
0795: mat[7] = 0.0;
0796: mat[8] = 0.0;
0797: mat[9] = 0.0;
0798: mat[10] = 0.0;
0799: mat[11] = 0.0;
0800: mat[12] = 0.0;
0801: mat[13] = 0.0;
0802: mat[14] = 0.0;
0803: mat[15] = 0.0;
0804:
0805: type = ZERO | ORTHO;
0806: dirtyBits = SCALE_BIT | ROTATION_BIT;
0807: }
0808:
0809: /**
0810: * Adds this transform to transform t1 and places the result into
0811: * this: this = this + t1.
0812: * @param t1 the transform to be added to this transform
0813: */
0814: public final void add(Transform3D t1) {
0815: for (int i = 0; i < 16; i++) {
0816: mat[i] += t1.mat[i];
0817: }
0818:
0819: dirtyBits = ALL_DIRTY;
0820:
0821: if (autoNormalize) {
0822: normalize();
0823: }
0824:
0825: }
0826:
0827: /**
0828: * Adds transforms t1 and t2 and places the result into this transform.
0829: * @param t1 the transform to be added
0830: * @param t2 the transform to be added
0831: */
0832: public final void add(Transform3D t1, Transform3D t2) {
0833: for (int i = 0; i < 16; i++) {
0834: mat[i] = t1.mat[i] + t2.mat[i];
0835: }
0836:
0837: dirtyBits = ALL_DIRTY;
0838:
0839: if (autoNormalize) {
0840: normalize();
0841: }
0842:
0843: }
0844:
0845: /**
0846: * Subtracts transform t1 from this transform and places the result
0847: * into this: this = this - t1.
0848: * @param t1 the transform to be subtracted from this transform
0849: */
0850: public final void sub(Transform3D t1) {
0851: for (int i = 0; i < 16; i++) {
0852: mat[i] -= t1.mat[i];
0853: }
0854:
0855: dirtyBits = ALL_DIRTY;
0856:
0857: if (autoNormalize) {
0858: normalize();
0859: }
0860: }
0861:
0862: /**
0863: * Subtracts transform t2 from transform t1 and places the result into
0864: * this: this = t1 - t2.
0865: * @param t1 the left transform
0866: * @param t2 the right transform
0867: */
0868: public final void sub(Transform3D t1, Transform3D t2) {
0869: for (int i = 0; i < 16; i++) {
0870: mat[i] = t1.mat[i] - t2.mat[i];
0871: }
0872:
0873: dirtyBits = ALL_DIRTY;
0874:
0875: if (autoNormalize) {
0876: normalize();
0877: }
0878:
0879: }
0880:
0881: /**
0882: * Transposes this matrix in place.
0883: */
0884: public final void transpose() {
0885: double temp;
0886:
0887: temp = mat[4];
0888: mat[4] = mat[1];
0889: mat[1] = temp;
0890:
0891: temp = mat[8];
0892: mat[8] = mat[2];
0893: mat[2] = temp;
0894:
0895: temp = mat[12];
0896: mat[12] = mat[3];
0897: mat[3] = temp;
0898:
0899: temp = mat[9];
0900: mat[9] = mat[6];
0901: mat[6] = temp;
0902:
0903: temp = mat[13];
0904: mat[13] = mat[7];
0905: mat[7] = temp;
0906:
0907: temp = mat[14];
0908: mat[14] = mat[11];
0909: mat[11] = temp;
0910:
0911: dirtyBits = ALL_DIRTY;
0912:
0913: if (autoNormalize) {
0914: normalize();
0915: }
0916: }
0917:
0918: /**
0919: * Transposes transform t1 and places the value into this transform.
0920: * The transform t1 is not modified.
0921: * @param t1 the transform whose transpose is placed into this transform
0922: */
0923: public final void transpose(Transform3D t1) {
0924:
0925: if (this != t1) {
0926: mat[0] = t1.mat[0];
0927: mat[1] = t1.mat[4];
0928: mat[2] = t1.mat[8];
0929: mat[3] = t1.mat[12];
0930: mat[4] = t1.mat[1];
0931: mat[5] = t1.mat[5];
0932: mat[6] = t1.mat[9];
0933: mat[7] = t1.mat[13];
0934: mat[8] = t1.mat[2];
0935: mat[9] = t1.mat[6];
0936: mat[10] = t1.mat[10];
0937: mat[11] = t1.mat[14];
0938: mat[12] = t1.mat[3];
0939: mat[13] = t1.mat[7];
0940: mat[14] = t1.mat[11];
0941: mat[15] = t1.mat[15];
0942:
0943: dirtyBits = ALL_DIRTY;
0944:
0945: if (autoNormalize) {
0946: normalize();
0947: }
0948: } else {
0949: this .transpose();
0950: }
0951:
0952: }
0953:
0954: /**
0955: * Sets the value of this transform to the matrix conversion of the
0956: * single precision quaternion argument; the non-rotational
0957: * components are set as if this were an identity matrix.
0958: * @param q1 the quaternion to be converted
0959: */
0960: public final void set(Quat4f q1) {
0961:
0962: mat[0] = (1.0f - 2.0f * q1.y * q1.y - 2.0f * q1.z * q1.z);
0963: mat[4] = (2.0f * (q1.x * q1.y + q1.w * q1.z));
0964: mat[8] = (2.0f * (q1.x * q1.z - q1.w * q1.y));
0965:
0966: mat[1] = (2.0f * (q1.x * q1.y - q1.w * q1.z));
0967: mat[5] = (1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.z * q1.z);
0968: mat[9] = (2.0f * (q1.y * q1.z + q1.w * q1.x));
0969:
0970: mat[2] = (2.0f * (q1.x * q1.z + q1.w * q1.y));
0971: mat[6] = (2.0f * (q1.y * q1.z - q1.w * q1.x));
0972: mat[10] = (1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.y * q1.y);
0973:
0974: mat[3] = 0.0;
0975: mat[7] = 0.0;
0976: mat[11] = 0.0;
0977:
0978: mat[12] = 0.0;
0979: mat[13] = 0.0;
0980: mat[14] = 0.0;
0981: mat[15] = 1.0;
0982:
0983: // Issue 253: set all dirty bits if input is infinity or NaN
0984: if (isInfOrNaN(q1)) {
0985: dirtyBits = ALL_DIRTY;
0986: return;
0987: }
0988:
0989: dirtyBits = CLASSIFY_BIT | SCALE_BIT | ROTATION_BIT;
0990: type = RIGID | CONGRUENT | AFFINE | ORTHO;
0991: }
0992:
0993: /**
0994: * Sets the value of this transform to the matrix conversion of the
0995: * double precision quaternion argument; the non-rotational
0996: * components are set as if this were an identity matrix.
0997: * @param q1 the quaternion to be converted
0998: */
0999: public final void set(Quat4d q1) {
1000:
1001: mat[0] = (1.0 - 2.0 * q1.y * q1.y - 2.0 * q1.z * q1.z);
1002: mat[4] = (2.0 * (q1.x * q1.y + q1.w * q1.z));
1003: mat[8] = (2.0 * (q1.x * q1.z - q1.w * q1.y));
1004:
1005: mat[1] = (2.0 * (q1.x * q1.y - q1.w * q1.z));
1006: mat[5] = (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.z * q1.z);
1007: mat[9] = (2.0 * (q1.y * q1.z + q1.w * q1.x));
1008:
1009: mat[2] = (2.0 * (q1.x * q1.z + q1.w * q1.y));
1010: mat[6] = (2.0 * (q1.y * q1.z - q1.w * q1.x));
1011: mat[10] = (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.y * q1.y);
1012:
1013: mat[3] = 0.0;
1014: mat[7] = 0.0;
1015: mat[11] = 0.0;
1016:
1017: mat[12] = 0.0;
1018: mat[13] = 0.0;
1019: mat[14] = 0.0;
1020: mat[15] = 1.0;
1021:
1022: // Issue 253: set all dirty bits if input is infinity or NaN
1023: if (isInfOrNaN(q1)) {
1024: dirtyBits = ALL_DIRTY;
1025: return;
1026: }
1027:
1028: dirtyBits = CLASSIFY_BIT | SCALE_BIT | ROTATION_BIT;
1029: type = RIGID | CONGRUENT | AFFINE | ORTHO;
1030: }
1031:
1032: /**
1033: * Sets the rotational component (upper 3x3) of this transform to the
1034: * matrix values in the double precision Matrix3d argument; the other
1035: * elements of this transform are unchanged; any pre-existing scale
1036: * will be preserved; the argument matrix m1 will be checked for proper
1037: * normalization when this transform is internally classified.
1038: * @param m1 the double precision 3x3 matrix
1039: */
1040: public final void setRotation(Matrix3d m1) {
1041:
1042: if ((dirtyBits & SCALE_BIT) != 0) {
1043: computeScales(false);
1044: }
1045:
1046: mat[0] = m1.m00 * scales[0];
1047: mat[1] = m1.m01 * scales[1];
1048: mat[2] = m1.m02 * scales[2];
1049: mat[4] = m1.m10 * scales[0];
1050: mat[5] = m1.m11 * scales[1];
1051: mat[6] = m1.m12 * scales[2];
1052: mat[8] = m1.m20 * scales[0];
1053: mat[9] = m1.m21 * scales[1];
1054: mat[10] = m1.m22 * scales[2];
1055:
1056: // Issue 253: set all dirty bits
1057: dirtyBits = ALL_DIRTY;
1058:
1059: if (autoNormalize) {
1060: // the matrix pass in may not normalize
1061: normalize();
1062: }
1063: }
1064:
1065: /**
1066: * Sets the rotational component (upper 3x3) of this transform to the
1067: * matrix values in the single precision Matrix3f argument; the other
1068: * elements of this transform are unchanged; any pre-existing scale
1069: * will be preserved; the argument matrix m1 will be checked for proper
1070: * normalization when this transform is internally classified.
1071: * @param m1 the single precision 3x3 matrix
1072: */
1073: public final void setRotation(Matrix3f m1) {
1074:
1075: if ((dirtyBits & SCALE_BIT) != 0) {
1076: computeScales(false);
1077: }
1078:
1079: mat[0] = m1.m00 * scales[0];
1080: mat[1] = m1.m01 * scales[1];
1081: mat[2] = m1.m02 * scales[2];
1082: mat[4] = m1.m10 * scales[0];
1083: mat[5] = m1.m11 * scales[1];
1084: mat[6] = m1.m12 * scales[2];
1085: mat[8] = m1.m20 * scales[0];
1086: mat[9] = m1.m21 * scales[1];
1087: mat[10] = m1.m22 * scales[2];
1088:
1089: // Issue 253: set all dirty bits
1090: dirtyBits = ALL_DIRTY;
1091:
1092: if (autoNormalize) {
1093: normalize();
1094: }
1095: }
1096:
1097: /**
1098: * Sets the rotational component (upper 3x3) of this transform to the
1099: * matrix equivalent values of the quaternion argument; the other
1100: * elements of this transform are unchanged; any pre-existing scale
1101: * in the transform is preserved.
1102: * @param q1 the quaternion that specifies the rotation
1103: */
1104: public final void setRotation(Quat4f q1) {
1105:
1106: if ((dirtyBits & SCALE_BIT) != 0) {
1107: computeScales(false);
1108: }
1109:
1110: mat[0] = (1.0 - 2.0 * q1.y * q1.y - 2.0 * q1.z * q1.z)
1111: * scales[0];
1112: mat[4] = (2.0 * (q1.x * q1.y + q1.w * q1.z)) * scales[0];
1113: mat[8] = (2.0 * (q1.x * q1.z - q1.w * q1.y)) * scales[0];
1114:
1115: mat[1] = (2.0 * (q1.x * q1.y - q1.w * q1.z)) * scales[1];
1116: mat[5] = (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.z * q1.z)
1117: * scales[1];
1118: mat[9] = (2.0 * (q1.y * q1.z + q1.w * q1.x)) * scales[1];
1119:
1120: mat[2] = (2.0 * (q1.x * q1.z + q1.w * q1.y)) * scales[2];
1121: mat[6] = (2.0 * (q1.y * q1.z - q1.w * q1.x)) * scales[2];
1122: mat[10] = (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.y * q1.y)
1123: * scales[2];
1124:
1125: // Issue 253: set all dirty bits if input is infinity or NaN
1126: if (isInfOrNaN(q1)) {
1127: dirtyBits = ALL_DIRTY;
1128: return;
1129: }
1130:
1131: dirtyBits |= CLASSIFY_BIT | ROTATION_BIT;
1132: dirtyBits &= ~ORTHO_BIT;
1133: type |= ORTHO;
1134: type &= ~(ORTHOGONAL | IDENTITY | SCALE | TRANSLATION | SCALE | ZERO);
1135: }
1136:
1137: /**
1138: * Sets the rotational component (upper 3x3) of this transform to the
1139: * matrix equivalent values of the quaternion argument; the other
1140: * elements of this transform are unchanged; any pre-existing scale
1141: * in the transform is preserved.
1142: * @param q1 the quaternion that specifies the rotation
1143: */
1144: public final void setRotation(Quat4d q1) {
1145:
1146: if ((dirtyBits & SCALE_BIT) != 0) {
1147: computeScales(false);
1148: }
1149:
1150: mat[0] = (1.0 - 2.0 * q1.y * q1.y - 2.0 * q1.z * q1.z)
1151: * scales[0];
1152: mat[4] = (2.0 * (q1.x * q1.y + q1.w * q1.z)) * scales[0];
1153: mat[8] = (2.0 * (q1.x * q1.z - q1.w * q1.y)) * scales[0];
1154:
1155: mat[1] = (2.0 * (q1.x * q1.y - q1.w * q1.z)) * scales[1];
1156: mat[5] = (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.z * q1.z)
1157: * scales[1];
1158: mat[9] = (2.0 * (q1.y * q1.z + q1.w * q1.x)) * scales[1];
1159:
1160: mat[2] = (2.0 * (q1.x * q1.z + q1.w * q1.y)) * scales[2];
1161: mat[6] = (2.0 * (q1.y * q1.z - q1.w * q1.x)) * scales[2];
1162: mat[10] = (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.y * q1.y)
1163: * scales[2];
1164:
1165: // Issue 253: set all dirty bits if input is infinity or NaN
1166: if (isInfOrNaN(q1)) {
1167: dirtyBits = ALL_DIRTY;
1168: return;
1169: }
1170:
1171: dirtyBits |= CLASSIFY_BIT | ROTATION_BIT;
1172: dirtyBits &= ~ORTHO_BIT;
1173: type |= ORTHO;
1174: type &= ~(ORTHOGONAL | IDENTITY | SCALE | TRANSLATION | SCALE | ZERO);
1175: }
1176:
1177: /**
1178: * Sets the value of this transform to the matrix conversion
1179: * of the single precision axis-angle argument; all of the matrix
1180: * values are modified.
1181: * @param a1 the axis-angle to be converted (x, y, z, angle)
1182: */
1183: public final void set(AxisAngle4f a1) {
1184:
1185: double mag = Math.sqrt(a1.x * a1.x + a1.y * a1.y + a1.z * a1.z);
1186:
1187: if (almostZero(mag)) {
1188: setIdentity();
1189: } else {
1190: mag = 1.0 / mag;
1191: double ax = a1.x * mag;
1192: double ay = a1.y * mag;
1193: double az = a1.z * mag;
1194:
1195: double sinTheta = Math.sin((double) a1.angle);
1196: double cosTheta = Math.cos((double) a1.angle);
1197: double t = 1.0 - cosTheta;
1198:
1199: double xz = ax * az;
1200: double xy = ax * ay;
1201: double yz = ay * az;
1202:
1203: mat[0] = t * ax * ax + cosTheta;
1204: mat[1] = t * xy - sinTheta * az;
1205: mat[2] = t * xz + sinTheta * ay;
1206: mat[3] = 0.0;
1207:
1208: mat[4] = t * xy + sinTheta * az;
1209: mat[5] = t * ay * ay + cosTheta;
1210: mat[6] = t * yz - sinTheta * ax;
1211: mat[7] = 0.0;
1212:
1213: mat[8] = t * xz - sinTheta * ay;
1214: mat[9] = t * yz + sinTheta * ax;
1215: mat[10] = t * az * az + cosTheta;
1216: mat[11] = 0.0;
1217:
1218: mat[12] = 0.0;
1219: mat[13] = 0.0;
1220: mat[14] = 0.0;
1221: mat[15] = 1.0;
1222:
1223: // Issue 253: set all dirty bits if input is infinity or NaN
1224: if (isInfOrNaN(a1)) {
1225: dirtyBits = ALL_DIRTY;
1226: return;
1227: }
1228:
1229: type = CONGRUENT | AFFINE | RIGID | ORTHO;
1230: dirtyBits = CLASSIFY_BIT | ROTATION_BIT | SCALE_BIT;
1231: }
1232: }
1233:
1234: /**
1235: * Sets the value of this transform to the matrix conversion
1236: * of the double precision axis-angle argument; all of the matrix
1237: * values are modified.
1238: * @param a1 the axis-angle to be converted (x, y, z, angle)
1239: */
1240: public final void set(AxisAngle4d a1) {
1241:
1242: double mag = Math.sqrt(a1.x * a1.x + a1.y * a1.y + a1.z * a1.z);
1243:
1244: if (almostZero(mag)) {
1245: setIdentity();
1246: } else {
1247: mag = 1.0 / mag;
1248: double ax = a1.x * mag;
1249: double ay = a1.y * mag;
1250: double az = a1.z * mag;
1251:
1252: double sinTheta = Math.sin(a1.angle);
1253: double cosTheta = Math.cos(a1.angle);
1254: double t = 1.0 - cosTheta;
1255:
1256: double xz = ax * az;
1257: double xy = ax * ay;
1258: double yz = ay * az;
1259:
1260: mat[0] = t * ax * ax + cosTheta;
1261: mat[1] = t * xy - sinTheta * az;
1262: mat[2] = t * xz + sinTheta * ay;
1263: mat[3] = 0.0;
1264:
1265: mat[4] = t * xy + sinTheta * az;
1266: mat[5] = t * ay * ay + cosTheta;
1267: mat[6] = t * yz - sinTheta * ax;
1268: mat[7] = 0.0;
1269:
1270: mat[8] = t * xz - sinTheta * ay;
1271: mat[9] = t * yz + sinTheta * ax;
1272: mat[10] = t * az * az + cosTheta;
1273: mat[11] = 0.0;
1274:
1275: mat[12] = 0.0;
1276: mat[13] = 0.0;
1277: mat[14] = 0.0;
1278: mat[15] = 1.0;
1279:
1280: // Issue 253: set all dirty bits if input is infinity or NaN
1281: if (isInfOrNaN(a1)) {
1282: dirtyBits = ALL_DIRTY;
1283: return;
1284: }
1285:
1286: type = CONGRUENT | AFFINE | RIGID | ORTHO;
1287: dirtyBits = CLASSIFY_BIT | ROTATION_BIT | SCALE_BIT;
1288: }
1289: }
1290:
1291: /**
1292: * Sets the rotational component (upper 3x3) of this transform to the
1293: * matrix equivalent values of the axis-angle argument; the other
1294: * elements of this transform are unchanged; any pre-existing scale
1295: * in the transform is preserved.
1296: * @param a1 the axis-angle to be converted (x, y, z, angle)
1297: */
1298: public final void setRotation(AxisAngle4d a1) {
1299:
1300: if ((dirtyBits & SCALE_BIT) != 0) {
1301: computeScales(false);
1302: }
1303:
1304: double mag = Math.sqrt(a1.x * a1.x + a1.y * a1.y + a1.z * a1.z);
1305:
1306: if (almostZero(mag)) {
1307: mat[0] = scales[0];
1308: mat[1] = 0.0;
1309: mat[2] = 0.0;
1310: mat[4] = 0.0;
1311: mat[5] = scales[1];
1312: mat[6] = 0.0;
1313: mat[8] = 0.0;
1314: mat[9] = 0.0;
1315: mat[10] = scales[2];
1316: } else {
1317: mag = 1.0 / mag;
1318: double ax = a1.x * mag;
1319: double ay = a1.y * mag;
1320: double az = a1.z * mag;
1321:
1322: double sinTheta = Math.sin(a1.angle);
1323: double cosTheta = Math.cos(a1.angle);
1324: double t = 1.0 - cosTheta;
1325:
1326: double xz = ax * az;
1327: double xy = ax * ay;
1328: double yz = ay * az;
1329:
1330: mat[0] = (t * ax * ax + cosTheta) * scales[0];
1331: mat[1] = (t * xy - sinTheta * az) * scales[1];
1332: mat[2] = (t * xz + sinTheta * ay) * scales[2];
1333:
1334: mat[4] = (t * xy + sinTheta * az) * scales[0];
1335: mat[5] = (t * ay * ay + cosTheta) * scales[1];
1336: mat[6] = (t * yz - sinTheta * ax) * scales[2];
1337:
1338: mat[8] = (t * xz - sinTheta * ay) * scales[0];
1339: mat[9] = (t * yz + sinTheta * ax) * scales[1];
1340: mat[10] = (t * az * az + cosTheta) * scales[2];
1341: }
1342:
1343: // Issue 253: set all dirty bits if input is infinity or NaN
1344: if (isInfOrNaN(a1)) {
1345: dirtyBits = ALL_DIRTY;
1346: return;
1347: }
1348:
1349: // Rigid remain rigid, congruent remain congruent after
1350: // set rotation
1351: dirtyBits |= CLASSIFY_BIT | ROTATION_BIT;
1352: dirtyBits &= ~ORTHO_BIT;
1353: type |= ORTHO;
1354: type &= ~(ORTHOGONAL | IDENTITY | SCALE | TRANSLATION | SCALE | ZERO);
1355: }
1356:
1357: /**
1358: * Sets the rotational component (upper 3x3) of this transform to the
1359: * matrix equivalent values of the axis-angle argument; the other
1360: * elements of this transform are unchanged; any pre-existing scale
1361: * in the transform is preserved.
1362: * @param a1 the axis-angle to be converted (x, y, z, angle)
1363: */
1364: public final void setRotation(AxisAngle4f a1) {
1365:
1366: if ((dirtyBits & SCALE_BIT) != 0) {
1367: computeScales(false);
1368: }
1369:
1370: double mag = Math.sqrt(a1.x * a1.x + a1.y * a1.y + a1.z * a1.z);
1371:
1372: if (almostZero(mag)) {
1373: mat[0] = scales[0];
1374: mat[1] = 0.0;
1375: mat[2] = 0.0;
1376: mat[4] = 0.0;
1377: mat[5] = scales[1];
1378: mat[6] = 0.0;
1379: mat[8] = 0.0;
1380: mat[9] = 0.0;
1381: mat[10] = scales[2];
1382: } else {
1383: mag = 1.0 / mag;
1384: double ax = a1.x * mag;
1385: double ay = a1.y * mag;
1386: double az = a1.z * mag;
1387:
1388: double sinTheta = Math.sin(a1.angle);
1389: double cosTheta = Math.cos(a1.angle);
1390: double t = 1.0 - cosTheta;
1391:
1392: double xz = ax * az;
1393: double xy = ax * ay;
1394: double yz = ay * az;
1395:
1396: mat[0] = (t * ax * ax + cosTheta) * scales[0];
1397: mat[1] = (t * xy - sinTheta * az) * scales[1];
1398: mat[2] = (t * xz + sinTheta * ay) * scales[2];
1399:
1400: mat[4] = (t * xy + sinTheta * az) * scales[0];
1401: mat[5] = (t * ay * ay + cosTheta) * scales[1];
1402: mat[6] = (t * yz - sinTheta * ax) * scales[2];
1403:
1404: mat[8] = (t * xz - sinTheta * ay) * scales[0];
1405: mat[9] = (t * yz + sinTheta * ax) * scales[1];
1406: mat[10] = (t * az * az + cosTheta) * scales[2];
1407: }
1408:
1409: // Issue 253: set all dirty bits if input is infinity or NaN
1410: if (isInfOrNaN(a1)) {
1411: dirtyBits = ALL_DIRTY;
1412: return;
1413: }
1414:
1415: // Rigid remain rigid, congruent remain congruent after
1416: // set rotation
1417: dirtyBits |= CLASSIFY_BIT | ROTATION_BIT;
1418: dirtyBits &= (~ORTHO_BIT | ~SVD_BIT);
1419: type |= ORTHO;
1420: type &= ~(ORTHOGONAL | IDENTITY | SCALE | TRANSLATION | SCALE | ZERO);
1421: }
1422:
1423: /**
1424: * Sets the value of this transform to a counter clockwise rotation
1425: * about the x axis. All of the non-rotational components are set as
1426: * if this were an identity matrix.
1427: * @param angle the angle to rotate about the X axis in radians
1428: */
1429: public void rotX(double angle) {
1430: double sinAngle = Math.sin(angle);
1431: double cosAngle = Math.cos(angle);
1432:
1433: mat[0] = 1.0;
1434: mat[1] = 0.0;
1435: mat[2] = 0.0;
1436: mat[3] = 0.0;
1437:
1438: mat[4] = 0.0;
1439: mat[5] = cosAngle;
1440: mat[6] = -sinAngle;
1441: mat[7] = 0.0;
1442:
1443: mat[8] = 0.0;
1444: mat[9] = sinAngle;
1445: mat[10] = cosAngle;
1446: mat[11] = 0.0;
1447:
1448: mat[12] = 0.0;
1449: mat[13] = 0.0;
1450: mat[14] = 0.0;
1451: mat[15] = 1.0;
1452:
1453: // Issue 253: set all dirty bits if input is infinity or NaN
1454: if (isInfOrNaN(angle)) {
1455: dirtyBits = ALL_DIRTY;
1456: return;
1457: }
1458:
1459: type = CONGRUENT | AFFINE | RIGID | ORTHO;
1460: dirtyBits = CLASSIFY_BIT | ROTATION_BIT | SCALE_BIT;
1461: }
1462:
1463: /**
1464: * Sets the value of this transform to a counter clockwise rotation about
1465: * the y axis. All of the non-rotational components are set as if this
1466: * were an identity matrix.
1467: * @param angle the angle to rotate about the Y axis in radians
1468: */
1469: public void rotY(double angle) {
1470: double sinAngle = Math.sin(angle);
1471: double cosAngle = Math.cos(angle);
1472:
1473: mat[0] = cosAngle;
1474: mat[1] = 0.0;
1475: mat[2] = sinAngle;
1476: mat[3] = 0.0;
1477:
1478: mat[4] = 0.0;
1479: mat[5] = 1.0;
1480: mat[6] = 0.0;
1481: mat[7] = 0.0;
1482:
1483: mat[8] = -sinAngle;
1484: mat[9] = 0.0;
1485: mat[10] = cosAngle;
1486: mat[11] = 0.0;
1487:
1488: mat[12] = 0.0;
1489: mat[13] = 0.0;
1490: mat[14] = 0.0;
1491: mat[15] = 1.0;
1492:
1493: // Issue 253: set all dirty bits if input is infinity or NaN
1494: if (isInfOrNaN(angle)) {
1495: dirtyBits = ALL_DIRTY;
1496: return;
1497: }
1498:
1499: type = CONGRUENT | AFFINE | RIGID | ORTHO;
1500: dirtyBits = CLASSIFY_BIT | ROTATION_BIT | SCALE_BIT;
1501: }
1502:
1503: /**
1504: * Sets the value of this transform to a counter clockwise rotation
1505: * about the z axis. All of the non-rotational components are set
1506: * as if this were an identity matrix.
1507: * @param angle the angle to rotate about the Z axis in radians
1508: */
1509: public void rotZ(double angle) {
1510: double sinAngle = Math.sin(angle);
1511: double cosAngle = Math.cos(angle);
1512:
1513: mat[0] = cosAngle;
1514: mat[1] = -sinAngle;
1515: mat[2] = 0.0;
1516: mat[3] = 0.0;
1517:
1518: mat[4] = sinAngle;
1519: mat[5] = cosAngle;
1520: mat[6] = 0.0;
1521: mat[7] = 0.0;
1522:
1523: mat[8] = 0.0;
1524: mat[9] = 0.0;
1525: mat[10] = 1.0;
1526: mat[11] = 0.0;
1527:
1528: mat[12] = 0.0;
1529: mat[13] = 0.0;
1530: mat[14] = 0.0;
1531: mat[15] = 1.0;
1532:
1533: // Issue 253: set all dirty bits if input is infinity or NaN
1534: if (isInfOrNaN(angle)) {
1535: dirtyBits = ALL_DIRTY;
1536: return;
1537: }
1538:
1539: type = CONGRUENT | AFFINE | RIGID | ORTHO;
1540: dirtyBits = CLASSIFY_BIT | ROTATION_BIT | SCALE_BIT;
1541: }
1542:
1543: /**
1544: * Sets the translational value of this matrix to the Vector3f parameter
1545: * values, and sets the other components of the matrix as if this
1546: * transform were an identity matrix.
1547: * @param trans the translational component
1548: */
1549: public final void set(Vector3f trans) {
1550: mat[0] = 1.0;
1551: mat[1] = 0.0;
1552: mat[2] = 0.0;
1553: mat[3] = trans.x;
1554: mat[4] = 0.0;
1555: mat[5] = 1.0;
1556: mat[6] = 0.0;
1557: mat[7] = trans.y;
1558: mat[8] = 0.0;
1559: mat[9] = 0.0;
1560: mat[10] = 1.0;
1561: mat[11] = trans.z;
1562: mat[12] = 0.0;
1563: mat[13] = 0.0;
1564: mat[14] = 0.0;
1565: mat[15] = 1.0;
1566:
1567: // Issue 253: set all dirty bits if input is infinity or NaN
1568: if (isInfOrNaN(trans)) {
1569: dirtyBits = ALL_DIRTY;
1570: return;
1571: }
1572:
1573: type = CONGRUENT | AFFINE | RIGID | ORTHO;
1574: dirtyBits = CLASSIFY_BIT | ROTATION_BIT | SCALE_BIT;
1575: }
1576:
1577: /**
1578: * Sets the translational value of this matrix to the Vector3d paramter
1579: * values, and sets the other components of the matrix as if this
1580: * transform were an identity matrix.
1581: * @param trans the translational component
1582: */
1583: public final void set(Vector3d trans) {
1584: mat[0] = 1.0;
1585: mat[1] = 0.0;
1586: mat[2] = 0.0;
1587: mat[3] = trans.x;
1588: mat[4] = 0.0;
1589: mat[5] = 1.0;
1590: mat[6] = 0.0;
1591: mat[7] = trans.y;
1592: mat[8] = 0.0;
1593: mat[9] = 0.0;
1594: mat[10] = 1.0;
1595: mat[11] = trans.z;
1596: mat[12] = 0.0;
1597: mat[13] = 0.0;
1598: mat[14] = 0.0;
1599: mat[15] = 1.0;
1600:
1601: // Issue 253: set all dirty bits if input is infinity or NaN
1602: if (isInfOrNaN(trans)) {
1603: dirtyBits = ALL_DIRTY;
1604: return;
1605: }
1606:
1607: type = CONGRUENT | AFFINE | RIGID | ORTHO;
1608: dirtyBits = CLASSIFY_BIT | ROTATION_BIT | SCALE_BIT;
1609: }
1610:
1611: /**
1612: * Sets the scale component of the current transform; any existing
1613: * scale is first factored out of the existing transform before
1614: * the new scale is applied.
1615: * @param scale the new scale amount
1616: */
1617: public final void setScale(double scale) {
1618: if ((dirtyBits & ROTATION_BIT) != 0) {
1619: computeScaleRotation(false);
1620: }
1621:
1622: scales[0] = scales[1] = scales[2] = scale;
1623: mat[0] = rot[0] * scale;
1624: mat[1] = rot[1] * scale;
1625: mat[2] = rot[2] * scale;
1626: mat[4] = rot[3] * scale;
1627: mat[5] = rot[4] * scale;
1628: mat[6] = rot[5] * scale;
1629: mat[8] = rot[6] * scale;
1630: mat[9] = rot[7] * scale;
1631: mat[10] = rot[8] * scale;
1632:
1633: // Issue 253: set all dirty bits if input is infinity or NaN
1634: if (isInfOrNaN(scale)) {
1635: dirtyBits = ALL_DIRTY;
1636: return;
1637: }
1638:
1639: dirtyBits |= (CLASSIFY_BIT | RIGID_BIT | CONGRUENT_BIT | SVD_BIT);
1640: dirtyBits &= ~SCALE_BIT;
1641: }
1642:
1643: /**
1644: * Sets the possibly non-uniform scale component of the current
1645: * transform; any existing scale is first factored out of the
1646: * existing transform before the new scale is applied.
1647: * @param scale the new x,y,z scale values
1648: */
1649: public final void setScale(Vector3d scale) {
1650:
1651: if ((dirtyBits & ROTATION_BIT) != 0) {
1652: computeScaleRotation(false);
1653: }
1654:
1655: scales[0] = scale.x;
1656: scales[1] = scale.y;
1657: scales[2] = scale.z;
1658:
1659: mat[0] = rot[0] * scale.x;
1660: mat[1] = rot[1] * scale.y;
1661: mat[2] = rot[2] * scale.z;
1662: mat[4] = rot[3] * scale.x;
1663: mat[5] = rot[4] * scale.y;
1664: mat[6] = rot[5] * scale.z;
1665: mat[8] = rot[6] * scale.x;
1666: mat[9] = rot[7] * scale.y;
1667: mat[10] = rot[8] * scale.z;
1668:
1669: // Issue 253: set all dirty bits if input is infinity or NaN
1670: if (isInfOrNaN(scale)) {
1671: dirtyBits = ALL_DIRTY;
1672: return;
1673: }
1674:
1675: dirtyBits |= (CLASSIFY_BIT | RIGID_BIT | CONGRUENT_BIT | SVD_BIT);
1676: dirtyBits &= ~SCALE_BIT;
1677: }
1678:
1679: /**
1680: * Replaces the current transform with a non-uniform scale transform.
1681: * All values of the existing transform are replaced.
1682: * @param xScale the new X scale amount
1683: * @param yScale the new Y scale amount
1684: * @param zScale the new Z scale amount
1685: * @deprecated Use setScale(Vector3d) instead of setNonUniformScale;
1686: * note that the setScale only modifies the scale component
1687: */
1688: public final void setNonUniformScale(double xScale, double yScale,
1689: double zScale) {
1690: if (scales == null)
1691: scales = new double[3];
1692:
1693: scales[0] = xScale;
1694: scales[1] = yScale;
1695: scales[2] = zScale;
1696: mat[0] = xScale;
1697: mat[1] = 0.0;
1698: mat[2] = 0.0;
1699: mat[3] = 0.0;
1700: mat[4] = 0.0;
1701: mat[5] = yScale;
1702: mat[6] = 0.0;
1703: mat[7] = 0.0;
1704: mat[8] = 0.0;
1705: mat[9] = 0.0;
1706: mat[10] = zScale;
1707: mat[11] = 0.0;
1708: mat[12] = 0.0;
1709: mat[13] = 0.0;
1710: mat[14] = 0.0;
1711: mat[15] = 1.0;
1712:
1713: // Issue 253: set all dirty bits
1714: dirtyBits = ALL_DIRTY;
1715: }
1716:
1717: /**
1718: * Replaces the translational components of this transform to the values
1719: * in the Vector3f argument; the other values of this transform are not
1720: * modified.
1721: * @param trans the translational component
1722: */
1723: public final void setTranslation(Vector3f trans) {
1724: mat[3] = trans.x;
1725: mat[7] = trans.y;
1726: mat[11] = trans.z;
1727:
1728: // Issue 253: set all dirty bits if input is infinity or NaN
1729: if (isInfOrNaN(trans)) {
1730: dirtyBits = ALL_DIRTY;
1731: return;
1732: }
1733:
1734: // Only preserve CONGRUENT, RIGID, ORTHO
1735: type &= ~(ORTHOGONAL | IDENTITY | SCALE | TRANSLATION | SCALE | ZERO);
1736: dirtyBits |= CLASSIFY_BIT;
1737: }
1738:
1739: /**
1740: * Replaces the translational components of this transform to the values
1741: * in the Vector3d argument; the other values of this transform are not
1742: * modified.
1743: * @param trans the translational component
1744: */
1745: public final void setTranslation(Vector3d trans) {
1746: mat[3] = trans.x;
1747: mat[7] = trans.y;
1748: mat[11] = trans.z;
1749:
1750: // Issue 253: set all dirty bits if input is infinity or NaN
1751: if (isInfOrNaN(trans)) {
1752: dirtyBits = ALL_DIRTY;
1753: return;
1754: }
1755:
1756: type &= ~(ORTHOGONAL | IDENTITY | SCALE | TRANSLATION | SCALE | ZERO);
1757: dirtyBits |= CLASSIFY_BIT;
1758: }
1759:
1760: /**
1761: * Sets the value of this matrix from the rotation expressed
1762: * by the quaternion q1, the translation t1, and the scale s.
1763: * @param q1 the rotation expressed as a quaternion
1764: * @param t1 the translation
1765: * @param s the scale value
1766: */
1767: public final void set(Quat4d q1, Vector3d t1, double s) {
1768: if (scales == null)
1769: scales = new double[3];
1770:
1771: scales[0] = scales[1] = scales[2] = s;
1772:
1773: mat[0] = (1.0 - 2.0 * q1.y * q1.y - 2.0 * q1.z * q1.z) * s;
1774: mat[4] = (2.0 * (q1.x * q1.y + q1.w * q1.z)) * s;
1775: mat[8] = (2.0 * (q1.x * q1.z - q1.w * q1.y)) * s;
1776:
1777: mat[1] = (2.0 * (q1.x * q1.y - q1.w * q1.z)) * s;
1778: mat[5] = (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.z * q1.z) * s;
1779: mat[9] = (2.0 * (q1.y * q1.z + q1.w * q1.x)) * s;
1780:
1781: mat[2] = (2.0 * (q1.x * q1.z + q1.w * q1.y)) * s;
1782: mat[6] = (2.0 * (q1.y * q1.z - q1.w * q1.x)) * s;
1783: mat[10] = (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.y * q1.y) * s;
1784:
1785: mat[3] = t1.x;
1786: mat[7] = t1.y;
1787: mat[11] = t1.z;
1788: mat[12] = 0.0;
1789: mat[13] = 0.0;
1790: mat[14] = 0.0;
1791: mat[15] = 1.0;
1792:
1793: // Issue 253: set all dirty bits
1794: dirtyBits = ALL_DIRTY;
1795: }
1796:
1797: /**
1798: * Sets the value of this matrix from the rotation expressed
1799: * by the quaternion q1, the translation t1, and the scale s.
1800: * @param q1 the rotation expressed as a quaternion
1801: * @param t1 the translation
1802: * @param s the scale value
1803: */
1804: public final void set(Quat4f q1, Vector3d t1, double s) {
1805: if (scales == null)
1806: scales = new double[3];
1807:
1808: scales[0] = scales[1] = scales[2] = s;
1809:
1810: mat[0] = (1.0f - 2.0f * q1.y * q1.y - 2.0f * q1.z * q1.z) * s;
1811: mat[4] = (2.0f * (q1.x * q1.y + q1.w * q1.z)) * s;
1812: mat[8] = (2.0f * (q1.x * q1.z - q1.w * q1.y)) * s;
1813:
1814: mat[1] = (2.0f * (q1.x * q1.y - q1.w * q1.z)) * s;
1815: mat[5] = (1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.z * q1.z) * s;
1816: mat[9] = (2.0f * (q1.y * q1.z + q1.w * q1.x)) * s;
1817:
1818: mat[2] = (2.0f * (q1.x * q1.z + q1.w * q1.y)) * s;
1819: mat[6] = (2.0f * (q1.y * q1.z - q1.w * q1.x)) * s;
1820: mat[10] = (1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.y * q1.y) * s;
1821:
1822: mat[3] = t1.x;
1823: mat[7] = t1.y;
1824: mat[11] = t1.z;
1825: mat[12] = 0.0;
1826: mat[13] = 0.0;
1827: mat[14] = 0.0;
1828: mat[15] = 1.0;
1829:
1830: // Issue 253: set all dirty bits
1831: dirtyBits = ALL_DIRTY;
1832: }
1833:
1834: /**
1835: * Sets the value of this matrix from the rotation expressed
1836: * by the quaternion q1, the translation t1, and the scale s.
1837: * @param q1 the rotation expressed as a quaternion
1838: * @param t1 the translation
1839: * @param s the scale value
1840: */
1841: public final void set(Quat4f q1, Vector3f t1, float s) {
1842: if (scales == null)
1843: scales = new double[3];
1844:
1845: scales[0] = scales[1] = scales[2] = s;
1846:
1847: mat[0] = (1.0f - 2.0f * q1.y * q1.y - 2.0f * q1.z * q1.z) * s;
1848: mat[4] = (2.0f * (q1.x * q1.y + q1.w * q1.z)) * s;
1849: mat[8] = (2.0f * (q1.x * q1.z - q1.w * q1.y)) * s;
1850:
1851: mat[1] = (2.0f * (q1.x * q1.y - q1.w * q1.z)) * s;
1852: mat[5] = (1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.z * q1.z) * s;
1853: mat[9] = (2.0f * (q1.y * q1.z + q1.w * q1.x)) * s;
1854:
1855: mat[2] = (2.0f * (q1.x * q1.z + q1.w * q1.y)) * s;
1856: mat[6] = (2.0f * (q1.y * q1.z - q1.w * q1.x)) * s;
1857: mat[10] = (1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.y * q1.y) * s;
1858:
1859: mat[3] = t1.x;
1860: mat[7] = t1.y;
1861: mat[11] = t1.z;
1862: mat[12] = 0.0;
1863: mat[13] = 0.0;
1864: mat[14] = 0.0;
1865: mat[15] = 1.0;
1866:
1867: // Issue 253: set all dirty bits
1868: dirtyBits = ALL_DIRTY;
1869: }
1870:
1871: /**
1872: * Sets the value of this matrix from the rotation expressed
1873: * by the rotation matrix m1, the translation t1, and the scale s.
1874: * The scale is only applied to the
1875: * rotational component of the matrix (upper 3x3) and not to the
1876: * translational component of the matrix.
1877: * @param m1 the rotation matrix
1878: * @param t1 the translation
1879: * @param s the scale value
1880: */
1881: public final void set(Matrix3f m1, Vector3f t1, float s) {
1882: mat[0] = m1.m00 * s;
1883: mat[1] = m1.m01 * s;
1884: mat[2] = m1.m02 * s;
1885: mat[3] = t1.x;
1886: mat[4] = m1.m10 * s;
1887: mat[5] = m1.m11 * s;
1888: mat[6] = m1.m12 * s;
1889: mat[7] = t1.y;
1890: mat[8] = m1.m20 * s;
1891: mat[9] = m1.m21 * s;
1892: mat[10] = m1.m22 * s;
1893: mat[11] = t1.z;
1894: mat[12] = 0.0;
1895: mat[13] = 0.0;
1896: mat[14] = 0.0;
1897: mat[15] = 1.0;
1898:
1899: // Issue 253: set all dirty bits
1900: dirtyBits = ALL_DIRTY;
1901:
1902: if (autoNormalize) {
1903: // input matrix may not normalize
1904: normalize();
1905: }
1906: }
1907:
1908: /**
1909: * Sets the value of this matrix from the rotation expressed
1910: * by the rotation matrix m1, the translation t1, and the scale s.
1911: * The scale is only applied to the
1912: * rotational component of the matrix (upper 3x3) and not to the
1913: * translational component of the matrix.
1914: * @param m1 the rotation matrix
1915: * @param t1 the translation
1916: * @param s the scale value
1917: */
1918: public final void set(Matrix3f m1, Vector3d t1, double s) {
1919: mat[0] = m1.m00 * s;
1920: mat[1] = m1.m01 * s;
1921: mat[2] = m1.m02 * s;
1922: mat[3] = t1.x;
1923: mat[4] = m1.m10 * s;
1924: mat[5] = m1.m11 * s;
1925: mat[6] = m1.m12 * s;
1926: mat[7] = t1.y;
1927: mat[8] = m1.m20 * s;
1928: mat[9] = m1.m21 * s;
1929: mat[10] = m1.m22 * s;
1930: mat[11] = t1.z;
1931: mat[12] = 0.0;
1932: mat[13] = 0.0;
1933: mat[14] = 0.0;
1934: mat[15] = 1.0;
1935:
1936: // Issue 253: set all dirty bits
1937: dirtyBits = ALL_DIRTY;
1938:
1939: if (autoNormalize) {
1940: normalize();
1941: }
1942: }
1943:
1944: /**
1945: * Sets the value of this matrix from the rotation expressed
1946: * by the rotation matrix m1, the translation t1, and the scale s.
1947: * The scale is only applied to the
1948: * rotational component of the matrix (upper 3x3) and not to the
1949: * translational component of the matrix.
1950: * @param m1 the rotation matrix
1951: * @param t1 the translation
1952: * @param s the scale value
1953: */
1954: public final void set(Matrix3d m1, Vector3d t1, double s) {
1955: mat[0] = m1.m00 * s;
1956: mat[1] = m1.m01 * s;
1957: mat[2] = m1.m02 * s;
1958: mat[3] = t1.x;
1959: mat[4] = m1.m10 * s;
1960: mat[5] = m1.m11 * s;
1961: mat[6] = m1.m12 * s;
1962: mat[7] = t1.y;
1963: mat[8] = m1.m20 * s;
1964: mat[9] = m1.m21 * s;
1965: mat[10] = m1.m22 * s;
1966: mat[11] = t1.z;
1967: mat[12] = 0.0;
1968: mat[13] = 0.0;
1969: mat[14] = 0.0;
1970: mat[15] = 1.0;
1971:
1972: // Issue 253: set all dirty bits
1973: dirtyBits = ALL_DIRTY;
1974:
1975: if (autoNormalize) {
1976: normalize();
1977: }
1978: }
1979:
1980: /**
1981: * Sets the matrix values of this transform to the matrix values in the
1982: * upper 4x4 corner of the GMatrix parameter. If the parameter matrix is
1983: * smaller than 4x4, the remaining elements in the transform matrix are
1984: * assigned to zero. The transform matrix type is classified
1985: * internally by the Transform3D class.
1986: * @param matrix the general matrix from which the Transform3D matrix is derived
1987: */
1988: public final void set(GMatrix matrix) {
1989: int i, j, k;
1990: int numRows = matrix.getNumRow();
1991: int numCol = matrix.getNumCol();
1992:
1993: for (i = 0; i < 4; i++) {
1994: k = i * 4;
1995: for (j = 0; j < 4; j++) {
1996: if (i >= numRows || j >= numCol)
1997: mat[k + j] = 0.0;
1998: else
1999: mat[k + j] = matrix.getElement(i, j);
2000: }
2001: }
2002:
2003: dirtyBits = ALL_DIRTY;
2004:
2005: if (autoNormalize) {
2006: normalize();
2007: }
2008:
2009: }
2010:
2011: /**
2012: * Sets the matrix, type, and state of this transform to the matrix,
2013: * type, and state of transform t1.
2014: * @param t1 the transform to be copied
2015: */
2016: public final void set(Transform3D t1) {
2017: mat[0] = t1.mat[0];
2018: mat[1] = t1.mat[1];
2019: mat[2] = t1.mat[2];
2020: mat[3] = t1.mat[3];
2021: mat[4] = t1.mat[4];
2022: mat[5] = t1.mat[5];
2023: mat[6] = t1.mat[6];
2024: mat[7] = t1.mat[7];
2025: mat[8] = t1.mat[8];
2026: mat[9] = t1.mat[9];
2027: mat[10] = t1.mat[10];
2028: mat[11] = t1.mat[11];
2029: mat[12] = t1.mat[12];
2030: mat[13] = t1.mat[13];
2031: mat[14] = t1.mat[14];
2032: mat[15] = t1.mat[15];
2033: type = t1.type;
2034:
2035: // don't copy rot[] and scales[]
2036: dirtyBits = t1.dirtyBits | ROTATION_BIT | SCALE_BIT;
2037: autoNormalize = t1.autoNormalize;
2038: }
2039:
2040: // This version gets a lock before doing the set. It is used internally
2041: synchronized void setWithLock(Transform3D t1) {
2042: this .set(t1);
2043: }
2044:
2045: // This version gets a lock before doing the get. It is used internally
2046: synchronized void getWithLock(Transform3D t1) {
2047: t1.set(this );
2048: }
2049:
2050: /**
2051: * Sets the matrix values of this transform to the matrix values in the
2052: * double precision array parameter. The matrix type is classified
2053: * internally by the Transform3D class.
2054: * @param matrix the double precision array of length 16 in row major format
2055: */
2056: public final void set(double[] matrix) {
2057: mat[0] = matrix[0];
2058: mat[1] = matrix[1];
2059: mat[2] = matrix[2];
2060: mat[3] = matrix[3];
2061: mat[4] = matrix[4];
2062: mat[5] = matrix[5];
2063: mat[6] = matrix[6];
2064: mat[7] = matrix[7];
2065: mat[8] = matrix[8];
2066: mat[9] = matrix[9];
2067: mat[10] = matrix[10];
2068: mat[11] = matrix[11];
2069: mat[12] = matrix[12];
2070: mat[13] = matrix[13];
2071: mat[14] = matrix[14];
2072: mat[15] = matrix[15];
2073:
2074: dirtyBits = ALL_DIRTY;
2075:
2076: if (autoNormalize) {
2077: normalize();
2078: }
2079:
2080: }
2081:
2082: /**
2083: * Sets the matrix values of this transform to the matrix values in the
2084: * single precision array parameter. The matrix type is classified
2085: * internally by the Transform3D class.
2086: * @param matrix the single precision array of length 16 in row major format
2087: */
2088: public final void set(float[] matrix) {
2089: mat[0] = matrix[0];
2090: mat[1] = matrix[1];
2091: mat[2] = matrix[2];
2092: mat[3] = matrix[3];
2093: mat[4] = matrix[4];
2094: mat[5] = matrix[5];
2095: mat[6] = matrix[6];
2096: mat[7] = matrix[7];
2097: mat[8] = matrix[8];
2098: mat[9] = matrix[9];
2099: mat[10] = matrix[10];
2100: mat[11] = matrix[11];
2101: mat[12] = matrix[12];
2102: mat[13] = matrix[13];
2103: mat[14] = matrix[14];
2104: mat[15] = matrix[15];
2105:
2106: dirtyBits = ALL_DIRTY;
2107:
2108: if (autoNormalize) {
2109: normalize();
2110: }
2111:
2112: }
2113:
2114: /**
2115: * Sets the matrix values of this transform to the matrix values in the
2116: * double precision Matrix4d argument. The transform type is classified
2117: * internally by the Transform3D class.
2118: * @param m1 the double precision 4x4 matrix
2119: */
2120: public final void set(Matrix4d m1) {
2121: mat[0] = m1.m00;
2122: mat[1] = m1.m01;
2123: mat[2] = m1.m02;
2124: mat[3] = m1.m03;
2125: mat[4] = m1.m10;
2126: mat[5] = m1.m11;
2127: mat[6] = m1.m12;
2128: mat[7] = m1.m13;
2129: mat[8] = m1.m20;
2130: mat[9] = m1.m21;
2131: mat[10] = m1.m22;
2132: mat[11] = m1.m23;
2133: mat[12] = m1.m30;
2134: mat[13] = m1.m31;
2135: mat[14] = m1.m32;
2136: mat[15] = m1.m33;
2137:
2138: dirtyBits = ALL_DIRTY;
2139:
2140: if (autoNormalize) {
2141: normalize();
2142: }
2143: }
2144:
2145: /**
2146: * Sets the matrix values of this transform to the matrix values in the
2147: * single precision Matrix4f argument. The transform type is classified
2148: * internally by the Transform3D class.
2149: * @param m1 the single precision 4x4 matrix
2150: */
2151: public final void set(Matrix4f m1) {
2152: mat[0] = m1.m00;
2153: mat[1] = m1.m01;
2154: mat[2] = m1.m02;
2155: mat[3] = m1.m03;
2156: mat[4] = m1.m10;
2157: mat[5] = m1.m11;
2158: mat[6] = m1.m12;
2159: mat[7] = m1.m13;
2160: mat[8] = m1.m20;
2161: mat[9] = m1.m21;
2162: mat[10] = m1.m22;
2163: mat[11] = m1.m23;
2164: mat[12] = m1.m30;
2165: mat[13] = m1.m31;
2166: mat[14] = m1.m32;
2167: mat[15] = m1.m33;
2168:
2169: dirtyBits = ALL_DIRTY;
2170:
2171: if (autoNormalize) {
2172: normalize();
2173: }
2174: }
2175:
2176: /**
2177: * Sets the rotational component (upper 3x3) of this transform to the
2178: * matrix values in the single precision Matrix3f argument; the other
2179: * elements of this transform are initialized as if this were an identity
2180: * matrix (i.e., affine matrix with no translational component).
2181: * @param m1 the single precision 3x3 matrix
2182: */
2183: public final void set(Matrix3f m1) {
2184: mat[0] = m1.m00;
2185: mat[1] = m1.m01;
2186: mat[2] = m1.m02;
2187: mat[3] = 0.0;
2188: mat[4] = m1.m10;
2189: mat[5] = m1.m11;
2190: mat[6] = m1.m12;
2191: mat[7] = 0.0;
2192: mat[8] = m1.m20;
2193: mat[9] = m1.m21;
2194: mat[10] = m1.m22;
2195: mat[11] = 0.0;
2196: mat[12] = 0.0;
2197: mat[13] = 0.0;
2198: mat[14] = 0.0;
2199: mat[15] = 1.0;
2200:
2201: // Issue 253: set all dirty bits
2202: dirtyBits = ALL_DIRTY;
2203:
2204: if (autoNormalize) {
2205: normalize();
2206: }
2207: }
2208:
2209: /**
2210: * Sets the rotational component (upper 3x3) of this transform to the
2211: * matrix values in the double precision Matrix3d argument; the other
2212: * elements of this transform are initialized as if this were an identity
2213: * matrix (ie, affine matrix with no translational component).
2214: * @param m1 the double precision 3x3 matrix
2215: */
2216: public final void set(Matrix3d m1) {
2217: mat[0] = m1.m00;
2218: mat[1] = m1.m01;
2219: mat[2] = m1.m02;
2220: mat[3] = 0.0;
2221: mat[4] = m1.m10;
2222: mat[5] = m1.m11;
2223: mat[6] = m1.m12;
2224: mat[7] = 0.0;
2225: mat[8] = m1.m20;
2226: mat[9] = m1.m21;
2227: mat[10] = m1.m22;
2228: mat[11] = 0.0;
2229: mat[12] = 0.0;
2230: mat[13] = 0.0;
2231: mat[14] = 0.0;
2232: mat[15] = 1.0;
2233:
2234: // Issue 253: set all dirty bits
2235: dirtyBits = ALL_DIRTY;
2236:
2237: if (autoNormalize) {
2238: normalize();
2239: }
2240:
2241: }
2242:
2243: /**
2244: * Sets the rotational component (upper 3x3) of this transform to the
2245: * rotation matrix converted from the Euler angles provided; the other
2246: * non-rotational elements are set as if this were an identity matrix.
2247: * The euler parameter is a Vector3d consisting of three rotation angles
2248: * applied first about the X, then Y then Z axis.
2249: * These rotations are applied using a static frame of reference. In
2250: * other words, the orientation of the Y rotation axis is not affected
2251: * by the X rotation and the orientation of the Z rotation axis is not
2252: * affected by the X or Y rotation.
2253: * @param euler the Vector3d consisting of three rotation angles about X,Y,Z
2254: *
2255: */
2256: public final void setEuler(Vector3d euler) {
2257: double sina, sinb, sinc;
2258: double cosa, cosb, cosc;
2259:
2260: sina = Math.sin(euler.x);
2261: sinb = Math.sin(euler.y);
2262: sinc = Math.sin(euler.z);
2263: cosa = Math.cos(euler.x);
2264: cosb = Math.cos(euler.y);
2265: cosc = Math.cos(euler.z);
2266:
2267: mat[0] = cosb * cosc;
2268: mat[1] = -(cosa * sinc) + (sina * sinb * cosc);
2269: mat[2] = (sina * sinc) + (cosa * sinb * cosc);
2270: mat[3] = 0.0;
2271:
2272: mat[4] = cosb * sinc;
2273: mat[5] = (cosa * cosc) + (sina * sinb * sinc);
2274: mat[6] = -(sina * cosc) + (cosa * sinb * sinc);
2275: mat[7] = 0.0;
2276:
2277: mat[8] = -sinb;
2278: mat[9] = sina * cosb;
2279: mat[10] = cosa * cosb;
2280: mat[11] = 0.0;
2281:
2282: mat[12] = 0.0;
2283: mat[13] = 0.0;
2284: mat[14] = 0.0;
2285: mat[15] = 1.0;
2286:
2287: // Issue 253: set all dirty bits if input is infinity or NaN
2288: if (isInfOrNaN(euler)) {
2289: dirtyBits = ALL_DIRTY;
2290: return;
2291: }
2292:
2293: type = AFFINE | CONGRUENT | RIGID | ORTHO;
2294: dirtyBits = CLASSIFY_BIT | SCALE_BIT | ROTATION_BIT;
2295: }
2296:
2297: /**
2298: * Places the values of this transform into the double precision array
2299: * of length 16. The first four elements of the array will contain
2300: * the top row of the transform matrix, etc.
2301: * @param matrix the double precision array of length 16
2302: */
2303: public final void get(double[] matrix) {
2304: matrix[0] = mat[0];
2305: matrix[1] = mat[1];
2306: matrix[2] = mat[2];
2307: matrix[3] = mat[3];
2308: matrix[4] = mat[4];
2309: matrix[5] = mat[5];
2310: matrix[6] = mat[6];
2311: matrix[7] = mat[7];
2312: matrix[8] = mat[8];
2313: matrix[9] = mat[9];
2314: matrix[10] = mat[10];
2315: matrix[11] = mat[11];
2316: matrix[12] = mat[12];
2317: matrix[13] = mat[13];
2318: matrix[14] = mat[14];
2319: matrix[15] = mat[15];
2320: }
2321:
2322: /**
2323: * Places the values of this transform into the single precision array
2324: * of length 16. The first four elements of the array will contain
2325: * the top row of the transform matrix, etc.
2326: * @param matrix the single precision array of length 16
2327: */
2328: public final void get(float[] matrix) {
2329: matrix[0] = (float) mat[0];
2330: matrix[1] = (float) mat[1];
2331: matrix[2] = (float) mat[2];
2332: matrix[3] = (float) mat[3];
2333: matrix[4] = (float) mat[4];
2334: matrix[5] = (float) mat[5];
2335: matrix[6] = (float) mat[6];
2336: matrix[7] = (float) mat[7];
2337: matrix[8] = (float) mat[8];
2338: matrix[9] = (float) mat[9];
2339: matrix[10] = (float) mat[10];
2340: matrix[11] = (float) mat[11];
2341: matrix[12] = (float) mat[12];
2342: matrix[13] = (float) mat[13];
2343: matrix[14] = (float) mat[14];
2344: matrix[15] = (float) mat[15];
2345: }
2346:
2347: /**
2348: * Places the normalized rotational component of this transform
2349: * into the 3x3 matrix argument.
2350: * @param m1 the matrix into which the rotational component is placed
2351: */
2352: public final void get(Matrix3d m1) {
2353: if ((dirtyBits & ROTATION_BIT) != 0) {
2354: computeScaleRotation(false);
2355: }
2356: m1.m00 = rot[0];
2357: m1.m01 = rot[1];
2358: m1.m02 = rot[2];
2359: m1.m10 = rot[3];
2360: m1.m11 = rot[4];
2361: m1.m12 = rot[5];
2362: m1.m20 = rot[6];
2363: m1.m21 = rot[7];
2364: m1.m22 = rot[8];
2365: }
2366:
2367: /**
2368: * Places the normalized rotational component of this transform
2369: * into the 3x3 matrix argument.
2370: * @param m1 the matrix into which the rotational component is placed
2371: */
2372: public final void get(Matrix3f m1) {
2373: if ((dirtyBits & ROTATION_BIT) != 0) {
2374: computeScaleRotation(false);
2375: }
2376: m1.m00 = (float) rot[0];
2377: m1.m01 = (float) rot[1];
2378: m1.m02 = (float) rot[2];
2379: m1.m10 = (float) rot[3];
2380: m1.m11 = (float) rot[4];
2381: m1.m12 = (float) rot[5];
2382: m1.m20 = (float) rot[6];
2383: m1.m21 = (float) rot[7];
2384: m1.m22 = (float) rot[8];
2385: }
2386:
2387: /**
2388: * Places the quaternion equivalent of the normalized rotational
2389: * component of this transform into the quaternion parameter.
2390: * @param q1 the quaternion into which the rotation component is placed
2391: */
2392: public final void get(Quat4f q1) {
2393: if ((dirtyBits & ROTATION_BIT) != 0) {
2394: computeScaleRotation(false);
2395: }
2396:
2397: double ww = 0.25 * (1.0 + rot[0] + rot[4] + rot[8]);
2398: if (!((ww < 0 ? -ww : ww) < 1.0e-10)) {
2399: q1.w = (float) Math.sqrt(ww);
2400: ww = 0.25 / q1.w;
2401: q1.x = (float) ((rot[7] - rot[5]) * ww);
2402: q1.y = (float) ((rot[2] - rot[6]) * ww);
2403: q1.z = (float) ((rot[3] - rot[1]) * ww);
2404: return;
2405: }
2406:
2407: q1.w = 0.0f;
2408: ww = -0.5 * (rot[4] + rot[8]);
2409: if (!((ww < 0 ? -ww : ww) < 1.0e-10)) {
2410: q1.x = (float) Math.sqrt(ww);
2411: ww = 0.5 / q1.x;
2412: q1.y = (float) (rot[3] * ww);
2413: q1.z = (float) (rot[6] * ww);
2414: return;
2415: }
2416:
2417: q1.x = 0.0f;
2418: ww = 0.5 * (1.0 - rot[8]);
2419: if (!((ww < 0 ? -ww : ww) < 1.0e-10)) {
2420: q1.y = (float) Math.sqrt(ww);
2421: q1.z = (float) (rot[7] / (2.0 * q1.y));
2422: return;
2423: }
2424:
2425: q1.y = 0.0f;
2426: q1.z = 1.0f;
2427: }
2428:
2429: /**
2430: * Places the quaternion equivalent of the normalized rotational
2431: * component of this transform into the quaternion parameter.
2432: * @param q1 the quaternion into which the rotation component is placed
2433: */
2434: public final void get(Quat4d q1) {
2435: if ((dirtyBits & ROTATION_BIT) != 0) {
2436: computeScaleRotation(false);
2437: }
2438:
2439: double ww = 0.25 * (1.0 + rot[0] + rot[4] + rot[8]);
2440: if (!((ww < 0 ? -ww : ww) < 1.0e-10)) {
2441: q1.w = Math.sqrt(ww);
2442: ww = 0.25 / q1.w;
2443: q1.x = (rot[7] - rot[5]) * ww;
2444: q1.y = (rot[2] - rot[6]) * ww;
2445: q1.z = (rot[3] - rot[1]) * ww;
2446: return;
2447: }
2448:
2449: q1.w = 0.0;
2450: ww = -0.5 * (rot[4] + rot[8]);
2451: if (!((ww < 0 ? -ww : ww) < 1.0e-10)) {
2452: q1.x = Math.sqrt(ww);
2453: ww = 0.5 / q1.x;
2454: q1.y = rot[3] * ww;
2455: q1.z = rot[6] * ww;
2456: return;
2457: }
2458:
2459: q1.x = 0.0;
2460: ww = 0.5 * (1.0 - rot[8]);
2461: if (!((ww < 0 ? -ww : ww) < 1.0e-10)) {
2462: q1.y = Math.sqrt(ww);
2463: q1.z = rot[7] / (2.0 * q1.y);
2464: return;
2465: }
2466:
2467: q1.y = 0.0;
2468: q1.z = 1.0;
2469: }
2470:
2471: /**
2472: * Places the values of this transform into the double precision
2473: * matrix argument.
2474: * @param matrix the double precision matrix
2475: */
2476: public final void get(Matrix4d matrix) {
2477: matrix.m00 = mat[0];
2478: matrix.m01 = mat[1];
2479: matrix.m02 = mat[2];
2480: matrix.m03 = mat[3];
2481: matrix.m10 = mat[4];
2482: matrix.m11 = mat[5];
2483: matrix.m12 = mat[6];
2484: matrix.m13 = mat[7];
2485: matrix.m20 = mat[8];
2486: matrix.m21 = mat[9];
2487: matrix.m22 = mat[10];
2488: matrix.m23 = mat[11];
2489: matrix.m30 = mat[12];
2490: matrix.m31 = mat[13];
2491: matrix.m32 = mat[14];
2492: matrix.m33 = mat[15];
2493: }
2494:
2495: /**
2496: * Places the values of this transform into the single precision matrix
2497: * argument.
2498: * @param matrix the single precision matrix
2499: */
2500: public final void get(Matrix4f matrix) {
2501: matrix.m00 = (float) mat[0];
2502: matrix.m01 = (float) mat[1];
2503: matrix.m02 = (float) mat[2];
2504: matrix.m03 = (float) mat[3];
2505: matrix.m10 = (float) mat[4];
2506: matrix.m11 = (float) mat[5];
2507: matrix.m12 = (float) mat[6];
2508: matrix.m13 = (float) mat[7];
2509: matrix.m20 = (float) mat[8];
2510: matrix.m21 = (float) mat[9];
2511: matrix.m22 = (float) mat[10];
2512: matrix.m23 = (float) mat[11];
2513: matrix.m30 = (float) mat[12];
2514: matrix.m31 = (float) mat[13];
2515: matrix.m32 = (float) mat[14];
2516: matrix.m33 = (float) mat[15];
2517: }
2518:
2519: /**
2520: * Places the quaternion equivalent of the normalized rotational
2521: * component of this transform into the quaternion parameter;
2522: * places the translational component into the Vector parameter.
2523: * @param q1 the quaternion representing the rotation
2524: * @param t1 the translation component
2525: * @return the scale component of this transform
2526: */
2527: public final double get(Quat4d q1, Vector3d t1) {
2528:
2529: if ((dirtyBits & ROTATION_BIT) != 0) {
2530: computeScaleRotation(false);
2531: } else if ((dirtyBits & SCALE_BIT) != 0) {
2532: computeScales(false);
2533: }
2534:
2535: t1.x = mat[3];
2536: t1.y = mat[7];
2537: t1.z = mat[11];
2538:
2539: double maxScale = max3(scales);
2540:
2541: double ww = 0.25 * (1.0 + rot[0] + rot[4] + rot[8]);
2542: if (!((ww < 0 ? -ww : ww) < EPSILON)) {
2543: q1.w = Math.sqrt(ww);
2544: ww = 0.25 / q1.w;
2545: q1.x = (rot[7] - rot[5]) * ww;
2546: q1.y = (rot[2] - rot[6]) * ww;
2547: q1.z = (rot[3] - rot[1]) * ww;
2548: return maxScale;
2549: }
2550:
2551: q1.w = 0.0;
2552: ww = -0.5 * (rot[4] + rot[8]);
2553: if (!((ww < 0 ? -ww : ww) < EPSILON)) {
2554: q1.x = Math.sqrt(ww);
2555: ww = 0.5 / q1.x;
2556: q1.y = rot[3] * ww;
2557: q1.z = rot[6] * ww;
2558: return maxScale;
2559: }
2560:
2561: q1.x = 0.0;
2562: ww = 0.5 * (1.0 - rot[8]);
2563: if (!((ww < 0 ? -ww : ww) < EPSILON)) {
2564: q1.y = Math.sqrt(ww);
2565: q1.z = rot[7] / (2.0 * q1.y);
2566: return maxScale;
2567: }
2568:
2569: q1.y = 0.0;
2570: q1.z = 1.0;
2571: return maxScale;
2572: }
2573:
2574: /**
2575: * Places the quaternion equivalent of the normalized rotational
2576: * component of this transform into the quaternion parameter;
2577: * places the translational component into the Vector parameter.
2578: * @param q1 the quaternion representing the rotation
2579: * @param t1 the translation component
2580: * @return the scale component of this transform
2581: */
2582: public final float get(Quat4f q1, Vector3f t1) {
2583:
2584: if ((dirtyBits & ROTATION_BIT) != 0) {
2585: computeScaleRotation(false);
2586: } else if ((dirtyBits & SCALE_BIT) != 0) {
2587: computeScales(false);
2588: }
2589:
2590: double maxScale = max3(scales);
2591: t1.x = (float) mat[3];
2592: t1.y = (float) mat[7];
2593: t1.z = (float) mat[11];
2594:
2595: double ww = 0.25 * (1.0 + rot[0] + rot[4] + rot[8]);
2596: if (!((ww < 0 ? -ww : ww) < EPSILON)) {
2597: q1.w = (float) Math.sqrt(ww);
2598: ww = 0.25 / q1.w;
2599: q1.x = (float) ((rot[7] - rot[5]) * ww);
2600: q1.y = (float) ((rot[2] - rot[6]) * ww);
2601: q1.z = (float) ((rot[3] - rot[1]) * ww);
2602: return (float) maxScale;
2603: }
2604:
2605: q1.w = 0.0f;
2606: ww = -0.5 * (rot[4] + rot[8]);
2607: if (!((ww < 0 ? -ww : ww) < EPSILON)) {
2608: q1.x = (float) Math.sqrt(ww);
2609: ww = 0.5 / q1.x;
2610: q1.y = (float) (rot[3] * ww);
2611: q1.z = (float) (rot[6] * ww);
2612: return (float) maxScale;
2613: }
2614:
2615: q1.x = 0.0f;
2616: ww = 0.5 * (1.0 - rot[8]);
2617: if (!((ww < 0 ? -ww : ww) < EPSILON)) {
2618: q1.y = (float) Math.sqrt(ww);
2619: q1.z = (float) (rot[7] / (2.0 * q1.y));
2620: return (float) maxScale;
2621: }
2622:
2623: q1.y = 0.0f;
2624: q1.z = 1.0f;
2625: return (float) maxScale;
2626: }
2627:
2628: /**
2629: * Places the quaternion equivalent of the normalized rotational
2630: * component of this transform into the quaternion parameter;
2631: * places the translational component into the Vector parameter.
2632: * @param q1 the quaternion representing the rotation
2633: * @param t1 the translation component
2634: * @return the scale component of this transform
2635: */
2636: public final double get(Quat4f q1, Vector3d t1) {
2637:
2638: if ((dirtyBits & ROTATION_BIT) != 0) {
2639: computeScaleRotation(false);
2640: } else if ((dirtyBits & SCALE_BIT) != 0) {
2641: computeScales(false);
2642: }
2643:
2644: double maxScale = max3(scales);
2645:
2646: t1.x = mat[3];
2647: t1.y = mat[7];
2648: t1.z = mat[11];
2649:
2650: double ww = 0.25 * (1.0 + rot[0] + rot[4] + rot[8]);
2651: if (!((ww < 0 ? -ww : ww) < EPSILON)) {
2652: q1.w = (float) Math.sqrt(ww);
2653: ww = 0.25 / q1.w;
2654: q1.x = (float) ((rot[7] - rot[5]) * ww);
2655: q1.y = (float) ((rot[2] - rot[6]) * ww);
2656: q1.z = (float) ((rot[3] - rot[1]) * ww);
2657: return maxScale;
2658: }
2659:
2660: q1.w = 0.0f;
2661: ww = -0.5 * (rot[4] + rot[8]);
2662: if (!((ww < 0 ? -ww : ww) < EPSILON)) {
2663: q1.x = (float) Math.sqrt(ww);
2664: ww = 0.5 / q1.x;
2665: q1.y = (float) (rot[3] * ww);
2666: q1.z = (float) (rot[6] * ww);
2667: return maxScale;
2668: }
2669:
2670: q1.x = 0.0f;
2671: ww = 0.5 * (1.0 - rot[8]);
2672: if (!((ww < 0 ? -ww : ww) < EPSILON)) {
2673: q1.y = (float) Math.sqrt(ww);
2674: q1.z = (float) (rot[7] / (2.0 * q1.y));
2675: return maxScale;
2676: }
2677:
2678: q1.y = 0.0f;
2679: q1.z = 1.0f;
2680: return maxScale;
2681: }
2682:
2683: /**
2684: * Places the normalized rotational component of this transform
2685: * into the matrix parameter; place the translational component
2686: * into the vector parameter.
2687: * @param m1 the normalized matrix representing the rotation
2688: * @param t1 the translation component
2689: * @return the scale component of this transform
2690: */
2691: public final double get(Matrix3d m1, Vector3d t1) {
2692:
2693: if ((dirtyBits & ROTATION_BIT) != 0) {
2694: computeScaleRotation(false);
2695: } else if ((dirtyBits & SCALE_BIT) != 0) {
2696: computeScales(false);
2697: }
2698:
2699: t1.x = mat[3];
2700: t1.y = mat[7];
2701: t1.z = mat[11];
2702:
2703: m1.m00 = rot[0];
2704: m1.m01 = rot[1];
2705: m1.m02 = rot[2];
2706:
2707: m1.m10 = rot[3];
2708: m1.m11 = rot[4];
2709: m1.m12 = rot[5];
2710:
2711: m1.m20 = rot[6];
2712: m1.m21 = rot[7];
2713: m1.m22 = rot[8];
2714:
2715: return max3(scales);
2716: }
2717:
2718: /**
2719: * Places the normalized rotational component of this transform
2720: * into the matrix parameter; place the translational component
2721: * into the vector parameter.
2722: * @param m1 the normalized matrix representing the rotation
2723: * @param t1 the translation component
2724: * @return the scale component of this transform
2725: */
2726: public final float get(Matrix3f m1, Vector3f t1) {
2727:
2728: if ((dirtyBits & ROTATION_BIT) != 0) {
2729: computeScaleRotation(false);
2730: } else if ((dirtyBits & SCALE_BIT) != 0) {
2731: computeScales(false);
2732: }
2733:
2734: t1.x = (float) mat[3];
2735: t1.y = (float) mat[7];
2736: t1.z = (float) mat[11];
2737:
2738: m1.m00 = (float) rot[0];
2739: m1.m01 = (float) rot[1];
2740: m1.m02 = (float) rot[2];
2741:
2742: m1.m10 = (float) rot[3];
2743: m1.m11 = (float) rot[4];
2744: m1.m12 = (float) rot[5];
2745:
2746: m1.m20 = (float) rot[6];
2747: m1.m21 = (float) rot[7];
2748: m1.m22 = (float) rot[8];
2749:
2750: return (float) max3(scales);
2751: }
2752:
2753: /**
2754: * Places the normalized rotational component of this transform
2755: * into the matrix parameter; place the translational component
2756: * into the vector parameter.
2757: * @param m1 the normalized matrix representing the rotation
2758: * @param t1 the translation component
2759: * @return the scale component of this transform
2760: */
2761: public final double get(Matrix3f m1, Vector3d t1) {
2762: if ((dirtyBits & ROTATION_BIT) != 0) {
2763: computeScaleRotation(false);
2764: } else if ((dirtyBits & SCALE_BIT) != 0) {
2765: computeScales(false);
2766: }
2767:
2768: t1.x = mat[3];
2769: t1.y = mat[7];
2770: t1.z = mat[11];
2771:
2772: m1.m00 = (float) rot[0];
2773: m1.m01 = (float) rot[1];
2774: m1.m02 = (float) rot[2];
2775:
2776: m1.m10 = (float) rot[3];
2777: m1.m11 = (float) rot[4];
2778: m1.m12 = (float) rot[5];
2779:
2780: m1.m20 = (float) rot[6];
2781: m1.m21 = (float) rot[7];
2782: m1.m22 = (float) rot[8];
2783:
2784: return max3(scales);
2785: }
2786:
2787: /**
2788: * Returns the uniform scale factor of this matrix.
2789: * If the matrix has non-uniform scale factors, the largest of the
2790: * x, y, and z scale factors will be returned.
2791: * @return the scale factor of this matrix
2792: */
2793: public final double getScale() {
2794: if ((dirtyBits & SCALE_BIT) != 0) {
2795: computeScales(false);
2796: }
2797: return max3(scales);
2798: }
2799:
2800: /**
2801: * Gets the possibly non-uniform scale components of the current
2802: * transform and places them into the scale vector.
2803: * @param scale the vector into which the x,y,z scale values will be placed
2804: */
2805: public final void getScale(Vector3d scale) {
2806: if ((dirtyBits & SCALE_BIT) != 0) {
2807: computeScales(false);
2808: }
2809: scale.x = scales[0];
2810: scale.y = scales[1];
2811: scale.z = scales[2];
2812: }
2813:
2814: /**
2815: * Retrieves the translational components of this transform.
2816: * @param trans the vector that will receive the translational component
2817: */
2818: public final void get(Vector3f trans) {
2819: trans.x = (float) mat[3];
2820: trans.y = (float) mat[7];
2821: trans.z = (float) mat[11];
2822: }
2823:
2824: /**
2825: * Retrieves the translational components of this transform.
2826: * @param trans the vector that will receive the translational component
2827: */
2828: public final void get(Vector3d trans) {
2829: trans.x = mat[3];
2830: trans.y = mat[7];
2831: trans.z = mat[11];
2832: }
2833:
2834: /**
2835: * Sets the value of this transform to the inverse of the passed
2836: * Transform3D parameter. This method uses the transform type
2837: * to determine the optimal algorithm for inverting transform t1.
2838: * @param t1 the transform to be inverted
2839: * @exception SingularMatrixException thrown if transform t1 is
2840: * not invertible
2841: */
2842: public final void invert(Transform3D t1) {
2843: if (t1 == this ) {
2844: invert();
2845: } else if (t1.isAffine()) {
2846: // We can't use invertOrtho() because of numerical
2847: // instability unless we set tolerance of ortho test to 0
2848: invertAffine(t1);
2849: } else {
2850: invertGeneral(t1);
2851: }
2852: }
2853:
2854: /**
2855: * Inverts this transform in place. This method uses the transform
2856: * type to determine the optimal algorithm for inverting this transform.
2857: * @exception SingularMatrixException thrown if this transform is
2858: * not invertible
2859: */
2860: public final void invert() {
2861: if (isAffine()) {
2862: invertAffine();
2863: } else {
2864: invertGeneral(this );
2865: }
2866: }
2867:
2868: /**
2869: * Congruent invert routine.
2870: *
2871: * if uniform scale s
2872: *
2873: * [R | t] => [R^T/s*s | -R^T * t/s*s]
2874: *
2875: */
2876:
2877: /*
2878: final void invertOrtho() {
2879: double tmp, s1, s2, s3;
2880:
2881: // do not force classifyRigid()
2882: if (((dirtyBits & CONGRUENT_BIT) == 0) &&
2883: ((type & CONGRUENT) != 0)) {
2884: s1 = mat[0]*mat[0] + mat[4]*mat[4] + mat[8]*mat[8];
2885: if (s1 == 0) {
2886: throw new SingularMatrixException(J3dI18N.getString("Transform3D1"));
2887: }
2888: s1 = s2 = s3 = 1/s1;
2889: dirtyBits |= ROTSCALESVD_DIRTY;
2890: } else {
2891: // non-uniform scale matrix
2892: s1 = mat[0]*mat[0] + mat[4]*mat[4] + mat[8]*mat[8];
2893: s2 = mat[1]*mat[1] + mat[5]*mat[5] + mat[9]*mat[9];
2894: s3 = mat[2]*mat[2] + mat[6]*mat[6] + mat[10]*mat[10];
2895: if ((s1 == 0) || (s2 == 0) || (s3 == 0)) {
2896: throw new SingularMatrixException(J3dI18N.getString("Transform3D1"));
2897: }
2898: s1 = 1/s1;
2899: s2 = 1/s2;
2900: s3 = 1/s3;
2901: dirtyBits |= ROTSCALESVD_DIRTY | ORTHO_BIT | CONGRUENT_BIT
2902: | RIGID_BIT | CLASSIFY_BIT;
2903: }
2904: // multiple by 1/s will cause loss in numerical value
2905: tmp = mat[1];
2906: mat[1] = mat[4]*s1;
2907: mat[4] = tmp*s2;
2908: tmp = mat[2];
2909: mat[2] = mat[8]*s1;
2910: mat[8] = tmp*s3;
2911: tmp = mat[6];
2912: mat[6] = mat[9]*s2;
2913: mat[9] = tmp*s3;
2914: mat[0] *= s1;
2915: mat[5] *= s2;
2916: mat[10] *= s3;
2917:
2918: tmp = mat[3];
2919: s1 = mat[7];
2920: mat[3] = -(tmp * mat[0] + s1 * mat[1] + mat[11] * mat[2]);
2921: mat[7] = -(tmp * mat[4] + s1 * mat[5] + mat[11] * mat[6]);
2922: mat[11]= -(tmp * mat[8] + s1 * mat[9] + mat[11] * mat[10]);
2923: mat[12] = mat[13] = mat[14] = 0.0;
2924: mat[15] = 1.0;
2925: }
2926: */
2927:
2928: /**
2929: * Orthogonal matrix invert routine.
2930: * Inverts t1 and places the result in "this".
2931: */
2932: /*
2933: final void invertOrtho(Transform3D t1) {
2934: double s1, s2, s3;
2935:
2936: // do not force classifyRigid()
2937: if (((t1.dirtyBits & CONGRUENT_BIT) == 0) &&
2938: ((t1.type & CONGRUENT) != 0)) {
2939: s1 = t1.mat[0]*t1.mat[0] + t1.mat[4]*t1.mat[4] +
2940: t1.mat[8]*t1.mat[8];
2941: if (s1 == 0) {
2942: throw new SingularMatrixException(J3dI18N.getString("Transform3D1"));
2943: }
2944: s1 = s2 = s3 = 1/s1;
2945: dirtyBits = t1.dirtyBits | ROTSCALESVD_DIRTY;
2946: } else {
2947: // non-uniform scale matrix
2948: s1 = t1.mat[0]*t1.mat[0] + t1.mat[4]*t1.mat[4] +
2949: t1.mat[8]*t1.mat[8];
2950: s2 = t1.mat[1]*t1.mat[1] + t1.mat[5]*t1.mat[5] +
2951: t1.mat[9]*t1.mat[9];
2952: s3 = t1.mat[2]*t1.mat[2] + t1.mat[6]*t1.mat[6] +
2953: t1.mat[10]*t1.mat[10];
2954:
2955: if ((s1 == 0) || (s2 == 0) || (s3 == 0)) {
2956: throw new SingularMatrixException(J3dI18N.getString("Transform3D1"));
2957: }
2958: s1 = 1/s1;
2959: s2 = 1/s2;
2960: s3 = 1/s3;
2961: dirtyBits = t1.dirtyBits | ROTSCALESVD_DIRTY | ORTHO_BIT |
2962: CONGRUENT_BIT | RIGID_BIT;
2963: }
2964:
2965: mat[0] = t1.mat[0]*s1;
2966: mat[1] = t1.mat[4]*s1;
2967: mat[2] = t1.mat[8]*s1;
2968: mat[4] = t1.mat[1]*s2;
2969: mat[5] = t1.mat[5]*s2;
2970: mat[6] = t1.mat[9]*s2;
2971: mat[8] = t1.mat[2]*s3;
2972: mat[9] = t1.mat[6]*s3;
2973: mat[10] = t1.mat[10]*s3;
2974:
2975: mat[3] = -(t1.mat[3] * mat[0] + t1.mat[7] * mat[1] +
2976: t1.mat[11] * mat[2]);
2977: mat[7] = -(t1.mat[3] * mat[4] + t1.mat[7] * mat[5] +
2978: t1.mat[11] * mat[6]);
2979: mat[11]= -(t1.mat[3] * mat[8] + t1.mat[7] * mat[9] +
2980: t1.mat[11] * mat[10]);
2981: mat[12] = mat[13] = mat[14] = 0.0;
2982: mat[15] = 1.0;
2983: type = t1.type;
2984: }
2985: */
2986:
2987: /**
2988: * Affine invert routine. Inverts t1 and places the result in "this".
2989: */
2990: final void invertAffine(Transform3D t1) {
2991: double determinant = t1.affineDeterminant();
2992:
2993: if (determinant == 0.0)
2994: throw new SingularMatrixException(J3dI18N
2995: .getString("Transform3D1"));
2996:
2997: double s = (t1.mat[0] * t1.mat[0] + t1.mat[1] * t1.mat[1]
2998: + t1.mat[2] * t1.mat[2] + t1.mat[3] * t1.mat[3])
2999: * (t1.mat[4] * t1.mat[4] + t1.mat[5] * t1.mat[5]
3000: + t1.mat[6] * t1.mat[6] + t1.mat[7] * t1.mat[7])
3001: * (t1.mat[8] * t1.mat[8] + t1.mat[9] * t1.mat[9]
3002: + t1.mat[10] * t1.mat[10] + t1.mat[11]
3003: * t1.mat[11]);
3004:
3005: if ((determinant * determinant) < (EPS * s)) {
3006: // using invertGeneral is numerically more stable for
3007: //this case see bug 4227733
3008: invertGeneral(t1);
3009: return;
3010: }
3011: s = 1.0 / determinant;
3012:
3013: mat[0] = (t1.mat[5] * t1.mat[10] - t1.mat[9] * t1.mat[6]) * s;
3014: mat[1] = -(t1.mat[1] * t1.mat[10] - t1.mat[9] * t1.mat[2]) * s;
3015: mat[2] = (t1.mat[1] * t1.mat[6] - t1.mat[5] * t1.mat[2]) * s;
3016: mat[4] = -(t1.mat[4] * t1.mat[10] - t1.mat[8] * t1.mat[6]) * s;
3017: mat[5] = (t1.mat[0] * t1.mat[10] - t1.mat[8] * t1.mat[2]) * s;
3018: mat[6] = -(t1.mat[0] * t1.mat[6] - t1.mat[4] * t1.mat[2]) * s;
3019: mat[8] = (t1.mat[4] * t1.mat[9] - t1.mat[8] * t1.mat[5]) * s;
3020: mat[9] = -(t1.mat[0] * t1.mat[9] - t1.mat[8] * t1.mat[1]) * s;
3021: mat[10] = (t1.mat[0] * t1.mat[5] - t1.mat[4] * t1.mat[1]) * s;
3022: mat[3] = -(t1.mat[3] * mat[0] + t1.mat[7] * mat[1] + t1.mat[11]
3023: * mat[2]);
3024: mat[7] = -(t1.mat[3] * mat[4] + t1.mat[7] * mat[5] + t1.mat[11]
3025: * mat[6]);
3026: mat[11] = -(t1.mat[3] * mat[8] + t1.mat[7] * mat[9] + t1.mat[11]
3027: * mat[10]);
3028:
3029: mat[12] = mat[13] = mat[14] = 0.0;
3030: mat[15] = 1.0;
3031:
3032: dirtyBits = t1.dirtyBits | ROTSCALESVD_DIRTY | CLASSIFY_BIT
3033: | ORTHO_BIT;
3034: type = t1.type;
3035: }
3036:
3037: /**
3038: * Affine invert routine. Inverts "this" matrix in place.
3039: */
3040: final void invertAffine() {
3041: double determinant = affineDeterminant();
3042:
3043: if (determinant == 0.0)
3044: throw new SingularMatrixException(J3dI18N
3045: .getString("Transform3D1"));
3046:
3047: double s = (mat[0] * mat[0] + mat[1] * mat[1] + mat[2] * mat[2] + mat[3]
3048: * mat[3])
3049: * (mat[4] * mat[4] + mat[5] * mat[5] + mat[6] * mat[6] + mat[7]
3050: * mat[7])
3051: * (mat[8] * mat[8] + mat[9] * mat[9] + mat[10]
3052: * mat[10] + mat[11] * mat[11]);
3053:
3054: if ((determinant * determinant) < (EPS * s)) {
3055: invertGeneral(this );
3056: return;
3057: }
3058: s = 1.0 / determinant;
3059: double tmp0 = (mat[5] * mat[10] - mat[9] * mat[6]) * s;
3060: double tmp1 = -(mat[1] * mat[10] - mat[9] * mat[2]) * s;
3061: double tmp2 = (mat[1] * mat[6] - mat[5] * mat[2]) * s;
3062: double tmp4 = -(mat[4] * mat[10] - mat[8] * mat[6]) * s;
3063: double tmp5 = (mat[0] * mat[10] - mat[8] * mat[2]) * s;
3064: double tmp6 = -(mat[0] * mat[6] - mat[4] * mat[2]) * s;
3065: double tmp8 = (mat[4] * mat[9] - mat[8] * mat[5]) * s;
3066: double tmp9 = -(mat[0] * mat[9] - mat[8] * mat[1]) * s;
3067: double tmp10 = (mat[0] * mat[5] - mat[4] * mat[1]) * s;
3068: double tmp3 = -(mat[3] * tmp0 + mat[7] * tmp1 + mat[11] * tmp2);
3069: double tmp7 = -(mat[3] * tmp4 + mat[7] * tmp5 + mat[11] * tmp6);
3070: mat[11] = -(mat[3] * tmp8 + mat[7] * tmp9 + mat[11] * tmp10);
3071:
3072: mat[0] = tmp0;
3073: mat[1] = tmp1;
3074: mat[2] = tmp2;
3075: mat[3] = tmp3;
3076: mat[4] = tmp4;
3077: mat[5] = tmp5;
3078: mat[6] = tmp6;
3079: mat[7] = tmp7;
3080: mat[8] = tmp8;
3081: mat[9] = tmp9;
3082: mat[10] = tmp10;
3083: mat[12] = mat[13] = mat[14] = 0.0;
3084: mat[15] = 1.0;
3085: dirtyBits |= ROTSCALESVD_DIRTY | CLASSIFY_BIT | ORTHO_BIT;
3086: }
3087:
3088: /**
3089: * General invert routine. Inverts t1 and places the result in "this".
3090: * Note that this routine handles both the "this" version and the
3091: * non-"this" version.
3092: *
3093: * Also note that since this routine is slow anyway, we won't worry
3094: * about allocating a little bit of garbage.
3095: */
3096: final void invertGeneral(Transform3D t1) {
3097: double tmp[] = new double[16];
3098: int row_perm[] = new int[4];
3099: int i, r, c;
3100:
3101: // Use LU decomposition and backsubstitution code specifically
3102: // for floating-point 4x4 matrices.
3103:
3104: // Copy source matrix to tmp
3105: System.arraycopy(t1.mat, 0, tmp, 0, tmp.length);
3106:
3107: // Calculate LU decomposition: Is the matrix singular?
3108: if (!luDecomposition(tmp, row_perm)) {
3109: // Matrix has no inverse
3110: throw new SingularMatrixException(J3dI18N
3111: .getString("Transform3D1"));
3112: }
3113:
3114: // Perform back substitution on the identity matrix
3115: // luDecomposition will set rot[] & scales[] for use
3116: // in luBacksubstituation
3117: mat[0] = 1.0;
3118: mat[1] = 0.0;
3119: mat[2] = 0.0;
3120: mat[3] = 0.0;
3121: mat[4] = 0.0;
3122: mat[5] = 1.0;
3123: mat[6] = 0.0;
3124: mat[7] = 0.0;
3125: mat[8] = 0.0;
3126: mat[9] = 0.0;
3127: mat[10] = 1.0;
3128: mat[11] = 0.0;
3129: mat[12] = 0.0;
3130: mat[13] = 0.0;
3131: mat[14] = 0.0;
3132: mat[15] = 1.0;
3133: luBacksubstitution(tmp, row_perm, this .mat);
3134:
3135: type = 0;
3136: dirtyBits = ALL_DIRTY;
3137: }
3138:
3139: /**
3140: * Given a 4x4 array "matrix0", this function replaces it with the
3141: * LU decomposition of a row-wise permutation of itself. The input
3142: * parameters are "matrix0" and "dimen". The array "matrix0" is also
3143: * an output parameter. The vector "row_perm[4]" is an output
3144: * parameter that contains the row permutations resulting from partial
3145: * pivoting. The output parameter "even_row_xchg" is 1 when the
3146: * number of row exchanges is even, or -1 otherwise. Assumes data
3147: * type is always double.
3148: *
3149: * This function is similar to luDecomposition, except that it
3150: * is tuned specifically for 4x4 matrices.
3151: *
3152: * @return true if the matrix is nonsingular, or false otherwise.
3153: */
3154: //
3155: // Reference: Press, Flannery, Teukolsky, Vetterling,
3156: // _Numerical_Recipes_in_C_, Cambridge University Press,
3157: // 1988, pp 40-45.
3158: //
3159: static boolean luDecomposition(double[] matrix0, int[] row_perm) {
3160:
3161: // Can't re-use this temporary since the method is static.
3162: double row_scale[] = new double[4];
3163:
3164: // Determine implicit scaling information by looping over rows
3165: {
3166: int i, j;
3167: int ptr, rs;
3168: double big, temp;
3169:
3170: ptr = 0;
3171: rs = 0;
3172:
3173: // For each row ...
3174: i = 4;
3175: while (i-- != 0) {
3176: big = 0.0;
3177:
3178: // For each column, find the largest element in the row
3179: j = 4;
3180: while (j-- != 0) {
3181: temp = matrix0[ptr++];
3182: temp = Math.abs(temp);
3183: if (temp > big) {
3184: big = temp;
3185: }
3186: }
3187:
3188: // Is the matrix singular?
3189: if (big == 0.0) {
3190: return false;
3191: }
3192: row_scale[rs++] = 1.0 / big;
3193: }
3194: }
3195:
3196: {
3197: int j;
3198: int mtx;
3199:
3200: mtx = 0;
3201:
3202: // For all columns, execute Crout's method
3203: for (j = 0; j < 4; j++) {
3204: int i, imax, k;
3205: int target, p1, p2;
3206: double sum, big, temp;
3207:
3208: // Determine elements of upper diagonal matrix U
3209: for (i = 0; i < j; i++) {
3210: target = mtx + (4 * i) + j;
3211: sum = matrix0[target];
3212: k = i;
3213: p1 = mtx + (4 * i);
3214: p2 = mtx + j;
3215: while (k-- != 0) {
3216: sum -= matrix0[p1] * matrix0[p2];
3217: p1++;
3218: p2 += 4;
3219: }
3220: matrix0[target] = sum;
3221: }
3222:
3223: // Search for largest pivot element and calculate
3224: // intermediate elements of lower diagonal matrix L.
3225: big = 0.0;
3226: imax = -1;
3227: for (i = j; i < 4; i++) {
3228: target = mtx + (4 * i) + j;
3229: sum = matrix0[target];
3230: k = j;
3231: p1 = mtx + (4 * i);
3232: p2 = mtx + j;
3233: while (k-- != 0) {
3234: sum -= matrix0[p1] * matrix0[p2];
3235: p1++;
3236: p2 += 4;
3237: }
3238: matrix0[target] = sum;
3239:
3240: // Is this the best pivot so far?
3241: if ((temp = row_scale[i] * Math.abs(sum)) >= big) {
3242: big = temp;
3243: imax = i;
3244: }
3245: }
3246:
3247: if (imax < 0) {
3248: return false;
3249: }
3250:
3251: // Is a row exchange necessary?
3252: if (j != imax) {
3253: // Yes: exchange rows
3254: k = 4;
3255: p1 = mtx + (4 * imax);
3256: p2 = mtx + (4 * j);
3257: while (k-- != 0) {
3258: temp = matrix0[p1];
3259: matrix0[p1++] = matrix0[p2];
3260: matrix0[p2++] = temp;
3261: }
3262:
3263: // Record change in scale factor
3264: row_scale[imax] = row_scale[j];
3265: }
3266:
3267: // Record row permutation
3268: row_perm[j] = imax;
3269:
3270: // Is the matrix singular
3271: if (matrix0[(mtx + (4 * j) + j)] == 0.0) {
3272: return false;
3273: }
3274:
3275: // Divide elements of lower diagonal matrix L by pivot
3276: if (j != (4 - 1)) {
3277: temp = 1.0 / (matrix0[(mtx + (4 * j) + j)]);
3278: target = mtx + (4 * (j + 1)) + j;
3279: i = 3 - j;
3280: while (i-- != 0) {
3281: matrix0[target] *= temp;
3282: target += 4;
3283: }
3284: }
3285: }
3286: }
3287:
3288: return true;
3289: }
3290:
3291: /**
3292: * Solves a set of linear equations. The input parameters "matrix1",
3293: * and "row_perm" come from luDecompostionD4x4 and do not change
3294: * here. The parameter "matrix2" is a set of column vectors assembled
3295: * into a 4x4 matrix of floating-point values. The procedure takes each
3296: * column of "matrix2" in turn and treats it as the right-hand side of the
3297: * matrix equation Ax = LUx = b. The solution vector replaces the
3298: * original column of the matrix.
3299: *
3300: * If "matrix2" is the identity matrix, the procedure replaces its contents
3301: * with the inverse of the matrix from which "matrix1" was originally
3302: * derived.
3303: */
3304: //
3305: // Reference: Press, Flannery, Teukolsky, Vetterling,
3306: // _Numerical_Recipes_in_C_, Cambridge University Press,
3307: // 1988, pp 44-45.
3308: //
3309: static void luBacksubstitution(double[] matrix1, int[] row_perm,
3310: double[] matrix2) {
3311:
3312: int i, ii, ip, j, k;
3313: int rp;
3314: int cv, rv;
3315:
3316: // rp = row_perm;
3317: rp = 0;
3318:
3319: // For each column vector of matrix2 ...
3320: for (k = 0; k < 4; k++) {
3321: // cv = &(matrix2[0][k]);
3322: cv = k;
3323: ii = -1;
3324:
3325: // Forward substitution
3326: for (i = 0; i < 4; i++) {
3327: double sum;
3328:
3329: ip = row_perm[rp + i];
3330: sum = matrix2[cv + 4 * ip];
3331: matrix2[cv + 4 * ip] = matrix2[cv + 4 * i];
3332: if (ii >= 0) {
3333: // rv = &(matrix1[i][0]);
3334: rv = i * 4;
3335: for (j = ii; j <= i - 1; j++) {
3336: sum -= matrix1[rv + j] * matrix2[cv + 4 * j];
3337: }
3338: } else if (sum != 0.0) {
3339: ii = i;
3340: }
3341: matrix2[cv + 4 * i] = sum;
3342: }
3343:
3344: // Backsubstitution
3345: // rv = &(matrix1[3][0]);
3346: rv = 3 * 4;
3347: matrix2[cv + 4 * 3] /= matrix1[rv + 3];
3348:
3349: rv -= 4;
3350: matrix2[cv + 4 * 2] = (matrix2[cv + 4 * 2] - matrix1[rv + 3]
3351: * matrix2[cv + 4 * 3])
3352: / matrix1[rv + 2];
3353:
3354: rv -= 4;
3355: matrix2[cv + 4 * 1] = (matrix2[cv + 4 * 1]
3356: - matrix1[rv + 2] * matrix2[cv + 4 * 2] - matrix1[rv + 3]
3357: * matrix2[cv + 4 * 3])
3358: / matrix1[rv + 1];
3359:
3360: rv -= 4;
3361: matrix2[cv + 4 * 0] = (matrix2[cv + 4 * 0]
3362: - matrix1[rv + 1] * matrix2[cv + 4 * 1]
3363: - matrix1[rv + 2] * matrix2[cv + 4 * 2] - matrix1[rv + 3]
3364: * matrix2[cv + 4 * 3])
3365: / matrix1[rv + 0];
3366: }
3367: }
3368:
3369: // given that this matrix is affine
3370: final double affineDeterminant() {
3371: return mat[0] * (mat[5] * mat[10] - mat[6] * mat[9]) - mat[1]
3372: * (mat[4] * mat[10] - mat[6] * mat[8]) + mat[2]
3373: * (mat[4] * mat[9] - mat[5] * mat[8]);
3374: }
3375:
3376: /**
3377: * Calculates and returns the determinant of this transform.
3378: * @return the double precision determinant
3379: */
3380: public final double determinant() {
3381:
3382: if (isAffine()) {
3383: return mat[0] * (mat[5] * mat[10] - mat[6] * mat[9])
3384: - mat[1] * (mat[4] * mat[10] - mat[6] * mat[8])
3385: + mat[2] * (mat[4] * mat[9] - mat[5] * mat[8]);
3386: }
3387: // cofactor exapainsion along first row
3388: return mat[0]
3389: * (mat[5] * (mat[10] * mat[15] - mat[11] * mat[14])
3390: - mat[6]
3391: * (mat[9] * mat[15] - mat[11] * mat[13]) + mat[7]
3392: * (mat[9] * mat[14] - mat[10] * mat[13]))
3393: - mat[1]
3394: * (mat[4] * (mat[10] * mat[15] - mat[11] * mat[14])
3395: - mat[6]
3396: * (mat[8] * mat[15] - mat[11] * mat[12]) + mat[7]
3397: * (mat[8] * mat[14] - mat[10] * mat[12]))
3398: + mat[2]
3399: * (mat[4] * (mat[9] * mat[15] - mat[11] * mat[13])
3400: - mat[5]
3401: * (mat[8] * mat[15] - mat[11] * mat[12]) + mat[7]
3402: * (mat[8] * mat[13] - mat[9] * mat[12]))
3403: - mat[3]
3404: * (mat[4] * (mat[9] * mat[14] - mat[10] * mat[13])
3405: - mat[5]
3406: * (mat[8] * mat[14] - mat[10] * mat[12]) + mat[6]
3407: * (mat[8] * mat[13] - mat[9] * mat[12]));
3408: }
3409:
3410: /**
3411: * Sets the value of this transform to a uniform scale; all of
3412: * the matrix values are modified.
3413: * @param scale the scale factor for the transform
3414: */
3415: public final void set(double scale) {
3416: setScaleTranslation(0, 0, 0, scale);
3417: }
3418:
3419: /**
3420: * Sets the value of this transform to a scale and translation
3421: * matrix; the scale is not applied to the translation and all
3422: * of the matrix values are modified.
3423: * @param scale the scale factor for the transform
3424: * @param v1 the translation amount
3425: */
3426: public final void set(double scale, Vector3d v1) {
3427: setScaleTranslation(v1.x, v1.y, v1.z, scale);
3428: }
3429:
3430: /**
3431: * Sets the value of this transform to a scale and translation
3432: * matrix; the scale is not applied to the translation and all
3433: * of the matrix values are modified.
3434: * @param scale the scale factor for the transform
3435: * @param v1 the translation amount
3436: */
3437: public final void set(float scale, Vector3f v1) {
3438: setScaleTranslation(v1.x, v1.y, v1.z, scale);
3439: }
3440:
3441: /**
3442: * Sets the value of this transform to a scale and translation matrix;
3443: * the translation is scaled by the scale factor and all of the
3444: * matrix values are modified.
3445: * @param v1 the translation amount
3446: * @param scale the scale factor for the transform AND the translation
3447: */
3448: public final void set(Vector3d v1, double scale) {
3449: setScaleTranslation(v1.x * scale, v1.y * scale, v1.z * scale,
3450: scale);
3451: }
3452:
3453: /**
3454: * Sets the value of this transform to a scale and translation matrix;
3455: * the translation is scaled by the scale factor and all of the
3456: * matrix values are modified.
3457: * @param v1 the translation amount
3458: * @param scale the scale factor for the transform AND the translation
3459: */
3460: public final void set(Vector3f v1, float scale) {
3461: setScaleTranslation(v1.x * scale, v1.y * scale, v1.z * scale,
3462: scale);
3463: }
3464:
3465: private final void setScaleTranslation(double x, double y,
3466: double z, double scale) {
3467: mat[0] = scale;
3468: mat[1] = 0.0;
3469: mat[2] = 0.0;
3470: mat[3] = x;
3471: mat[4] = 0.0;
3472: mat[5] = scale;
3473: mat[6] = 0.0;
3474: mat[7] = y;
3475: mat[8] = 0.0;
3476: mat[9] = 0.0;
3477: mat[10] = scale;
3478: mat[11] = z;
3479: mat[12] = 0.0;
3480: mat[13] = 0.0;
3481: mat[14] = 0.0;
3482: mat[15] = 1.0;
3483:
3484: if (scales == null)
3485: scales = new double[3];
3486:
3487: scales[0] = scales[1] = scales[2] = scale;
3488:
3489: // Issue 253: set all dirty bits if input is infinity or NaN
3490: if (isInfOrNaN(x) || isInfOrNaN(y) || isInfOrNaN(z)
3491: || isInfOrNaN(scale)) {
3492: dirtyBits = ALL_DIRTY;
3493: return;
3494: }
3495:
3496: type = AFFINE | CONGRUENT | ORTHO;
3497: dirtyBits = CLASSIFY_BIT | ROTATION_BIT | RIGID_BIT;
3498: }
3499:
3500: /**
3501: * Multiplies each element of this transform by a scalar.
3502: * @param scalar the scalar multiplier
3503: */
3504: public final void mul(double scalar) {
3505: for (int i = 0; i < 16; i++) {
3506: mat[i] *= scalar;
3507: }
3508: dirtyBits = ALL_DIRTY;
3509: }
3510:
3511: /**
3512: * Multiplies each element of transform t1 by a scalar and places
3513: * the result into this. Transform t1 is not modified.
3514: * @param scalar the scalar multiplier
3515: * @param t1 the original transform
3516: */
3517: public final void mul(double scalar, Transform3D t1) {
3518: for (int i = 0; i < 16; i++) {
3519: mat[i] = t1.mat[i] * scalar;
3520: }
3521: dirtyBits = ALL_DIRTY;
3522:
3523: if (autoNormalize) {
3524: normalize();
3525: }
3526:
3527: }
3528:
3529: /**
3530: * Sets the value of this transform to the result of multiplying itself
3531: * with transform t1 (this = this * t1).
3532: * @param t1 the other transform
3533: */
3534: public final void mul(Transform3D t1) {
3535: double tmp0, tmp1, tmp2, tmp3;
3536: double tmp4, tmp5, tmp6, tmp7;
3537: double tmp8, tmp9, tmp10, tmp11;
3538: boolean aff = false;
3539:
3540: if (t1.isAffine()) {
3541: tmp0 = mat[0] * t1.mat[0] + mat[1] * t1.mat[4] + mat[2]
3542: * t1.mat[8];
3543: tmp1 = mat[0] * t1.mat[1] + mat[1] * t1.mat[5] + mat[2]
3544: * t1.mat[9];
3545: tmp2 = mat[0] * t1.mat[2] + mat[1] * t1.mat[6] + mat[2]
3546: * t1.mat[10];
3547: tmp3 = mat[0] * t1.mat[3] + mat[1] * t1.mat[7] + mat[2]
3548: * t1.mat[11] + mat[3];
3549: tmp4 = mat[4] * t1.mat[0] + mat[5] * t1.mat[4] + mat[6]
3550: * t1.mat[8];
3551: tmp5 = mat[4] * t1.mat[1] + mat[5] * t1.mat[5] + mat[6]
3552: * t1.mat[9];
3553: tmp6 = mat[4] * t1.mat[2] + mat[5] * t1.mat[6] + mat[6]
3554: * t1.mat[10];
3555: tmp7 = mat[4] * t1.mat[3] + mat[5] * t1.mat[7] + mat[6]
3556: * t1.mat[11] + mat[7];
3557: tmp8 = mat[8] * t1.mat[0] + mat[9] * t1.mat[4] + mat[10]
3558: * t1.mat[8];
3559: tmp9 = mat[8] * t1.mat[1] + mat[9] * t1.mat[5] + mat[10]
3560: * t1.mat[9];
3561: tmp10 = mat[8] * t1.mat[2] + mat[9] * t1.mat[6] + mat[10]
3562: * t1.mat[10];
3563: tmp11 = mat[8] * t1.mat[3] + mat[9] * t1.mat[7] + mat[10]
3564: * t1.mat[11] + mat[11];
3565: if (isAffine()) {
3566: mat[12] = mat[13] = mat[14] = 0;
3567: mat[15] = 1;
3568: aff = true;
3569: } else {
3570: double tmp12 = mat[12] * t1.mat[0] + mat[13]
3571: * t1.mat[4] + mat[14] * t1.mat[8];
3572: double tmp13 = mat[12] * t1.mat[1] + mat[13]
3573: * t1.mat[5] + mat[14] * t1.mat[9];
3574: double tmp14 = mat[12] * t1.mat[2] + mat[13]
3575: * t1.mat[6] + mat[14] * t1.mat[10];
3576: double tmp15 = mat[12] * t1.mat[3] + mat[13]
3577: * t1.mat[7] + mat[14] * t1.mat[11] + mat[15];
3578: mat[12] = tmp12;
3579: mat[13] = tmp13;
3580: mat[14] = tmp14;
3581: mat[15] = tmp15;
3582: }
3583: } else {
3584: tmp0 = mat[0] * t1.mat[0] + mat[1] * t1.mat[4] + mat[2]
3585: * t1.mat[8] + mat[3] * t1.mat[12];
3586: tmp1 = mat[0] * t1.mat[1] + mat[1] * t1.mat[5] + mat[2]
3587: * t1.mat[9] + mat[3] * t1.mat[13];
3588: tmp2 = mat[0] * t1.mat[2] + mat[1] * t1.mat[6] + mat[2]
3589: * t1.mat[10] + mat[3] * t1.mat[14];
3590: tmp3 = mat[0] * t1.mat[3] + mat[1] * t1.mat[7] + mat[2]
3591: * t1.mat[11] + mat[3] * t1.mat[15];
3592: tmp4 = mat[4] * t1.mat[0] + mat[5] * t1.mat[4] + mat[6]
3593: * t1.mat[8] + mat[7] * t1.mat[12];
3594: tmp5 = mat[4] * t1.mat[1] + mat[5] * t1.mat[5] + mat[6]
3595: * t1.mat[9] + mat[7] * t1.mat[13];
3596: tmp6 = mat[4] * t1.mat[2] + mat[5] * t1.mat[6] + mat[6]
3597: * t1.mat[10] + mat[7] * t1.mat[14];
3598: tmp7 = mat[4] * t1.mat[3] + mat[5] * t1.mat[7] + mat[6]
3599: * t1.mat[11] + mat[7] * t1.mat[15];
3600: tmp8 = mat[8] * t1.mat[0] + mat[9] * t1.mat[4] + mat[10]
3601: * t1.mat[8] + mat[11] * t1.mat[12];
3602: tmp9 = mat[8] * t1.mat[1] + mat[9] * t1.mat[5] + mat[10]
3603: * t1.mat[9] + mat[11] * t1.mat[13];
3604: tmp10 = mat[8] * t1.mat[2] + mat[9] * t1.mat[6] + mat[10]
3605: * t1.mat[10] + mat[11] * t1.mat[14];
3606: tmp11 = mat[8] * t1.mat[3] + mat[9] * t1.mat[7] + mat[10]
3607: * t1.mat[11] + mat[11] * t1.mat[15];
3608:
3609: if (isAffine()) {
3610: mat[12] = t1.mat[12];
3611: mat[13] = t1.mat[13];
3612: mat[14] = t1.mat[14];
3613: mat[15] = t1.mat[15];
3614: } else {
3615: double tmp12 = mat[12] * t1.mat[0] + mat[13]
3616: * t1.mat[4] + mat[14] * t1.mat[8] + mat[15]
3617: * t1.mat[12];
3618: double tmp13 = mat[12] * t1.mat[1] + mat[13]
3619: * t1.mat[5] + mat[14] * t1.mat[9] + mat[15]
3620: * t1.mat[13];
3621: double tmp14 = mat[12] * t1.mat[2] + mat[13]
3622: * t1.mat[6] + mat[14] * t1.mat[10] + mat[15]
3623: * t1.mat[14];
3624: double tmp15 = mat[12] * t1.mat[3] + mat[13]
3625: * t1.mat[7] + mat[14] * t1.mat[11] + mat[15]
3626: * t1.mat[15];
3627: mat[12] = tmp12;
3628: mat[13] = tmp13;
3629: mat[14] = tmp14;
3630: mat[15] = tmp15;
3631: }
3632: }
3633:
3634: mat[0] = tmp0;
3635: mat[1] = tmp1;
3636: mat[2] = tmp2;
3637: mat[3] = tmp3;
3638: mat[4] = tmp4;
3639: mat[5] = tmp5;
3640: mat[6] = tmp6;
3641: mat[7] = tmp7;
3642: mat[8] = tmp8;
3643: mat[9] = tmp9;
3644: mat[10] = tmp10;
3645: mat[11] = tmp11;
3646:
3647: if (((dirtyBits & CONGRUENT_BIT) == 0)
3648: && ((type & CONGRUENT) != 0)
3649: && ((t1.dirtyBits & CONGRUENT_BIT) == 0)
3650: && ((t1.type & CONGRUENT) != 0)) {
3651: type &= t1.type;
3652: dirtyBits |= t1.dirtyBits | CLASSIFY_BIT
3653: | ROTSCALESVD_DIRTY | RIGID_BIT;
3654: } else {
3655: if (aff) {
3656: dirtyBits = ORTHO_BIT | CONGRUENT_BIT | RIGID_BIT
3657: | CLASSIFY_BIT | ROTSCALESVD_DIRTY;
3658: } else {
3659: dirtyBits = ALL_DIRTY;
3660: }
3661: }
3662:
3663: if (autoNormalize) {
3664: normalize();
3665: }
3666:
3667: }
3668:
3669: /**
3670: * Sets the value of this transform to the result of multiplying transform
3671: * t1 by transform t2 (this = t1*t2).
3672: * @param t1 the left transform
3673: * @param t2 the right transform
3674: */
3675: public final void mul(Transform3D t1, Transform3D t2) {
3676: boolean aff = false;
3677: if ((this != t1) && (this != t2)) {
3678: if (t2.isAffine()) {
3679:
3680: mat[0] = t1.mat[0] * t2.mat[0] + t1.mat[1] * t2.mat[4]
3681: + t1.mat[2] * t2.mat[8];
3682: mat[1] = t1.mat[0] * t2.mat[1] + t1.mat[1] * t2.mat[5]
3683: + t1.mat[2] * t2.mat[9];
3684: mat[2] = t1.mat[0] * t2.mat[2] + t1.mat[1] * t2.mat[6]
3685: + t1.mat[2] * t2.mat[10];
3686: mat[3] = t1.mat[0] * t2.mat[3] + t1.mat[1] * t2.mat[7]
3687: + t1.mat[2] * t2.mat[11] + t1.mat[3];
3688: mat[4] = t1.mat[4] * t2.mat[0] + t1.mat[5] * t2.mat[4]
3689: + t1.mat[6] * t2.mat[8];
3690: mat[5] = t1.mat[4] * t2.mat[1] + t1.mat[5] * t2.mat[5]
3691: + t1.mat[6] * t2.mat[9];
3692: mat[6] = t1.mat[4] * t2.mat[2] + t1.mat[5] * t2.mat[6]
3693: + t1.mat[6] * t2.mat[10];
3694: mat[7] = t1.mat[4] * t2.mat[3] + t1.mat[5] * t2.mat[7]
3695: + t1.mat[6] * t2.mat[11] + t1.mat[7];
3696: mat[8] = t1.mat[8] * t2.mat[0] + t1.mat[9] * t2.mat[4]
3697: + t1.mat[10] * t2.mat[8];
3698: mat[9] = t1.mat[8] * t2.mat[1] + t1.mat[9] * t2.mat[5]
3699: + t1.mat[10] * t2.mat[9];
3700: mat[10] = t1.mat[8] * t2.mat[2] + t1.mat[9] * t2.mat[6]
3701: + t1.mat[10] * t2.mat[10];
3702: mat[11] = t1.mat[8] * t2.mat[3] + t1.mat[9] * t2.mat[7]
3703: + t1.mat[10] * t2.mat[11] + t1.mat[11];
3704: if (t1.isAffine()) {
3705: aff = true;
3706: mat[12] = mat[13] = mat[14] = 0;
3707: mat[15] = 1;
3708: } else {
3709: mat[12] = t1.mat[12] * t2.mat[0] + t1.mat[13]
3710: * t2.mat[4] + t1.mat[14] * t2.mat[8];
3711: mat[13] = t1.mat[12] * t2.mat[1] + t1.mat[13]
3712: * t2.mat[5] + t1.mat[14] * t2.mat[9];
3713: mat[14] = t1.mat[12] * t2.mat[2] + t1.mat[13]
3714: * t2.mat[6] + t1.mat[14] * t2.mat[10];
3715: mat[15] = t1.mat[12] * t2.mat[3] + t1.mat[13]
3716: * t2.mat[7] + t1.mat[14] * t2.mat[11]
3717: + t1.mat[15];
3718: }
3719: } else {
3720: mat[0] = t1.mat[0] * t2.mat[0] + t1.mat[1] * t2.mat[4]
3721: + t1.mat[2] * t2.mat[8] + t1.mat[3]
3722: * t2.mat[12];
3723: mat[1] = t1.mat[0] * t2.mat[1] + t1.mat[1] * t2.mat[5]
3724: + t1.mat[2] * t2.mat[9] + t1.mat[3]
3725: * t2.mat[13];
3726: mat[2] = t1.mat[0] * t2.mat[2] + t1.mat[1] * t2.mat[6]
3727: + t1.mat[2] * t2.mat[10] + t1.mat[3]
3728: * t2.mat[14];
3729: mat[3] = t1.mat[0] * t2.mat[3] + t1.mat[1] * t2.mat[7]
3730: + t1.mat[2] * t2.mat[11] + t1.mat[3]
3731: * t2.mat[15];
3732: mat[4] = t1.mat[4] * t2.mat[0] + t1.mat[5] * t2.mat[4]
3733: + t1.mat[6] * t2.mat[8] + t1.mat[7]
3734: * t2.mat[12];
3735: mat[5] = t1.mat[4] * t2.mat[1] + t1.mat[5] * t2.mat[5]
3736: + t1.mat[6] * t2.mat[9] + t1.mat[7]
3737: * t2.mat[13];
3738: mat[6] = t1.mat[4] * t2.mat[2] + t1.mat[5] * t2.mat[6]
3739: + t1.mat[6] * t2.mat[10] + t1.mat[7]
3740: * t2.mat[14];
3741: mat[7] = t1.mat[4] * t2.mat[3] + t1.mat[5] * t2.mat[7]
3742: + t1.mat[6] * t2.mat[11] + t1.mat[7]
3743: * t2.mat[15];
3744: mat[8] = t1.mat[8] * t2.mat[0] + t1.mat[9] * t2.mat[4]
3745: + t1.mat[10] * t2.mat[8] + t1.mat[11]
3746: * t2.mat[12];
3747: mat[9] = t1.mat[8] * t2.mat[1] + t1.mat[9] * t2.mat[5]
3748: + t1.mat[10] * t2.mat[9] + t1.mat[11]
3749: * t2.mat[13];
3750: mat[10] = t1.mat[8] * t2.mat[2] + t1.mat[9] * t2.mat[6]
3751: + t1.mat[10] * t2.mat[10] + t1.mat[11]
3752: * t2.mat[14];
3753: mat[11] = t1.mat[8] * t2.mat[3] + t1.mat[9] * t2.mat[7]
3754: + t1.mat[10] * t2.mat[11] + t1.mat[11]
3755: * t2.mat[15];
3756: if (t1.isAffine()) {
3757: mat[12] = t2.mat[12];
3758: mat[13] = t2.mat[13];
3759: mat[14] = t2.mat[14];
3760: mat[15] = t2.mat[15];
3761: } else {
3762: mat[12] = t1.mat[12] * t2.mat[0] + t1.mat[13]
3763: * t2.mat[4] + t1.mat[14] * t2.mat[8]
3764: + t1.mat[15] * t2.mat[12];
3765: mat[13] = t1.mat[12] * t2.mat[1] + t1.mat[13]
3766: * t2.mat[5] + t1.mat[14] * t2.mat[9]
3767: + t1.mat[15] * t2.mat[13];
3768: mat[14] = t1.mat[12] * t2.mat[2] + t1.mat[13]
3769: * t2.mat[6] + t1.mat[14] * t2.mat[10]
3770: + t1.mat[15] * t2.mat[14];
3771: mat[15] = t1.mat[12] * t2.mat[3] + t1.mat[13]
3772: * t2.mat[7] + t1.mat[14] * t2.mat[11]
3773: + t1.mat[15] * t2.mat[15];
3774: }
3775: }
3776: } else {
3777: double tmp0, tmp1, tmp2, tmp3;
3778: double tmp4, tmp5, tmp6, tmp7;
3779: double tmp8, tmp9, tmp10, tmp11;
3780:
3781: if (t2.isAffine()) {
3782: tmp0 = t1.mat[0] * t2.mat[0] + t1.mat[1] * t2.mat[4]
3783: + t1.mat[2] * t2.mat[8];
3784: tmp1 = t1.mat[0] * t2.mat[1] + t1.mat[1] * t2.mat[5]
3785: + t1.mat[2] * t2.mat[9];
3786: tmp2 = t1.mat[0] * t2.mat[2] + t1.mat[1] * t2.mat[6]
3787: + t1.mat[2] * t2.mat[10];
3788: tmp3 = t1.mat[0] * t2.mat[3] + t1.mat[1] * t2.mat[7]
3789: + t1.mat[2] * t2.mat[11] + t1.mat[3];
3790: tmp4 = t1.mat[4] * t2.mat[0] + t1.mat[5] * t2.mat[4]
3791: + t1.mat[6] * t2.mat[8];
3792: tmp5 = t1.mat[4] * t2.mat[1] + t1.mat[5] * t2.mat[5]
3793: + t1.mat[6] * t2.mat[9];
3794: tmp6 = t1.mat[4] * t2.mat[2] + t1.mat[5] * t2.mat[6]
3795: + t1.mat[6] * t2.mat[10];
3796: tmp7 = t1.mat[4] * t2.mat[3] + t1.mat[5] * t2.mat[7]
3797: + t1.mat[6] * t2.mat[11] + t1.mat[7];
3798: tmp8 = t1.mat[8] * t2.mat[0] + t1.mat[9] * t2.mat[4]
3799: + t1.mat[10] * t2.mat[8];
3800: tmp9 = t1.mat[8] * t2.mat[1] + t1.mat[9] * t2.mat[5]
3801: + t1.mat[10] * t2.mat[9];
3802: tmp10 = t1.mat[8] * t2.mat[2] + t1.mat[9] * t2.mat[6]
3803: + t1.mat[10] * t2.mat[10];
3804: tmp11 = t1.mat[8] * t2.mat[3] + t1.mat[9] * t2.mat[7]
3805: + t1.mat[10] * t2.mat[11] + t1.mat[11];
3806: if (t1.isAffine()) {
3807: aff = true;
3808: mat[12] = mat[13] = mat[14] = 0;
3809: mat[15] = 1;
3810: } else {
3811: double tmp12 = t1.mat[12] * t2.mat[0] + t1.mat[13]
3812: * t2.mat[4] + t1.mat[14] * t2.mat[8];
3813: double tmp13 = t1.mat[12] * t2.mat[1] + t1.mat[13]
3814: * t2.mat[5] + t1.mat[14] * t2.mat[9];
3815: double tmp14 = t1.mat[12] * t2.mat[2] + t1.mat[13]
3816: * t2.mat[6] + t1.mat[14] * t2.mat[10];
3817: double tmp15 = t1.mat[12] * t2.mat[3] + t1.mat[13]
3818: * t2.mat[7] + t1.mat[14] * t2.mat[11]
3819: + t1.mat[15];
3820: mat[12] = tmp12;
3821: mat[13] = tmp13;
3822: mat[14] = tmp14;
3823: mat[15] = tmp15;
3824: }
3825: } else {
3826: tmp0 = t1.mat[0] * t2.mat[0] + t1.mat[1] * t2.mat[4]
3827: + t1.mat[2] * t2.mat[8] + t1.mat[3]
3828: * t2.mat[12];
3829: tmp1 = t1.mat[0] * t2.mat[1] + t1.mat[1] * t2.mat[5]
3830: + t1.mat[2] * t2.mat[9] + t1.mat[3]
3831: * t2.mat[13];
3832: tmp2 = t1.mat[0] * t2.mat[2] + t1.mat[1] * t2.mat[6]
3833: + t1.mat[2] * t2.mat[10] + t1.mat[3]
3834: * t2.mat[14];
3835: tmp3 = t1.mat[0] * t2.mat[3] + t1.mat[1] * t2.mat[7]
3836: + t1.mat[2] * t2.mat[11] + t1.mat[3]
3837: * t2.mat[15];
3838: tmp4 = t1.mat[4] * t2.mat[0] + t1.mat[5] * t2.mat[4]
3839: + t1.mat[6] * t2.mat[8] + t1.mat[7]
3840: * t2.mat[12];
3841: tmp5 = t1.mat[4] * t2.mat[1] + t1.mat[5] * t2.mat[5]
3842: + t1.mat[6] * t2.mat[9] + t1.mat[7]
3843: * t2.mat[13];
3844: tmp6 = t1.mat[4] * t2.mat[2] + t1.mat[5] * t2.mat[6]
3845: + t1.mat[6] * t2.mat[10] + t1.mat[7]
3846: * t2.mat[14];
3847: tmp7 = t1.mat[4] * t2.mat[3] + t1.mat[5] * t2.mat[7]
3848: + t1.mat[6] * t2.mat[11] + t1.mat[7]
3849: * t2.mat[15];
3850: tmp8 = t1.mat[8] * t2.mat[0] + t1.mat[9] * t2.mat[4]
3851: + t1.mat[10] * t2.mat[8] + t1.mat[11]
3852: * t2.mat[12];
3853: tmp9 = t1.mat[8] * t2.mat[1] + t1.mat[9] * t2.mat[5]
3854: + t1.mat[10] * t2.mat[9] + t1.mat[11]
3855: * t2.mat[13];
3856: tmp10 = t1.mat[8] * t2.mat[2] + t1.mat[9] * t2.mat[6]
3857: + t1.mat[10] * t2.mat[10] + t1.mat[11]
3858: * t2.mat[14];
3859: tmp11 = t1.mat[8] * t2.mat[3] + t1.mat[9] * t2.mat[7]
3860: + t1.mat[10] * t2.mat[11] + t1.mat[11]
3861: * t2.mat[15];
3862:
3863: if (t1.isAffine()) {
3864: mat[12] = t2.mat[12];
3865: mat[13] = t2.mat[13];
3866: mat[14] = t2.mat[14];
3867: mat[15] = t2.mat[15];
3868: } else {
3869: double tmp12 = t1.mat[12] * t2.mat[0] + t1.mat[13]
3870: * t2.mat[4] + t1.mat[14] * t2.mat[8]
3871: + t1.mat[15] * t2.mat[12];
3872: double tmp13 = t1.mat[12] * t2.mat[1] + t1.mat[13]
3873: * t2.mat[5] + t1.mat[14] * t2.mat[9]
3874: + t1.mat[15] * t2.mat[13];
3875: double tmp14 = t1.mat[12] * t2.mat[2] + t1.mat[13]
3876: * t2.mat[6] + t1.mat[14] * t2.mat[10]
3877: + t1.mat[15] * t2.mat[14];
3878: double tmp15 = t1.mat[12] * t2.mat[3] + t1.mat[13]
3879: * t2.mat[7] + t1.mat[14] * t2.mat[11]
3880: + t1.mat[15] * t2.mat[15];
3881: mat[12] = tmp12;
3882: mat[13] = tmp13;
3883: mat[14] = tmp14;
3884: mat[15] = tmp15;
3885: }
3886: }
3887: mat[0] = tmp0;
3888: mat[1] = tmp1;
3889: mat[2] = tmp2;
3890: mat[3] = tmp3;
3891: mat[4] = tmp4;
3892: mat[5] = tmp5;
3893: mat[6] = tmp6;
3894: mat[7] = tmp7;
3895: mat[8] = tmp8;
3896: mat[9] = tmp9;
3897: mat[10] = tmp10;
3898: mat[11] = tmp11;
3899: }
3900:
3901: if (((t1.dirtyBits & CONGRUENT_BIT) == 0)
3902: && ((t1.type & CONGRUENT) != 0)
3903: && ((t2.dirtyBits & CONGRUENT_BIT) == 0)
3904: && ((t2.type & CONGRUENT) != 0)) {
3905: type = t1.type & t2.type;
3906: dirtyBits = t1.dirtyBits | t2.dirtyBits | CLASSIFY_BIT
3907: | ROTSCALESVD_DIRTY | RIGID_BIT;
3908: } else {
3909: if (aff) {
3910: dirtyBits = ORTHO_BIT | CONGRUENT_BIT | RIGID_BIT
3911: | CLASSIFY_BIT | ROTSCALESVD_DIRTY;
3912: } else {
3913: dirtyBits = ALL_DIRTY;
3914: }
3915: }
3916:
3917: if (autoNormalize) {
3918: normalize();
3919: }
3920: }
3921:
3922: /**
3923: * Multiplies this transform by the inverse of transform t1. The final
3924: * value is placed into this matrix (this = this*t1^-1).
3925: * @param t1 the matrix whose inverse is computed.
3926: */
3927: public final void mulInverse(Transform3D t1) {
3928: Transform3D t2 = new Transform3D();
3929: t2.autoNormalize = false;
3930: t2.invert(t1);
3931: this .mul(t2);
3932: }
3933:
3934: /**
3935: * Multiplies transform t1 by the inverse of transform t2. The final
3936: * value is placed into this matrix (this = t1*t2^-1).
3937: * @param t1 the left transform in the multiplication
3938: * @param t2 the transform whose inverse is computed.
3939: */
3940: public final void mulInverse(Transform3D t1, Transform3D t2) {
3941: Transform3D t3 = new Transform3D();
3942: t3.autoNormalize = false;
3943: t3.invert(t2);
3944: this .mul(t1, t3);
3945: }
3946:
3947: /**
3948: * Multiplies transform t1 by the transpose of transform t2 and places
3949: * the result into this transform (this = t1 * transpose(t2)).
3950: * @param t1 the transform on the left hand side of the multiplication
3951: * @param t2 the transform whose transpose is computed
3952: */
3953: public final void mulTransposeRight(Transform3D t1, Transform3D t2) {
3954: Transform3D t3 = new Transform3D();
3955: t3.autoNormalize = false;
3956: t3.transpose(t2);
3957: mul(t1, t3);
3958: }
3959:
3960: /**
3961: * Multiplies the transpose of transform t1 by transform t2 and places
3962: * the result into this matrix (this = transpose(t1) * t2).
3963: * @param t1 the transform whose transpose is computed
3964: * @param t2 the transform on the right hand side of the multiplication
3965: */
3966: public final void mulTransposeLeft(Transform3D t1, Transform3D t2) {
3967: Transform3D t3 = new Transform3D();
3968: t3.autoNormalize = false;
3969: t3.transpose(t1);
3970: mul(t3, t2);
3971: }
3972:
3973: /**
3974: * Multiplies the transpose of transform t1 by the transpose of
3975: * transform t2 and places the result into this transform
3976: * (this = transpose(t1) * transpose(t2)).
3977: * @param t1 the transform on the left hand side of the multiplication
3978: * @param t2 the transform on the right hand side of the multiplication
3979: */
3980: public final void mulTransposeBoth(Transform3D t1, Transform3D t2) {
3981: Transform3D t3 = new Transform3D();
3982: Transform3D t4 = new Transform3D();
3983: t3.autoNormalize = false;
3984: t4.autoNormalize = false;
3985: t3.transpose(t1);
3986: t4.transpose(t2);
3987: mul(t3, t4);
3988: }
3989:
3990: /**
3991: * Normalizes the rotational components (upper 3x3) of this matrix
3992: * in place using a Singular Value Decomposition (SVD).
3993: * This operation ensures that the column vectors of this matrix
3994: * are orthogonal to each other. The primary use of this method
3995: * is to correct for floating point errors that accumulate over
3996: * time when concatenating a large number of rotation matrices.
3997: * Note that the scale of the matrix is not altered by this method.
3998: */
3999: public final void normalize() {
4000: // Issue 253: Unable to normalize matrices with infinity or NaN
4001: if (!isAffine() && isInfOrNaN()) {
4002: return;
4003: }
4004:
4005: if ((dirtyBits & (ROTATION_BIT | SVD_BIT)) != 0) {
4006: computeScaleRotation(true);
4007: } else if ((dirtyBits & (SCALE_BIT | SVD_BIT)) != 0) {
4008: computeScales(true);
4009: }
4010:
4011: mat[0] = rot[0] * scales[0];
4012: mat[1] = rot[1] * scales[1];
4013: mat[2] = rot[2] * scales[2];
4014: mat[4] = rot[3] * scales[0];
4015: mat[5] = rot[4] * scales[1];
4016: mat[6] = rot[5] * scales[2];
4017: mat[8] = rot[6] * scales[0];
4018: mat[9] = rot[7] * scales[1];
4019: mat[10] = rot[8] * scales[2];
4020: dirtyBits |= CLASSIFY_BIT;
4021: dirtyBits &= ~ORTHO_BIT;
4022: type |= ORTHO;
4023: }
4024:
4025: /**
4026: * Normalizes the rotational components (upper 3x3) of transform t1
4027: * using a Singular Value Decomposition (SVD), and places the result
4028: * into this transform.
4029: * This operation ensures that the column vectors of this matrix
4030: * are orthogonal to each other. The primary use of this method
4031: * is to correct for floating point errors that accumulate over
4032: * time when concatenating a large number of rotation matrices.
4033: * Note that the scale of the matrix is not altered by this method.
4034: *
4035: * @param t1 the source transform, which is not modified
4036: */
4037: public final void normalize(Transform3D t1) {
4038: set(t1);
4039: normalize();
4040: }
4041:
4042: /**
4043: * Normalizes the rotational components (upper 3x3) of this transform
4044: * in place using a Cross Product (CP) normalization.
4045: * This operation ensures that the column vectors of this matrix
4046: * are orthogonal to each other. The primary use of this method
4047: * is to correct for floating point errors that accumulate over
4048: * time when concatenating a large number of rotation matrices.
4049: * Note that the scale of the matrix is not altered by this method.
4050: */
4051: public final void normalizeCP() {
4052: // Issue 253: Unable to normalize matrices with infinity or NaN
4053: if (!isAffine() && isInfOrNaN()) {
4054: return;
4055: }
4056:
4057: if ((dirtyBits & SCALE_BIT) != 0) {
4058: computeScales(false);
4059: }
4060:
4061: double mag = mat[0] * mat[0] + mat[4] * mat[4] + mat[8]
4062: * mat[8];
4063:
4064: if (mag != 0) {
4065: mag = 1.0 / Math.sqrt(mag);
4066: mat[0] = mat[0] * mag;
4067: mat[4] = mat[4] * mag;
4068: mat[8] = mat[8] * mag;
4069: }
4070:
4071: mag = mat[1] * mat[1] + mat[5] * mat[5] + mat[9] * mat[9];
4072:
4073: if (mag != 0) {
4074: mag = 1.0 / Math.sqrt(mag);
4075: mat[1] = mat[1] * mag;
4076: mat[5] = mat[5] * mag;
4077: mat[9] = mat[9] * mag;
4078: }
4079: mat[2] = (mat[4] * mat[9] - mat[5] * mat[8]) * scales[0];
4080: mat[6] = (mat[1] * mat[8] - mat[0] * mat[9]) * scales[1];
4081: mat[10] = (mat[0] * mat[5] - mat[1] * mat[4]) * scales[2];
4082:
4083: mat[0] *= scales[0];
4084: mat[1] *= scales[0];
4085: mat[4] *= scales[1];
4086: mat[5] *= scales[1];
4087: mat[8] *= scales[2];
4088: mat[9] *= scales[2];
4089:
4090: // leave the AFFINE bit
4091: dirtyBits |= CONGRUENT_BIT | RIGID_BIT | CLASSIFY_BIT
4092: | ROTATION_BIT | SVD_BIT;
4093: dirtyBits &= ~ORTHO_BIT;
4094: type |= ORTHO;
4095: }
4096:
4097: /**
4098: * Normalizes the rotational components (upper 3x3) of transform t1
4099: * using a Cross Product (CP) normalization, and
4100: * places the result into this transform.
4101: * This operation ensures that the column vectors of this matrix
4102: * are orthogonal to each other. The primary use of this method
4103: * is to correct for floating point errors that accumulate over
4104: * time when concatenating a large number of rotation matrices.
4105: * Note that the scale of the matrix is not altered by this method.
4106: *
4107: * @param t1 the transform to be normalized
4108: */
4109: public final void normalizeCP(Transform3D t1) {
4110: set(t1);
4111: normalizeCP();
4112: }
4113:
4114: /**
4115: * Returns true if all of the data members of transform t1 are
4116: * equal to the corresponding data members in this Transform3D.
4117: * @param t1 the transform with which the comparison is made
4118: * @return true or false
4119: */
4120: public boolean equals(Transform3D t1) {
4121: return (t1 != null) && (mat[0] == t1.mat[0])
4122: && (mat[1] == t1.mat[1]) && (mat[2] == t1.mat[2])
4123: && (mat[3] == t1.mat[3]) && (mat[4] == t1.mat[4])
4124: && (mat[5] == t1.mat[5]) && (mat[6] == t1.mat[6])
4125: && (mat[7] == t1.mat[7]) && (mat[8] == t1.mat[8])
4126: && (mat[9] == t1.mat[9]) && (mat[10] == t1.mat[10])
4127: && (mat[11] == t1.mat[11]) && (mat[12] == t1.mat[12])
4128: && (mat[13] == t1.mat[13]) && (mat[14] == t1.mat[14])
4129: && (mat[15] == t1.mat[15]);
4130: }
4131:
4132: /**
4133: * Returns true if the Object o1 is of type Transform3D and all of the
4134: * data members of o1 are equal to the corresponding data members in
4135: * this Transform3D.
4136: * @param o1 the object with which the comparison is made.
4137: * @return true or false
4138: */
4139: public boolean equals(Object o1) {
4140: return (o1 instanceof Transform3D) && equals((Transform3D) o1);
4141: }
4142:
4143: /**
4144: * Returns true if the L-infinite distance between this matrix
4145: * and matrix m1 is less than or equal to the epsilon parameter,
4146: * otherwise returns false. The L-infinite
4147: * distance is equal to
4148: * MAX[i=0,1,2,3 ; j=0,1,2,3 ; abs[(this.m(i,j) - m1.m(i,j)]
4149: * @param t1 the transform to be compared to this transform
4150: * @param epsilon the threshold value
4151: */
4152: public boolean epsilonEquals(Transform3D t1, double epsilon) {
4153: double diff;
4154:
4155: for (int i = 0; i < 16; i++) {
4156: diff = mat[i] - t1.mat[i];
4157: if ((diff < 0 ? -diff : diff) > epsilon) {
4158: return false;
4159: }
4160: }
4161: return true;
4162: }
4163:
4164: /**
4165: * Returns a hash code value based on the data values in this
4166: * object. Two different Transform3D objects with identical data
4167: * values (i.e., Transform3D.equals returns true) will return the
4168: * same hash number. Two Transform3D objects with different data
4169: * members may return the same hash value, although this is not
4170: * likely.
4171: * @return the integer hash code value
4172: */
4173: public int hashCode() {
4174: long bits = 1L;
4175:
4176: for (int i = 0; i < 16; i++) {
4177: bits = 31L * bits + HashCodeUtil.doubleToLongBits(mat[i]);
4178: }
4179: return (int) (bits ^ (bits >> 32));
4180: }
4181:
4182: /**
4183: * Transform the vector vec using this transform and place the
4184: * result into vecOut.
4185: * @param vec the double precision vector to be transformed
4186: * @param vecOut the vector into which the transformed values are placed
4187: */
4188: public final void transform(Vector4d vec, Vector4d vecOut) {
4189:
4190: if (vec != vecOut) {
4191: vecOut.x = (mat[0] * vec.x + mat[1] * vec.y + mat[2]
4192: * vec.z + mat[3] * vec.w);
4193: vecOut.y = (mat[4] * vec.x + mat[5] * vec.y + mat[6]
4194: * vec.z + mat[7] * vec.w);
4195: vecOut.z = (mat[8] * vec.x + mat[9] * vec.y + mat[10]
4196: * vec.z + mat[11] * vec.w);
4197: vecOut.w = (mat[12] * vec.x + mat[13] * vec.y + mat[14]
4198: * vec.z + mat[15] * vec.w);
4199: } else {
4200: transform(vec);
4201: }
4202: }
4203:
4204: /**
4205: * Transform the vector vec using this Transform and place the
4206: * result back into vec.
4207: * @param vec the double precision vector to be transformed
4208: */
4209: public final void transform(Vector4d vec) {
4210: double x = (mat[0] * vec.x + mat[1] * vec.y + mat[2] * vec.z + mat[3]
4211: * vec.w);
4212: double y = (mat[4] * vec.x + mat[5] * vec.y + mat[6] * vec.z + mat[7]
4213: * vec.w);
4214: double z = (mat[8] * vec.x + mat[9] * vec.y + mat[10] * vec.z + mat[11]
4215: * vec.w);
4216: vec.w = (mat[12] * vec.x + mat[13] * vec.y + mat[14] * vec.z + mat[15]
4217: * vec.w);
4218: vec.x = x;
4219: vec.y = y;
4220: vec.z = z;
4221: }
4222:
4223: /**
4224: * Transform the vector vec using this Transform and place the
4225: * result into vecOut.
4226: * @param vec the single precision vector to be transformed
4227: * @param vecOut the vector into which the transformed values are placed
4228: */
4229: public final void transform(Vector4f vec, Vector4f vecOut) {
4230: if (vecOut != vec) {
4231: vecOut.x = (float) (mat[0] * vec.x + mat[1] * vec.y
4232: + mat[2] * vec.z + mat[3] * vec.w);
4233: vecOut.y = (float) (mat[4] * vec.x + mat[5] * vec.y
4234: + mat[6] * vec.z + mat[7] * vec.w);
4235: vecOut.z = (float) (mat[8] * vec.x + mat[9] * vec.y
4236: + mat[10] * vec.z + mat[11] * vec.w);
4237: vecOut.w = (float) (mat[12] * vec.x + mat[13] * vec.y
4238: + mat[14] * vec.z + mat[15] * vec.w);
4239: } else {
4240: transform(vec);
4241: }
4242: }
4243:
4244: /**
4245: * Transform the vector vec using this Transform and place the
4246: * result back into vec.
4247: * @param vec the single precision vector to be transformed
4248: */
4249: public final void transform(Vector4f vec) {
4250: float x = (float) (mat[0] * vec.x + mat[1] * vec.y + mat[2]
4251: * vec.z + mat[3] * vec.w);
4252: float y = (float) (mat[4] * vec.x + mat[5] * vec.y + mat[6]
4253: * vec.z + mat[7] * vec.w);
4254: float z = (float) (mat[8] * vec.x + mat[9] * vec.y + mat[10]
4255: * vec.z + mat[11] * vec.w);
4256: vec.w = (float) (mat[12] * vec.x + mat[13] * vec.y + mat[14]
4257: * vec.z + mat[15] * vec.w);
4258: vec.x = x;
4259: vec.y = y;
4260: vec.z = z;
4261: }
4262:
4263: /**
4264: * Transforms the point parameter with this transform and
4265: * places the result into pointOut. The fourth element of the
4266: * point input paramter is assumed to be one.
4267: * @param point the input point to be transformed
4268: * @param pointOut the transformed point
4269: */
4270: public final void transform(Point3d point, Point3d pointOut) {
4271: if (point != pointOut) {
4272: pointOut.x = mat[0] * point.x + mat[1] * point.y + mat[2]
4273: * point.z + mat[3];
4274: pointOut.y = mat[4] * point.x + mat[5] * point.y + mat[6]
4275: * point.z + mat[7];
4276: pointOut.z = mat[8] * point.x + mat[9] * point.y + mat[10]
4277: * point.z + mat[11];
4278: } else {
4279: transform(point);
4280: }
4281: }
4282:
4283: /**
4284: * Transforms the point parameter with this transform and
4285: * places the result back into point. The fourth element of the
4286: * point input paramter is assumed to be one.
4287: * @param point the input point to be transformed
4288: */
4289: public final void transform(Point3d point) {
4290: double x = mat[0] * point.x + mat[1] * point.y + mat[2]
4291: * point.z + mat[3];
4292: double y = mat[4] * point.x + mat[5] * point.y + mat[6]
4293: * point.z + mat[7];
4294: point.z = mat[8] * point.x + mat[9] * point.y + mat[10]
4295: * point.z + mat[11];
4296: point.x = x;
4297: point.y = y;
4298: }
4299:
4300: /**
4301: * Transforms the normal parameter by this transform and places the value
4302: * into normalOut. The fourth element of the normal is assumed to be zero.
4303: * @param normal the input normal to be transformed
4304: * @param normalOut the transformed normal
4305: */
4306: public final void transform(Vector3d normal, Vector3d normalOut) {
4307: if (normalOut != normal) {
4308: normalOut.x = mat[0] * normal.x + mat[1] * normal.y
4309: + mat[2] * normal.z;
4310: normalOut.y = mat[4] * normal.x + mat[5] * normal.y
4311: + mat[6] * normal.z;
4312: normalOut.z = mat[8] * normal.x + mat[9] * normal.y
4313: + mat[10] * normal.z;
4314: } else {
4315: transform(normal);
4316: }
4317: }
4318:
4319: /**
4320: * Transforms the normal parameter by this transform and places the value
4321: * back into normal. The fourth element of the normal is assumed to be zero.
4322: * @param normal the input normal to be transformed
4323: */
4324: public final void transform(Vector3d normal) {
4325: double x = mat[0] * normal.x + mat[1] * normal.y + mat[2]
4326: * normal.z;
4327: double y = mat[4] * normal.x + mat[5] * normal.y + mat[6]
4328: * normal.z;
4329: normal.z = mat[8] * normal.x + mat[9] * normal.y + mat[10]
4330: * normal.z;
4331: normal.x = x;
4332: normal.y = y;
4333: }
4334:
4335: /**
4336: * Transforms the point parameter with this transform and
4337: * places the result into pointOut. The fourth element of the
4338: * point input paramter is assumed to be one.
4339: * @param point the input point to be transformed
4340: * @param pointOut the transformed point
4341: */
4342: public final void transform(Point3f point, Point3f pointOut) {
4343: if (point != pointOut) {
4344: pointOut.x = (float) (mat[0] * point.x + mat[1] * point.y
4345: + mat[2] * point.z + mat[3]);
4346: pointOut.y = (float) (mat[4] * point.x + mat[5] * point.y
4347: + mat[6] * point.z + mat[7]);
4348: pointOut.z = (float) (mat[8] * point.x + mat[9] * point.y
4349: + mat[10] * point.z + mat[11]);
4350: } else {
4351: transform(point);
4352: }
4353: }
4354:
4355: /**
4356: * Transforms the point parameter with this transform and
4357: * places the result back into point. The fourth element of the
4358: * point input paramter is assumed to be one.
4359: * @param point the input point to be transformed
4360: */
4361: public final void transform(Point3f point) {
4362: float x = (float) (mat[0] * point.x + mat[1] * point.y + mat[2]
4363: * point.z + mat[3]);
4364: float y = (float) (mat[4] * point.x + mat[5] * point.y + mat[6]
4365: * point.z + mat[7]);
4366: point.z = (float) (mat[8] * point.x + mat[9] * point.y
4367: + mat[10] * point.z + mat[11]);
4368: point.x = x;
4369: point.y = y;
4370: }
4371:
4372: /**
4373: * Transforms the normal parameter by this transform and places the value
4374: * into normalOut. The fourth element of the normal is assumed to be zero.
4375: * Note: For correct lighting results, if a transform has uneven scaling
4376: * surface normals should transformed by the inverse transpose of
4377: * the transform. This the responsibility of the application and is not
4378: * done automatically by this method.
4379: * @param normal the input normal to be transformed
4380: * @param normalOut the transformed normal
4381: */
4382: public final void transform(Vector3f normal, Vector3f normalOut) {
4383: if (normal != normalOut) {
4384: normalOut.x = (float) (mat[0] * normal.x + mat[1]
4385: * normal.y + mat[2] * normal.z);
4386: normalOut.y = (float) (mat[4] * normal.x + mat[5]
4387: * normal.y + mat[6] * normal.z);
4388: normalOut.z = (float) (mat[8] * normal.x + mat[9]
4389: * normal.y + mat[10] * normal.z);
4390: } else {
4391: transform(normal);
4392: }
4393: }
4394:
4395: /**
4396: * Transforms the normal parameter by this transform and places the value
4397: * back into normal. The fourth element of the normal is assumed to be zero.
4398: * Note: For correct lighting results, if a transform has uneven scaling
4399: * surface normals should transformed by the inverse transpose of
4400: * the transform. This the responsibility of the application and is not
4401: * done automatically by this method.
4402: * @param normal the input normal to be transformed
4403: */
4404: public final void transform(Vector3f normal) {
4405: float x = (float) (mat[0] * normal.x + mat[1] * normal.y + mat[2]
4406: * normal.z);
4407: float y = (float) (mat[4] * normal.x + mat[5] * normal.y + mat[6]
4408: * normal.z);
4409: normal.z = (float) (mat[8] * normal.x + mat[9] * normal.y + mat[10]
4410: * normal.z);
4411: normal.x = x;
4412: normal.y = y;
4413: }
4414:
4415: /**
4416: * Replaces the upper 3x3 matrix values of this transform with the
4417: * values in the matrix m1.
4418: * @param m1 the matrix that will be the new upper 3x3
4419: */
4420: public final void setRotationScale(Matrix3f m1) {
4421: mat[0] = m1.m00;
4422: mat[1] = m1.m01;
4423: mat[2] = m1.m02;
4424: mat[4] = m1.m10;
4425: mat[5] = m1.m11;
4426: mat[6] = m1.m12;
4427: mat[8] = m1.m20;
4428: mat[9] = m1.m21;
4429: mat[10] = m1.m22;
4430:
4431: // Issue 253: set all dirty bits
4432: dirtyBits = ALL_DIRTY;
4433:
4434: if (autoNormalize) {
4435: normalize();
4436: }
4437: }
4438:
4439: /**
4440: * Replaces the upper 3x3 matrix values of this transform with the
4441: * values in the matrix m1.
4442: * @param m1 the matrix that will be the new upper 3x3
4443: */
4444: public final void setRotationScale(Matrix3d m1) {
4445: mat[0] = m1.m00;
4446: mat[1] = m1.m01;
4447: mat[2] = m1.m02;
4448: mat[4] = m1.m10;
4449: mat[5] = m1.m11;
4450: mat[6] = m1.m12;
4451: mat[8] = m1.m20;
4452: mat[9] = m1.m21;
4453: mat[10] = m1.m22;
4454:
4455: // Issue 253: set all dirty bits
4456: dirtyBits = ALL_DIRTY;
4457:
4458: if (autoNormalize) {
4459: normalize();
4460: }
4461: }
4462:
4463: /**
4464: * Scales transform t1 by a Uniform scale matrix with scale
4465: * factor s and then adds transform t2 (this = S*t1 + t2).
4466: * @param s the scale factor
4467: * @param t1 the transform to be scaled
4468: * @param t2 the transform to be added
4469: */
4470: public final void scaleAdd(double s, Transform3D t1, Transform3D t2) {
4471: for (int i = 0; i < 16; i++) {
4472: mat[i] = s * t1.mat[i] + t2.mat[i];
4473: }
4474:
4475: dirtyBits = ALL_DIRTY;
4476:
4477: if (autoNormalize) {
4478: normalize();
4479: }
4480: }
4481:
4482: /**
4483: * Scales this transform by a Uniform scale matrix with scale factor
4484: * s and then adds transform t1 (this = S*this + t1).
4485: * @param s the scale factor
4486: * @param t1 the transform to be added
4487: */
4488: public final void scaleAdd(double s, Transform3D t1) {
4489: for (int i = 0; i < 16; i++) {
4490: mat[i] = s * mat[i] + t1.mat[i];
4491: }
4492:
4493: dirtyBits = ALL_DIRTY;
4494:
4495: if (autoNormalize) {
4496: normalize();
4497: }
4498: }
4499:
4500: /**
4501: * Gets the upper 3x3 values of this matrix and places them into
4502: * the matrix m1.
4503: * @param m1 the matrix that will hold the values
4504: */
4505: public final void getRotationScale(Matrix3f m1) {
4506: m1.m00 = (float) mat[0];
4507: m1.m01 = (float) mat[1];
4508: m1.m02 = (float) mat[2];
4509: m1.m10 = (float) mat[4];
4510: m1.m11 = (float) mat[5];
4511: m1.m12 = (float) mat[6];
4512: m1.m20 = (float) mat[8];
4513: m1.m21 = (float) mat[9];
4514: m1.m22 = (float) mat[10];
4515: }
4516:
4517: /**
4518: * Gets the upper 3x3 values of this matrix and places them into
4519: * the matrix m1.
4520: * @param m1 the matrix that will hold the values
4521: */
4522: public final void getRotationScale(Matrix3d m1) {
4523: m1.m00 = mat[0];
4524: m1.m01 = mat[1];
4525: m1.m02 = mat[2];
4526: m1.m10 = mat[4];
4527: m1.m11 = mat[5];
4528: m1.m12 = mat[6];
4529: m1.m20 = mat[8];
4530: m1.m21 = mat[9];
4531: m1.m22 = mat[10];
4532: }
4533:
4534: /**
4535: * Helping function that specifies the position and orientation of a
4536: * view matrix. The inverse of this transform can be used to control
4537: * the ViewPlatform object within the scene graph.
4538: * @param eye the location of the eye
4539: * @param center a point in the virtual world where the eye is looking
4540: * @param up an up vector specifying the frustum's up direction
4541: */
4542: public void lookAt(Point3d eye, Point3d center, Vector3d up) {
4543: double forwardx, forwardy, forwardz, invMag;
4544: double upx, upy, upz;
4545: double sidex, sidey, sidez;
4546:
4547: forwardx = eye.x - center.x;
4548: forwardy = eye.y - center.y;
4549: forwardz = eye.z - center.z;
4550:
4551: invMag = 1.0 / Math.sqrt(forwardx * forwardx + forwardy
4552: * forwardy + forwardz * forwardz);
4553: forwardx = forwardx * invMag;
4554: forwardy = forwardy * invMag;
4555: forwardz = forwardz * invMag;
4556:
4557: invMag = 1.0 / Math.sqrt(up.x * up.x + up.y * up.y + up.z
4558: * up.z);
4559: upx = up.x * invMag;
4560: upy = up.y * invMag;
4561: upz = up.z * invMag;
4562:
4563: // side = Up cross forward
4564: sidex = upy * forwardz - forwardy * upz;
4565: sidey = upz * forwardx - upx * forwardz;
4566: sidez = upx * forwardy - upy * forwardx;
4567:
4568: invMag = 1.0 / Math.sqrt(sidex * sidex + sidey * sidey + sidez
4569: * sidez);
4570: sidex *= invMag;
4571: sidey *= invMag;
4572: sidez *= invMag;
4573:
4574: // recompute up = forward cross side
4575:
4576: upx = forwardy * sidez - sidey * forwardz;
4577: upy = forwardz * sidex - forwardx * sidez;
4578: upz = forwardx * sidey - forwardy * sidex;
4579:
4580: // transpose because we calculated the inverse of what we want
4581: mat[0] = sidex;
4582: mat[1] = sidey;
4583: mat[2] = sidez;
4584:
4585: mat[4] = upx;
4586: mat[5] = upy;
4587: mat[6] = upz;
4588:
4589: mat[8] = forwardx;
4590: mat[9] = forwardy;
4591: mat[10] = forwardz;
4592:
4593: mat[3] = -eye.x * mat[0] + -eye.y * mat[1] + -eye.z * mat[2];
4594: mat[7] = -eye.x * mat[4] + -eye.y * mat[5] + -eye.z * mat[6];
4595: mat[11] = -eye.x * mat[8] + -eye.y * mat[9] + -eye.z * mat[10];
4596:
4597: mat[12] = mat[13] = mat[14] = 0;
4598: mat[15] = 1;
4599:
4600: // Issue 253: set all dirty bits
4601: dirtyBits = ALL_DIRTY;
4602: }
4603:
4604: /**
4605: * Creates a perspective projection transform that mimics a standard,
4606: * camera-based,
4607: * view-model. This transform maps coordinates from Eye Coordinates (EC)
4608: * to Clipping Coordinates (CC). Note that unlike the similar function
4609: * in OpenGL, the clipping coordinates generated by the resulting
4610: * transform are in a right-handed coordinate system
4611: * (as are all other coordinate systems in Java 3D).
4612: * <p>
4613: * The frustum function-call establishes a view model with the eye
4614: * at the apex of a symmetric view frustum. The arguments
4615: * define the frustum and its associated perspective projection:
4616: * (left, bottom, -near) and (right, top, -near) specify the
4617: * point on the near clipping plane that maps onto the
4618: * lower-left and upper-right corners of the window respectively,
4619: * assuming the eye is located at (0, 0, 0).
4620: * @param left the vertical line on the left edge of the near
4621: * clipping plane mapped to the left edge of the graphics window
4622: * @param right the vertical line on the right edge of the near
4623: * clipping plane mapped to the right edge of the graphics window
4624: * @param bottom the horizontal line on the bottom edge of the near
4625: * clipping plane mapped to the bottom edge of the graphics window
4626: * @param top the horizontal line on the top edge of the near
4627: * @param near the distance to the frustum's near clipping plane.
4628: * This value must be positive, (the value -near is the location of the
4629: * near clip plane).
4630: * @param far the distance to the frustum's far clipping plane.
4631: * This value must be positive, and must be greater than near.
4632: */
4633: public void frustum(double left, double right, double bottom,
4634: double top, double near, double far) {
4635: double dx = 1 / (right - left);
4636: double dy = 1 / (top - bottom);
4637: double dz = 1 / (far - near);
4638:
4639: mat[0] = (2.0 * near) * dx;
4640: mat[5] = (2.0 * near) * dy;
4641: mat[10] = (far + near) * dz;
4642: mat[2] = (right + left) * dx;
4643: mat[6] = (top + bottom) * dy;
4644: mat[11] = (2.0 * far * near) * dz;
4645: mat[14] = -1.0;
4646: mat[1] = mat[3] = mat[4] = mat[7] = mat[8] = mat[9] = mat[12] = mat[13] = mat[15] = 0;
4647:
4648: // Matrix is a projection transform
4649: type = 0;
4650: // Issue 253: set all dirty bits
4651: dirtyBits = ALL_DIRTY;
4652: }
4653:
4654: /**
4655: * Creates a perspective projection transform that mimics a standard,
4656: * camera-based,
4657: * view-model. This transform maps coordinates from Eye Coordinates (EC)
4658: * to Clipping Coordinates (CC). Note that unlike the similar function
4659: * in OpenGL, the clipping coordinates generated by the resulting
4660: * transform are in a right-handed coordinate system
4661: * (as are all other coordinate systems in Java 3D). Also note that the
4662: * field of view is specified in radians.
4663: * @param fovx specifies the field of view in the x direction, in radians
4664: * @param aspect specifies the aspect ratio and thus the field of
4665: * view in the x direction. The aspect ratio is the ratio of x to y,
4666: * or width to height.
4667: * @param zNear the distance to the frustum's near clipping plane.
4668: * This value must be positive, (the value -zNear is the location of the
4669: * near clip plane).
4670: * @param zFar the distance to the frustum's far clipping plane
4671: */
4672: public void perspective(double fovx, double aspect, double zNear,
4673: double zFar) {
4674: double sine, cotangent, deltaZ;
4675: double half_fov = fovx * 0.5;
4676: double x, y;
4677: Vector3d v1, v2, v3, v4;
4678: Vector3d norm = new Vector3d();
4679:
4680: deltaZ = zFar - zNear;
4681: sine = Math.sin(half_fov);
4682: // if ((deltaZ == 0.0) || (sine == 0.0) || (aspect == 0.0)) {
4683: // return;
4684: // }
4685: cotangent = Math.cos(half_fov) / sine;
4686:
4687: mat[0] = cotangent;
4688: mat[5] = cotangent * aspect;
4689: mat[10] = (zFar + zNear) / deltaZ;
4690: mat[11] = 2.0 * zNear * zFar / deltaZ;
4691: mat[14] = -1.0;
4692: mat[1] = mat[2] = mat[3] = mat[4] = mat[6] = mat[7] = mat[8] = mat[9] = mat[12] = mat[13] = mat[15] = 0;
4693:
4694: // Matrix is a projection transform
4695: type = 0;
4696: // Issue 253: set all dirty bits
4697: dirtyBits = ALL_DIRTY;
4698: }
4699:
4700: /**
4701: * Creates an orthographic projection transform that mimics a standard,
4702: * camera-based,
4703: * view-model. This transform maps coordinates from Eye Coordinates (EC)
4704: * to Clipping Coordinates (CC). Note that unlike the similar function
4705: * in OpenGL, the clipping coordinates generated by the resulting
4706: * transform are in a right-handed coordinate system
4707: * (as are all other coordinate systems in Java 3D).
4708: * @param left the vertical line on the left edge of the near
4709: * clipping plane mapped to the left edge of the graphics window
4710: * @param right the vertical line on the right edge of the near
4711: * clipping plane mapped to the right edge of the graphics window
4712: * @param bottom the horizontal line on the bottom edge of the near
4713: * clipping plane mapped to the bottom edge of the graphics window
4714: * @param top the horizontal line on the top edge of the near
4715: * clipping plane mapped to the top edge of the graphics window
4716: * @param near the distance to the frustum's near clipping plane
4717: * (the value -near is the location of the near clip plane)
4718: * @param far the distance to the frustum's far clipping plane
4719: */
4720: public void ortho(double left, double right, double bottom,
4721: double top, double near, double far) {
4722: double deltax = 1 / (right - left);
4723: double deltay = 1 / (top - bottom);
4724: double deltaz = 1 / (far - near);
4725:
4726: // if ((deltax == 0.0) || (deltay == 0.0) || (deltaz == 0.0)) {
4727: // return;
4728: // }
4729:
4730: mat[0] = 2.0 * deltax;
4731: mat[3] = -(right + left) * deltax;
4732: mat[5] = 2.0 * deltay;
4733: mat[7] = -(top + bottom) * deltay;
4734: mat[10] = 2.0 * deltaz;
4735: mat[11] = (far + near) * deltaz;
4736: mat[1] = mat[2] = mat[4] = mat[6] = mat[8] = mat[9] = mat[12] = mat[13] = mat[14] = 0;
4737: mat[15] = 1;
4738:
4739: // Issue 253: set all dirty bits
4740: dirtyBits = ALL_DIRTY;
4741: }
4742:
4743: /**
4744: * get the scaling factor of matrix in this transform,
4745: * use for distance scaling
4746: */
4747: double getDistanceScale() {
4748: // The caller know that this matrix is affine
4749: // orthogonal before invoke this procedure
4750:
4751: if ((dirtyBits & SCALE_BIT) != 0) {
4752: double max = mat[0] * mat[0] + mat[4] * mat[4] + mat[8]
4753: * mat[8];
4754: if (((dirtyBits & CONGRUENT_BIT) == 0)
4755: && ((type & CONGRUENT) != 0)) {
4756: // in most case it is congruent
4757: return Math.sqrt(max);
4758: }
4759: double tmp = mat[1] * mat[1] + mat[5] * mat[5] + mat[9]
4760: * mat[9];
4761: if (tmp > max) {
4762: max = tmp;
4763: }
4764: tmp = mat[2] * mat[2] + mat[6] * mat[6] + mat[10] * mat[10];
4765: return Math.sqrt((tmp > max) ? tmp : max);
4766: }
4767: return max3(scales);
4768: }
4769:
4770: static private void mat_mul(double[] m1, double[] m2, double[] m3) {
4771:
4772: double[] result = m3;
4773: if ((m1 == m3) || (m2 == m3)) {
4774: result = new double[9];
4775: }
4776:
4777: result[0] = m1[0] * m2[0] + m1[1] * m2[3] + m1[2] * m2[6];
4778: result[1] = m1[0] * m2[1] + m1[1] * m2[4] + m1[2] * m2[7];
4779: result[2] = m1[0] * m2[2] + m1[1] * m2[5] + m1[2] * m2[8];
4780:
4781: result[3] = m1[3] * m2[0] + m1[4] * m2[3] + m1[5] * m2[6];
4782: result[4] = m1[3] * m2[1] + m1[4] * m2[4] + m1[5] * m2[7];
4783: result[5] = m1[3] * m2[2] + m1[4] * m2[5] + m1[5] * m2[8];
4784:
4785: result[6] = m1[6] * m2[0] + m1[7] * m2[3] + m1[8] * m2[6];
4786: result[7] = m1[6] * m2[1] + m1[7] * m2[4] + m1[8] * m2[7];
4787: result[8] = m1[6] * m2[2] + m1[7] * m2[5] + m1[8] * m2[8];
4788:
4789: if (result != m3) {
4790: for (int i = 0; i < 9; i++) {
4791: m3[i] = result[i];
4792: }
4793: }
4794: }
4795:
4796: static private void transpose_mat(double[] in, double[] out) {
4797: out[0] = in[0];
4798: out[1] = in[3];
4799: out[2] = in[6];
4800:
4801: out[3] = in[1];
4802: out[4] = in[4];
4803: out[5] = in[7];
4804:
4805: out[6] = in[2];
4806: out[7] = in[5];
4807: out[8] = in[8];
4808: }
4809:
4810: final static private void multipleScale(double m[], double s[]) {
4811: m[0] *= s[0];
4812: m[1] *= s[0];
4813: m[2] *= s[0];
4814: m[4] *= s[1];
4815: m[5] *= s[1];
4816: m[6] *= s[1];
4817: m[8] *= s[2];
4818: m[9] *= s[2];
4819: m[10] *= s[2];
4820: }
4821:
4822: private void compute_svd(Transform3D matrix, double[] outScale,
4823: double[] outRot) {
4824:
4825: int i, j;
4826: double g, scale;
4827: double m[] = new double[9];
4828:
4829: // if (!svdAllocd) {
4830: double[] u1 = new double[9];
4831: double[] v1 = new double[9];
4832: double[] t1 = new double[9];
4833: double[] t2 = new double[9];
4834: // double[] ts = new double[9];
4835: // double[] svdTmp = new double[9]; It is replaced by t1
4836: double[] svdRot = new double[9];
4837: // double[] single_values = new double[3]; replaced by t2
4838:
4839: double[] e = new double[3];
4840: double[] svdScales = new double[3];
4841:
4842: // XXXX: initialize to 0's if alread allocd? Should not have to, since
4843: // no operations depend on these being init'd to zero.
4844:
4845: int converged, negCnt = 0;
4846: double cs, sn;
4847: double c1, c2, c3, c4;
4848: double s1, s2, s3, s4;
4849: double cl1, cl2, cl3;
4850:
4851: svdRot[0] = m[0] = matrix.mat[0];
4852: svdRot[1] = m[1] = matrix.mat[1];
4853: svdRot[2] = m[2] = matrix.mat[2];
4854: svdRot[3] = m[3] = matrix.mat[4];
4855: svdRot[4] = m[4] = matrix.mat[5];
4856: svdRot[5] = m[5] = matrix.mat[6];
4857: svdRot[6] = m[6] = matrix.mat[8];
4858: svdRot[7] = m[7] = matrix.mat[9];
4859: svdRot[8] = m[8] = matrix.mat[10];
4860:
4861: // u1
4862:
4863: if (m[3] * m[3] < EPS) {
4864: u1[0] = 1.0;
4865: u1[1] = 0.0;
4866: u1[2] = 0.0;
4867: u1[3] = 0.0;
4868: u1[4] = 1.0;
4869: u1[5] = 0.0;
4870: u1[6] = 0.0;
4871: u1[7] = 0.0;
4872: u1[8] = 1.0;
4873: } else if (m[0] * m[0] < EPS) {
4874: t1[0] = m[0];
4875: t1[1] = m[1];
4876: t1[2] = m[2];
4877: m[0] = m[3];
4878: m[1] = m[4];
4879: m[2] = m[5];
4880:
4881: m[3] = -t1[0]; // zero
4882: m[4] = -t1[1];
4883: m[5] = -t1[2];
4884:
4885: u1[0] = 0.0;
4886: u1[1] = 1.0;
4887: u1[2] = 0.0;
4888: u1[3] = -1.0;
4889: u1[4] = 0.0;
4890: u1[5] = 0.0;
4891: u1[6] = 0.0;
4892: u1[7] = 0.0;
4893: u1[8] = 1.0;
4894: } else {
4895: g = 1.0 / Math.sqrt(m[0] * m[0] + m[3] * m[3]);
4896: c1 = m[0] * g;
4897: s1 = m[3] * g;
4898: t1[0] = c1 * m[0] + s1 * m[3];
4899: t1[1] = c1 * m[1] + s1 * m[4];
4900: t1[2] = c1 * m[2] + s1 * m[5];
4901:
4902: m[3] = -s1 * m[0] + c1 * m[3]; // zero
4903: m[4] = -s1 * m[1] + c1 * m[4];
4904: m[5] = -s1 * m[2] + c1 * m[5];
4905:
4906: m[0] = t1[0];
4907: m[1] = t1[1];
4908: m[2] = t1[2];
4909: u1[0] = c1;
4910: u1[1] = s1;
4911: u1[2] = 0.0;
4912: u1[3] = -s1;
4913: u1[4] = c1;
4914: u1[5] = 0.0;
4915: u1[6] = 0.0;
4916: u1[7] = 0.0;
4917: u1[8] = 1.0;
4918: }
4919:
4920: // u2
4921:
4922: if (m[6] * m[6] < EPS) {
4923: } else if (m[0] * m[0] < EPS) {
4924: t1[0] = m[0];
4925: t1[1] = m[1];
4926: t1[2] = m[2];
4927: m[0] = m[6];
4928: m[1] = m[7];
4929: m[2] = m[8];
4930:
4931: m[6] = -t1[0]; // zero
4932: m[7] = -t1[1];
4933: m[8] = -t1[2];
4934:
4935: t1[0] = u1[0];
4936: t1[1] = u1[1];
4937: t1[2] = u1[2];
4938: u1[0] = u1[6];
4939: u1[1] = u1[7];
4940: u1[2] = u1[8];
4941:
4942: u1[6] = -t1[0]; // zero
4943: u1[7] = -t1[1];
4944: u1[8] = -t1[2];
4945: } else {
4946: g = 1.0 / Math.sqrt(m[0] * m[0] + m[6] * m[6]);
4947: c2 = m[0] * g;
4948: s2 = m[6] * g;
4949: t1[0] = c2 * m[0] + s2 * m[6];
4950: t1[1] = c2 * m[1] + s2 * m[7];
4951: t1[2] = c2 * m[2] + s2 * m[8];
4952:
4953: m[6] = -s2 * m[0] + c2 * m[6];
4954: m[7] = -s2 * m[1] + c2 * m[7];
4955: m[8] = -s2 * m[2] + c2 * m[8];
4956: m[0] = t1[0];
4957: m[1] = t1[1];
4958: m[2] = t1[2];
4959:
4960: t1[0] = c2 * u1[0];
4961: t1[1] = c2 * u1[1];
4962: u1[2] = s2;
4963:
4964: t1[6] = -u1[0] * s2;
4965: t1[7] = -u1[1] * s2;
4966: u1[8] = c2;
4967: u1[0] = t1[0];
4968: u1[1] = t1[1];
4969: u1[6] = t1[6];
4970: u1[7] = t1[7];
4971: }
4972:
4973: // v1
4974:
4975: if (m[2] * m[2] < EPS) {
4976: v1[0] = 1.0;
4977: v1[1] = 0.0;
4978: v1[2] = 0.0;
4979: v1[3] = 0.0;
4980: v1[4] = 1.0;
4981: v1[5] = 0.0;
4982: v1[6] = 0.0;
4983: v1[7] = 0.0;
4984: v1[8] = 1.0;
4985: } else if (m[1] * m[1] < EPS) {
4986: t1[2] = m[2];
4987: t1[5] = m[5];
4988: t1[8] = m[8];
4989: m[2] = -m[1];
4990: m[5] = -m[4];
4991: m[8] = -m[7];
4992:
4993: m[1] = t1[2]; // zero
4994: m[4] = t1[5];
4995: m[7] = t1[8];
4996:
4997: v1[0] = 1.0;
4998: v1[1] = 0.0;
4999: v1[2] = 0.0;
5000: v1[3] = 0.0;
5001: v1[4] = 0.0;
5002: v1[5] = -1.0;
5003: v1[6] = 0.0;
5004: v1[7] = 1.0;
5005: v1[8] = 0.0;
5006: } else {
5007: g = 1.0 / Math.sqrt(m[1] * m[1] + m[2] * m[2]);
5008: c3 = m[1] * g;
5009: s3 = m[2] * g;
5010: t1[1] = c3 * m[1] + s3 * m[2]; // can assign to m[1]?
5011: m[2] = -s3 * m[1] + c3 * m[2]; // zero
5012: m[1] = t1[1];
5013:
5014: t1[4] = c3 * m[4] + s3 * m[5];
5015: m[5] = -s3 * m[4] + c3 * m[5];
5016: m[4] = t1[4];
5017:
5018: t1[7] = c3 * m[7] + s3 * m[8];
5019: m[8] = -s3 * m[7] + c3 * m[8];
5020: m[7] = t1[7];
5021:
5022: v1[0] = 1.0;
5023: v1[1] = 0.0;
5024: v1[2] = 0.0;
5025: v1[3] = 0.0;
5026: v1[4] = c3;
5027: v1[5] = -s3;
5028: v1[6] = 0.0;
5029: v1[7] = s3;
5030: v1[8] = c3;
5031: }
5032:
5033: // u3
5034:
5035: if (m[7] * m[7] < EPS) {
5036: } else if (m[4] * m[4] < EPS) {
5037: t1[3] = m[3];
5038: t1[4] = m[4];
5039: t1[5] = m[5];
5040: m[3] = m[6]; // zero
5041: m[4] = m[7];
5042: m[5] = m[8];
5043:
5044: m[6] = -t1[3]; // zero
5045: m[7] = -t1[4]; // zero
5046: m[8] = -t1[5];
5047:
5048: t1[3] = u1[3];
5049: t1[4] = u1[4];
5050: t1[5] = u1[5];
5051: u1[3] = u1[6];
5052: u1[4] = u1[7];
5053: u1[5] = u1[8];
5054:
5055: u1[6] = -t1[3]; // zero
5056: u1[7] = -t1[4];
5057: u1[8] = -t1[5];
5058:
5059: } else {
5060: g = 1.0 / Math.sqrt(m[4] * m[4] + m[7] * m[7]);
5061: c4 = m[4] * g;
5062: s4 = m[7] * g;
5063: t1[3] = c4 * m[3] + s4 * m[6];
5064: m[6] = -s4 * m[3] + c4 * m[6]; // zero
5065: m[3] = t1[3];
5066:
5067: t1[4] = c4 * m[4] + s4 * m[7];
5068: m[7] = -s4 * m[4] + c4 * m[7];
5069: m[4] = t1[4];
5070:
5071: t1[5] = c4 * m[5] + s4 * m[8];
5072: m[8] = -s4 * m[5] + c4 * m[8];
5073: m[5] = t1[5];
5074:
5075: t1[3] = c4 * u1[3] + s4 * u1[6];
5076: u1[6] = -s4 * u1[3] + c4 * u1[6];
5077: u1[3] = t1[3];
5078:
5079: t1[4] = c4 * u1[4] + s4 * u1[7];
5080: u1[7] = -s4 * u1[4] + c4 * u1[7];
5081: u1[4] = t1[4];
5082:
5083: t1[5] = c4 * u1[5] + s4 * u1[8];
5084: u1[8] = -s4 * u1[5] + c4 * u1[8];
5085: u1[5] = t1[5];
5086: }
5087:
5088: t2[0] = m[0];
5089: t2[1] = m[4];
5090: t2[2] = m[8];
5091: e[0] = m[1];
5092: e[1] = m[5];
5093:
5094: if (e[0] * e[0] > EPS || e[1] * e[1] > EPS) {
5095: compute_qr(t2, e, u1, v1);
5096: }
5097:
5098: svdScales[0] = t2[0];
5099: svdScales[1] = t2[1];
5100: svdScales[2] = t2[2];
5101:
5102: // Do some optimization here. If scale is unity, simply return the rotation matric.
5103: if (almostOne(Math.abs(svdScales[0]))
5104: && almostOne(Math.abs(svdScales[1]))
5105: && almostOne(Math.abs(svdScales[2]))) {
5106:
5107: for (i = 0; i < 3; i++)
5108: if (svdScales[i] < 0.0)
5109: negCnt++;
5110:
5111: if ((negCnt == 0) || (negCnt == 2)) {
5112: //System.err.println("Optimize!!");
5113: outScale[0] = outScale[1] = outScale[2] = 1.0;
5114: for (i = 0; i < 9; i++)
5115: outRot[i] = svdRot[i];
5116:
5117: return;
5118: }
5119: }
5120:
5121: // XXXX: could eliminate use of t1 and t1 by making a new method which
5122: // transposes and multiplies two matricies
5123: transpose_mat(u1, t1);
5124: transpose_mat(v1, t2);
5125:
5126: svdReorder(m, t1, t2, svdRot, svdScales, outRot, outScale);
5127: }
5128:
5129: private void svdReorder(double[] m, double[] t1, double[] t2,
5130: double[] rot, double[] scales, double[] outRot,
5131: double[] outScale) {
5132:
5133: int in0, in1, in2, index, i;
5134: int[] svdOut = new int[3];
5135: double[] svdMag = new double[3];
5136:
5137: // check for rotation information in the scales
5138: if (scales[0] < 0.0) { // move the rotation info to rotation matrix
5139: scales[0] = -scales[0];
5140: t2[0] = -t2[0];
5141: t2[1] = -t2[1];
5142: t2[2] = -t2[2];
5143: }
5144: if (scales[1] < 0.0) { // move the rotation info to rotation matrix
5145: scales[1] = -scales[1];
5146: t2[3] = -t2[3];
5147: t2[4] = -t2[4];
5148: t2[5] = -t2[5];
5149: }
5150: if (scales[2] < 0.0) { // move the rotation info to rotation matrix
5151: scales[2] = -scales[2];
5152: t2[6] = -t2[6];
5153: t2[7] = -t2[7];
5154: t2[8] = -t2[8];
5155: }
5156:
5157: mat_mul(t1, t2, rot);
5158:
5159: // check for equal scales case and do not reorder
5160: if (almostEqual(Math.abs(scales[0]), Math.abs(scales[1]))
5161: && almostEqual(Math.abs(scales[1]), Math.abs(scales[2]))) {
5162: for (i = 0; i < 9; i++) {
5163: outRot[i] = rot[i];
5164: }
5165: for (i = 0; i < 3; i++) {
5166: outScale[i] = scales[i];
5167: }
5168:
5169: } else {
5170:
5171: // sort the order of the results of SVD
5172: if (scales[0] > scales[1]) {
5173: if (scales[0] > scales[2]) {
5174: if (scales[2] > scales[1]) {
5175: svdOut[0] = 0;
5176: svdOut[1] = 2;
5177: svdOut[2] = 1; // xzy
5178: } else {
5179: svdOut[0] = 0;
5180: svdOut[1] = 1;
5181: svdOut[2] = 2; // xyz
5182: }
5183: } else {
5184: svdOut[0] = 2;
5185: svdOut[1] = 0;
5186: svdOut[2] = 1; // zxy
5187: }
5188: } else { // y > x
5189: if (scales[1] > scales[2]) {
5190: if (scales[2] > scales[0]) {
5191: svdOut[0] = 1;
5192: svdOut[1] = 2;
5193: svdOut[2] = 0; // yzx
5194: } else {
5195: svdOut[0] = 1;
5196: svdOut[1] = 0;
5197: svdOut[2] = 2; // yxz
5198: }
5199: } else {
5200: svdOut[0] = 2;
5201: svdOut[1] = 1;
5202: svdOut[2] = 0; // zyx
5203: }
5204: }
5205:
5206: // sort the order of the input matrix
5207: svdMag[0] = (m[0] * m[0] + m[1] * m[1] + m[2] * m[2]);
5208: svdMag[1] = (m[3] * m[3] + m[4] * m[4] + m[5] * m[5]);
5209: svdMag[2] = (m[6] * m[6] + m[7] * m[7] + m[8] * m[8]);
5210:
5211: if (svdMag[0] > svdMag[1]) {
5212: if (svdMag[0] > svdMag[2]) {
5213: if (svdMag[2] > svdMag[1]) {
5214: // 0 - 2 - 1
5215: in0 = 0;
5216: in2 = 1;
5217: in1 = 2;// xzy
5218: } else {
5219: // 0 - 1 - 2
5220: in0 = 0;
5221: in1 = 1;
5222: in2 = 2; // xyz
5223: }
5224: } else {
5225: // 2 - 0 - 1
5226: in2 = 0;
5227: in0 = 1;
5228: in1 = 2; // zxy
5229: }
5230: } else { // y > x 1>0
5231: if (svdMag[1] > svdMag[2]) { // 1>2
5232: if (svdMag[2] > svdMag[0]) { // 2>0
5233: // 1 - 2 - 0
5234: in1 = 0;
5235: in2 = 1;
5236: in0 = 2; // yzx
5237: } else {
5238: // 1 - 0 - 2
5239: in1 = 0;
5240: in0 = 1;
5241: in2 = 2; // yxz
5242: }
5243: } else {
5244: // 2 - 1 - 0
5245: in2 = 0;
5246: in1 = 1;
5247: in0 = 2; // zyx
5248: }
5249: }
5250:
5251: index = svdOut[in0];
5252: outScale[0] = scales[index];
5253:
5254: index = svdOut[in1];
5255: outScale[1] = scales[index];
5256:
5257: index = svdOut[in2];
5258: outScale[2] = scales[index];
5259:
5260: index = svdOut[in0];
5261: if (outRot == null) {
5262: MasterControl.getCoreLogger().severe("outRot == null");
5263: }
5264: if (rot == null) {
5265: MasterControl.getCoreLogger().severe("rot == null");
5266: }
5267:
5268: outRot[0] = rot[index];
5269:
5270: index = svdOut[in0] + 3;
5271: outRot[0 + 3] = rot[index];
5272:
5273: index = svdOut[in0] + 6;
5274: outRot[0 + 6] = rot[index];
5275:
5276: index = svdOut[in1];
5277: outRot[1] = rot[index];
5278:
5279: index = svdOut[in1] + 3;
5280: outRot[1 + 3] = rot[index];
5281:
5282: index = svdOut[in1] + 6;
5283: outRot[1 + 6] = rot[index];
5284:
5285: index = svdOut[in2];
5286: outRot[2] = rot[index];
5287:
5288: index = svdOut[in2] + 3;
5289: outRot[2 + 3] = rot[index];
5290:
5291: index = svdOut[in2] + 6;
5292: outRot[2 + 6] = rot[index];
5293: }
5294:
5295: }
5296:
5297: private int compute_qr(double[] s, double[] e, double[] u,
5298: double[] v) {
5299: int i, j, k;
5300: boolean converged;
5301: double shift, ssmin, ssmax, r;
5302:
5303: double utemp, vtemp;
5304: double f, g;
5305:
5306: final int MAX_INTERATIONS = 10;
5307: final double CONVERGE_TOL = 4.89E-15;
5308:
5309: double[] cosl = new double[2];
5310: double[] cosr = new double[2];
5311: double[] sinl = new double[2];
5312: double[] sinr = new double[2];
5313: double[] qr_m = new double[9];
5314:
5315: double c_b48 = 1.;
5316: double c_b71 = -1.;
5317: int first;
5318: converged = false;
5319:
5320: first = 1;
5321:
5322: if (Math.abs(e[1]) < CONVERGE_TOL
5323: || Math.abs(e[0]) < CONVERGE_TOL)
5324: converged = true;
5325:
5326: for (k = 0; k < MAX_INTERATIONS && !converged; k++) {
5327: shift = compute_shift(s[1], e[1], s[2]);
5328: f = (Math.abs(s[0]) - shift)
5329: * (d_sign(c_b48, s[0]) + shift / s[0]);
5330: g = e[0];
5331: r = compute_rot(f, g, sinr, cosr, 0, first);
5332: f = cosr[0] * s[0] + sinr[0] * e[0];
5333: e[0] = cosr[0] * e[0] - sinr[0] * s[0];
5334: g = sinr[0] * s[1];
5335: s[1] = cosr[0] * s[1];
5336:
5337: r = compute_rot(f, g, sinl, cosl, 0, first);
5338: first = 0;
5339: s[0] = r;
5340: f = cosl[0] * e[0] + sinl[0] * s[1];
5341: s[1] = cosl[0] * s[1] - sinl[0] * e[0];
5342: g = sinl[0] * e[1];
5343: e[1] = cosl[0] * e[1];
5344:
5345: r = compute_rot(f, g, sinr, cosr, 1, first);
5346: e[0] = r;
5347: f = cosr[1] * s[1] + sinr[1] * e[1];
5348: e[1] = cosr[1] * e[1] - sinr[1] * s[1];
5349: g = sinr[1] * s[2];
5350: s[2] = cosr[1] * s[2];
5351:
5352: r = compute_rot(f, g, sinl, cosl, 1, first);
5353: s[1] = r;
5354: f = cosl[1] * e[1] + sinl[1] * s[2];
5355: s[2] = cosl[1] * s[2] - sinl[1] * e[1];
5356: e[1] = f;
5357:
5358: // update u matrices
5359: utemp = u[0];
5360: u[0] = cosl[0] * utemp + sinl[0] * u[3];
5361: u[3] = -sinl[0] * utemp + cosl[0] * u[3];
5362: utemp = u[1];
5363: u[1] = cosl[0] * utemp + sinl[0] * u[4];
5364: u[4] = -sinl[0] * utemp + cosl[0] * u[4];
5365: utemp = u[2];
5366: u[2] = cosl[0] * utemp + sinl[0] * u[5];
5367: u[5] = -sinl[0] * utemp + cosl[0] * u[5];
5368:
5369: utemp = u[3];
5370: u[3] = cosl[1] * utemp + sinl[1] * u[6];
5371: u[6] = -sinl[1] * utemp + cosl[1] * u[6];
5372: utemp = u[4];
5373: u[4] = cosl[1] * utemp + sinl[1] * u[7];
5374: u[7] = -sinl[1] * utemp + cosl[1] * u[7];
5375: utemp = u[5];
5376: u[5] = cosl[1] * utemp + sinl[1] * u[8];
5377: u[8] = -sinl[1] * utemp + cosl[1] * u[8];
5378:
5379: // update v matrices
5380:
5381: vtemp = v[0];
5382: v[0] = cosr[0] * vtemp + sinr[0] * v[1];
5383: v[1] = -sinr[0] * vtemp + cosr[0] * v[1];
5384: vtemp = v[3];
5385: v[3] = cosr[0] * vtemp + sinr[0] * v[4];
5386: v[4] = -sinr[0] * vtemp + cosr[0] * v[4];
5387: vtemp = v[6];
5388: v[6] = cosr[0] * vtemp + sinr[0] * v[7];
5389: v[7] = -sinr[0] * vtemp + cosr[0] * v[7];
5390:
5391: vtemp = v[1];
5392: v[1] = cosr[1] * vtemp + sinr[1] * v[2];
5393: v[2] = -sinr[1] * vtemp + cosr[1] * v[2];
5394: vtemp = v[4];
5395: v[4] = cosr[1] * vtemp + sinr[1] * v[5];
5396: v[5] = -sinr[1] * vtemp + cosr[1] * v[5];
5397: vtemp = v[7];
5398: v[7] = cosr[1] * vtemp + sinr[1] * v[8];
5399: v[8] = -sinr[1] * vtemp + cosr[1] * v[8];
5400:
5401: // if(debug)System.err.println("\n*********************** iteration #"+k+" ***********************\n");
5402:
5403: qr_m[0] = s[0];
5404: qr_m[1] = e[0];
5405: qr_m[2] = 0.0;
5406: qr_m[3] = 0.0;
5407: qr_m[4] = s[1];
5408: qr_m[5] = e[1];
5409: qr_m[6] = 0.0;
5410: qr_m[7] = 0.0;
5411: qr_m[8] = s[2];
5412:
5413: if (Math.abs(e[1]) < CONVERGE_TOL
5414: || Math.abs(e[0]) < CONVERGE_TOL)
5415: converged = true;
5416: }
5417:
5418: if (Math.abs(e[1]) < CONVERGE_TOL) {
5419: compute_2X2(s[0], e[0], s[1], s, sinl, cosl, sinr, cosr, 0);
5420:
5421: utemp = u[0];
5422: u[0] = cosl[0] * utemp + sinl[0] * u[3];
5423: u[3] = -sinl[0] * utemp + cosl[0] * u[3];
5424: utemp = u[1];
5425: u[1] = cosl[0] * utemp + sinl[0] * u[4];
5426: u[4] = -sinl[0] * utemp + cosl[0] * u[4];
5427: utemp = u[2];
5428: u[2] = cosl[0] * utemp + sinl[0] * u[5];
5429: u[5] = -sinl[0] * utemp + cosl[0] * u[5];
5430:
5431: // update v matrices
5432:
5433: vtemp = v[0];
5434: v[0] = cosr[0] * vtemp + sinr[0] * v[1];
5435: v[1] = -sinr[0] * vtemp + cosr[0] * v[1];
5436: vtemp = v[3];
5437: v[3] = cosr[0] * vtemp + sinr[0] * v[4];
5438: v[4] = -sinr[0] * vtemp + cosr[0] * v[4];
5439: vtemp = v[6];
5440: v[6] = cosr[0] * vtemp + sinr[0] * v[7];
5441: v[7] = -sinr[0] * vtemp + cosr[0] * v[7];
5442: } else {
5443: compute_2X2(s[1], e[1], s[2], s, sinl, cosl, sinr, cosr, 1);
5444:
5445: utemp = u[3];
5446: u[3] = cosl[0] * utemp + sinl[0] * u[6];
5447: u[6] = -sinl[0] * utemp + cosl[0] * u[6];
5448: utemp = u[4];
5449: u[4] = cosl[0] * utemp + sinl[0] * u[7];
5450: u[7] = -sinl[0] * utemp + cosl[0] * u[7];
5451: utemp = u[5];
5452: u[5] = cosl[0] * utemp + sinl[0] * u[8];
5453: u[8] = -sinl[0] * utemp + cosl[0] * u[8];
5454:
5455: // update v matrices
5456:
5457: vtemp = v[1];
5458: v[1] = cosr[0] * vtemp + sinr[0] * v[2];
5459: v[2] = -sinr[0] * vtemp + cosr[0] * v[2];
5460: vtemp = v[4];
5461: v[4] = cosr[0] * vtemp + sinr[0] * v[5];
5462: v[5] = -sinr[0] * vtemp + cosr[0] * v[5];
5463: vtemp = v[7];
5464: v[7] = cosr[0] * vtemp + sinr[0] * v[8];
5465: v[8] = -sinr[0] * vtemp + cosr[0] * v[8];
5466: }
5467:
5468: return (0);
5469: }
5470:
5471: static final double max(double a, double b) {
5472: return (a > b ? a : b);
5473: }
5474:
5475: static final double min(double a, double b) {
5476: return (a < b ? a : b);
5477: }
5478:
5479: static final double d_sign(double a, double b) {
5480: double x = (a >= 0 ? a : -a);
5481: return (b >= 0 ? x : -x);
5482: }
5483:
5484: static final double compute_shift(double f, double g, double h) {
5485: double d__1, d__2;
5486: double fhmn, fhmx, c, fa, ga, ha, as, at, au;
5487: double ssmin;
5488:
5489: fa = Math.abs(f);
5490: ga = Math.abs(g);
5491: ha = Math.abs(h);
5492: fhmn = min(fa, ha);
5493: fhmx = max(fa, ha);
5494: if (fhmn == 0.) {
5495: ssmin = 0.;
5496: if (fhmx == 0.) {
5497: } else {
5498: d__1 = min(fhmx, ga) / max(fhmx, ga);
5499: }
5500: } else {
5501: if (ga < fhmx) {
5502: as = fhmn / fhmx + 1.;
5503: at = (fhmx - fhmn) / fhmx;
5504: d__1 = ga / fhmx;
5505: au = d__1 * d__1;
5506: c = 2. / (Math.sqrt(as * as + au) + Math.sqrt(at * at
5507: + au));
5508: ssmin = fhmn * c;
5509: } else {
5510: au = fhmx / ga;
5511: if (au == 0.) {
5512:
5513: ssmin = fhmn * fhmx / ga;
5514: } else {
5515: as = fhmn / fhmx + 1.;
5516: at = (fhmx - fhmn) / fhmx;
5517: d__1 = as * au;
5518: d__2 = at * au;
5519: c = 1. / (Math.sqrt(d__1 * d__1 + 1.) + Math
5520: .sqrt(d__2 * d__2 + 1.));
5521: ssmin = fhmn * c * au;
5522: ssmin += ssmin;
5523: }
5524: }
5525: }
5526:
5527: return (ssmin);
5528: }
5529:
5530: static int compute_2X2(double f, double g, double h,
5531: double[] single_values, double[] snl, double[] csl,
5532: double[] snr, double[] csr, int index) {
5533:
5534: double c_b3 = 2.;
5535: double c_b4 = 1.;
5536:
5537: double d__1;
5538: int pmax;
5539: double temp;
5540: boolean swap;
5541: double a, d, l, m, r, s, t, tsign, fa, ga, ha;
5542: double ft, gt, ht, mm;
5543: boolean gasmal;
5544: double tt, clt, crt, slt, srt;
5545: double ssmin, ssmax;
5546:
5547: ssmax = single_values[0];
5548: ssmin = single_values[1];
5549: clt = 0.0;
5550: crt = 0.0;
5551: slt = 0.0;
5552: srt = 0.0;
5553: tsign = 0.0;
5554:
5555: ft = f;
5556: fa = Math.abs(ft);
5557: ht = h;
5558: ha = Math.abs(h);
5559:
5560: pmax = 1;
5561: if (ha > fa)
5562: swap = true;
5563: else
5564: swap = false;
5565:
5566: if (swap) {
5567: pmax = 3;
5568: temp = ft;
5569: ft = ht;
5570: ht = temp;
5571: temp = fa;
5572: fa = ha;
5573: ha = temp;
5574:
5575: }
5576: gt = g;
5577: ga = Math.abs(gt);
5578: if (ga == 0.) {
5579:
5580: single_values[1] = ha;
5581: single_values[0] = fa;
5582: clt = 1.;
5583: crt = 1.;
5584: slt = 0.;
5585: srt = 0.;
5586: } else {
5587: gasmal = true;
5588:
5589: if (ga > fa) {
5590: pmax = 2;
5591: if (fa / ga < EPS) {
5592:
5593: gasmal = false;
5594: ssmax = ga;
5595: if (ha > 1.) {
5596: ssmin = fa / (ga / ha);
5597: } else {
5598: ssmin = fa / ga * ha;
5599: }
5600: clt = 1.;
5601: slt = ht / gt;
5602: srt = 1.;
5603: crt = ft / gt;
5604: }
5605: }
5606: if (gasmal) {
5607:
5608: d = fa - ha;
5609: if (d == fa) {
5610:
5611: l = 1.;
5612: } else {
5613: l = d / fa;
5614: }
5615:
5616: m = gt / ft;
5617:
5618: t = 2. - l;
5619:
5620: mm = m * m;
5621: tt = t * t;
5622: s = Math.sqrt(tt + mm);
5623:
5624: if (l == 0.) {
5625: r = Math.abs(m);
5626: } else {
5627: r = Math.sqrt(l * l + mm);
5628: }
5629:
5630: a = (s + r) * .5;
5631:
5632: if (ga > fa) {
5633: pmax = 2;
5634: if (fa / ga < EPS) {
5635:
5636: gasmal = false;
5637: ssmax = ga;
5638: if (ha > 1.) {
5639: ssmin = fa / (ga / ha);
5640: } else {
5641: ssmin = fa / ga * ha;
5642: }
5643: clt = 1.;
5644: slt = ht / gt;
5645: srt = 1.;
5646: crt = ft / gt;
5647: }
5648: }
5649: if (gasmal) {
5650:
5651: d = fa - ha;
5652: if (d == fa) {
5653:
5654: l = 1.;
5655: } else {
5656: l = d / fa;
5657: }
5658:
5659: m = gt / ft;
5660:
5661: t = 2. - l;
5662:
5663: mm = m * m;
5664: tt = t * t;
5665: s = Math.sqrt(tt + mm);
5666:
5667: if (l == 0.) {
5668: r = Math.abs(m);
5669: } else {
5670: r = Math.sqrt(l * l + mm);
5671: }
5672:
5673: a = (s + r) * .5;
5674:
5675: ssmin = ha / a;
5676: ssmax = fa * a;
5677: if (mm == 0.) {
5678:
5679: if (l == 0.) {
5680: t = d_sign(c_b3, ft) * d_sign(c_b4, gt);
5681: } else {
5682: t = gt / d_sign(d, ft) + m / t;
5683: }
5684: } else {
5685: t = (m / (s + t) + m / (r + l)) * (a + 1.);
5686: }
5687: l = Math.sqrt(t * t + 4.);
5688: crt = 2. / l;
5689: srt = t / l;
5690: clt = (crt + srt * m) / a;
5691: slt = ht / ft * srt / a;
5692: }
5693: }
5694: if (swap) {
5695: csl[0] = srt;
5696: snl[0] = crt;
5697: csr[0] = slt;
5698: snr[0] = clt;
5699: } else {
5700: csl[0] = clt;
5701: snl[0] = slt;
5702: csr[0] = crt;
5703: snr[0] = srt;
5704: }
5705:
5706: if (pmax == 1) {
5707: tsign = d_sign(c_b4, csr[0]) * d_sign(c_b4, csl[0])
5708: * d_sign(c_b4, f);
5709: }
5710: if (pmax == 2) {
5711: tsign = d_sign(c_b4, snr[0]) * d_sign(c_b4, csl[0])
5712: * d_sign(c_b4, g);
5713: }
5714: if (pmax == 3) {
5715: tsign = d_sign(c_b4, snr[0]) * d_sign(c_b4, snl[0])
5716: * d_sign(c_b4, h);
5717: }
5718: single_values[index] = d_sign(ssmax, tsign);
5719: d__1 = tsign * d_sign(c_b4, f) * d_sign(c_b4, h);
5720: single_values[index + 1] = d_sign(ssmin, d__1);
5721:
5722: }
5723: return 0;
5724: }
5725:
5726: static double compute_rot(double f, double g, double[] sin,
5727: double[] cos, int index, int first) {
5728: int i__1;
5729: double d__1, d__2;
5730: double cs, sn;
5731: int i;
5732: double scale;
5733: int count;
5734: double f1, g1;
5735: double r;
5736: final double safmn2 = 2.002083095183101E-146;
5737: final double safmx2 = 4.994797680505588E+145;
5738:
5739: if (g == 0.) {
5740: cs = 1.;
5741: sn = 0.;
5742: r = f;
5743: } else if (f == 0.) {
5744: cs = 0.;
5745: sn = 1.;
5746: r = g;
5747: } else {
5748: f1 = f;
5749: g1 = g;
5750: scale = max(Math.abs(f1), Math.abs(g1));
5751: if (scale >= safmx2) {
5752: count = 0;
5753: while (scale >= safmx2) {
5754: ++count;
5755: f1 *= safmn2;
5756: g1 *= safmn2;
5757: scale = max(Math.abs(f1), Math.abs(g1));
5758: }
5759: r = Math.sqrt(f1 * f1 + g1 * g1);
5760: cs = f1 / r;
5761: sn = g1 / r;
5762: i__1 = count;
5763: for (i = 1; i <= count; ++i) {
5764: r *= safmx2;
5765: }
5766: } else if (scale <= safmn2) {
5767: count = 0;
5768: while (scale <= safmn2) {
5769: ++count;
5770: f1 *= safmx2;
5771: g1 *= safmx2;
5772: scale = max(Math.abs(f1), Math.abs(g1));
5773: }
5774: r = Math.sqrt(f1 * f1 + g1 * g1);
5775: cs = f1 / r;
5776: sn = g1 / r;
5777: i__1 = count;
5778: for (i = 1; i <= count; ++i) {
5779: r *= safmn2;
5780: }
5781: } else {
5782: r = Math.sqrt(f1 * f1 + g1 * g1);
5783: cs = f1 / r;
5784: sn = g1 / r;
5785: }
5786: if (Math.abs(f) > Math.abs(g) && cs < 0.) {
5787: cs = -cs;
5788: sn = -sn;
5789: r = -r;
5790: }
5791: }
5792: sin[index] = sn;
5793: cos[index] = cs;
5794: return r;
5795:
5796: }
5797:
5798: static final private double max3(double[] values) {
5799: if (values[0] > values[1]) {
5800: if (values[0] > values[2])
5801: return (values[0]);
5802: else
5803: return (values[2]);
5804: } else {
5805: if (values[1] > values[2])
5806: return (values[1]);
5807: else
5808: return (values[2]);
5809: }
5810: }
5811:
5812: final private void computeScales(boolean forceSVD) {
5813:
5814: if (scales == null)
5815: scales = new double[3];
5816:
5817: if ((!forceSVD || ((dirtyBits & SVD_BIT) == 0)) && isAffine()) {
5818: if (isCongruent()) {
5819: if (((dirtyBits & RIGID_BIT) == 0)
5820: && ((type & RIGID) != 0)) {
5821: scales[0] = scales[1] = scales[2] = 1;
5822: dirtyBits &= ~SCALE_BIT;
5823: return;
5824: }
5825: scales[0] = scales[1] = scales[2] = Math.sqrt(mat[0]
5826: * mat[0] + mat[4] * mat[4] + mat[8] * mat[8]);
5827: dirtyBits &= ~SCALE_BIT;
5828: return;
5829: }
5830: if (isOrtho()) {
5831: scales[0] = Math.sqrt(mat[0] * mat[0] + mat[4] * mat[4]
5832: + mat[8] * mat[8]);
5833: scales[1] = Math.sqrt(mat[1] * mat[1] + mat[5] * mat[5]
5834: + mat[9] * mat[9]);
5835: scales[2] = Math.sqrt(mat[2] * mat[2] + mat[6] * mat[6]
5836: + mat[10] * mat[10]);
5837: dirtyBits &= ~SCALE_BIT;
5838: return;
5839: }
5840: }
5841: // fall back to use SVD decomposition
5842: if (rot == null)
5843: rot = new double[9];
5844:
5845: compute_svd(this , scales, rot);
5846: dirtyBits &= ~ROTSCALESVD_DIRTY;
5847: }
5848:
5849: final private void computeScaleRotation(boolean forceSVD) {
5850:
5851: if (rot == null)
5852: rot = new double[9];
5853:
5854: if (scales == null)
5855: scales = new double[3];
5856:
5857: if ((!forceSVD || ((dirtyBits & SVD_BIT) == 0)) && isAffine()) {
5858: if (isCongruent()) {
5859: if (((dirtyBits & RIGID_BIT) == 0)
5860: && ((type & RIGID) != 0)) {
5861: rot[0] = mat[0];
5862: rot[1] = mat[1];
5863: rot[2] = mat[2];
5864: rot[3] = mat[4];
5865: rot[4] = mat[5];
5866: rot[5] = mat[6];
5867: rot[6] = mat[8];
5868: rot[7] = mat[9];
5869: rot[8] = mat[10];
5870: scales[0] = scales[1] = scales[2] = 1;
5871: dirtyBits &= (~ROTATION_BIT | ~SCALE_BIT);
5872: return;
5873: }
5874: double s = Math.sqrt(mat[0] * mat[0] + mat[4] * mat[4]
5875: + mat[8] * mat[8]);
5876: if (s == 0) {
5877: compute_svd(this , scales, rot);
5878: return;
5879: }
5880: scales[0] = scales[1] = scales[2] = s;
5881: s = 1 / s;
5882: rot[0] = mat[0] * s;
5883: rot[1] = mat[1] * s;
5884: rot[2] = mat[2] * s;
5885: rot[3] = mat[4] * s;
5886: rot[4] = mat[5] * s;
5887: rot[5] = mat[6] * s;
5888: rot[6] = mat[8] * s;
5889: rot[7] = mat[9] * s;
5890: rot[8] = mat[10] * s;
5891: dirtyBits &= (~ROTATION_BIT | ~SCALE_BIT);
5892: return;
5893: }
5894: if (isOrtho()) {
5895: double s;
5896:
5897: scales[0] = Math.sqrt(mat[0] * mat[0] + mat[4] * mat[4]
5898: + mat[8] * mat[8]);
5899: scales[1] = Math.sqrt(mat[1] * mat[1] + mat[5] * mat[5]
5900: + mat[9] * mat[9]);
5901: scales[2] = Math.sqrt(mat[2] * mat[2] + mat[6] * mat[6]
5902: + mat[10] * mat[10]);
5903:
5904: if ((scales[0] == 0) || (scales[1] == 0)
5905: || (scales[2] == 0)) {
5906: compute_svd(this , scales, rot);
5907: return;
5908: }
5909: s = 1 / scales[0];
5910: rot[0] = mat[0] * s;
5911: rot[3] = mat[4] * s;
5912: rot[6] = mat[8] * s;
5913: s = 1 / scales[1];
5914: rot[1] = mat[1] * s;
5915: rot[4] = mat[5] * s;
5916: rot[7] = mat[9] * s;
5917: s = 1 / scales[2];
5918: rot[2] = mat[2] * s;
5919: rot[5] = mat[6] * s;
5920: rot[8] = mat[10] * s;
5921: dirtyBits &= (~ROTATION_BIT | ~SCALE_BIT);
5922: return;
5923: }
5924: }
5925: // fall back to use SVD decomposition
5926: compute_svd(this , scales, rot);
5927: dirtyBits &= ~ROTSCALESVD_DIRTY;
5928: }
5929:
5930: final void getRotation(Transform3D t) {
5931: if ((dirtyBits & ROTATION_BIT) != 0) {
5932: computeScaleRotation(false);
5933: }
5934:
5935: t.mat[3] = t.mat[7] = t.mat[11] = t.mat[12] = t.mat[13] = t.mat[14] = 0;
5936: t.mat[15] = 1;
5937: t.mat[0] = rot[0];
5938: t.mat[1] = rot[1];
5939: t.mat[2] = rot[2];
5940: t.mat[4] = rot[3];
5941: t.mat[5] = rot[4];
5942: t.mat[6] = rot[5];
5943: t.mat[8] = rot[6];
5944: t.mat[9] = rot[7];
5945: t.mat[10] = rot[8];
5946:
5947: // Issue 253: set all dirty bits
5948: t.dirtyBits = ALL_DIRTY;
5949: }
5950:
5951: // somehow CanvasViewCache will directly modify mat[]
5952: // instead of calling ortho(). So we need to reset dirty bit
5953: final void setOrthoDirtyBit() {
5954: // Issue 253: set all dirty bits
5955: dirtyBits = ALL_DIRTY;
5956: type = 0;
5957: }
5958:
5959: // Fix for Issue 167 -- don't classify matrices with Infinity or NaN values
5960: // as affine
5961: private final boolean isInfOrNaN() {
5962: // The following is a faster version of the check.
5963: // Instead of 3 tests per array element (Double.isInfinite is 2 tests),
5964: // for a total of 48 tests, we will do 16 multiplies and 1 test.
5965: double d = 0.0;
5966: for (int i = 0; i < 16; i++) {
5967: d *= mat[i];
5968: }
5969:
5970: return d != 0.0;
5971: }
5972:
5973: // Fix for Issue 253
5974: // Methods to check input parameters for Infinity or NaN values
5975: private final boolean isInfOrNaN(Quat4f q) {
5976: return (Float.isNaN(q.x) || Float.isInfinite(q.x)
5977: || Float.isNaN(q.y) || Float.isInfinite(q.y)
5978: || Float.isNaN(q.z) || Float.isInfinite(q.z)
5979: || Float.isNaN(q.w) || Float.isInfinite(q.w));
5980: }
5981:
5982: private boolean isInfOrNaN(Quat4d q) {
5983: return (Double.isNaN(q.x) || Double.isInfinite(q.x)
5984: || Double.isNaN(q.y) || Double.isInfinite(q.y)
5985: || Double.isNaN(q.z) || Double.isInfinite(q.z)
5986: || Double.isNaN(q.w) || Double.isInfinite(q.w));
5987: }
5988:
5989: private boolean isInfOrNaN(AxisAngle4f a) {
5990: return (Float.isNaN(a.x) || Float.isInfinite(a.x)
5991: || Float.isNaN(a.y) || Float.isInfinite(a.y)
5992: || Float.isNaN(a.z) || Float.isInfinite(a.z)
5993: || Float.isNaN(a.angle) || Float.isInfinite(a.angle));
5994: }
5995:
5996: private boolean isInfOrNaN(AxisAngle4d a) {
5997: return (Double.isNaN(a.x) || Double.isInfinite(a.x)
5998: || Double.isNaN(a.y) || Double.isInfinite(a.y)
5999: || Double.isNaN(a.z) || Double.isInfinite(a.z)
6000: || Double.isNaN(a.angle) || Double.isInfinite(a.angle));
6001: }
6002:
6003: private boolean isInfOrNaN(double val) {
6004: return Double.isNaN(val) || Double.isInfinite(val);
6005: }
6006:
6007: private boolean isInfOrNaN(Vector3f v) {
6008: return (Float.isNaN(v.x) || Float.isInfinite(v.x)
6009: || Float.isNaN(v.y) || Float.isInfinite(v.y)
6010: || Float.isNaN(v.z) || Float.isInfinite(v.z));
6011: }
6012:
6013: private boolean isInfOrNaN(Vector3d v) {
6014: return (Double.isNaN(v.x) || Double.isInfinite(v.x)
6015: || Double.isNaN(v.y) || Double.isInfinite(v.y)
6016: || Double.isNaN(v.z) || Double.isInfinite(v.z));
6017: }
6018: }
|