package com.jme3.scene.plugins.blender.math;
import java.text.DecimalFormat;
import org.ejml.ops.CommonOps;
import org.ejml.simple.SimpleMatrix;
import org.ejml.simple.SimpleSVD;
import com.jme3.math.FastMath;
/**
* Encapsulates a 4x4 matrix
*
*
*/
public class Matrix extends SimpleMatrix {
private static final long serialVersionUID = 2396600537315902559L;
public Matrix(int rows, int cols) {
super(rows, cols);
}
/**
* Copy constructor
*/
public Matrix(SimpleMatrix m) {
super(m);
}
public Matrix(double[][] data) {
super(data);
}
public static Matrix identity(int size) {
Matrix result = new Matrix(size, size);
CommonOps.setIdentity(result.mat);
return result;
}
public Matrix pseudoinverse() {
return this.pseudoinverse(1);
}
@SuppressWarnings("unchecked")
public Matrix pseudoinverse(double lambda) {
SimpleSVD<SimpleMatrix> simpleSVD = this.svd();
SimpleMatrix U = simpleSVD.getU();
SimpleMatrix S = simpleSVD.getW();
SimpleMatrix V = simpleSVD.getV();
int N = Math.min(this.numRows(),this.numCols());
double maxSingular = 0;
for( int i = 0; i < N; ++i ) {
if( S.get(i, i) > maxSingular ) {
maxSingular = S.get(i, i);
}
}
double tolerance = FastMath.DBL_EPSILON * Math.max(this.numRows(),this.numCols()) * maxSingular;
for(int i=0;i<Math.min(S.numRows(), S.numCols());++i) {
double a = S.get(i, i);
if(a <= tolerance) {
a = 0;
} else {
a = a/(a * a + lambda * lambda);
}
S.set(i, i, a);
}
return new Matrix(V.mult(S.transpose()).mult(U.transpose()));
}
public void setColumn(Vector3d col, int column) {
this.setColumn(column, 0, col.x, col.y, col.z);
}
/**
* Just for some debug informations in order to compare the results with the scilab computation program.
* @param name the name of the matrix
* @param m the matrix to print out
* @return the String format of the matrix to easily input it to Scilab
*/
public String toScilabString(String name, SimpleMatrix m) {
String result = name + " = [";
for(int i=0;i<m.numRows();++i) {
for(int j=0;j<m.numCols();++j) {
result += m.get(i, j) + " ";
}
result += ";";
}
return result;
}
/**
* @return a String representation of the matrix
*/
@Override
public String toString() {
DecimalFormat df = new DecimalFormat("#.0000");
StringBuilder buf = new StringBuilder();
for (int r = 0; r < this.numRows(); ++r) {
buf.append("\n| ");
for (int c = 0; c < this.numCols(); ++c) {
buf.append(df.format(this.get(r, c))).append(' ');
}
buf.append('|');
}
return buf.toString();
}
public void setTranslation(Vector3d translation) {
this.setColumn(translation, 3);
}
/**
* Sets the scale.
*
* @param scale
* the scale vector to set
*/
public void setScale(Vector3d scale) {
this.setScale(scale.x, scale.y, scale.z);
}
/**
* Sets the scale.
*
* @param x
* the X scale
* @param y
* the Y scale
* @param z
* the Z scale
*/
public void setScale(double x, double y, double z) {
Vector3d vect1 = new Vector3d(this.get(0, 0), this.get(1, 0), this.get(2, 0));
vect1.normalizeLocal().multLocal(x);
this.set(0, 0, vect1.x);
this.set(1, 0, vect1.y);
this.set(2, 0, vect1.z);
vect1.set(this.get(0, 1), this.get(1, 1), this.get(2, 1));
vect1.normalizeLocal().multLocal(y);
this.set(0, 1, vect1.x);
this.set(1, 1, vect1.y);
this.set(2, 1, vect1.z);
vect1.set(this.get(0, 2), this.get(1, 2), this.get(2, 2));
vect1.normalizeLocal().multLocal(z);
this.set(0, 2, vect1.x);
this.set(1, 2, vect1.y);
this.set(2, 2, vect1.z);
}
/**
* <code>setRotationQuaternion</code> builds a rotation from a
* <code>Quaternion</code>.
*
* @param quat
* the quaternion to build the rotation from.
* @throws NullPointerException
* if quat is null.
*/
public void setRotationQuaternion(DQuaternion quat) {
quat.toRotationMatrix(this);
}
public DTransform toTransform() {
DTransform result = new DTransform();
result.setTranslation(this.toTranslationVector());
result.setRotation(this.toRotationQuat());
result.setScale(this.toScaleVector());
return result;
}
public Vector3d toTranslationVector() {
return new Vector3d(this.get(0, 3), this.get(1, 3), this.get(2, 3));
}
public DQuaternion toRotationQuat() {
DQuaternion quat = new DQuaternion();
quat.fromRotationMatrix(this.get(0, 0), this.get(0, 1), this.get(0, 2), this.get(1, 0), this.get(1, 1), this.get(1, 2), this.get(2, 0), this.get(2, 1), this.get(2, 2));
return quat;
}
/**
* Retreives the scale vector from the matrix and stores it into a given
* vector.
*
* @param the
* vector where the scale will be stored
*/
public Vector3d toScaleVector() {
Vector3d result = new Vector3d();
this.toScaleVector(result);
return result;
}
/**
* Retreives the scale vector from the matrix and stores it into a given
* vector.
*
* @param the
* vector where the scale will be stored
*/
public void toScaleVector(Vector3d vector) {
double scaleX = Math.sqrt(this.get(0, 0) * this.get(0, 0) + this.get(1, 0) * this.get(1, 0) + this.get(2, 0) * this.get(2, 0));
double scaleY = Math.sqrt(this.get(0, 1) * this.get(0, 1) + this.get(1, 1) * this.get(1, 1) + this.get(2, 1) * this.get(2, 1));
double scaleZ = Math.sqrt(this.get(0, 2) * this.get(0, 2) + this.get(1, 2) * this.get(1, 2) + this.get(2, 2) * this.get(2, 2));
vector.set(scaleX, scaleY, scaleZ);
}
}