toxiclibs 0.2-java → 0.5.0-java

data/src/toxi/geom/LocalGridTesselator.java

@@ -0,0 +1,57 @@
+package toxi.geom;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import toxi.math.ScaleMap;
+
+/**
+ * A concrete implementation of the abstract {@link GridTesselator} using a grid
+ * in polygon-local coordinate space for generating additional points within a
+ * polygon. The resolution setting of this class defines number of desired grid
+ * points in X & Y direction. E.g. a resolution of 10 means up to 10x10 grid
+ * points are created a within the polygon bounding rect. For smaller polygons,
+ * the resulting triangles will simply be smaller too. This resolution is used
+ * independently on polygon size. Use the {@link GlobalGridTesselator} for an
+ * alternative behavior, resulting in more uniformly sized triangles.
+ * 
+ * @see GridTesselator
+ * @see GlobalGridTesselator
+ * @see PolygonTesselator
+ */
+public class LocalGridTesselator extends GridTesselator {
+
+    /**
+     *
+     * @param res
+     */
+    public LocalGridTesselator(int res) {
+        super(res);
+    }
+
+    /**
+     *
+     * @param poly
+     * @param bounds
+     * @return
+     */
+    @Override
+    protected List<Vec2D> createInsidePoints(Polygon2D poly, Rect bounds) {
+        List<Vec2D> points = new ArrayList<>();
+        int ires = (int) res;
+        ScaleMap xmap = new ScaleMap(0, ires, bounds.getLeft(),
+                bounds.getRight());
+        ScaleMap ymap = new ScaleMap(0, ires, bounds.getTop(),
+                bounds.getBottom());
+        for (int y = 0; y < ires; y++) {
+            float yy = (float) ymap.getMappedValueFor(y);
+            for (int x = 0; x < ires; x++) {
+                Vec2D p = new Vec2D((float) xmap.getMappedValueFor(x), yy);
+                if (poly.containsPoint(p)) {
+                    points.add(p);
+                }
+            }
+        }
+        return points;
+    }
+}

data/src/toxi/geom/Matrix3d.java

@@ -0,0 +1,3048 @@
+/*
+ * $RCSfile$
+ *
+ * Copyright 1996-2008 Sun Microsystems, Inc.  All Rights Reserved.
+ * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
+ *
+ * This code is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 only, as
+ * published by the Free Software Foundation.  Sun designates this
+ * particular file as subject to the "Classpath" exception as provided
+ * by Sun in the LICENSE file that accompanied this code.
+ *
+ * This code is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+ * version 2 for more details (a copy is included in the LICENSE file that
+ * accompanied this code).
+ *
+ * You should have received a copy of the GNU General Public License version
+ * 2 along with this work; if not, write to the Free Software Foundation,
+ * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
+ * CA 95054 USA or visit www.sun.com if you need additional information or
+ * have any questions.
+ *
+ * $Revision: 127 $
+ * $Date: 2008-02-28 20:18:51 +0000 (Thu, 28 Feb 2008) $
+ * $State$
+ */
+
+package toxi.geom;
+
+import toxi.math.MathUtils;
+
+/**
+ * A double precision floating point 3 by 3 matrix. Primarily to support 3D
+ * rotations.
+ */
+public class Matrix3d implements java.io.Serializable, Cloneable {
+
+    // Compatible with 1.1
+    static final long serialVersionUID = 6837536777072402710L;
+
+    private static boolean almostEqual(double a, double b) {
+        if (a == b) {
+            return true;
+        }
+
+        final double EPSILON_ABSOLUTE = 1.0e-6;
+        final double EPSILON_RELATIVE = 1.0e-4;
+        double diff = Math.abs(a - b);
+        double absA = Math.abs(a);
+        double absB = Math.abs(b);
+        double max = (absA >= absB) ? absA : absB;
+
+        if (diff < EPSILON_ABSOLUTE) {
+            return true;
+        }
+        return ((diff / max) < EPSILON_RELATIVE);
+    }
+
+    static int compute_2X2(double f, double g, double h,
+            double[] single_values, double[] snl, double[] csl, double[] snr,
+            double[] csr, int index) {
+
+        double c_b3 = 2.;
+        double c_b4 = 1.;
+
+        double d__1;
+        int pmax;
+        double temp;
+        boolean swap;
+        double a, d, l, m, r, s, t, tsign, fa, ga, ha;
+        double ft, gt, ht, mm;
+        boolean gasmal;
+        double tt, clt, crt, slt, srt;
+        double ssmin, ssmax;
+
+        ssmax = single_values[0];
+        ssmin = single_values[1];
+        clt = 0.0;
+        crt = 0.0;
+        slt = 0.0;
+        srt = 0.0;
+        tsign = 0.0;
+
+        ft = f;
+        fa = Math.abs(ft);
+        ht = h;
+        ha = Math.abs(h);
+        pmax = 1;
+        swap = ha > fa;
+
+        if (swap) {
+            pmax = 3;
+            temp = ft;
+            ft = ht;
+            ht = temp;
+            temp = fa;
+            fa = ha;
+            ha = temp;
+
+        }
+        gt = g;
+        ga = Math.abs(gt);
+        if (ga == 0.) {
+
+            single_values[1] = ha;
+            single_values[0] = fa;
+            clt = 1.;
+            crt = 1.;
+            slt = 0.;
+            srt = 0.;
+        } else {
+            gasmal = true;
+
+            if (ga > fa) {
+                pmax = 2;
+                if (fa / ga < EPS) {
+
+                    gasmal = false;
+                    ssmax = ga;
+                    if (ha > 1.) {
+                        ssmin = fa / (ga / ha);
+                    } else {
+                        ssmin = fa / ga * ha;
+                    }
+                    clt = 1.;
+                    slt = ht / gt;
+                    srt = 1.;
+                    crt = ft / gt;
+                }
+            }
+            if (gasmal) {
+
+                d = fa - ha;
+                if (d == fa) {
+
+                    l = 1.;
+                } else {
+                    l = d / fa;
+                }
+
+                m = gt / ft;
+
+                t = 2. - l;
+
+                mm = m * m;
+                tt = t * t;
+                s = Math.sqrt(tt + mm);
+
+                if (l == 0.) {
+                    r = Math.abs(m);
+                } else {
+                    r = Math.sqrt(l * l + mm);
+                }
+
+                a = (s + r) * .5;
+
+                if (ga > fa) {
+                    pmax = 2;
+                    if (fa / ga < EPS) {
+
+                        gasmal = false;
+                        ssmax = ga;
+                        if (ha > 1.) {
+                            ssmin = fa / (ga / ha);
+                        } else {
+                            ssmin = fa / ga * ha;
+                        }
+                        clt = 1.;
+                        slt = ht / gt;
+                        srt = 1.;
+                        crt = ft / gt;
+                    }
+                }
+                if (gasmal) {
+
+                    d = fa - ha;
+                    if (d == fa) {
+
+                        l = 1.;
+                    } else {
+                        l = d / fa;
+                    }
+
+                    m = gt / ft;
+
+                    t = 2. - l;
+
+                    mm = m * m;
+                    tt = t * t;
+                    s = Math.sqrt(tt + mm);
+
+                    if (l == 0.) {
+                        r = Math.abs(m);
+                    } else {
+                        r = Math.sqrt(l * l + mm);
+                    }
+
+                    a = (s + r) * .5;
+
+                    ssmin = ha / a;
+                    ssmax = fa * a;
+                    if (mm == 0.) {
+
+                        if (l == 0.) {
+                            t = MathUtils.dualSign(c_b3, ft)
+                                    * MathUtils.dualSign(c_b4, gt);
+                        } else {
+                            t = gt / MathUtils.dualSign(d, ft) + m / t;
+                        }
+                    } else {
+                        t = (m / (s + t) + m / (r + l)) * (a + 1.);
+                    }
+                    l = Math.sqrt(t * t + 4.);
+                    crt = 2. / l;
+                    srt = t / l;
+                    clt = (crt + srt * m) / a;
+                    slt = ht / ft * srt / a;
+                }
+            }
+            if (swap) {
+                csl[0] = srt;
+                snl[0] = crt;
+                csr[0] = slt;
+                snr[0] = clt;
+            } else {
+                csl[0] = clt;
+                snl[0] = slt;
+                csr[0] = crt;
+                snr[0] = srt;
+            }
+
+            if (pmax == 1) {
+                tsign = MathUtils.dualSign(c_b4, csr[0])
+                        * MathUtils.dualSign(c_b4, csl[0])
+                        * MathUtils.dualSign(c_b4, f);
+            }
+            if (pmax == 2) {
+                tsign = MathUtils.dualSign(c_b4, snr[0])
+                        * MathUtils.dualSign(c_b4, csl[0])
+                        * MathUtils.dualSign(c_b4, g);
+            }
+            if (pmax == 3) {
+                tsign = MathUtils.dualSign(c_b4, snr[0])
+                        * MathUtils.dualSign(c_b4, snl[0])
+                        * MathUtils.dualSign(c_b4, h);
+            }
+            single_values[index] = MathUtils.dualSign(ssmax, tsign);
+            d__1 = tsign * MathUtils.dualSign(c_b4, f)
+                    * MathUtils.dualSign(c_b4, h);
+            single_values[index + 1] = MathUtils.dualSign(ssmin, d__1);
+
+        }
+        return 0;
+    }
+
+    static int compute_qr(double[] s, double[] e, double[] u, double[] v) {
+        int k;
+        boolean converged;
+        double shift, r;
+        double[] cosl = new double[2];
+        double[] cosr = new double[2];
+        double[] sinl = new double[2];
+        double[] sinr = new double[2];
+        double[] m = new double[9];
+
+        double utemp, vtemp;
+        double f, g;
+
+        final int MAX_INTERATIONS = 10;
+        final double CONVERGE_TOL = 4.89E-15;
+
+        double c_b48 = 1.;
+        int first;
+        converged = false;
+
+        first = 1;
+
+        if (Math.abs(e[1]) < CONVERGE_TOL || Math.abs(e[0]) < CONVERGE_TOL) {
+            converged = true;
+        }
+
+        for (k = 0; k < MAX_INTERATIONS && !converged; k++) {
+            shift = compute_shift(s[1], e[1], s[2]);
+            f = (Math.abs(s[0]) - shift)
+                    * (MathUtils.dualSign(c_b48, s[0]) + shift / s[0]);
+            g = e[0];
+            r = compute_rot(f, g, sinr, cosr, 0, first);
+            f = cosr[0] * s[0] + sinr[0] * e[0];
+            e[0] = cosr[0] * e[0] - sinr[0] * s[0];
+            g = sinr[0] * s[1];
+            s[1] = cosr[0] * s[1];
+
+            r = compute_rot(f, g, sinl, cosl, 0, first);
+            first = 0;
+            s[0] = r;
+            f = cosl[0] * e[0] + sinl[0] * s[1];
+            s[1] = cosl[0] * s[1] - sinl[0] * e[0];
+            g = sinl[0] * e[1];
+            e[1] = cosl[0] * e[1];
+
+            r = compute_rot(f, g, sinr, cosr, 1, first);
+            e[0] = r;
+            f = cosr[1] * s[1] + sinr[1] * e[1];
+            e[1] = cosr[1] * e[1] - sinr[1] * s[1];
+            g = sinr[1] * s[2];
+            s[2] = cosr[1] * s[2];
+
+            r = compute_rot(f, g, sinl, cosl, 1, first);
+            s[1] = r;
+            f = cosl[1] * e[1] + sinl[1] * s[2];
+            s[2] = cosl[1] * s[2] - sinl[1] * e[1];
+            e[1] = f;
+
+            // update u matrices
+            utemp = u[0];
+            u[0] = cosl[0] * utemp + sinl[0] * u[3];
+            u[3] = -sinl[0] * utemp + cosl[0] * u[3];
+            utemp = u[1];
+            u[1] = cosl[0] * utemp + sinl[0] * u[4];
+            u[4] = -sinl[0] * utemp + cosl[0] * u[4];
+            utemp = u[2];
+            u[2] = cosl[0] * utemp + sinl[0] * u[5];
+            u[5] = -sinl[0] * utemp + cosl[0] * u[5];
+
+            utemp = u[3];
+            u[3] = cosl[1] * utemp + sinl[1] * u[6];
+            u[6] = -sinl[1] * utemp + cosl[1] * u[6];
+            utemp = u[4];
+            u[4] = cosl[1] * utemp + sinl[1] * u[7];
+            u[7] = -sinl[1] * utemp + cosl[1] * u[7];
+            utemp = u[5];
+            u[5] = cosl[1] * utemp + sinl[1] * u[8];
+            u[8] = -sinl[1] * utemp + cosl[1] * u[8];
+
+            // update v matrices
+
+            vtemp = v[0];
+            v[0] = cosr[0] * vtemp + sinr[0] * v[1];
+            v[1] = -sinr[0] * vtemp + cosr[0] * v[1];
+            vtemp = v[3];
+            v[3] = cosr[0] * vtemp + sinr[0] * v[4];
+            v[4] = -sinr[0] * vtemp + cosr[0] * v[4];
+            vtemp = v[6];
+            v[6] = cosr[0] * vtemp + sinr[0] * v[7];
+            v[7] = -sinr[0] * vtemp + cosr[0] * v[7];
+
+            vtemp = v[1];
+            v[1] = cosr[1] * vtemp + sinr[1] * v[2];
+            v[2] = -sinr[1] * vtemp + cosr[1] * v[2];
+            vtemp = v[4];
+            v[4] = cosr[1] * vtemp + sinr[1] * v[5];
+            v[5] = -sinr[1] * vtemp + cosr[1] * v[5];
+            vtemp = v[7];
+            v[7] = cosr[1] * vtemp + sinr[1] * v[8];
+            v[8] = -sinr[1] * vtemp + cosr[1] * v[8];
+
+            m[0] = s[0];
+            m[1] = e[0];
+            m[2] = 0.0;
+            m[3] = 0.0;
+            m[4] = s[1];
+            m[5] = e[1];
+            m[6] = 0.0;
+            m[7] = 0.0;
+            m[8] = s[2];
+
+            if (Math.abs(e[1]) < CONVERGE_TOL || Math.abs(e[0]) < CONVERGE_TOL) {
+                converged = true;
+            }
+        }
+
+        if (Math.abs(e[1]) < CONVERGE_TOL) {
+            compute_2X2(s[0], e[0], s[1], s, sinl, cosl, sinr, cosr, 0);
+
+            utemp = u[0];
+            u[0] = cosl[0] * utemp + sinl[0] * u[3];
+            u[3] = -sinl[0] * utemp + cosl[0] * u[3];
+            utemp = u[1];
+            u[1] = cosl[0] * utemp + sinl[0] * u[4];
+            u[4] = -sinl[0] * utemp + cosl[0] * u[4];
+            utemp = u[2];
+            u[2] = cosl[0] * utemp + sinl[0] * u[5];
+            u[5] = -sinl[0] * utemp + cosl[0] * u[5];
+
+            // update v matrices
+
+            vtemp = v[0];
+            v[0] = cosr[0] * vtemp + sinr[0] * v[1];
+            v[1] = -sinr[0] * vtemp + cosr[0] * v[1];
+            vtemp = v[3];
+            v[3] = cosr[0] * vtemp + sinr[0] * v[4];
+            v[4] = -sinr[0] * vtemp + cosr[0] * v[4];
+            vtemp = v[6];
+            v[6] = cosr[0] * vtemp + sinr[0] * v[7];
+            v[7] = -sinr[0] * vtemp + cosr[0] * v[7];
+        } else {
+            compute_2X2(s[1], e[1], s[2], s, sinl, cosl, sinr, cosr, 1);
+
+            utemp = u[3];
+            u[3] = cosl[0] * utemp + sinl[0] * u[6];
+            u[6] = -sinl[0] * utemp + cosl[0] * u[6];
+            utemp = u[4];
+            u[4] = cosl[0] * utemp + sinl[0] * u[7];
+            u[7] = -sinl[0] * utemp + cosl[0] * u[7];
+            utemp = u[5];
+            u[5] = cosl[0] * utemp + sinl[0] * u[8];
+            u[8] = -sinl[0] * utemp + cosl[0] * u[8];
+
+            // update v matrices
+
+            vtemp = v[1];
+            v[1] = cosr[0] * vtemp + sinr[0] * v[2];
+            v[2] = -sinr[0] * vtemp + cosr[0] * v[2];
+            vtemp = v[4];
+            v[4] = cosr[0] * vtemp + sinr[0] * v[5];
+            v[5] = -sinr[0] * vtemp + cosr[0] * v[5];
+            vtemp = v[7];
+            v[7] = cosr[0] * vtemp + sinr[0] * v[8];
+            v[8] = -sinr[0] * vtemp + cosr[0] * v[8];
+        }
+
+        return (0);
+    }
+
+    static double compute_rot(double f, double g, double[] sin, double[] cos,
+            int index, int first) {
+        double cs, sn;
+        int i;
+        double scale;
+        int count;
+        double f1, g1;
+        double r;
+        final double safmn2 = 2.002083095183101E-146;
+        final double safmx2 = 4.994797680505588E+145;
+
+        if (g == 0.) {
+            cs = 1.;
+            sn = 0.;
+            r = f;
+        } else if (f == 0.) {
+            cs = 0.;
+            sn = 1.;
+            r = g;
+        } else {
+            f1 = f;
+            g1 = g;
+            scale = MathUtils.max(MathUtils.abs(f1), MathUtils.abs(g1));
+            if (scale >= safmx2) {
+                count = 0;
+                while (scale >= safmx2) {
+                    ++count;
+                    f1 *= safmn2;
+                    g1 *= safmn2;
+                    scale = MathUtils.max(MathUtils.abs(f1), MathUtils.abs(g1));
+                }
+                r = Math.sqrt(f1 * f1 + g1 * g1);
+                cs = f1 / r;
+                sn = g1 / r;
+                for (i = 1; i <= count; ++i) {
+                    r *= safmx2;
+                }
+            } else if (scale <= safmn2) {
+                count = 0;
+                while (scale <= safmn2) {
+                    ++count;
+                    f1 *= safmx2;
+                    g1 *= safmx2;
+                    scale = MathUtils.max(MathUtils.abs(f1), MathUtils.abs(g1));
+                }
+                r = Math.sqrt(f1 * f1 + g1 * g1);
+                cs = f1 / r;
+                sn = g1 / r;
+                for (i = 1; i <= count; ++i) {
+                    r *= safmn2;
+                }
+            } else {
+                r = Math.sqrt(f1 * f1 + g1 * g1);
+                cs = f1 / r;
+                sn = g1 / r;
+            }
+            if (Math.abs(f) > Math.abs(g) && cs < 0.) {
+                cs = -cs;
+                sn = -sn;
+                r = -r;
+            }
+        }
+        sin[index] = sn;
+        cos[index] = cs;
+        return r;
+
+    }
+
+    static double compute_shift(double f, double g, double h) {
+        double d__1, d__2;
+        double fhmn, fhmx, c, fa, ga, ha, as, at, au;
+        double ssmin;
+
+        fa = Math.abs(f);
+        ga = Math.abs(g);
+        ha = Math.abs(h);
+        fhmn = MathUtils.min(fa, ha);
+        fhmx = MathUtils.max(fa, ha);
+        if (fhmn == 0.) {
+            ssmin = 0.;
+            if (fhmx == 0.) {
+            } else {
+                d__1 = MathUtils.min(fhmx, ga) / MathUtils.max(fhmx, ga);
+            }
+        } else {
+            if (ga < fhmx) {
+                as = fhmn / fhmx + 1.;
+                at = (fhmx - fhmn) / fhmx;
+                d__1 = ga / fhmx;
+                au = d__1 * d__1;
+                c = 2. / (Math.sqrt(as * as + au) + Math.sqrt(at * at + au));
+                ssmin = fhmn * c;
+            } else {
+                au = fhmx / ga;
+                if (au == 0.) {
+                    ssmin = fhmn * fhmx / ga;
+                } else {
+                    as = fhmn / fhmx + 1.;
+                    at = (fhmx - fhmn) / fhmx;
+                    d__1 = as * au;
+                    d__2 = at * au;
+                    c = 1. / (Math.sqrt(d__1 * d__1 + 1.) + Math.sqrt(d__2
+                            * d__2 + 1.));
+                    ssmin = fhmn * c * au;
+                    ssmin += ssmin;
+                }
+            }
+        }
+
+        return (ssmin);
+    }
+
+    static void compute_svd(double[] m, double[] outScale, double[] outRot) {
+        int i;
+        double g;
+        double[] u1 = new double[9];
+        double[] v1 = new double[9];
+        double[] t1 = new double[9];
+        double[] t2 = new double[9];
+
+        double[] tmp = t1;
+        double[] single_values = t2;
+
+        double[] rot = new double[9];
+        double[] e = new double[3];
+        double[] scales = new double[3];
+
+        int negCnt = 0;
+        double c1, c2, c3, c4;
+        double s1, s2, s3, s4;
+
+        for (i = 0; i < 9; i++) {
+            rot[i] = m[i];
+        }
+
+        // u1
+
+        if (m[3] * m[3] < EPS) {
+            u1[0] = 1.0;
+            u1[1] = 0.0;
+            u1[2] = 0.0;
+            u1[3] = 0.0;
+            u1[4] = 1.0;
+            u1[5] = 0.0;
+            u1[6] = 0.0;
+            u1[7] = 0.0;
+            u1[8] = 1.0;
+        } else if (m[0] * m[0] < EPS) {
+            tmp[0] = m[0];
+            tmp[1] = m[1];
+            tmp[2] = m[2];
+            m[0] = m[3];
+            m[1] = m[4];
+            m[2] = m[5];
+
+            m[3] = -tmp[0]; // zero
+            m[4] = -tmp[1];
+            m[5] = -tmp[2];
+
+            u1[0] = 0.0;
+            u1[1] = 1.0;
+            u1[2] = 0.0;
+            u1[3] = -1.0;
+            u1[4] = 0.0;
+            u1[5] = 0.0;
+            u1[6] = 0.0;
+            u1[7] = 0.0;
+            u1[8] = 1.0;
+        } else {
+            g = 1.0 / Math.sqrt(m[0] * m[0] + m[3] * m[3]);
+            c1 = m[0] * g;
+            s1 = m[3] * g;
+            tmp[0] = c1 * m[0] + s1 * m[3];
+            tmp[1] = c1 * m[1] + s1 * m[4];
+            tmp[2] = c1 * m[2] + s1 * m[5];
+
+            m[3] = -s1 * m[0] + c1 * m[3]; // zero
+            m[4] = -s1 * m[1] + c1 * m[4];
+            m[5] = -s1 * m[2] + c1 * m[5];
+
+            m[0] = tmp[0];
+            m[1] = tmp[1];
+            m[2] = tmp[2];
+            u1[0] = c1;
+            u1[1] = s1;
+            u1[2] = 0.0;
+            u1[3] = -s1;
+            u1[4] = c1;
+            u1[5] = 0.0;
+            u1[6] = 0.0;
+            u1[7] = 0.0;
+            u1[8] = 1.0;
+        }
+
+        // u2
+
+        if (m[6] * m[6] < EPS) {
+        } else if (m[0] * m[0] < EPS) {
+            tmp[0] = m[0];
+            tmp[1] = m[1];
+            tmp[2] = m[2];
+            m[0] = m[6];
+            m[1] = m[7];
+            m[2] = m[8];
+
+            m[6] = -tmp[0]; // zero
+            m[7] = -tmp[1];
+            m[8] = -tmp[2];
+
+            tmp[0] = u1[0];
+            tmp[1] = u1[1];
+            tmp[2] = u1[2];
+            u1[0] = u1[6];
+            u1[1] = u1[7];
+            u1[2] = u1[8];
+
+            u1[6] = -tmp[0]; // zero
+            u1[7] = -tmp[1];
+            u1[8] = -tmp[2];
+        } else {
+            g = 1.0 / Math.sqrt(m[0] * m[0] + m[6] * m[6]);
+            c2 = m[0] * g;
+            s2 = m[6] * g;
+            tmp[0] = c2 * m[0] + s2 * m[6];
+            tmp[1] = c2 * m[1] + s2 * m[7];
+            tmp[2] = c2 * m[2] + s2 * m[8];
+
+            m[6] = -s2 * m[0] + c2 * m[6];
+            m[7] = -s2 * m[1] + c2 * m[7];
+            m[8] = -s2 * m[2] + c2 * m[8];
+            m[0] = tmp[0];
+            m[1] = tmp[1];
+            m[2] = tmp[2];
+
+            tmp[0] = c2 * u1[0];
+            tmp[1] = c2 * u1[1];
+            u1[2] = s2;
+
+            tmp[6] = -u1[0] * s2;
+            tmp[7] = -u1[1] * s2;
+            u1[8] = c2;
+            u1[0] = tmp[0];
+            u1[1] = tmp[1];
+            u1[6] = tmp[6];
+            u1[7] = tmp[7];
+        }
+
+        // v1
+
+        if (m[2] * m[2] < EPS) {
+            v1[0] = 1.0;
+            v1[1] = 0.0;
+            v1[2] = 0.0;
+            v1[3] = 0.0;
+            v1[4] = 1.0;
+            v1[5] = 0.0;
+            v1[6] = 0.0;
+            v1[7] = 0.0;
+            v1[8] = 1.0;
+        } else if (m[1] * m[1] < EPS) {
+            tmp[2] = m[2];
+            tmp[5] = m[5];
+            tmp[8] = m[8];
+            m[2] = -m[1];
+            m[5] = -m[4];
+            m[8] = -m[7];
+
+            m[1] = tmp[2]; // zero
+            m[4] = tmp[5];
+            m[7] = tmp[8];
+
+            v1[0] = 1.0;
+            v1[1] = 0.0;
+            v1[2] = 0.0;
+            v1[3] = 0.0;
+            v1[4] = 0.0;
+            v1[5] = -1.0;
+            v1[6] = 0.0;
+            v1[7] = 1.0;
+            v1[8] = 0.0;
+        } else {
+            g = 1.0 / Math.sqrt(m[1] * m[1] + m[2] * m[2]);
+            c3 = m[1] * g;
+            s3 = m[2] * g;
+            tmp[1] = c3 * m[1] + s3 * m[2]; // can assign to m[1]?
+            m[2] = -s3 * m[1] + c3 * m[2]; // zero
+            m[1] = tmp[1];
+
+            tmp[4] = c3 * m[4] + s3 * m[5];
+            m[5] = -s3 * m[4] + c3 * m[5];
+            m[4] = tmp[4];
+
+            tmp[7] = c3 * m[7] + s3 * m[8];
+            m[8] = -s3 * m[7] + c3 * m[8];
+            m[7] = tmp[7];
+
+            v1[0] = 1.0;
+            v1[1] = 0.0;
+            v1[2] = 0.0;
+            v1[3] = 0.0;
+            v1[4] = c3;
+            v1[5] = -s3;
+            v1[6] = 0.0;
+            v1[7] = s3;
+            v1[8] = c3;
+        }
+
+        // u3
+
+        if (m[7] * m[7] < EPS) {
+        } else if (m[4] * m[4] < EPS) {
+            tmp[3] = m[3];
+            tmp[4] = m[4];
+            tmp[5] = m[5];
+            m[3] = m[6]; // zero
+            m[4] = m[7];
+            m[5] = m[8];
+
+            m[6] = -tmp[3]; // zero
+            m[7] = -tmp[4]; // zero
+            m[8] = -tmp[5];
+
+            tmp[3] = u1[3];
+            tmp[4] = u1[4];
+            tmp[5] = u1[5];
+            u1[3] = u1[6];
+            u1[4] = u1[7];
+            u1[5] = u1[8];
+
+            u1[6] = -tmp[3]; // zero
+            u1[7] = -tmp[4];
+            u1[8] = -tmp[5];
+
+        } else {
+            g = 1.0 / Math.sqrt(m[4] * m[4] + m[7] * m[7]);
+            c4 = m[4] * g;
+            s4 = m[7] * g;
+            tmp[3] = c4 * m[3] + s4 * m[6];
+            m[6] = -s4 * m[3] + c4 * m[6]; // zero
+            m[3] = tmp[3];
+
+            tmp[4] = c4 * m[4] + s4 * m[7];
+            m[7] = -s4 * m[4] + c4 * m[7];
+            m[4] = tmp[4];
+
+            tmp[5] = c4 * m[5] + s4 * m[8];
+            m[8] = -s4 * m[5] + c4 * m[8];
+            m[5] = tmp[5];
+
+            tmp[3] = c4 * u1[3] + s4 * u1[6];
+            u1[6] = -s4 * u1[3] + c4 * u1[6];
+            u1[3] = tmp[3];
+
+            tmp[4] = c4 * u1[4] + s4 * u1[7];
+            u1[7] = -s4 * u1[4] + c4 * u1[7];
+            u1[4] = tmp[4];
+
+            tmp[5] = c4 * u1[5] + s4 * u1[8];
+            u1[8] = -s4 * u1[5] + c4 * u1[8];
+            u1[5] = tmp[5];
+        }
+
+        single_values[0] = m[0];
+        single_values[1] = m[4];
+        single_values[2] = m[8];
+        e[0] = m[1];
+        e[1] = m[5];
+
+        if (e[0] * e[0] < EPS && e[1] * e[1] < EPS) {
+
+        } else {
+            compute_qr(single_values, e, u1, v1);
+        }
+
+        scales[0] = single_values[0];
+        scales[1] = single_values[1];
+        scales[2] = single_values[2];
+
+        // Do some optimization here. If scale is unity, simply return the
+        // rotation matric.
+        if (almostEqual(Math.abs(scales[0]), 1.0)
+                && almostEqual(Math.abs(scales[1]), 1.0)
+                && almostEqual(Math.abs(scales[2]), 1.0)) {
+            for (i = 0; i < 3; i++) {
+                if (scales[i] < 0.0) {
+                    negCnt++;
+                }
+            }
+
+            if ((negCnt == 0) || (negCnt == 2)) {
+                outScale[0] = outScale[1] = outScale[2] = 1.0;
+                for (i = 0; i < 9; i++) {
+                    outRot[i] = rot[i];
+                }
+
+                return;
+            }
+        }
+
+        transpose_mat(u1, t1);
+        transpose_mat(v1, t2);
+
+        svdReorder(m, t1, t2, scales, outRot, outScale);
+    }
+
+    /**
+     * Solves a set of linear equations. The input parameters "matrix1", and
+     * "row_perm" come from luDecompostionD3x3 and do not change here. The
+     * parameter "matrix2" is a set of column vectors assembled into a 3x3
+     * matrix of floating-point values. The procedure takes each column of
+     * "matrix2" in turn and treats it as the right-hand side of the matrix
+     * equation Ax = LUx = b. The solution vector replaces the original column
+     * of the matrix.
+     * 
+     * If "matrix2" is the identity matrix, the procedure replaces its contents
+     * with the inverse of the matrix from which "matrix1" was originally
+     * derived.
+     */
+    //
+    // Reference: Press, Flannery, Teukolsky, Vetterling,
+    // _Numerical_Recipes_in_C_, Cambridge University Press,
+    // 1988, pp 44-45.
+    //
+    static void luBacksubstitution(double[] matrix1, int[] row_perm,
+            double[] matrix2) {
+
+        int i, ii, ip, j, k;
+        int rp;
+        int cv, rv;
+
+        // rp = row_perm;
+        rp = 0;
+
+        // For each column vector of matrix2 ...
+        for (k = 0; k < 3; k++) {
+            // cv = &(matrix2[0][k]);
+            cv = k;
+            ii = -1;
+
+            // Forward substitution
+            for (i = 0; i < 3; i++) {
+                double sum;
+
+                ip = row_perm[rp + i];
+                sum = matrix2[cv + 3 * ip];
+                matrix2[cv + 3 * ip] = matrix2[cv + 3 * i];
+                if (ii >= 0) {
+                    // rv = &(matrix1[i][0]);
+                    rv = i * 3;
+                    for (j = ii; j <= i - 1; j++) {
+                        sum -= matrix1[rv + j] * matrix2[cv + 3 * j];
+                    }
+                } else if (sum != 0.0) {
+                    ii = i;
+                }
+                matrix2[cv + 3 * i] = sum;
+            }
+
+            // Backsubstitution
+            // rv = &(matrix1[3][0]);
+            rv = 2 * 3;
+            matrix2[cv + 3 * 2] /= matrix1[rv + 2];
+
+            rv -= 3;
+            matrix2[cv + 3 * 1] = (matrix2[cv + 3 * 1] - matrix1[rv + 2]
+                    * matrix2[cv + 3 * 2])
+                    / matrix1[rv + 1];
+
+            rv -= 3;
+            matrix2[cv + 4 * 0] = (matrix2[cv + 3 * 0] - matrix1[rv + 1]
+                    * matrix2[cv + 3 * 1] - matrix1[rv + 2]
+                    * matrix2[cv + 3 * 2])
+                    / matrix1[rv + 0];
+        }
+    }
+
+    static void mat_mul(double[] m1, double[] m2, double[] m3) {
+        int i;
+        double[] tmp = new double[9];
+
+        tmp[0] = m1[0] * m2[0] + m1[1] * m2[3] + m1[2] * m2[6];
+        tmp[1] = m1[0] * m2[1] + m1[1] * m2[4] + m1[2] * m2[7];
+        tmp[2] = m1[0] * m2[2] + m1[1] * m2[5] + m1[2] * m2[8];
+
+        tmp[3] = m1[3] * m2[0] + m1[4] * m2[3] + m1[5] * m2[6];
+        tmp[4] = m1[3] * m2[1] + m1[4] * m2[4] + m1[5] * m2[7];
+        tmp[5] = m1[3] * m2[2] + m1[4] * m2[5] + m1[5] * m2[8];
+
+        tmp[6] = m1[6] * m2[0] + m1[7] * m2[3] + m1[8] * m2[6];
+        tmp[7] = m1[6] * m2[1] + m1[7] * m2[4] + m1[8] * m2[7];
+        tmp[8] = m1[6] * m2[2] + m1[7] * m2[5] + m1[8] * m2[8];
+
+        for (i = 0; i < 9; i++) {
+            m3[i] = tmp[i];
+        }
+    }
+
+    static void print_det(double[] mat) {
+        double det;
+
+        det = mat[0] * mat[4] * mat[8] + mat[1] * mat[5] * mat[6] + mat[2]
+                * mat[3] * mat[7] - mat[2] * mat[4] * mat[6] - mat[0] * mat[5]
+                * mat[7] - mat[1] * mat[3] * mat[8];
+        System.out.println("det= " + det);
+    }
+
+    static void print_mat(double[] mat) {
+        int i;
+        for (i = 0; i < 3; i++) {
+            System.out.println(mat[i * 3 + 0] + " " + mat[i * 3 + 1] + " "
+                    + mat[i * 3 + 2] + "\n");
+        }
+
+    }
+
+    static void svdReorder(double[] m, double[] t1, double[] t2,
+            double[] scales, double[] outRot, double[] outScale) {
+
+        int[] out = new int[3];
+        int[] in = new int[3];
+        int in0, in1, in2, index, i;
+        double[] mag = new double[3];
+        double[] rot = new double[9];
+
+        // check for rotation information in the scales
+        if (scales[0] < 0.0) { // move the rotation info to rotation matrix
+            scales[0] = -scales[0];
+            t2[0] = -t2[0];
+            t2[1] = -t2[1];
+            t2[2] = -t2[2];
+        }
+        if (scales[1] < 0.0) { // move the rotation info to rotation matrix
+            scales[1] = -scales[1];
+            t2[3] = -t2[3];
+            t2[4] = -t2[4];
+            t2[5] = -t2[5];
+        }
+        if (scales[2] < 0.0) { // move the rotation info to rotation matrix
+            scales[2] = -scales[2];
+            t2[6] = -t2[6];
+            t2[7] = -t2[7];
+            t2[8] = -t2[8];
+        }
+
+        mat_mul(t1, t2, rot);
+
+        // check for equal scales case and do not reorder
+        if (almostEqual(Math.abs(scales[0]), Math.abs(scales[1]))
+                && almostEqual(Math.abs(scales[1]), Math.abs(scales[2]))) {
+            for (i = 0; i < 9; i++) {
+                outRot[i] = rot[i];
+            }
+            for (i = 0; i < 3; i++) {
+                outScale[i] = scales[i];
+            }
+
+        } else {
+
+            // sort the order of the results of SVD
+            if (scales[0] > scales[1]) {
+                if (scales[0] > scales[2]) {
+                    if (scales[2] > scales[1]) {
+                        out[0] = 0;
+                        out[1] = 2;
+                        out[2] = 1; // xzy
+                    } else {
+                        out[0] = 0;
+                        out[1] = 1;
+                        out[2] = 2; // xyz
+                    }
+                } else {
+                    out[0] = 2;
+                    out[1] = 0;
+                    out[2] = 1; // zxy
+                }
+            } else { // y > x
+                if (scales[1] > scales[2]) {
+                    if (scales[2] > scales[0]) {
+                        out[0] = 1;
+                        out[1] = 2;
+                        out[2] = 0; // yzx
+                    } else {
+                        out[0] = 1;
+                        out[1] = 0;
+                        out[2] = 2; // yxz
+                    }
+                } else {
+                    out[0] = 2;
+                    out[1] = 1;
+                    out[2] = 0; // zyx
+                }
+            }
+
+            /*
+             * System.out.println("\nscales="+scales[0]+" "+scales[1]+" "+scales[
+             * 2]); System.out.println("\nrot="+rot[0]+" "+rot[1]+" "+rot[2]);
+             * System.out.println("rot="+rot[3]+" "+rot[4]+" "+rot[5]);
+             * System.out.println("rot="+rot[6]+" "+rot[7]+" "+rot[8]);
+             */
+
+            // sort the order of the input matrix
+            mag[0] = (m[0] * m[0] + m[1] * m[1] + m[2] * m[2]);
+            mag[1] = (m[3] * m[3] + m[4] * m[4] + m[5] * m[5]);
+            mag[2] = (m[6] * m[6] + m[7] * m[7] + m[8] * m[8]);
+
+            if (mag[0] > mag[1]) {
+                if (mag[0] > mag[2]) {
+                    if (mag[2] > mag[1]) {
+                        // 0 - 2 - 1
+                        in0 = 0;
+                        in2 = 1;
+                        in1 = 2;// xzy
+                    } else {
+                        // 0 - 1 - 2
+                        in0 = 0;
+                        in1 = 1;
+                        in2 = 2; // xyz
+                    }
+                } else {
+                    // 2 - 0 - 1
+                    in2 = 0;
+                    in0 = 1;
+                    in1 = 2; // zxy
+                }
+            } else { // y > x 1>0
+                if (mag[1] > mag[2]) {
+                    if (mag[2] > mag[0]) {
+                        // 1 - 2 - 0
+                        in1 = 0;
+                        in2 = 1;
+                        in0 = 2; // yzx
+                    } else {
+                        // 1 - 0 - 2
+                        in1 = 0;
+                        in0 = 1;
+                        in2 = 2; // yxz
+                    }
+                } else {
+                    // 2 - 1 - 0
+                    in2 = 0;
+                    in1 = 1;
+                    in0 = 2; // zyx
+                }
+            }
+
+            index = out[in0];
+            outScale[0] = scales[index];
+
+            index = out[in1];
+            outScale[1] = scales[index];
+
+            index = out[in2];
+            outScale[2] = scales[index];
+
+            index = out[in0];
+            outRot[0] = rot[index];
+
+            index = out[in0] + 3;
+            outRot[0 + 3] = rot[index];
+
+            index = out[in0] + 6;
+            outRot[0 + 6] = rot[index];
+
+            index = out[in1];
+            outRot[1] = rot[index];
+
+            index = out[in1] + 3;
+            outRot[1 + 3] = rot[index];
+
+            index = out[in1] + 6;
+            outRot[1 + 6] = rot[index];
+
+            index = out[in2];
+            outRot[2] = rot[index];
+
+            index = out[in2] + 3;
+            outRot[2 + 3] = rot[index];
+
+            index = out[in2] + 6;
+            outRot[2 + 6] = rot[index];
+        }
+    }
+
+    static void transpose_mat(double[] in, double[] out) {
+        out[0] = in[0];
+        out[1] = in[3];
+        out[2] = in[6];
+
+        out[3] = in[1];
+        out[4] = in[4];
+        out[5] = in[7];
+
+        out[6] = in[2];
+        out[7] = in[5];
+        out[8] = in[8];
+    }
+
+    /**
+     * The first matrix element in the first row.
+     */
+    public double m00;
+
+    /**
+     * The second matrix element in the first row.
+     */
+    public double m01;
+
+    /**
+     * The third matrix element in the first row.
+     */
+    public double m02;
+
+    /**
+     * The first matrix element in the second row.
+     */
+    public double m10;
+
+    /**
+     * The second matrix element in the second row.
+     */
+    public double m11;
+
+    /**
+     * The third matrix element in the second row.
+     */
+    public double m12;
+
+    /**
+     * The first matrix element in the third row.
+     */
+    public double m20;
+
+    /**
+     * The second matrix element in the third row.
+     */
+    public double m21;
+
+    /**
+     * The third matrix element in the third row.
+     */
+    public double m22;
+
+    // double[] tmp = new double[9]; // scratch matrix
+    // double[] tmp_rot = new double[9]; // scratch matrix
+    // double[] tmp_scale = new double[3]; // scratch matrix
+    private static final double EPS = 1.110223024E-16;
+
+    private static final double ERR_EPS = 1.0E-8;
+
+    private static double xin, yin, zin, xout, yout, zout;
+
+    /**
+     * Constructs and initializes a Matrix3d to all zeros.
+     */
+    public Matrix3d() {
+        this.m00 = 0.0;
+        this.m01 = 0.0;
+        this.m02 = 0.0;
+
+        this.m10 = 0.0;
+        this.m11 = 0.0;
+        this.m12 = 0.0;
+
+        this.m20 = 0.0;
+        this.m21 = 0.0;
+        this.m22 = 0.0;
+
+    }
+
+    /**
+     * Constructs and initializes a Matrix3d from the specified nine values.
+     * 
+     * @param m00
+     *            the [0][0] element
+     * @param m01
+     *            the [0][1] element
+     * @param m02
+     *            the [0][2] element
+     * @param m10
+     *            the [1][0] element
+     * @param m11
+     *            the [1][1] element
+     * @param m12
+     *            the [1][2] element
+     * @param m20
+     *            the [2][0] element
+     * @param m21
+     *            the [2][1] element
+     * @param m22
+     *            the [2][2] element
+     */
+    public Matrix3d(double m00, double m01, double m02, double m10, double m11,
+            double m12, double m20, double m21, double m22) {
+        this.m00 = m00;
+        this.m01 = m01;
+        this.m02 = m02;
+
+        this.m10 = m10;
+        this.m11 = m11;
+        this.m12 = m12;
+
+        this.m20 = m20;
+        this.m21 = m21;
+        this.m22 = m22;
+
+    }
+
+    /**
+     * Constructs and initializes a Matrix3d from the specified nine- element
+     * array.
+     * 
+     * @param v
+     *            the array of length 9 containing in order
+     */
+    public Matrix3d(double[] v) {
+        this.m00 = v[0];
+        this.m01 = v[1];
+        this.m02 = v[2];
+
+        this.m10 = v[3];
+        this.m11 = v[4];
+        this.m12 = v[5];
+
+        this.m20 = v[6];
+        this.m21 = v[7];
+        this.m22 = v[8];
+
+    }
+
+    /**
+     * Constructs a new matrix with the same values as the Matrix3d parameter.
+     * 
+     * @param m1
+     *            the source matrix
+     */
+    public Matrix3d(Matrix3d m1) {
+        this.m00 = m1.m00;
+        this.m01 = m1.m01;
+        this.m02 = m1.m02;
+
+        this.m10 = m1.m10;
+        this.m11 = m1.m11;
+        this.m12 = m1.m12;
+
+        this.m20 = m1.m20;
+        this.m21 = m1.m21;
+        this.m22 = m1.m22;
+
+    }
+
+    /**
+     * Adds a scalar to each component of this matrix.
+     * 
+     * @param scalar
+     *            the scalar adder
+     */
+    public final void add(double scalar) {
+        m00 += scalar;
+        m01 += scalar;
+        m02 += scalar;
+
+        m10 += scalar;
+        m11 += scalar;
+        m12 += scalar;
+
+        m20 += scalar;
+        m21 += scalar;
+        m22 += scalar;
+
+    }
+
+    /**
+     * Adds a scalar to each component of the matrix m1 and places the result
+     * into this. Matrix m1 is not modified.
+     * 
+     * @param scalar
+     *            the scalar adder
+     * @param m1
+     *            the original matrix values
+     */
+    public final void add(double scalar, Matrix3d m1) {
+        this.m00 = m1.m00 + scalar;
+        this.m01 = m1.m01 + scalar;
+        this.m02 = m1.m02 + scalar;
+
+        this.m10 = m1.m10 + scalar;
+        this.m11 = m1.m11 + scalar;
+        this.m12 = m1.m12 + scalar;
+
+        this.m20 = m1.m20 + scalar;
+        this.m21 = m1.m21 + scalar;
+        this.m22 = m1.m22 + scalar;
+    }
+
+    /**
+     * Sets the value of this matrix to the sum of itself and matrix m1.
+     * 
+     * @param m1
+     *            the other matrix
+     */
+    public final void add(Matrix3d m1) {
+        this.m00 += m1.m00;
+        this.m01 += m1.m01;
+        this.m02 += m1.m02;
+
+        this.m10 += m1.m10;
+        this.m11 += m1.m11;
+        this.m12 += m1.m12;
+
+        this.m20 += m1.m20;
+        this.m21 += m1.m21;
+        this.m22 += m1.m22;
+    }
+
+    /**
+     * Sets the value of this matrix to the matrix sum of matrices m1 and m2.
+     * 
+     * @param m1
+     *            the first matrix
+     * @param m2
+     *            the second matrix
+     */
+    public final void add(Matrix3d m1, Matrix3d m2) {
+        this.m00 = m1.m00 + m2.m00;
+        this.m01 = m1.m01 + m2.m01;
+        this.m02 = m1.m02 + m2.m02;
+
+        this.m10 = m1.m10 + m2.m10;
+        this.m11 = m1.m11 + m2.m11;
+        this.m12 = m1.m12 + m2.m12;
+
+        this.m20 = m1.m20 + m2.m20;
+        this.m21 = m1.m21 + m2.m21;
+        this.m22 = m1.m22 + m2.m22;
+    }
+
+    /**
+     * Creates a new object of the same class as this object.
+     * 
+     * @return a clone of this instance.
+     * @exception OutOfMemoryError
+     *                if there is not enough memory.
+     * @see java.lang.Cloneable
+     * @since vecmath 1.3
+     */
+//    public Object clone() {
+//        Matrix3d m1 = null;
+//        try {
+//            m1 = (Matrix3d) super.clone();
+//        } catch (CloneNotSupportedException e) {
+//            // this shouldn't happen, since we are Cloneable
+//            throw new InternalError();
+//        }
+
+        // Also need to create new tmp arrays (no need to actually clone them)
+//       return m1;
+//   }
+
+    /**
+     * Computes the determinant of this matrix.
+     * 
+     * @return the determinant of the matrix
+     */
+    public final double determinant() {
+        double total;
+
+        total = this.m00 * (this.m11 * this.m22 - this.m12 * this.m21)
+                + this.m01 * (this.m12 * this.m20 - this.m10 * this.m22)
+                + this.m02 * (this.m10 * this.m21 - this.m11 * this.m20);
+        return total;
+    }
+
+    /**
+     * Returns true if the L-infinite distance between this matrix and matrix m1
+     * is less than or equal to the epsilon parameter, otherwise returns false.
+     * The L-infinite distance is equal to MAX[i=0,1,2 ; j=0,1,2 ;
+     * abs(this.m(i,j) - m1.m(i,j)]
+     * 
+     * @param m1
+     *            the matrix to be compared to this matrix
+     * @param epsilon
+     *            the threshold value
+     * @return 
+     */
+    public boolean epsilonEquals(Matrix3d m1, double epsilon) {
+        double diff;
+
+        diff = m00 - m1.m00;
+        if ((diff < 0 ? -diff : diff) > epsilon) {
+            return false;
+        }
+
+        diff = m01 - m1.m01;
+        if ((diff < 0 ? -diff : diff) > epsilon) {
+            return false;
+        }
+
+        diff = m02 - m1.m02;
+        if ((diff < 0 ? -diff : diff) > epsilon) {
+            return false;
+        }
+
+        diff = m10 - m1.m10;
+        if ((diff < 0 ? -diff : diff) > epsilon) {
+            return false;
+        }
+
+        diff = m11 - m1.m11;
+        if ((diff < 0 ? -diff : diff) > epsilon) {
+            return false;
+        }
+
+        diff = m12 - m1.m12;
+        if ((diff < 0 ? -diff : diff) > epsilon) {
+            return false;
+        }
+
+        diff = m20 - m1.m20;
+        if ((diff < 0 ? -diff : diff) > epsilon) {
+            return false;
+        }
+
+        diff = m21 - m1.m21;
+        if ((diff < 0 ? -diff : diff) > epsilon) {
+            return false;
+        }
+
+        diff = m22 - m1.m22;
+        return (diff < 0 ? -diff : diff) <= epsilon;
+    }
+
+    /**
+     * Returns true if all of the data members of Matrix3d m1 are equal to the
+     * corresponding data members in this Matrix3d.
+     * 
+     * @param m1
+     *            the matrix with which the comparison is made
+     * @return true or false
+     */
+    public boolean equals(Matrix3d m1) {
+        try {
+            return (this.m00 == m1.m00 && this.m01 == m1.m01
+                    && this.m02 == m1.m02 && this.m10 == m1.m10
+                    && this.m11 == m1.m11 && this.m12 == m1.m12
+                    && this.m20 == m1.m20 && this.m21 == m1.m21 && this.m22 == m1.m22);
+        } catch (NullPointerException e2) {
+            return false;
+        }
+
+    }
+
+    /**
+     * Returns true if the Object t1 is of type Matrix3d and all of the data
+     * members of t1 are equal to the corresponding data members in this
+     * Matrix3d.
+     * 
+     * @param t1
+     *            the matrix with which the comparison is made
+     * @return true or false
+     */
+    @Override
+    public boolean equals(Object t1) {
+        if (t1 instanceof Matrix3d) {
+            Matrix3d m2 = (Matrix3d) t1;
+            return (this.m00 == m2.m00 && this.m01 == m2.m01
+                    && this.m02 == m2.m02 && this.m10 == m2.m10
+                    && this.m11 == m2.m11 && this.m12 == m2.m12
+                    && this.m20 == m2.m20 && this.m21 == m2.m21 && this.m22 == m2.m22);
+        }
+        return false;
+    }
+
+    /**
+     * Retrieves the value at the specified row and column of the specified
+     * matrix.
+     * 
+     * @param row
+     *            the row number to be retrieved (zero indexed)
+     * @param column
+     *            the column number to be retrieved (zero indexed)
+     * @return the value at the indexed element.
+     */
+    public final double get(int row, int column) {
+        switch (row) {
+            case 0:
+                switch (column) {
+                    case 0:
+                        return (this.m00);
+                    case 1:
+                        return (this.m01);
+                    case 2:
+                        return (this.m02);
+                    default:
+                        break;
+                }
+                break;
+            case 1:
+                switch (column) {
+                    case 0:
+                        return (this.m10);
+                    case 1:
+                        return (this.m11);
+                    case 2:
+                        return (this.m12);
+                    default:
+                        break;
+                }
+                break;
+
+            case 2:
+                switch (column) {
+                    case 0:
+                        return (this.m20);
+                    case 1:
+                        return (this.m21);
+                    case 2:
+                        return (this.m22);
+                    default:
+                        break;
+                }
+                break;
+
+            default:
+                break;
+        }
+        throw new ArrayIndexOutOfBoundsException();
+    }
+
+    /**
+     * Copies the matrix values in the specified column into the array
+     * parameter.
+     * 
+     * @param column
+     *            the matrix column
+     * @param v
+     *            the array into which the matrix row values will be copied
+     */
+    public final void getColumn(int column, double v[]) {
+        if (column == 0) {
+            v[0] = m00;
+            v[1] = m10;
+            v[2] = m20;
+        } else if (column == 1) {
+            v[0] = m01;
+            v[1] = m11;
+            v[2] = m21;
+        } else if (column == 2) {
+            v[0] = m02;
+            v[1] = m12;
+            v[2] = m22;
+        } else {
+            throw new ArrayIndexOutOfBoundsException();
+        }
+
+    }
+
+    /**
+     * Copies the matrix values in the specified column into the vector
+     * parameter.
+     * 
+     * @param column
+     *            the matrix column
+     * @param v
+     *            the vector into which the matrix row values will be copied
+     */
+    public final void getColumn(int column, Vec3D v) {
+        if (column == 0) {
+            v.x = (float) m00;
+            v.y = (float) m10;
+            v.z = (float) m20;
+        } else if (column == 1) {
+            v.x = (float) m01;
+            v.y = (float) m11;
+            v.z = (float) m21;
+        } else if (column == 2) {
+            v.x = (float) m02;
+            v.y = (float) m12;
+            v.z = (float) m22;
+        } else {
+            throw new ArrayIndexOutOfBoundsException();
+        }
+
+    }
+
+    /**
+     * Copies the matrix values in the specified row into the array parameter.
+     * 
+     * @param row
+     *            the matrix row
+     * @param v
+     *            the array into which the matrix row values will be copied
+     */
+    public final void getRow(int row, double v[]) {
+        if (row == 0) {
+            v[0] = m00;
+            v[1] = m01;
+            v[2] = m02;
+        } else if (row == 1) {
+            v[0] = m10;
+            v[1] = m11;
+            v[2] = m12;
+        } else if (row == 2) {
+            v[0] = m20;
+            v[1] = m21;
+            v[2] = m22;
+        } else {
+            throw new ArrayIndexOutOfBoundsException();
+        }
+    }
+
+    /**
+     * Copies the matrix values in the specified row into the vector parameter.
+     * 
+     * @param row
+     *            the matrix row
+     * @param v
+     *            the vector into which the matrix row values will be copied
+     */
+    public final void getRow(int row, Vec3D v) {
+        if (row == 0) {
+            v.x = (float) m00;
+            v.y = (float) m01;
+            v.z = (float) m02;
+        } else if (row == 1) {
+            v.x = (float) m10;
+            v.y = (float) m11;
+            v.z = (float) m12;
+        } else if (row == 2) {
+            v.x = (float) m20;
+            v.y = (float) m21;
+            v.z = (float) m22;
+        } else {
+            throw new ArrayIndexOutOfBoundsException();
+        }
+
+    }
+
+    /**
+     * Performs an SVD normalization of this matrix to calculate and return the
+     * uniform scale factor. If the matrix has non-uniform scale factors, the
+     * largest of the x, y, and z scale factors will be returned. This matrix is
+     * not modified.
+     * 
+     * @return the scale factor of this matrix
+     */
+    public final double getScale() {
+        double[] tmp_scale = new double[3];
+        double[] tmp_rot = new double[9];
+        getScaleRotate(tmp_scale, tmp_rot);
+        return (MathUtils.max(tmp_scale));
+    }
+
+    /**
+     * perform SVD (if necessary to get rotational component
+     */
+    final void getScaleRotate(double scales[], double rots[]) {
+        double[] tmp = new double[9];
+        tmp[0] = m00;
+        tmp[1] = m01;
+        tmp[2] = m02;
+
+        tmp[3] = m10;
+        tmp[4] = m11;
+        tmp[5] = m12;
+
+        tmp[6] = m20;
+        tmp[7] = m21;
+        tmp[8] = m22;
+        compute_svd(tmp, scales, rots);
+    }
+
+    /**
+     * Returns a hash code value based on the data values in this object. Two
+     * different Matrix3d objects with identical data values (i.e.,
+     * Matrix3d.equals returns true) will return the same hash code value. Two
+     * objects with different data members may return the same hash value,
+     * although this is not likely.
+     * 
+     * @return the integer hash code value
+     */
+    @Override
+    public int hashCode() {
+        long bits = 1L;
+        bits = 31L * bits + VecMathUtil.doubleToLongBits(m00);
+        bits = 31L * bits + VecMathUtil.doubleToLongBits(m01);
+        bits = 31L * bits + VecMathUtil.doubleToLongBits(m02);
+        bits = 31L * bits + VecMathUtil.doubleToLongBits(m10);
+        bits = 31L * bits + VecMathUtil.doubleToLongBits(m11);
+        bits = 31L * bits + VecMathUtil.doubleToLongBits(m12);
+        bits = 31L * bits + VecMathUtil.doubleToLongBits(m20);
+        bits = 31L * bits + VecMathUtil.doubleToLongBits(m21);
+        bits = 31L * bits + VecMathUtil.doubleToLongBits(m22);
+        return (int) (bits ^ (bits >> 32));
+    }
+
+    /**
+     * Inverts this matrix in place.
+     */
+    public final void invert() {
+        invertGeneral(this);
+    }
+
+    /**
+     * Sets the value of this matrix to the matrix inverse of the passed matrix
+     * m1.
+     * 
+     * @param m1
+     *            the matrix to be inverted
+     */
+    public final void invert(Matrix3d m1) {
+        invertGeneral(m1);
+    }
+
+    /**
+     * General invert routine. Inverts m1 and places the result in "this". Note
+     * that this routine handles both the "this" version and the non-"this"
+     * version.
+     * 
+     * Also note that since this routine is slow anyway, we won't worry about
+     * allocating a little bit of garbage.
+     */
+    private void invertGeneral(Matrix3d m1) {
+        double result[] = new double[9];
+        int row_perm[] = new int[3];
+        int i, r, c;
+        double[] tmp = new double[9]; // scratch matrix
+
+        // Use LU decomposition and backsubstitution code specifically
+        // for floating-point 3x3 matrices.
+
+        // Copy source matrix to t1tmp
+        tmp[0] = m1.m00;
+        tmp[1] = m1.m01;
+        tmp[2] = m1.m02;
+
+        tmp[3] = m1.m10;
+        tmp[4] = m1.m11;
+        tmp[5] = m1.m12;
+
+        tmp[6] = m1.m20;
+        tmp[7] = m1.m21;
+        tmp[8] = m1.m22;
+
+        // Calculate LU decomposition: Is the matrix singular?
+        if (!Matrix4x4.LUDecomposition(tmp, row_perm, 3)) {
+            // Matrix has no inverse
+            throw new SingularMatrixException();
+        }
+
+        // Perform back substitution on the identity matrix
+        for (i = 0; i < 9; i++) {
+            result[i] = 0.0;
+        }
+        result[0] = 1.0;
+        result[4] = 1.0;
+        result[8] = 1.0;
+        luBacksubstitution(tmp, row_perm, result);
+
+        this.m00 = result[0];
+        this.m01 = result[1];
+        this.m02 = result[2];
+
+        this.m10 = result[3];
+        this.m11 = result[4];
+        this.m12 = result[5];
+
+        this.m20 = result[6];
+        this.m21 = result[7];
+        this.m22 = result[8];
+
+    }
+
+    /**
+     * Multiplies each element of this matrix by a scalar.
+     * 
+     * @param scalar
+     *            The scalar multiplier.
+     */
+    public final void mul(double scalar) {
+        m00 *= scalar;
+        m01 *= scalar;
+        m02 *= scalar;
+
+        m10 *= scalar;
+        m11 *= scalar;
+        m12 *= scalar;
+
+        m20 *= scalar;
+        m21 *= scalar;
+        m22 *= scalar;
+
+    }
+
+    /**
+     * Multiplies each element of matrix m1 by a scalar and places the result
+     * into this. Matrix m1 is not modified.
+     * 
+     * @param scalar
+     *            the scalar multiplier
+     * @param m1
+     *            the original matrix
+     */
+    public final void mul(double scalar, Matrix3d m1) {
+        this.m00 = scalar * m1.m00;
+        this.m01 = scalar * m1.m01;
+        this.m02 = scalar * m1.m02;
+
+        this.m10 = scalar * m1.m10;
+        this.m11 = scalar * m1.m11;
+        this.m12 = scalar * m1.m12;
+
+        this.m20 = scalar * m1.m20;
+        this.m21 = scalar * m1.m21;
+        this.m22 = scalar * m1.m22;
+
+    }
+
+    /**
+     * Sets the value of this matrix to the result of multiplying itself with
+     * matrix m1.
+     * 
+     * @param m1
+     *            the other matrix
+     */
+    public final void mul(Matrix3d m1) {
+        double tm00, tm01, tm02, tm10, tm11, tm12, tm20, tm21, tm22;
+
+        tm00 = this.m00 * m1.m00 + this.m01 * m1.m10 + this.m02 * m1.m20;
+        tm01 = this.m00 * m1.m01 + this.m01 * m1.m11 + this.m02 * m1.m21;
+        tm02 = this.m00 * m1.m02 + this.m01 * m1.m12 + this.m02 * m1.m22;
+
+        tm10 = this.m10 * m1.m00 + this.m11 * m1.m10 + this.m12 * m1.m20;
+        tm11 = this.m10 * m1.m01 + this.m11 * m1.m11 + this.m12 * m1.m21;
+        tm12 = this.m10 * m1.m02 + this.m11 * m1.m12 + this.m12 * m1.m22;
+
+        tm20 = this.m20 * m1.m00 + this.m21 * m1.m10 + this.m22 * m1.m20;
+        tm21 = this.m20 * m1.m01 + this.m21 * m1.m11 + this.m22 * m1.m21;
+        tm22 = this.m20 * m1.m02 + this.m21 * m1.m12 + this.m22 * m1.m22;
+
+        this.m00 = tm00;
+        this.m01 = tm01;
+        this.m02 = tm02;
+        this.m10 = tm10;
+        this.m11 = tm11;
+        this.m12 = tm12;
+        this.m20 = tm20;
+        this.m21 = tm21;
+        this.m22 = tm22;
+    }
+
+    /**
+     * Sets the value of this matrix to the result of multiplying the two
+     * argument matrices together.
+     * 
+     * @param m1
+     *            the first matrix
+     * @param m2
+     *            the second matrix
+     */
+    public final void mul(Matrix3d m1, Matrix3d m2) {
+        if (this != m1 && this != m2) {
+            this.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20;
+            this.m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21;
+            this.m02 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22;
+
+            this.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20;
+            this.m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21;
+            this.m12 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22;
+
+            this.m20 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20;
+            this.m21 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21;
+            this.m22 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22;
+        } else {
+            double tm00, tm01, tm02, tm10, tm11, tm12, tm20, tm21, tm22; // vars for temp
+                                                                // result matrix
+
+            tm00 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20;
+            tm01 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21;
+            tm02 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22;
+
+            tm10 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20;
+            tm11 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21;
+            tm12 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22;
+
+            tm20 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20;
+            tm21 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21;
+            tm22 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22;
+
+            this.m00 = tm00;
+            this.m01 = tm01;
+            this.m02 = tm02;
+            this.m10 = tm10;
+            this.m11 = tm11;
+            this.m12 = tm12;
+            this.m20 = tm20;
+            this.m21 = tm21;
+            this.m22 = tm22;
+        }
+    }
+
+    /**
+     * Multiplies this matrix by matrix m1, does an SVD normalization of the
+     * result, and places the result back into this matrix this =
+     * SVDnorm(this*m1).
+     * 
+     * @param m1
+     *            the matrix on the right hand side of the multiplication
+     */
+    public final void mulNormalize(Matrix3d m1) {
+
+        double[] tmp = new double[9]; // scratch matrix
+        double[] tmp_rot = new double[9]; // scratch matrix
+        double[] tmp_scale = new double[3]; // scratch matrix
+
+        tmp[0] = this.m00 * m1.m00 + this.m01 * m1.m10 + this.m02 * m1.m20;
+        tmp[1] = this.m00 * m1.m01 + this.m01 * m1.m11 + this.m02 * m1.m21;
+        tmp[2] = this.m00 * m1.m02 + this.m01 * m1.m12 + this.m02 * m1.m22;
+
+        tmp[3] = this.m10 * m1.m00 + this.m11 * m1.m10 + this.m12 * m1.m20;
+        tmp[4] = this.m10 * m1.m01 + this.m11 * m1.m11 + this.m12 * m1.m21;
+        tmp[5] = this.m10 * m1.m02 + this.m11 * m1.m12 + this.m12 * m1.m22;
+
+        tmp[6] = this.m20 * m1.m00 + this.m21 * m1.m10 + this.m22 * m1.m20;
+        tmp[7] = this.m20 * m1.m01 + this.m21 * m1.m11 + this.m22 * m1.m21;
+        tmp[8] = this.m20 * m1.m02 + this.m21 * m1.m12 + this.m22 * m1.m22;
+
+        compute_svd(tmp, tmp_scale, tmp_rot);
+
+        this.m00 = tmp_rot[0];
+        this.m01 = tmp_rot[1];
+        this.m02 = tmp_rot[2];
+
+        this.m10 = tmp_rot[3];
+        this.m11 = tmp_rot[4];
+        this.m12 = tmp_rot[5];
+
+        this.m20 = tmp_rot[6];
+        this.m21 = tmp_rot[7];
+        this.m22 = tmp_rot[8];
+
+    }
+
+    /**
+     * Multiplies matrix m1 by matrix m2, does an SVD normalization of the
+     * result, and places the result into this matrix this = SVDnorm(m1*m2).
+     * 
+     * @param m1
+     *            the matrix on the left hand side of the multiplication
+     * @param m2
+     *            the matrix on the right hand side of the multiplication
+     */
+    public final void mulNormalize(Matrix3d m1, Matrix3d m2) {
+
+        double[] tmp = new double[9]; // scratch matrix
+        double[] tmp_rot = new double[9]; // scratch matrix
+        double[] tmp_scale = new double[3]; // scratch matrix
+
+        tmp[0] = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20;
+        tmp[1] = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21;
+        tmp[2] = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22;
+
+        tmp[3] = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20;
+        tmp[4] = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21;
+        tmp[5] = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22;
+
+        tmp[6] = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20;
+        tmp[7] = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21;
+        tmp[8] = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22;
+
+        compute_svd(tmp, tmp_scale, tmp_rot);
+
+        this.m00 = tmp_rot[0];
+        this.m01 = tmp_rot[1];
+        this.m02 = tmp_rot[2];
+
+        this.m10 = tmp_rot[3];
+        this.m11 = tmp_rot[4];
+        this.m12 = tmp_rot[5];
+
+        this.m20 = tmp_rot[6];
+        this.m21 = tmp_rot[7];
+        this.m22 = tmp_rot[8];
+
+    }
+
+    /**
+     * Multiplies the transpose of matrix m1 times the transpose of matrix m2,
+     * and places the result into this.
+     * 
+     * @param m1
+     *            the matrix on the left hand side of the multiplication
+     * @param m2
+     *            the matrix on the right hand side of the multiplication
+     */
+    public final void mulTransposeBoth(Matrix3d m1, Matrix3d m2) {
+        if (this != m1 && this != m2) {
+            this.m00 = m1.m00 * m2.m00 + m1.m10 * m2.m01 + m1.m20 * m2.m02;
+            this.m01 = m1.m00 * m2.m10 + m1.m10 * m2.m11 + m1.m20 * m2.m12;
+            this.m02 = m1.m00 * m2.m20 + m1.m10 * m2.m21 + m1.m20 * m2.m22;
+
+            this.m10 = m1.m01 * m2.m00 + m1.m11 * m2.m01 + m1.m21 * m2.m02;
+            this.m11 = m1.m01 * m2.m10 + m1.m11 * m2.m11 + m1.m21 * m2.m12;
+            this.m12 = m1.m01 * m2.m20 + m1.m11 * m2.m21 + m1.m21 * m2.m22;
+
+            this.m20 = m1.m02 * m2.m00 + m1.m12 * m2.m01 + m1.m22 * m2.m02;
+            this.m21 = m1.m02 * m2.m10 + m1.m12 * m2.m11 + m1.m22 * m2.m12;
+            this.m22 = m1.m02 * m2.m20 + m1.m12 * m2.m21 + m1.m22 * m2.m22;
+        } else {
+            double tm00, tm01, tm02, tm10, tm11, tm12, tm20, tm21, tm22; // vars for temp
+                                                                // result matrix
+
+            tm00 = m1.m00 * m2.m00 + m1.m10 * m2.m01 + m1.m20 * m2.m02;
+            tm01 = m1.m00 * m2.m10 + m1.m10 * m2.m11 + m1.m20 * m2.m12;
+            tm02 = m1.m00 * m2.m20 + m1.m10 * m2.m21 + m1.m20 * m2.m22;
+
+            tm10 = m1.m01 * m2.m00 + m1.m11 * m2.m01 + m1.m21 * m2.m02;
+            tm11 = m1.m01 * m2.m10 + m1.m11 * m2.m11 + m1.m21 * m2.m12;
+            tm12 = m1.m01 * m2.m20 + m1.m11 * m2.m21 + m1.m21 * m2.m22;
+
+            tm20 = m1.m02 * m2.m00 + m1.m12 * m2.m01 + m1.m22 * m2.m02;
+            tm21 = m1.m02 * m2.m10 + m1.m12 * m2.m11 + m1.m22 * m2.m12;
+            tm22 = m1.m02 * m2.m20 + m1.m12 * m2.m21 + m1.m22 * m2.m22;
+
+            this.m00 = tm00;
+            this.m01 = tm01;
+            this.m02 = tm02;
+            this.m10 = tm10;
+            this.m11 = tm11;
+            this.m12 = tm12;
+            this.m20 = tm20;
+            this.m21 = tm21;
+            this.m22 = tm22;
+        }
+
+    }
+
+    /**
+     * Multiplies the transpose of matrix m1 times matrix m2, and places the
+     * result into this.
+     * 
+     * @param m1
+     *            the matrix on the left hand side of the multiplication
+     * @param m2
+     *            the matrix on the right hand side of the multiplication
+     */
+    public final void mulTransposeLeft(Matrix3d m1, Matrix3d m2) {
+        if (this != m1 && this != m2) {
+            this.m00 = m1.m00 * m2.m00 + m1.m10 * m2.m10 + m1.m20 * m2.m20;
+            this.m01 = m1.m00 * m2.m01 + m1.m10 * m2.m11 + m1.m20 * m2.m21;
+            this.m02 = m1.m00 * m2.m02 + m1.m10 * m2.m12 + m1.m20 * m2.m22;
+
+            this.m10 = m1.m01 * m2.m00 + m1.m11 * m2.m10 + m1.m21 * m2.m20;
+            this.m11 = m1.m01 * m2.m01 + m1.m11 * m2.m11 + m1.m21 * m2.m21;
+            this.m12 = m1.m01 * m2.m02 + m1.m11 * m2.m12 + m1.m21 * m2.m22;
+
+            this.m20 = m1.m02 * m2.m00 + m1.m12 * m2.m10 + m1.m22 * m2.m20;
+            this.m21 = m1.m02 * m2.m01 + m1.m12 * m2.m11 + m1.m22 * m2.m21;
+            this.m22 = m1.m02 * m2.m02 + m1.m12 * m2.m12 + m1.m22 * m2.m22;
+        } else {
+            double tm00, tm01, tm02, tm10, tm11, tm12, tm20, tm21, tm22; // vars for temp
+                                                                // result matrix
+
+            tm00 = m1.m00 * m2.m00 + m1.m10 * m2.m10 + m1.m20 * m2.m20;
+            tm01 = m1.m00 * m2.m01 + m1.m10 * m2.m11 + m1.m20 * m2.m21;
+            tm02 = m1.m00 * m2.m02 + m1.m10 * m2.m12 + m1.m20 * m2.m22;
+
+            tm10 = m1.m01 * m2.m00 + m1.m11 * m2.m10 + m1.m21 * m2.m20;
+            tm11 = m1.m01 * m2.m01 + m1.m11 * m2.m11 + m1.m21 * m2.m21;
+            tm12 = m1.m01 * m2.m02 + m1.m11 * m2.m12 + m1.m21 * m2.m22;
+
+            tm20 = m1.m02 * m2.m00 + m1.m12 * m2.m10 + m1.m22 * m2.m20;
+            tm21 = m1.m02 * m2.m01 + m1.m12 * m2.m11 + m1.m22 * m2.m21;
+            tm22 = m1.m02 * m2.m02 + m1.m12 * m2.m12 + m1.m22 * m2.m22;
+
+            this.m00 = tm00;
+            this.m01 = tm01;
+            this.m02 = tm02;
+            this.m10 = tm10;
+            this.m11 = tm11;
+            this.m12 = tm12;
+            this.m20 = tm20;
+            this.m21 = tm21;
+            this.m22 = tm22;
+        }
+    }
+
+    /**
+     * Multiplies matrix m1 times the transpose of matrix m2, and places the
+     * result into this.
+     * 
+     * @param m1
+     *            the matrix on the left hand side of the multiplication
+     * @param m2
+     *            the matrix on the right hand side of the multiplication
+     */
+    public final void mulTransposeRight(Matrix3d m1, Matrix3d m2) {
+        if (this != m1 && this != m2) {
+            this.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m01 + m1.m02 * m2.m02;
+            this.m01 = m1.m00 * m2.m10 + m1.m01 * m2.m11 + m1.m02 * m2.m12;
+            this.m02 = m1.m00 * m2.m20 + m1.m01 * m2.m21 + m1.m02 * m2.m22;
+
+            this.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m01 + m1.m12 * m2.m02;
+            this.m11 = m1.m10 * m2.m10 + m1.m11 * m2.m11 + m1.m12 * m2.m12;
+            this.m12 = m1.m10 * m2.m20 + m1.m11 * m2.m21 + m1.m12 * m2.m22;
+
+            this.m20 = m1.m20 * m2.m00 + m1.m21 * m2.m01 + m1.m22 * m2.m02;
+            this.m21 = m1.m20 * m2.m10 + m1.m21 * m2.m11 + m1.m22 * m2.m12;
+            this.m22 = m1.m20 * m2.m20 + m1.m21 * m2.m21 + m1.m22 * m2.m22;
+        } else {
+            double tm00, tm01, tm02, tm10, tm11, tm12, tm20, tm21, tm22; // vars for temp
+                                                                // result matrix
+
+            tm00 = m1.m00 * m2.m00 + m1.m01 * m2.m01 + m1.m02 * m2.m02;
+            tm01 = m1.m00 * m2.m10 + m1.m01 * m2.m11 + m1.m02 * m2.m12;
+            tm02 = m1.m00 * m2.m20 + m1.m01 * m2.m21 + m1.m02 * m2.m22;
+
+            tm10 = m1.m10 * m2.m00 + m1.m11 * m2.m01 + m1.m12 * m2.m02;
+            tm11 = m1.m10 * m2.m10 + m1.m11 * m2.m11 + m1.m12 * m2.m12;
+            tm12 = m1.m10 * m2.m20 + m1.m11 * m2.m21 + m1.m12 * m2.m22;
+
+            tm20 = m1.m20 * m2.m00 + m1.m21 * m2.m01 + m1.m22 * m2.m02;
+            tm21 = m1.m20 * m2.m10 + m1.m21 * m2.m11 + m1.m22 * m2.m12;
+            tm22 = m1.m20 * m2.m20 + m1.m21 * m2.m21 + m1.m22 * m2.m22;
+
+            this.m00 = tm00;
+            this.m01 = tm01;
+            this.m02 = tm02;
+            this.m10 = tm10;
+            this.m11 = tm11;
+            this.m12 = tm12;
+            this.m20 = tm20;
+            this.m21 = tm21;
+            this.m22 = tm22;
+        }
+    }
+
+    /**
+     * Negates the value of this matrix: this = -this.
+     */
+    public final void negate() {
+        this.m00 = -this.m00;
+        this.m01 = -this.m01;
+        this.m02 = -this.m02;
+
+        this.m10 = -this.m10;
+        this.m11 = -this.m11;
+        this.m12 = -this.m12;
+
+        this.m20 = -this.m20;
+        this.m21 = -this.m21;
+        this.m22 = -this.m22;
+
+    }
+
+    /**
+     * Sets the value of this matrix equal to the negation of of the Matrix3d
+     * parameter.
+     * 
+     * @param m1
+     *            the source matrix
+     */
+    public final void negate(Matrix3d m1) {
+        this.m00 = -m1.m00;
+        this.m01 = -m1.m01;
+        this.m02 = -m1.m02;
+
+        this.m10 = -m1.m10;
+        this.m11 = -m1.m11;
+        this.m12 = -m1.m12;
+
+        this.m20 = -m1.m20;
+        this.m21 = -m1.m21;
+        this.m22 = -m1.m22;
+
+    }
+
+    /**
+     * Performs singular value decomposition normalization of this matrix.
+     */
+    public final void normalize() {
+        double[] tmp_rot = new double[9]; // scratch matrix
+        double[] tmp_scale = new double[3]; // scratch matrix
+
+        getScaleRotate(tmp_scale, tmp_rot);
+
+        this.m00 = tmp_rot[0];
+        this.m01 = tmp_rot[1];
+        this.m02 = tmp_rot[2];
+
+        this.m10 = tmp_rot[3];
+        this.m11 = tmp_rot[4];
+        this.m12 = tmp_rot[5];
+
+        this.m20 = tmp_rot[6];
+        this.m21 = tmp_rot[7];
+        this.m22 = tmp_rot[8];
+
+    }
+
+    /**
+     * Perform singular value decomposition normalization of matrix m1 and place
+     * the normalized values into this.
+     * 
+     * @param m1
+     *            Provides the matrix values to be normalized
+     */
+    public final void normalize(Matrix3d m1) {
+
+        double[] tmp = new double[9]; // scratch matrix
+        double[] tmp_rot = new double[9]; // scratch matrix
+        double[] tmp_scale = new double[3]; // scratch matrix
+
+        tmp[0] = m1.m00;
+        tmp[1] = m1.m01;
+        tmp[2] = m1.m02;
+
+        tmp[3] = m1.m10;
+        tmp[4] = m1.m11;
+        tmp[5] = m1.m12;
+
+        tmp[6] = m1.m20;
+        tmp[7] = m1.m21;
+        tmp[8] = m1.m22;
+
+        compute_svd(tmp, tmp_scale, tmp_rot);
+
+        this.m00 = tmp_rot[0];
+        this.m01 = tmp_rot[1];
+        this.m02 = tmp_rot[2];
+
+        this.m10 = tmp_rot[3];
+        this.m11 = tmp_rot[4];
+        this.m12 = tmp_rot[5];
+
+        this.m20 = tmp_rot[6];
+        this.m21 = tmp_rot[7];
+        this.m22 = tmp_rot[8];
+    }
+
+    /**
+     * Perform cross product normalization of this matrix.
+     */
+
+    public final void normalizeCP() {
+        double mag = 1.0 / Math.sqrt(m00 * m00 + m10 * m10 + m20 * m20);
+        m00 = m00 * mag;
+        m10 = m10 * mag;
+        m20 = m20 * mag;
+
+        mag = 1.0 / Math.sqrt(m01 * m01 + m11 * m11 + m21 * m21);
+        m01 = m01 * mag;
+        m11 = m11 * mag;
+        m21 = m21 * mag;
+
+        m02 = m10 * m21 - m11 * m20;
+        m12 = m01 * m20 - m00 * m21;
+        m22 = m00 * m11 - m01 * m10;
+    }
+
+    /**
+     * Perform cross product normalization of matrix m1 and place the normalized
+     * values into this.
+     * 
+     * @param m1
+     *            Provides the matrix values to be normalized
+     */
+    public final void normalizeCP(Matrix3d m1) {
+        double mag = 1.0 / Math.sqrt(m1.m00 * m1.m00 + m1.m10 * m1.m10 + m1.m20
+                * m1.m20);
+        m00 = m1.m00 * mag;
+        m10 = m1.m10 * mag;
+        m20 = m1.m20 * mag;
+
+        mag = 1.0 / Math.sqrt(m1.m01 * m1.m01 + m1.m11 * m1.m11 + m1.m21
+                * m1.m21);
+        m01 = m1.m01 * mag;
+        m11 = m1.m11 * mag;
+        m21 = m1.m21 * mag;
+
+        m02 = m10 * m21 - m11 * m20;
+        m12 = m01 * m20 - m00 * m21;
+        m22 = m00 * m11 - m01 * m10;
+    }
+
+    /**
+     * Sets the value of this matrix to a counter clockwise rotation about the x
+     * axis.
+     * 
+     * @param angle
+     *            the angle to rotate about the X axis in radians
+     */
+    public final void rotX(double angle) {
+        double sinAngle, cosAngle;
+
+        sinAngle = Math.sin(angle);
+        cosAngle = Math.cos(angle);
+
+        this.m00 = 1.0;
+        this.m01 = 0.0;
+        this.m02 = 0.0;
+
+        this.m10 = 0.0;
+        this.m11 = cosAngle;
+        this.m12 = -sinAngle;
+
+        this.m20 = 0.0;
+        this.m21 = sinAngle;
+        this.m22 = cosAngle;
+    }
+
+    /**
+     * Sets the value of this matrix to a counter clockwise rotation about the y
+     * axis.
+     * 
+     * @param angle
+     *            the angle to rotate about the Y axis in radians
+     */
+    public final void rotY(double angle) {
+        double sinAngle, cosAngle;
+
+        sinAngle = Math.sin(angle);
+        cosAngle = Math.cos(angle);
+
+        this.m00 = cosAngle;
+        this.m01 = 0.0;
+        this.m02 = sinAngle;
+
+        this.m10 = 0.0;
+        this.m11 = 1.0;
+        this.m12 = 0.0;
+
+        this.m20 = -sinAngle;
+        this.m21 = 0.0;
+        this.m22 = cosAngle;
+    }
+
+    /**
+     * Sets the value of this matrix to a counter clockwise rotation about the z
+     * axis.
+     * 
+     * @param angle
+     *            the angle to rotate about the Z axis in radians
+     */
+    public final void rotZ(double angle) {
+        double sinAngle, cosAngle;
+
+        sinAngle = Math.sin(angle);
+        cosAngle = Math.cos(angle);
+
+        this.m00 = cosAngle;
+        this.m01 = -sinAngle;
+        this.m02 = 0.0;
+
+        this.m10 = sinAngle;
+        this.m11 = cosAngle;
+        this.m12 = 0.0;
+
+        this.m20 = 0.0;
+        this.m21 = 0.0;
+        this.m22 = 1.0;
+    }
+
+    /**
+     * Sets the value of this matrix to a scale matrix with the passed scale
+     * amount.
+     * 
+     * @param scale
+     *            the scale factor for the matrix
+     */
+    public final void set(double scale) {
+        this.m00 = scale;
+        this.m01 = 0.0;
+        this.m02 = 0.0;
+
+        this.m10 = 0.0;
+        this.m11 = scale;
+        this.m12 = 0.0;
+
+        this.m20 = 0.0;
+        this.m21 = 0.0;
+        this.m22 = scale;
+    }
+
+    /**
+     * Sets the values in this Matrix3d equal to the row-major array parameter
+     * (ie, the first three elements of the array will be copied into the first
+     * row of this matrix, etc.).
+     * 
+     * @param m
+     *            the double precision array of length 9
+     */
+    public final void set(double[] m) {
+        m00 = m[0];
+        m01 = m[1];
+        m02 = m[2];
+
+        m10 = m[3];
+        m11 = m[4];
+        m12 = m[5];
+
+        m20 = m[6];
+        m21 = m[7];
+        m22 = m[8];
+
+    }
+
+    /**
+     * Sets the value of this matrix to the value of the Matrix3d argument.
+     * 
+     * @param m1
+     *            the source matrix3d
+     */
+    public final void set(Matrix3d m1) {
+        this.m00 = m1.m00;
+        this.m01 = m1.m01;
+        this.m02 = m1.m02;
+
+        this.m10 = m1.m10;
+        this.m11 = m1.m11;
+        this.m12 = m1.m12;
+
+        this.m20 = m1.m20;
+        this.m21 = m1.m21;
+        this.m22 = m1.m22;
+    }
+
+    /**
+     * Sets the value of this matrix to the matrix conversion of the single
+     * precision quaternion argument.
+     * 
+     * @param q1
+     *            the quaternion to be converted
+     */
+    public final void set(Quaternion q1) {
+        this.m00 = (1.0 - 2.0 * q1.y * q1.y - 2.0 * q1.z * q1.z);
+        this.m10 = (2.0 * (q1.x * q1.y + q1.w * q1.z));
+        this.m20 = (2.0 * (q1.x * q1.z - q1.w * q1.y));
+
+        this.m01 = (2.0 * (q1.x * q1.y - q1.w * q1.z));
+        this.m11 = (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.z * q1.z);
+        this.m21 = (2.0 * (q1.y * q1.z + q1.w * q1.x));
+
+        this.m02 = (2.0 * (q1.x * q1.z + q1.w * q1.y));
+        this.m12 = (2.0 * (q1.y * q1.z - q1.w * q1.x));
+        this.m22 = (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.y * q1.y);
+    }
+
+    /**
+     * Sets the specified column of this matrix3d to the three values provided.
+     * 
+     * @param column
+     *            the column number to be modified (zero indexed)
+     * @param v
+     *            the replacement column
+     */
+    public final void setColumn(int column, double v[]) {
+        switch (column) {
+            case 0:
+                this.m00 = v[0];
+                this.m10 = v[1];
+                this.m20 = v[2];
+                break;
+
+            case 1:
+                this.m01 = v[0];
+                this.m11 = v[1];
+                this.m21 = v[2];
+                break;
+
+            case 2:
+                this.m02 = v[0];
+                this.m12 = v[1];
+                this.m22 = v[2];
+                break;
+
+            default:
+                throw new ArrayIndexOutOfBoundsException();
+        }
+    }
+
+    /**
+     * Sets the specified column of this matrix3d to the three values provided.
+     * 
+     * @param column
+     *            the column number to be modified (zero indexed)
+     * @param x
+     *            the first row element
+     * @param y
+     *            the second row element
+     * @param z
+     *            the third row element
+     */
+    public final void setColumn(int column, double x, double y, double z) {
+        switch (column) {
+            case 0:
+                this.m00 = x;
+                this.m10 = y;
+                this.m20 = z;
+                break;
+
+            case 1:
+                this.m01 = x;
+                this.m11 = y;
+                this.m21 = z;
+                break;
+
+            case 2:
+                this.m02 = x;
+                this.m12 = y;
+                this.m22 = z;
+                break;
+
+            default:
+                throw new ArrayIndexOutOfBoundsException();
+        }
+    }
+
+    /**
+     * Sets the specified column of this matrix3d to the vector provided.
+     * 
+     * @param column
+     *            the column number to be modified (zero indexed)
+     * @param v
+     *            the replacement column
+     */
+    public final void setColumn(int column, Vec3D v) {
+        switch (column) {
+            case 0:
+                this.m00 = v.x;
+                this.m10 = v.y;
+                this.m20 = v.z;
+                break;
+
+            case 1:
+                this.m01 = v.x;
+                this.m11 = v.y;
+                this.m21 = v.z;
+                break;
+
+            case 2:
+                this.m02 = v.x;
+                this.m12 = v.y;
+                this.m22 = v.z;
+                break;
+
+            default:
+                throw new ArrayIndexOutOfBoundsException();
+        }
+    }
+
+    /**
+     * Sets the specified element of this matrix3f to the value provided.
+     * 
+     * @param row
+     *            the row number to be modified (zero indexed)
+     * @param column
+     *            the column number to be modified (zero indexed)
+     * @param value
+     *            the new value
+     */
+    public final void setElement(int row, int column, double value) {
+        switch (row) {
+            case 0:
+                switch (column) {
+                    case 0:
+                        this.m00 = value;
+                        break;
+                    case 1:
+                        this.m01 = value;
+                        break;
+                    case 2:
+                        this.m02 = value;
+                        break;
+                    default:
+                        throw new ArrayIndexOutOfBoundsException();
+                }
+                break;
+
+            case 1:
+                switch (column) {
+                    case 0:
+                        this.m10 = value;
+                        break;
+                    case 1:
+                        this.m11 = value;
+                        break;
+                    case 2:
+                        this.m12 = value;
+                        break;
+                    default:
+                        throw new ArrayIndexOutOfBoundsException();
+                }
+                break;
+
+            case 2:
+                switch (column) {
+                    case 0:
+                        this.m20 = value;
+                        break;
+                    case 1:
+                        this.m21 = value;
+                        break;
+                    case 2:
+                        this.m22 = value;
+                        break;
+                    default:
+                        throw new ArrayIndexOutOfBoundsException();
+                }
+                break;
+
+            default:
+                throw new ArrayIndexOutOfBoundsException();
+        }
+    }
+
+    /**
+     * Sets this Matrix3d to identity.
+     */
+    public final void setIdentity() {
+        this.m00 = 1.0;
+        this.m01 = 0.0;
+        this.m02 = 0.0;
+
+        this.m10 = 0.0;
+        this.m11 = 1.0;
+        this.m12 = 0.0;
+
+        this.m20 = 0.0;
+        this.m21 = 0.0;
+        this.m22 = 1.0;
+    }
+
+    /**
+     * Set the first matrix element in the first row.
+     * 
+     * @param m00
+     *            The m00 to set.
+     * 
+     * @since vecmath 1.5
+     */
+    public final void setM00(double m00) {
+        this.m00 = m00;
+    }
+
+    /**
+     * Set the second matrix element in the first row.
+     * 
+     * @param m01
+     *            The m01 to set.
+     * 
+     * @since vecmath 1.5
+     */
+    public final void setM01(double m01) {
+        this.m01 = m01;
+    }
+
+    /**
+     * Set the third matrix element in the first row.
+     * 
+     * @param m02
+     *            The m02 to set.
+     * 
+     * @since vecmath 1.5
+     */
+    public final void setM02(double m02) {
+        this.m02 = m02;
+    }
+
+    /**
+     * Set first matrix element in the second row.
+     * 
+     * @param m10
+     *            The m10 to set.
+     * 
+     * @since vecmath 1.5
+     */
+    public final void setM10(double m10) {
+        this.m10 = m10;
+    }
+
+    /**
+     * Set the second matrix element in the second row.
+     * 
+     * @param m11
+     *            The m11 to set.
+     * 
+     * @since vecmath 1.5
+     */
+    public final void setM11(double m11) {
+        this.m11 = m11;
+    }
+
+    /**
+     * Set the third matrix element in the second row.
+     * 
+     * @param m12
+     *            The m12 to set.
+     * 
+     * @since vecmath 1.5
+     */
+    public final void setM12(double m12) {
+        this.m12 = m12;
+    }
+
+    /**
+     * Set the first matrix element in the third row.
+     * 
+     * @param m20
+     *            The m20 to set.
+     * 
+     * @since vecmath 1.5
+     */
+    public final void setM20(double m20) {
+        this.m20 = m20;
+    }
+
+    /**
+     * Set the second matrix element in the third row.
+     * 
+     * @param m21
+     *            The m21 to set.
+     * 
+     * @since vecmath 1.5
+     */
+    public final void setM21(double m21) {
+        this.m21 = m21;
+    }
+
+    /**
+     * Set the third matrix element in the third row.
+     * 
+     * @param m22
+     *            The m22 to set.
+     * 
+     * @since vecmath 1.5
+     */
+    public final void setM22(double m22) {
+        this.m22 = m22;
+    }
+
+    /**
+     * Sets the specified row of this matrix3d to the three values provided.
+     * 
+     * @param row
+     *            the row number to be modified (zero indexed)
+     * @param v
+     *            the replacement row
+     */
+    public final void setRow(int row, double v[]) {
+        switch (row) {
+            case 0:
+                this.m00 = v[0];
+                this.m01 = v[1];
+                this.m02 = v[2];
+                break;
+
+            case 1:
+                this.m10 = v[0];
+                this.m11 = v[1];
+                this.m12 = v[2];
+                break;
+
+            case 2:
+                this.m20 = v[0];
+                this.m21 = v[1];
+                this.m22 = v[2];
+                break;
+
+            default:
+                throw new ArrayIndexOutOfBoundsException();
+        }
+    }
+
+    /**
+     * Sets the specified row of this matrix3d to the 4 values provided.
+     * 
+     * @param row
+     *            the row number to be modified (zero indexed)
+     * @param x
+     *            the first column element
+     * @param y
+     *            the second column element
+     * @param z
+     *            the third column element
+     */
+    public final void setRow(int row, double x, double y, double z) {
+        switch (row) {
+            case 0:
+                this.m00 = x;
+                this.m01 = y;
+                this.m02 = z;
+                break;
+
+            case 1:
+                this.m10 = x;
+                this.m11 = y;
+                this.m12 = z;
+                break;
+
+            case 2:
+                this.m20 = x;
+                this.m21 = y;
+                this.m22 = z;
+                break;
+
+            default:
+                throw new ArrayIndexOutOfBoundsException();
+        }
+    }
+
+    /**
+     * Sets the specified row of this matrix3d to the Vector provided.
+     * 
+     * @param row
+     *            the row number to be modified (zero indexed)
+     * @param v
+     *            the replacement row
+     */
+    public final void setRow(int row, Vec3D v) {
+        switch (row) {
+            case 0:
+                this.m00 = v.x;
+                this.m01 = v.y;
+                this.m02 = v.z;
+                break;
+
+            case 1:
+                this.m10 = v.x;
+                this.m11 = v.y;
+                this.m12 = v.z;
+                break;
+
+            case 2:
+                this.m20 = v.x;
+                this.m21 = v.y;
+                this.m22 = v.z;
+                break;
+
+            default:
+                throw new ArrayIndexOutOfBoundsException();
+        }
+    }
+
+    /**
+     * Sets the scale component of the current matrix by factoring out the
+     * current scale (by doing an SVD) and multiplying by the new scale.
+     * 
+     * @param scale
+     *            the new scale amount
+     */
+    public final void setScale(double scale) {
+
+        double[] tmp_rot = new double[9]; // scratch matrix
+        double[] tmp_scale = new double[3]; // scratch matrix
+
+        getScaleRotate(tmp_scale, tmp_rot);
+
+        this.m00 = tmp_rot[0] * scale;
+        this.m01 = tmp_rot[1] * scale;
+        this.m02 = tmp_rot[2] * scale;
+
+        this.m10 = tmp_rot[3] * scale;
+        this.m11 = tmp_rot[4] * scale;
+        this.m12 = tmp_rot[5] * scale;
+
+        this.m20 = tmp_rot[6] * scale;
+        this.m21 = tmp_rot[7] * scale;
+        this.m22 = tmp_rot[8] * scale;
+    }
+
+    /**
+     * Sets this matrix to all zeros.
+     */
+    public final void setZero() {
+        m00 = 0.0;
+        m01 = 0.0;
+        m02 = 0.0;
+
+        m10 = 0.0;
+        m11 = 0.0;
+        m12 = 0.0;
+
+        m20 = 0.0;
+        m21 = 0.0;
+        m22 = 0.0;
+
+    }
+
+    /**
+     * Sets the value of this matrix to the matrix difference of itself and
+     * matrix m1 (this = this - m1).
+     * 
+     * @param m1
+     *            the other matrix
+     */
+    public final void sub(Matrix3d m1) {
+        this.m00 -= m1.m00;
+        this.m01 -= m1.m01;
+        this.m02 -= m1.m02;
+
+        this.m10 -= m1.m10;
+        this.m11 -= m1.m11;
+        this.m12 -= m1.m12;
+
+        this.m20 -= m1.m20;
+        this.m21 -= m1.m21;
+        this.m22 -= m1.m22;
+    }
+
+    /**
+     * Sets the value of this matrix to the matrix difference of matrices m1 and
+     * m2.
+     * 
+     * @param m1
+     *            the first matrix
+     * @param m2
+     *            the second matrix
+     */
+    public final void sub(Matrix3d m1, Matrix3d m2) {
+        this.m00 = m1.m00 - m2.m00;
+        this.m01 = m1.m01 - m2.m01;
+        this.m02 = m1.m02 - m2.m02;
+
+        this.m10 = m1.m10 - m2.m10;
+        this.m11 = m1.m11 - m2.m11;
+        this.m12 = m1.m12 - m2.m12;
+
+        this.m20 = m1.m20 - m2.m20;
+        this.m21 = m1.m21 - m2.m21;
+        this.m22 = m1.m22 - m2.m22;
+    }
+
+    /**
+     * Returns a string that contains the values of this Matrix3d.
+     * 
+     * @return the String representation
+     */
+    @Override
+    public String toString() {
+        return this.m00 + ", " + this.m01 + ", " + this.m02 + "\n" + this.m10
+                + ", " + this.m11 + ", " + this.m12 + "\n" + this.m20 + ", "
+                + this.m21 + ", " + this.m22 + "\n";
+    }
+
+    /**
+     * Multiply this matrix by the tuple t and place the result back into the
+     * tuple (t = this*t).
+     * 
+     * @param t
+     *            the tuple to be multiplied by this matrix and then replaced
+     */
+    public final void transform(Vec3D t) {
+        float x, y, z;
+        x = (float) (m00 * t.x + m01 * t.y + m02 * t.z);
+        y = (float) (m10 * t.x + m11 * t.y + m12 * t.z);
+        z = (float) (m20 * t.x + m21 * t.y + m22 * t.z);
+        t.set(x, y, z);
+    }
+
+    /**
+     * Multiply this matrix by the tuple t and and place the result into the
+     * tuple "result" (result = this*t).
+     * 
+     * @param t
+     *            the tuple to be multiplied by this matrix
+     * @param result
+     *            the tuple into which the product is placed
+     */
+    public final void transform(Vec3D t, Vec3D result) {
+        float x, y, z;
+        x = (float) (m00 * t.x + m01 * t.y + m02 * t.z);
+        y = (float) (m10 * t.x + m11 * t.y + m12 * t.z);
+        result.z = (float) (m20 * t.x + m21 * t.y + m22 * t.z);
+        result.x = x;
+        result.y = y;
+    }
+
+    /**
+     * Sets the value of this matrix to its transpose.
+     */
+    public final void transpose() {
+        double temp;
+
+        temp = this.m10;
+        this.m10 = this.m01;
+        this.m01 = temp;
+
+        temp = this.m20;
+        this.m20 = this.m02;
+        this.m02 = temp;
+
+        temp = this.m21;
+        this.m21 = this.m12;
+        this.m12 = temp;
+    }
+
+    /**
+     * Sets the value of this matrix to the transpose of the argument matrix.
+     * 
+     * @param m1
+     *            the matrix to be transposed
+     */
+    public final void transpose(Matrix3d m1) {
+        if (this != m1) {
+            this.m00 = m1.m00;
+            this.m01 = m1.m10;
+            this.m02 = m1.m20;
+
+            this.m10 = m1.m01;
+            this.m11 = m1.m11;
+            this.m12 = m1.m21;
+
+            this.m20 = m1.m02;
+            this.m21 = m1.m12;
+            this.m22 = m1.m22;
+        } else {
+            this.transpose();
+        }
+    }
+}