/*******************************************************************************
* 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
*/
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
*/
// @Override // annotation omitted for GWT-compatibility
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! */
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.
*
* @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.
*
* @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;
}
}