/** * ***************************************************************************** * 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 = -3851554186221722070L; public final Vec2 ex, ey; /** * Convert the matrix to printable format. * @return */ @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 = new Vec2(c1); ey = new Vec2(c2); } /** * 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; } /** * 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; } }