public class Matrix {
public final static Matrix IDENTITY=new Matrix(Vec.X_AXIS, Vec.Y_AXIS, Vec.Z_AXIS);
public final Vec xAxis, yAxis, zAxis;
public Matrix(double ... e) {
if(e.length!=9) throw new RuntimeException();
xAxis = new Vec(e[0], e[3], e[6]);
yAxis = new Vec(e[1], e[4], e[7]);
zAxis = new Vec(e[2], e[5], e[8]);
}
public Matrix(Vec xAxis, Vec yAxis, Vec zAxis) {
this.xAxis = xAxis;
this.yAxis = yAxis;
this.zAxis = zAxis;
}
public Matrix(Vec axis, double theta) {
Vec u=axis.normalize();
double sin=Math.sin(theta);
double cos=Math.cos(theta);
double uxy=u.x*u.y*(1-cos);
double uyz=u.y*u.z*(1-cos);
double uxz=u.x*u.z*(1-cos);
double ux2=u.x*u.x*(1-cos);
double uy2=u.y*u.y*(1-cos);
double uz2=u.z*u.z*(1-cos);
double uxsin=u.x*sin;
double uysin=u.y*sin;
double uzsin=u.z*sin;
xAxis = new Vec(cos+ux2, uxy+uzsin, uxz-uysin);
yAxis = new Vec(uxy-uzsin, cos+uy2, uyz+uxsin);
zAxis = new Vec(uxz+uysin, uyz-uxsin, cos+uz2);
}
static public Matrix xRotationMatrix(double theta) {
double cos = Math.cos(theta), sin = Math.sin(theta);
return new Matrix(Vec.X_AXIS,
new Vec(0, cos, sin),
new Vec(0, -sin, cos));
}
static public Matrix yRotationMatrix(double theta) {
double cos = Math.cos(theta), sin = Math.sin(theta);
return new Matrix(new Vec(cos, 0, -sin),
Vec.Y_AXIS,
new Vec(sin, 0, cos));
}
static public Matrix zRotationMatrix(double theta) {
double cos = Math.cos(theta), sin = Math.sin(theta);
return new Matrix(new Vec(cos, sin, 0),
new Vec(-sin, cos, 0),
Vec.Z_AXIS);
}
public Matrix rotX(double theta) {
return xRotationMatrix(theta).mul(this);
}
public Matrix rotY(double theta) {
return yRotationMatrix(theta).mul(this);
}
public Matrix rotZ(double theta) {
return zRotationMatrix(theta).mul(this);
}
public Matrix mul(double d) {
return new Matrix(xAxis.mul(d), yAxis.mul(d), zAxis.mul(d));
}
public Vec mul(Vec v) {
return xAxis.mul(v.x).add(yAxis.mul(v.y)).add(zAxis.mul(v.z));
}
private Matrix mul(Matrix m) {
return new Matrix(mul(m.xAxis), mul(m.yAxis), mul(m.zAxis));
}
public Matrix rotateRel(Matrix m) {
return mul(m);
}
public Matrix rotateAbs(Matrix m) {
return m.mul(this);
}
public Matrix normalize() {
Vec vz=xAxis.crossProduct(yAxis);
Vec vy=vz.crossProduct(xAxis);
return new Matrix(xAxis.normalize(),
vy.normalize(),
vz.normalize());
}
public Matrix transpose() {
return new Matrix(xAxis.x, xAxis.y, xAxis.z,
yAxis.x, yAxis.y, yAxis.z,
zAxis.x, zAxis.y, zAxis.z);
}
public double determinant() {
return xAxis.x*(yAxis.y*zAxis.z-zAxis.y*yAxis.z) -
yAxis.x*(zAxis.z*xAxis.y-zAxis.y*xAxis.z) +
zAxis.x*(xAxis.y*yAxis.z-yAxis.y*xAxis.z);
}
public Matrix inverse() {
Vec A = yAxis.crossProduct(zAxis);
Vec B = zAxis.crossProduct(xAxis);
Vec C = xAxis.crossProduct(yAxis);
return new Matrix(A,B,C).transpose().mul(1/determinant());
}
public Matrix oppositeRotMatrix() {
return transpose();
}
@Override
public boolean equals(Object o) {
if(!(o instanceof Matrix)) return false;
Matrix m=(Matrix)o;
return m==this || xAxis.equals(m.xAxis) && yAxis.equals(m.yAxis) && zAxis.equals(m.zAxis);
}
public String toString() {
return String.format(java.util.Locale.ENGLISH,
"[%.3f, %.3f, %.3f]\n[%.3f, %.3f, %.3f]\n[%.3f, %.3f, %.3f]",
xAxis.x, yAxis.x, zAxis.x,
xAxis.y, yAxis.y, zAxis.y,
xAxis.z, yAxis.z, zAxis.z);
}
}
public class Vec {
public final static Vec X_AXIS=new Vec(1,0,0);
public final static Vec Y_AXIS=new Vec(0,1,0);
public final static Vec Z_AXIS=new Vec(0,0,1);
public final static Vec ORIGIN=new Vec(0,0,0);
public final double x,y,z;
public Vec(double x, double y, double z) {
this.x=x;
this.y=y;
this.z=z;
}
public double dotProduct(Vec v) {
return x*v.x+y*v.y+z*v.z;
}
public Vec crossProduct(Vec v) {
double rx=y*v.z-z*v.y;
double ry=z*v.x-x*v.z;
double rz=x*v.y-y*v.x;
return new Vec(rx, ry, rz);
}
public Vec newLength(double newLength) {
double length=getLength();
if(length==newLength) return this;
if(length==0) return X_AXIS.newLength(newLength);
return new Vec(x*newLength/length, y*newLength/length, z*newLength/length);
}
public Vec rotationAxis() {
return rotationAxis(X_AXIS);
}
//The rotation axis to rotate v onto this
public Vec rotationAxis(Vec v) {
return normalizedCrossProduct(v);
}
public Vec normalizedCrossProduct(Vec v) {
Vec r=crossProduct(v);
if(r.getLength()<0.0001) {
r=crossProduct(X_AXIS);
}
if(r.getLength()<0.0001) {
r=crossProduct(Y_AXIS);
}
if(r.getLength()<0.0001) {
return X_AXIS;
}
return r.normalize();
}
public double angle() {
return angleTo(X_AXIS);
}
public double angleTo(Vec v) {
double cosTheta=dotProduct(v)/(getLength()*v.getLength());
return Math.acos(cosTheta);
}
public double getLength() {
return Math.sqrt(x*x+y*y+z*z);
}
public Vec rotate(Matrix m) {
return m.mul(this);
}
public Vec normalize() {
double length=getLength();
if(length==1) return this;
if(length==0) return X_AXIS;
return new Vec(x/length, y/length, z/length);
}
public Vec addX(double a) {
return new Vec(x+a, y, z);
}
public Vec addY(double a) {
return new Vec(x, y+a, z);
}
public Vec addZ(double a) {
return new Vec(x, y, z+a);
}
public Vec add(Vec v) {
return new Vec(x+v.x, y+v.y, z+v.z);
}
public Vec sub(Vec v) {
return new Vec(x-v.x, y-v.y, z-v.z);
}
public Vec mul(double m) {
return new Vec(x*m, y*m, z*m);
}
public Vec div(double d) {
return new Vec(x/d, y/d, z/d);
}
public Vec neg() {
return new Vec(-x, -y, -z);
}
@Override
public boolean equals(Object o) {
if(!(o instanceof Vec)) return false;
Vec v=(Vec)o;
return v==this || sub(v).getLength() < 0.0001;
}
public String toString() {
return String.format(java.util.Locale.ENGLISH,
"(%.3f, %.3f, %.3f)", x, y, z);
}
}