pbox2d 0.6.0-java → 0.8.0-java

data/src/org/jbox2d/common/Mat22.java

@@ -0,0 +1,609 @@
+/**
+ * *****************************************************************************
+ * Copyright (c) 2013, Daniel Murphy
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ * 	* Redistributions of source code must retain the above copyright notice,
+ * 	  this list of conditions and the following disclaimer.
+ * 	* Redistributions in binary form must reproduce the above copyright notice,
+ * 	  this list of conditions and the following disclaimer in the documentation
+ * 	  and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
+ * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *****************************************************************************
+ */
+package org.jbox2d.common;
+
+import java.io.Serializable;
+
+/**
+ * A 2-by-2 matrix. Stored in column-major order.
+ */
+public class Mat22 implements Serializable {
+
+    private static final long serialVersionUID = 2L;
+
+    public final Vec2 ex, ey;
+
+    /**
+     * Convert the matrix to printable format.
+     */
+    @Override
+    public String toString() {
+        String s = "";
+        s += "[" + ex.x + "," + ey.x + "]\n";
+        s += "[" + ex.y + "," + ey.y + "]";
+        return s;
+    }
+
+    /**
+     * Construct zero matrix. Note: this is NOT an identity matrix! djm fixed
+     * double allocation problem
+     */
+    public Mat22() {
+        ex = new Vec2();
+        ey = new Vec2();
+    }
+
+    /**
+     * Create a matrix with given vectors as columns.
+     *
+     * @param c1 Column 1 of matrix
+     * @param c2 Column 2 of matrix
+     */
+    public Mat22(final Vec2 c1, final Vec2 c2) {
+        ex = c1.clone();
+        ey = c2.clone();
+    }
+
+    /**
+     * Create a matrix from four floats.
+     *
+     * @param exx
+     * @param col2x
+     * @param exy
+     * @param col2y
+     */
+    public Mat22(final float exx, final float col2x, final float exy, final float col2y) {
+        ex = new Vec2(exx, exy);
+        ey = new Vec2(col2x, col2y);
+    }
+
+    /**
+     * Set as a copy of another matrix.
+     *
+     * @param m Matrix to copy
+     * @return
+     */
+    public final Mat22 set(final Mat22 m) {
+        ex.x = m.ex.x;
+        ex.y = m.ex.y;
+        ey.x = m.ey.x;
+        ey.y = m.ey.y;
+        return this;
+    }
+
+    public final Mat22 set(final float exx, final float col2x, final float exy, final float col2y) {
+        ex.x = exx;
+        ex.y = exy;
+        ey.x = col2x;
+        ey.y = col2y;
+        return this;
+    }
+
+    /**
+     * Return a clone of this matrix. djm fixed double allocation
+     *
+     * @return
+     */
+    // @Override // annotation omitted for GWT-compatibility
+    @Override
+    public final Mat22 clone() {
+        return new Mat22(ex, ey);
+    }
+
+    /**
+     * Set as a matrix representing a rotation.
+     *
+     * @param angle Rotation (in radians) that matrix represents.
+     */
+    public final void set(final float angle) {
+        final float c = MathUtils.cos(angle), s = MathUtils.sin(angle);
+        ex.x = c;
+        ey.x = -s;
+        ex.y = s;
+        ey.y = c;
+    }
+
+    /**
+     * Set as the identity matrix.
+     */
+    public final void setIdentity() {
+        ex.x = 1.0f;
+        ey.x = 0.0f;
+        ex.y = 0.0f;
+        ey.y = 1.0f;
+    }
+
+    /**
+     * Set as the zero matrix.
+     */
+    public final void setZero() {
+        ex.x = 0.0f;
+        ey.x = 0.0f;
+        ex.y = 0.0f;
+        ey.y = 0.0f;
+    }
+
+    /**
+     * Extract the angle from this matrix (assumed to be a rotation matrix).
+     *
+     * @return
+     */
+    public final float getAngle() {
+        return MathUtils.atan2(ex.y, ex.x);
+    }
+
+    /**
+     * Set by column vectors.
+     *
+     * @param c1 Column 1
+     * @param c2 Column 2
+     */
+    public final void set(final Vec2 c1, final Vec2 c2) {
+        ex.x = c1.x;
+        ey.x = c2.x;
+        ex.y = c1.y;
+        ey.y = c2.y;
+    }
+
+    /**
+     * Returns the inverted Mat22 - does NOT invert the matrix locally!
+     *
+     * @return
+     */
+    public final Mat22 invert() {
+        final float a = ex.x, b = ey.x, c = ex.y, d = ey.y;
+        final Mat22 B = new Mat22();
+        float det = a * d - b * c;
+        if (det != 0) {
+            det = 1.0f / det;
+        }
+        B.ex.x = det * d;
+        B.ey.x = -det * b;
+        B.ex.y = -det * c;
+        B.ey.y = det * a;
+        return B;
+    }
+
+    public final Mat22 invertLocal() {
+        final float a = ex.x, b = ey.x, c = ex.y, d = ey.y;
+        float det = a * d - b * c;
+        if (det != 0) {
+            det = 1.0f / det;
+        }
+        ex.x = det * d;
+        ey.x = -det * b;
+        ex.y = -det * c;
+        ey.y = det * a;
+        return this;
+    }
+
+    public final void invertToOut(final Mat22 out) {
+        final float a = ex.x, b = ey.x, c = ex.y, d = ey.y;
+        float det = a * d - b * c;
+        // b2Assert(det != 0.0f);
+        det = 1.0f / det;
+        out.ex.x = det * d;
+        out.ey.x = -det * b;
+        out.ex.y = -det * c;
+        out.ey.y = det * a;
+    }
+
+    /**
+     * Return the matrix composed of the absolute values of all elements. djm:
+     * fixed double allocation
+     *
+     * @return Absolute value matrix
+     */
+    public final Mat22 abs() {
+        return new Mat22(MathUtils.abs(ex.x), MathUtils.abs(ey.x), MathUtils.abs(ex.y),
+                MathUtils.abs(ey.y));
+    }
+
+    /* djm: added */
+    public final void absLocal() {
+        ex.absLocal();
+        ey.absLocal();
+    }
+
+    /**
+     * Return the matrix composed of the absolute values of all elements.
+     *
+     * @param R
+     * @return Absolute value matrix
+     */
+    public final static Mat22 abs(final Mat22 R) {
+        return R.abs();
+    }
+
+    /* djm created */
+    public static void absToOut(final Mat22 R, final Mat22 out) {
+        out.ex.x = MathUtils.abs(R.ex.x);
+        out.ex.y = MathUtils.abs(R.ex.y);
+        out.ey.x = MathUtils.abs(R.ey.x);
+        out.ey.y = MathUtils.abs(R.ey.y);
+    }
+
+    /**
+     * Multiply a vector by this matrix.
+     *
+     * @param v Vector to multiply by matrix.
+     * @return Resulting vector
+     */
+    public final Vec2 mul(final Vec2 v) {
+        return new Vec2(ex.x * v.x + ey.x * v.y, ex.y * v.x + ey.y * v.y);
+    }
+
+    public final void mulToOut(final Vec2 v, final Vec2 out) {
+        final float tempy = ex.y * v.x + ey.y * v.y;
+        out.x = ex.x * v.x + ey.x * v.y;
+        out.y = tempy;
+    }
+
+    public final void mulToOutUnsafe(final Vec2 v, final Vec2 out) {
+        assert (v != out);
+        out.x = ex.x * v.x + ey.x * v.y;
+        out.y = ex.y * v.x + ey.y * v.y;
+    }
+
+    /**
+     * Multiply another matrix by this one (this one on left). djm optimized
+     *
+     * @param R
+     * @return
+     */
+    public final Mat22 mul(final Mat22 R) {
+        /*
+     * Mat22 C = new Mat22();C.set(this.mul(R.ex), this.mul(R.ey));return C;
+         */
+        final Mat22 C = new Mat22();
+        C.ex.x = ex.x * R.ex.x + ey.x * R.ex.y;
+        C.ex.y = ex.y * R.ex.x + ey.y * R.ex.y;
+        C.ey.x = ex.x * R.ey.x + ey.x * R.ey.y;
+        C.ey.y = ex.y * R.ey.x + ey.y * R.ey.y;
+        // C.set(ex,col2);
+        return C;
+    }
+
+    public final Mat22 mulLocal(final Mat22 R) {
+        mulToOut(R, this);
+        return this;
+    }
+
+    public final void mulToOut(final Mat22 R, final Mat22 out) {
+        final float tempy1 = this.ex.y * R.ex.x + this.ey.y * R.ex.y;
+        final float tempx1 = this.ex.x * R.ex.x + this.ey.x * R.ex.y;
+        out.ex.x = tempx1;
+        out.ex.y = tempy1;
+        final float tempy2 = this.ex.y * R.ey.x + this.ey.y * R.ey.y;
+        final float tempx2 = this.ex.x * R.ey.x + this.ey.x * R.ey.y;
+        out.ey.x = tempx2;
+        out.ey.y = tempy2;
+    }
+
+    public final void mulToOutUnsafe(final Mat22 R, final Mat22 out) {
+        assert (out != R);
+        assert (out != this);
+        out.ex.x = this.ex.x * R.ex.x + this.ey.x * R.ex.y;
+        out.ex.y = this.ex.y * R.ex.x + this.ey.y * R.ex.y;
+        out.ey.x = this.ex.x * R.ey.x + this.ey.x * R.ey.y;
+        out.ey.y = this.ex.y * R.ey.x + this.ey.y * R.ey.y;
+    }
+
+    /**
+     * Multiply another matrix by the transpose of this one (transpose of this
+     * one on left). djm: optimized
+     *
+     * @param B
+     * @return
+     */
+    public final Mat22 mulTrans(final Mat22 B) {
+        /*
+     * Vec2 c1 = new Vec2(Vec2.dot(this.ex, B.ex), Vec2.dot(this.ey, B.ex)); Vec2 c2 = new
+     * Vec2(Vec2.dot(this.ex, B.ey), Vec2.dot(this.ey, B.ey)); Mat22 C = new Mat22(); C.set(c1, c2);
+     * return C;
+         */
+        final Mat22 C = new Mat22();
+
+        C.ex.x = Vec2.dot(this.ex, B.ex);
+        C.ex.y = Vec2.dot(this.ey, B.ex);
+
+        C.ey.x = Vec2.dot(this.ex, B.ey);
+        C.ey.y = Vec2.dot(this.ey, B.ey);
+        return C;
+    }
+
+    public final Mat22 mulTransLocal(final Mat22 B) {
+        mulTransToOut(B, this);
+        return this;
+    }
+
+    public final void mulTransToOut(final Mat22 B, final Mat22 out) {
+        /*
+     * out.ex.x = Vec2.dot(this.ex, B.ex); out.ex.y = Vec2.dot(this.ey, B.ex); out.ey.x =
+     * Vec2.dot(this.ex, B.ey); out.ey.y = Vec2.dot(this.ey, B.ey);
+         */
+        final float x1 = this.ex.x * B.ex.x + this.ex.y * B.ex.y;
+        final float y1 = this.ey.x * B.ex.x + this.ey.y * B.ex.y;
+        final float x2 = this.ex.x * B.ey.x + this.ex.y * B.ey.y;
+        final float y2 = this.ey.x * B.ey.x + this.ey.y * B.ey.y;
+        out.ex.x = x1;
+        out.ey.x = x2;
+        out.ex.y = y1;
+        out.ey.y = y2;
+    }
+
+    public final void mulTransToOutUnsafe(final Mat22 B, final Mat22 out) {
+        assert (B != out);
+        assert (this != out);
+        out.ex.x = this.ex.x * B.ex.x + this.ex.y * B.ex.y;
+        out.ey.x = this.ex.x * B.ey.x + this.ex.y * B.ey.y;
+        out.ex.y = this.ey.x * B.ex.x + this.ey.y * B.ex.y;
+        out.ey.y = this.ey.x * B.ey.x + this.ey.y * B.ey.y;
+    }
+
+    /**
+     * Multiply a vector by the transpose of this matrix.
+     *
+     * @param v
+     * @return
+     */
+    public final Vec2 mulTrans(final Vec2 v) {
+        // return new Vec2(Vec2.dot(v, ex), Vec2.dot(v, col2));
+        return new Vec2((v.x * ex.x + v.y * ex.y), (v.x * ey.x + v.y * ey.y));
+    }
+
+    /* djm added */
+    public final void mulTransToOut(final Vec2 v, final Vec2 out) {
+        /*
+     * out.x = Vec2.dot(v, ex); out.y = Vec2.dot(v, col2);
+         */
+        final float tempx = v.x * ex.x + v.y * ex.y;
+        out.y = v.x * ey.x + v.y * ey.y;
+        out.x = tempx;
+    }
+
+    /**
+     * Add this matrix to B, return the result.
+     *
+     * @param B
+     * @return
+     */
+    public final Mat22 add(final Mat22 B) {
+        // return new Mat22(ex.add(B.ex), col2.add(B.ey));
+        Mat22 m = new Mat22();
+        m.ex.x = ex.x + B.ex.x;
+        m.ex.y = ex.y + B.ex.y;
+        m.ey.x = ey.x + B.ey.x;
+        m.ey.y = ey.y + B.ey.y;
+        return m;
+    }
+
+    /**
+     * Add B to this matrix locally.
+     *
+     * @param B
+     * @return
+     */
+    public final Mat22 addLocal(final Mat22 B) {
+        // ex.addLocal(B.ex);
+        // col2.addLocal(B.ey);
+        ex.x += B.ex.x;
+        ex.y += B.ex.y;
+        ey.x += B.ey.x;
+        ey.y += B.ey.y;
+        return this;
+    }
+
+    /**
+     * Solve A * x = b where A = this matrix.
+     *
+     * @param b
+     * @return The vector x that solves the above equation.
+     */
+    public final Vec2 solve(final Vec2 b) {
+        final float a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
+        float det = a11 * a22 - a12 * a21;
+        if (det != 0.0f) {
+            det = 1.0f / det;
+        }
+        final Vec2 x = new Vec2(det * (a22 * b.x - a12 * b.y), det * (a11 * b.y - a21 * b.x));
+        return x;
+    }
+
+    public final void solveToOut(final Vec2 b, final Vec2 out) {
+        final float a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
+        float det = a11 * a22 - a12 * a21;
+        if (det != 0.0f) {
+            det = 1.0f / det;
+        }
+        final float tempy = det * (a11 * b.y - a21 * b.x);
+        out.x = det * (a22 * b.x - a12 * b.y);
+        out.y = tempy;
+    }
+
+    public final static Vec2 mul(final Mat22 R, final Vec2 v) {
+        // return R.mul(v);
+        return new Vec2(R.ex.x * v.x + R.ey.x * v.y, R.ex.y * v.x + R.ey.y * v.y);
+    }
+
+    public final static void mulToOut(final Mat22 R, final Vec2 v, final Vec2 out) {
+        final float tempy = R.ex.y * v.x + R.ey.y * v.y;
+        out.x = R.ex.x * v.x + R.ey.x * v.y;
+        out.y = tempy;
+    }
+
+    public final static void mulToOutUnsafe(final Mat22 R, final Vec2 v, final Vec2 out) {
+        assert (v != out);
+        out.x = R.ex.x * v.x + R.ey.x * v.y;
+        out.y = R.ex.y * v.x + R.ey.y * v.y;
+    }
+
+    public final static Mat22 mul(final Mat22 A, final Mat22 B) {
+        // return A.mul(B);
+        final Mat22 C = new Mat22();
+        C.ex.x = A.ex.x * B.ex.x + A.ey.x * B.ex.y;
+        C.ex.y = A.ex.y * B.ex.x + A.ey.y * B.ex.y;
+        C.ey.x = A.ex.x * B.ey.x + A.ey.x * B.ey.y;
+        C.ey.y = A.ex.y * B.ey.x + A.ey.y * B.ey.y;
+        return C;
+    }
+
+    public final static void mulToOut(final Mat22 A, final Mat22 B, final Mat22 out) {
+        final float tempy1 = A.ex.y * B.ex.x + A.ey.y * B.ex.y;
+        final float tempx1 = A.ex.x * B.ex.x + A.ey.x * B.ex.y;
+        final float tempy2 = A.ex.y * B.ey.x + A.ey.y * B.ey.y;
+        final float tempx2 = A.ex.x * B.ey.x + A.ey.x * B.ey.y;
+        out.ex.x = tempx1;
+        out.ex.y = tempy1;
+        out.ey.x = tempx2;
+        out.ey.y = tempy2;
+    }
+
+    public final static void mulToOutUnsafe(final Mat22 A, final Mat22 B, final Mat22 out) {
+        assert (out != A);
+        assert (out != B);
+        out.ex.x = A.ex.x * B.ex.x + A.ey.x * B.ex.y;
+        out.ex.y = A.ex.y * B.ex.x + A.ey.y * B.ex.y;
+        out.ey.x = A.ex.x * B.ey.x + A.ey.x * B.ey.y;
+        out.ey.y = A.ex.y * B.ey.x + A.ey.y * B.ey.y;
+    }
+
+    public final static Vec2 mulTrans(final Mat22 R, final Vec2 v) {
+        return new Vec2((v.x * R.ex.x + v.y * R.ex.y), (v.x * R.ey.x + v.y * R.ey.y));
+    }
+
+    public final static void mulTransToOut(final Mat22 R, final Vec2 v, final Vec2 out) {
+        float outx = v.x * R.ex.x + v.y * R.ex.y;
+        out.y = v.x * R.ey.x + v.y * R.ey.y;
+        out.x = outx;
+    }
+
+    public final static void mulTransToOutUnsafe(final Mat22 R, final Vec2 v, final Vec2 out) {
+        assert (out != v);
+        out.y = v.x * R.ey.x + v.y * R.ey.y;
+        out.x = v.x * R.ex.x + v.y * R.ex.y;
+    }
+
+    public final static Mat22 mulTrans(final Mat22 A, final Mat22 B) {
+        final Mat22 C = new Mat22();
+        C.ex.x = A.ex.x * B.ex.x + A.ex.y * B.ex.y;
+        C.ex.y = A.ey.x * B.ex.x + A.ey.y * B.ex.y;
+        C.ey.x = A.ex.x * B.ey.x + A.ex.y * B.ey.y;
+        C.ey.y = A.ey.x * B.ey.x + A.ey.y * B.ey.y;
+        return C;
+    }
+
+    public final static void mulTransToOut(final Mat22 A, final Mat22 B, final Mat22 out) {
+        final float x1 = A.ex.x * B.ex.x + A.ex.y * B.ex.y;
+        final float y1 = A.ey.x * B.ex.x + A.ey.y * B.ex.y;
+        final float x2 = A.ex.x * B.ey.x + A.ex.y * B.ey.y;
+        final float y2 = A.ey.x * B.ey.x + A.ey.y * B.ey.y;
+
+        out.ex.x = x1;
+        out.ex.y = y1;
+        out.ey.x = x2;
+        out.ey.y = y2;
+    }
+
+    public final static void mulTransToOutUnsafe(final Mat22 A, final Mat22 B, final Mat22 out) {
+        assert (A != out);
+        assert (B != out);
+        out.ex.x = A.ex.x * B.ex.x + A.ex.y * B.ex.y;
+        out.ex.y = A.ey.x * B.ex.x + A.ey.y * B.ex.y;
+        out.ey.x = A.ex.x * B.ey.x + A.ex.y * B.ey.y;
+        out.ey.y = A.ey.x * B.ey.x + A.ey.y * B.ey.y;
+    }
+
+    public final static Mat22 createRotationalTransform(float angle) {
+        Mat22 mat = new Mat22();
+        final float c = MathUtils.cos(angle);
+        final float s = MathUtils.sin(angle);
+        mat.ex.x = c;
+        mat.ey.x = -s;
+        mat.ex.y = s;
+        mat.ey.y = c;
+        return mat;
+    }
+
+    public final static void createRotationalTransform(float angle, Mat22 out) {
+        final float c = MathUtils.cos(angle);
+        final float s = MathUtils.sin(angle);
+        out.ex.x = c;
+        out.ey.x = -s;
+        out.ex.y = s;
+        out.ey.y = c;
+    }
+
+    public final static Mat22 createScaleTransform(float scale) {
+        Mat22 mat = new Mat22();
+        mat.ex.x = scale;
+        mat.ey.y = scale;
+        return mat;
+    }
+
+    public final static void createScaleTransform(float scale, Mat22 out) {
+        out.ex.x = scale;
+        out.ey.y = scale;
+    }
+
+    @Override
+    public int hashCode() {
+        final int prime = 31;
+        int result = 1;
+        result = prime * result + ((ex == null) ? 0 : ex.hashCode());
+        result = prime * result + ((ey == null) ? 0 : ey.hashCode());
+        return result;
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (this == obj) {
+            return true;
+        }
+        if (obj == null) {
+            return false;
+        }
+        if (getClass() != obj.getClass()) {
+            return false;
+        }
+        Mat22 other = (Mat22) obj;
+        if (ex == null) {
+            if (other.ex != null) {
+                return false;
+            }
+        } else if (!ex.equals(other.ex)) {
+            return false;
+        }
+        if (ey == null) {
+            if (other.ey != null) {
+                return false;
+            }
+        } else if (!ey.equals(other.ey)) {
+            return false;
+        }
+        return true;
+    }
+}