/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
*/
package net.anzix.fsz.voxelworld;
import com.ardor3d.math.MathUtils;
import com.ardor3d.math.Matrix4;
import com.ardor3d.math.Quaternion;
import com.ardor3d.math.Vector3;
import com.ardor3d.math.type.ReadOnlyMatrix4;
import com.ardor3d.math.type.ReadOnlyVector3;
/**
* @author kovacsandras
*/
public class FszMatrix4 extends Matrix4 {
public FszMatrix4() {
set(IDENTITY);
}
public FszMatrix4(final ReadOnlyMatrix4 source) {
set(source);
}
public Vector3 applyPost(final ReadOnlyVector3 vector, Vector3 store) {
if (store == null) {
store = new Vector3();
}
final double x = vector.getX();
final double y = vector.getY();
final double z = vector.getZ();
store.setX(getM00() * x + getM01() * y + getM02() * z + getM03());
store.setY(getM10() * x + getM11() * y + getM12() * z + getM13());
store.setZ(getM20() * x + getM21() * y + getM22() * z + getM23());
return store;
}
public void setRotation(double x, double y, double z) {
Quaternion rotation = new Quaternion();
rotation.fromEulerAngles(x, y, z);
set(rotation);
}
public void setTranslation(double x, double y, double z) {
setM03(x);
setM13(y);
setM23(z);
setM33(1.0);
}
public void setTranslation(final ReadOnlyVector3 v) {
setM03(v.getX());
setM13(v.getY());
setM23(v.getZ());
setM33(1.0);
}
public Vector3 getTranslation() {
return new Vector3(getM03(), getM13(), getM23());
}
public void addTranslation(double x, double y, double z) {
setM03(getM03() + x);
setM13(getM13() + y);
setM23(getM23() + z);
}
public void addTranslation(final ReadOnlyVector3 v) {
setM03(getM03() + v.getX());
setM13(getM13() + v.getY());
setM23(getM23() + v.getZ());
}
public void createFromPointAndNormal(final ReadOnlyVector3 point, final ReadOnlyVector3 normal) {
setM03(point.getX());
setM13(point.getY());
setM23(point.getZ());
double x = normal.getX(); //normal must be normalized
double y = normal.getY();
double z = normal.getZ();
boolean Z_IS_BIG = z > 0.8;
double x2 = Z_IS_BIG ? 0.0 : y;
double y2 = Z_IS_BIG ? z : -x;
double z2 = Z_IS_BIG ? -y : 0.0;
double l2 = x2 * x2 + y2 * y2 + z2 * z2;
double f2 = MathUtils.inverseSqrt(l2);
x2 *= f2;
y2 *= f2;
z2 *= f2;
double x3 = (y * z2) - (z * y2);
double y3 = (z * x2) - (x * z2);
double z3 = (x * y2) - (y * x2);
double l3 = x3 * x3 + y3 * y3 + z3 * z3;
double f3 = MathUtils.inverseSqrt(l3);
x3 *= f3;
y3 *= f3;
z3 *= f3;
setM00(x2);
setM10(y2);
setM20(z2);
setM01(x);
setM11(y);
setM21(z);
setM02(x3);
setM12(y3);
setM22(z3);
}
}