Advertisement
Guest User

rotation matrix

a guest
Sep 25th, 2013
672
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 7.42 KB | None | 0 0
  1. public class Matrix {
  2.     public final static Matrix IDENTITY=new Matrix(Vec.X_AXIS, Vec.Y_AXIS, Vec.Z_AXIS);
  3.     public final Vec xAxis, yAxis, zAxis;
  4.    
  5.     public Matrix(double ... e) {
  6.         if(e.length!=9) throw new RuntimeException();
  7.         xAxis = new Vec(e[0], e[3], e[6]);
  8.         yAxis = new Vec(e[1], e[4], e[7]);
  9.         zAxis = new Vec(e[2], e[5], e[8]);
  10.     }
  11.    
  12.     public Matrix(Vec xAxis, Vec yAxis, Vec zAxis) {
  13.         this.xAxis = xAxis;
  14.         this.yAxis = yAxis;
  15.         this.zAxis = zAxis;
  16.     }
  17.    
  18.     public Matrix(Vec axis, double theta) {
  19.         Vec u=axis.normalize();
  20.         double sin=Math.sin(theta);
  21.         double cos=Math.cos(theta);
  22.         double uxy=u.x*u.y*(1-cos);
  23.         double uyz=u.y*u.z*(1-cos);
  24.         double uxz=u.x*u.z*(1-cos);
  25.         double ux2=u.x*u.x*(1-cos);
  26.         double uy2=u.y*u.y*(1-cos);
  27.         double uz2=u.z*u.z*(1-cos);
  28.         double uxsin=u.x*sin;
  29.         double uysin=u.y*sin;
  30.         double uzsin=u.z*sin;
  31.        
  32.         xAxis = new Vec(cos+ux2, uxy+uzsin, uxz-uysin);
  33.         yAxis = new Vec(uxy-uzsin, cos+uy2, uyz+uxsin);
  34.         zAxis = new Vec(uxz+uysin, uyz-uxsin, cos+uz2);
  35.     }
  36.    
  37.     static public Matrix xRotationMatrix(double theta) {
  38.         double cos = Math.cos(theta), sin = Math.sin(theta);
  39.         return new Matrix(Vec.X_AXIS,
  40.                           new Vec(0, cos, sin),
  41.                           new Vec(0, -sin, cos));
  42.     }
  43.    
  44.     static public Matrix yRotationMatrix(double theta) {
  45.         double cos = Math.cos(theta), sin = Math.sin(theta);
  46.         return new Matrix(new Vec(cos, 0, -sin),
  47.                           Vec.Y_AXIS,
  48.                           new Vec(sin, 0, cos));
  49.     }
  50.    
  51.     static public Matrix zRotationMatrix(double theta) {
  52.         double cos = Math.cos(theta), sin = Math.sin(theta);
  53.         return new Matrix(new Vec(cos, sin, 0),
  54.                           new Vec(-sin, cos, 0),
  55.                           Vec.Z_AXIS);
  56.     }
  57.    
  58.     public Matrix rotX(double theta) {
  59.         return xRotationMatrix(theta).mul(this);
  60.     }
  61.    
  62.     public Matrix rotY(double theta) {
  63.         return yRotationMatrix(theta).mul(this);
  64.     }
  65.    
  66.     public Matrix rotZ(double theta) {
  67.         return zRotationMatrix(theta).mul(this);
  68.     }
  69.    
  70.     public Matrix mul(double d) {
  71.         return new Matrix(xAxis.mul(d), yAxis.mul(d), zAxis.mul(d));
  72.     }
  73.    
  74.     public Vec mul(Vec v) {
  75.         return xAxis.mul(v.x).add(yAxis.mul(v.y)).add(zAxis.mul(v.z));
  76.     }
  77.    
  78.     private Matrix mul(Matrix m) {
  79.         return new Matrix(mul(m.xAxis), mul(m.yAxis), mul(m.zAxis));
  80.     }
  81.    
  82.     public Matrix rotateRel(Matrix m) {
  83.         return mul(m);
  84.     }
  85.    
  86.     public Matrix rotateAbs(Matrix m) {
  87.         return m.mul(this);
  88.     }
  89.    
  90.     public Matrix normalize() {
  91.         Vec vz=xAxis.crossProduct(yAxis);
  92.         Vec vy=vz.crossProduct(xAxis);        
  93.         return new Matrix(xAxis.normalize(),
  94.                           vy.normalize(),
  95.                           vz.normalize());
  96.     }
  97.    
  98.     public Matrix transpose() {
  99.         return new Matrix(xAxis.x, xAxis.y, xAxis.z,
  100.                           yAxis.x, yAxis.y, yAxis.z,
  101.                           zAxis.x, zAxis.y, zAxis.z);
  102.     }
  103.    
  104.     public double determinant() {
  105.         return xAxis.x*(yAxis.y*zAxis.z-zAxis.y*yAxis.z) -
  106.                yAxis.x*(zAxis.z*xAxis.y-zAxis.y*xAxis.z) +
  107.                zAxis.x*(xAxis.y*yAxis.z-yAxis.y*xAxis.z);
  108.     }
  109.    
  110.     public Matrix inverse() {
  111.         Vec A = yAxis.crossProduct(zAxis);
  112.         Vec B = zAxis.crossProduct(xAxis);
  113.         Vec C = xAxis.crossProduct(yAxis);
  114.         return new Matrix(A,B,C).transpose().mul(1/determinant());
  115.     }
  116.    
  117.     public Matrix oppositeRotMatrix() {
  118.         return transpose();
  119.     }
  120.    
  121.     @Override
  122.     public boolean equals(Object o) {
  123.         if(!(o instanceof Matrix)) return false;
  124.         Matrix m=(Matrix)o;
  125.         return m==this || xAxis.equals(m.xAxis) && yAxis.equals(m.yAxis) && zAxis.equals(m.zAxis);
  126.     }
  127.    
  128.     public String toString() {
  129.         return String.format(java.util.Locale.ENGLISH,
  130.             "[%.3f, %.3f, %.3f]\n[%.3f, %.3f, %.3f]\n[%.3f, %.3f, %.3f]",
  131.             xAxis.x, yAxis.x, zAxis.x,
  132.             xAxis.y, yAxis.y, zAxis.y,
  133.             xAxis.z, yAxis.z, zAxis.z);
  134.     }
  135.    
  136. }
  137.  
  138. public class Vec {
  139.     public final static Vec X_AXIS=new Vec(1,0,0);
  140.     public final static Vec Y_AXIS=new Vec(0,1,0);
  141.     public final static Vec Z_AXIS=new Vec(0,0,1);
  142.     public final static Vec ORIGIN=new Vec(0,0,0);
  143.  
  144.     public final double x,y,z;
  145.    
  146.     public Vec(double x, double y, double z) {
  147.         this.x=x;
  148.         this.y=y;
  149.         this.z=z;
  150.     }
  151.    
  152.     public double dotProduct(Vec v) {
  153.         return x*v.x+y*v.y+z*v.z;
  154.     }
  155.    
  156.     public Vec crossProduct(Vec v) {
  157.         double rx=y*v.z-z*v.y;
  158.         double ry=z*v.x-x*v.z;
  159.         double rz=x*v.y-y*v.x;
  160.         return new Vec(rx, ry, rz);
  161.     }
  162.    
  163.     public Vec newLength(double newLength) {
  164.         double length=getLength();
  165.         if(length==newLength) return this;
  166.         if(length==0) return X_AXIS.newLength(newLength);
  167.         return new Vec(x*newLength/length, y*newLength/length, z*newLength/length);
  168.     }
  169.    
  170.     public Vec rotationAxis() {
  171.         return rotationAxis(X_AXIS);
  172.     }
  173.    
  174.     //The rotation axis to rotate v onto this
  175.     public Vec rotationAxis(Vec v) {
  176.         return normalizedCrossProduct(v);
  177.     }
  178.    
  179.     public Vec normalizedCrossProduct(Vec v) {
  180.         Vec r=crossProduct(v);
  181.         if(r.getLength()<0.0001) {
  182.             r=crossProduct(X_AXIS);
  183.         }
  184.         if(r.getLength()<0.0001) {
  185.             r=crossProduct(Y_AXIS);
  186.         }
  187.         if(r.getLength()<0.0001) {
  188.             return X_AXIS;
  189.         }
  190.         return r.normalize();
  191.     }
  192.    
  193.     public double angle() {
  194.         return angleTo(X_AXIS);
  195.     }
  196.    
  197.     public double angleTo(Vec v) {
  198.         double cosTheta=dotProduct(v)/(getLength()*v.getLength());
  199.        
  200.         return Math.acos(cosTheta);
  201.     }
  202.    
  203.     public double getLength() {
  204.         return Math.sqrt(x*x+y*y+z*z);
  205.     }        
  206.    
  207.    
  208.     public Vec rotate(Matrix m) {
  209.         return m.mul(this);
  210.     }
  211.    
  212.     public Vec normalize() {
  213.         double length=getLength();
  214.        
  215.         if(length==1) return this;
  216.         if(length==0) return X_AXIS;
  217.         return new Vec(x/length, y/length, z/length);
  218.     }
  219.    
  220.     public Vec addX(double a) {
  221.         return new Vec(x+a, y, z);
  222.     }
  223.    
  224.     public Vec addY(double a) {
  225.         return new Vec(x, y+a, z);
  226.     }
  227.    
  228.     public Vec addZ(double a) {
  229.         return new Vec(x, y, z+a);
  230.     }
  231.    
  232.     public Vec add(Vec v) {
  233.         return new Vec(x+v.x, y+v.y, z+v.z);
  234.     }
  235.            
  236.     public Vec sub(Vec v) {
  237.         return new Vec(x-v.x, y-v.y, z-v.z);
  238.     }
  239.    
  240.     public Vec mul(double m) {
  241.         return new Vec(x*m, y*m, z*m);
  242.     }
  243.    
  244.     public Vec div(double d) {
  245.         return new Vec(x/d, y/d, z/d);
  246.     }
  247.    
  248.     public Vec neg() {
  249.         return new Vec(-x, -y, -z);
  250.     }
  251.    
  252.     @Override
  253.     public boolean equals(Object o) {
  254.         if(!(o instanceof Vec)) return false;
  255.         Vec v=(Vec)o;
  256.         return v==this || sub(v).getLength() < 0.0001;
  257.     }
  258.    
  259.     public String toString() {
  260.     return String.format(java.util.Locale.ENGLISH,
  261.         "(%.3f, %.3f, %.3f)", x, y, z);
  262.     }
  263.    
  264. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement