jmekaelthas
10 years ago
parent
e2d8fe8293
commit
f7a4fe9f25
@ -0,0 +1,214 @@ |
||||
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); |
||||
} |
||||
} |
||||
Loading…
Reference in new issue