Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- 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);
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement