com.almasb.fxgl.physics.box2d.common.Mat22 Maven / Gradle / Ivy
/*
* FXGL - JavaFX Game Library. The MIT License (MIT).
* Copyright (c) AlmasB ([email protected]).
* See LICENSE for details.
*/
package com.almasb.fxgl.physics.box2d.common;
import com.almasb.fxgl.core.math.FXGLMath;
import com.almasb.fxgl.core.math.Vec2;
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;
/**
* Construct zero matrix. Note: this is NOT an identity matrix!
*/
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.
*/
@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 = JBoxUtils.cos(angle), s = JBoxUtils.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 angle
*/
public final float getAngle() {
return JBoxUtils.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(FXGLMath.abs(ex.x), FXGLMath.abs(ey.x), FXGLMath.abs(ex.y),
FXGLMath.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 = FXGLMath.abs(R.ex.x);
out.ex.y = FXGLMath.abs(R.ex.y);
out.ey.x = FXGLMath.abs(R.ey.x);
out.ey.y = FXGLMath.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;
}
return new Vec2(det * (a22 * b.x - a12 * b.y), det * (a11 * b.y - a21 * b.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 = JBoxUtils.cos(angle);
final float s = JBoxUtils.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 = JBoxUtils.cos(angle);
final float s = JBoxUtils.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;
}
@Override
public String toString() {
return "[" + ex.x + "," + ey.x + "]\n"
+"[" + ex.y + "," + ey.y + "]";
}
}
© 2015 - 2025 Weber Informatics LLC | Privacy Policy