139 lines
4.8 KiB
Java
139 lines
4.8 KiB
Java
package org.dynmap.utils;
|
|
|
|
import org.json.simple.JSONArray;
|
|
|
|
/**
|
|
* Basic 3D matrix math class - prevent dependency on Java 3D for this
|
|
*/
|
|
public class Matrix3D {
|
|
private double m11, m12, m13, m21, m22, m23, m31, m32, m33;
|
|
/**
|
|
* Construct identity matrix
|
|
*/
|
|
public Matrix3D() {
|
|
m11 = m22 = m33 = 1.0;
|
|
m12 = m13 = m21 = m23 = m31 = m32 = 0.0;
|
|
}
|
|
/**
|
|
* Construct matrix with given parms
|
|
*
|
|
* @param m11 - first cell of first row
|
|
* @param m12 - second cell of first row
|
|
* @param m13 - third cell of first row
|
|
* @param m21 - first cell of second row
|
|
* @param m22 - second cell of second row
|
|
* @param m23 - third cell of second row
|
|
* @param m31 - first cell of third row
|
|
* @param m32 - second cell of third row
|
|
* @param m33 - third cell of third row
|
|
*/
|
|
public Matrix3D(double m11, double m12, double m13, double m21, double m22, double m23, double m31, double m32, double m33) {
|
|
this.m11 = m11; this.m12 = m12; this.m13 = m13;
|
|
this.m21 = m21; this.m22 = m22; this.m23 = m23;
|
|
this.m31 = m31; this.m32 = m32; this.m33 = m33;
|
|
}
|
|
/**
|
|
* Multiply matrix by another matrix (this = mat * this), and store result in self
|
|
* @param mat
|
|
*/
|
|
public void multiply(Matrix3D mat) {
|
|
double new_m11 = mat.m11*m11 + mat.m12*m21 + mat.m13*m31;
|
|
double new_m12 = mat.m11*m12 + mat.m12*m22 + mat.m13*m32;
|
|
double new_m13 = mat.m11*m13 + mat.m12*m23 + mat.m13*m33;
|
|
double new_m21 = mat.m21*m11 + mat.m22*m21 + mat.m23*m31;
|
|
double new_m22 = mat.m21*m12 + mat.m22*m22 + mat.m23*m32;
|
|
double new_m23 = mat.m21*m13 + mat.m22*m23 + mat.m23*m33;
|
|
double new_m31 = mat.m31*m11 + mat.m32*m21 + mat.m33*m31;
|
|
double new_m32 = mat.m31*m12 + mat.m32*m22 + mat.m33*m32;
|
|
double new_m33 = mat.m31*m13 + mat.m32*m23 + mat.m33*m33;
|
|
m11 = new_m11; m12 = new_m12; m13 = new_m13;
|
|
m21 = new_m21; m22 = new_m22; m23 = new_m23;
|
|
m31 = new_m31; m32 = new_m32; m33 = new_m33;
|
|
}
|
|
/**
|
|
* Scale each coordinate by given values
|
|
*
|
|
* @param s1
|
|
* @param s2
|
|
* @param s3
|
|
*/
|
|
public void scale(double s1, double s2, double s3) {
|
|
Matrix3D scalemat = new Matrix3D(s1, 0, 0, 0, s2, 0, 0, 0, s3);
|
|
multiply(scalemat);
|
|
}
|
|
/**
|
|
* Rotate XY clockwise around +Z axis
|
|
* @param rot_deg - degrees of rotation
|
|
*/
|
|
public void rotateXY(double rot_deg) {
|
|
double rot_rad = Math.toRadians(rot_deg);
|
|
double sin_rot = Math.sin(rot_rad);
|
|
double cos_rot = Math.cos(rot_rad);
|
|
Matrix3D rotmat = new Matrix3D(cos_rot, sin_rot, 0, -sin_rot, cos_rot, 0, 0, 0, 1);
|
|
multiply(rotmat);
|
|
}
|
|
/**
|
|
* Rotate YZ clockwise around +X axis
|
|
* @param rot_deg - degrees of rotation
|
|
*/
|
|
public void rotateYZ(double rot_deg) {
|
|
double rot_rad = Math.toRadians(rot_deg);
|
|
double sin_rot = Math.sin(rot_rad);
|
|
double cos_rot = Math.cos(rot_rad);
|
|
Matrix3D rotmat = new Matrix3D(1, 0, 0, 0, cos_rot, sin_rot, 0, -sin_rot, cos_rot);
|
|
multiply(rotmat);
|
|
}
|
|
/**
|
|
* Shear along Z axis by factor of X and Y
|
|
*/
|
|
public void shearZ(double x_fact, double y_fact) {
|
|
Matrix3D shearmat = new Matrix3D(1, 0, 0, 0, 1, 0, x_fact, y_fact, 1);
|
|
multiply(shearmat);
|
|
}
|
|
/**
|
|
* Transform a given vector using the matrix
|
|
*/
|
|
public final void transform(double[] v) {
|
|
double v1 = m11*v[0] + m12*v[1] + m13*v[2];
|
|
double v2 = m21*v[0] + m22*v[1] + m23*v[2];
|
|
double v3 = m31*v[0] + m32*v[1] + m33*v[2];
|
|
v[0] = v1; v[1] = v2; v[2] = v3;
|
|
}
|
|
/**
|
|
* Transform a given vector using the matrix
|
|
*/
|
|
public final void transform(Vector3D v) {
|
|
double v1 = m11*v.x + m12*v.y + m13*v.z;
|
|
double v2 = m21*v.x + m22*v.y + m23*v.z;
|
|
double v3 = m31*v.x + m32*v.y + m33*v.z;
|
|
v.x = v1; v.y = v2; v.z = v3;
|
|
}
|
|
|
|
/**
|
|
* Transform a given vector using the matrix - put result in provided output vector
|
|
*/
|
|
public final void transform(Vector3D v, Vector3D outv) {
|
|
outv.x = m11*v.x + m12*v.y + m13*v.z;
|
|
outv.y = m21*v.x + m22*v.y + m23*v.z;
|
|
outv.z = m31*v.x + m32*v.y + m33*v.z;
|
|
}
|
|
|
|
public String toString() {
|
|
return "[ [" + m11 + " " + m12 + " " + m13 + "] [" + m21 + " " + m22 + " " + m23 + "] [" + m31 + " " + m32 + " " + m33 + "] ]";
|
|
}
|
|
|
|
@SuppressWarnings("unchecked")
|
|
public JSONArray toJSON() {
|
|
JSONArray array = new JSONArray();
|
|
array.add(m11);
|
|
array.add(m12);
|
|
array.add(m13);
|
|
array.add(m21);
|
|
array.add(m22);
|
|
array.add(m23);
|
|
array.add(m31);
|
|
array.add(m32);
|
|
array.add(m33);
|
|
return array;
|
|
}
|
|
}
|