• API
• FAQ
• Tools
• Archive
daily pastebin goal
21%
SHARE
TWEET

# rotation matrix

a guest Sep 25th, 2013 424 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
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) {
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. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.

Top