1 | package com.framsticks.util.math; |
---|
2 | |
---|
3 | /** |
---|
4 | * @author Piotr Sniegowski |
---|
5 | */ |
---|
6 | public class Orientation { |
---|
7 | public final Point3d x; |
---|
8 | public final Point3d y; |
---|
9 | public final Point3d z; |
---|
10 | |
---|
11 | public Orientation(Point3d x, Point3d y, Point3d z) { |
---|
12 | this.x = x; |
---|
13 | this.y = y; |
---|
14 | this.z = z; |
---|
15 | } |
---|
16 | |
---|
17 | public Orientation() { |
---|
18 | x = new Point3d(1, 0, 0); |
---|
19 | y = new Point3d(0, 1, 0); |
---|
20 | z = new Point3d(0, 0, 1); |
---|
21 | } |
---|
22 | |
---|
23 | /** based on c++ void Orient::transform(Pt3D& target,const Pt3D &s) const */ |
---|
24 | public Point3d transform(Point3d source) { |
---|
25 | Point3d.Builder b = new Point3d.Builder(); |
---|
26 | for (int i = 0; i < 3; ++i) { |
---|
27 | double v = 0; |
---|
28 | for (int j = 0; j < 3; ++j) { |
---|
29 | v += source.get(j) * this.get(j).get(i); |
---|
30 | } |
---|
31 | b.set(i, v); |
---|
32 | } |
---|
33 | return b.build(); |
---|
34 | } |
---|
35 | |
---|
36 | /** based on c++ void Orient::transform(Orient& target,const Orient& src) const */ |
---|
37 | public Orientation transform(Orientation source) { |
---|
38 | return new Orientation(transform(source.x), transform(source.y), transform(source.z)); |
---|
39 | } |
---|
40 | |
---|
41 | protected static Point3d rotate2D(double sin, double cos, Point3d p, int a0, int a1, int im) { |
---|
42 | Point3d.Builder b = new Point3d.Builder(); |
---|
43 | b.set(a0, cos * p.get(a0) - sin * p.get(a1)); |
---|
44 | b.set(a1, sin * p.get(a0) + cos * p.get(a1)); |
---|
45 | b.set(im, p.get(im)); |
---|
46 | return b.build(); |
---|
47 | } |
---|
48 | |
---|
49 | /** based on c++ Orient::rotate(const Pt3D &v) */ |
---|
50 | public Orientation rotate(Point3d v) { |
---|
51 | Point3d[] p = new Point3d[] {x, y, z}; |
---|
52 | |
---|
53 | for (int i = 0; i < 3; ++i) { |
---|
54 | double sin = Math.sin(v.get(i)); |
---|
55 | double cos = Math.cos(v.get(i)); |
---|
56 | |
---|
57 | for (int j = 0; j < 3; ++j) { |
---|
58 | p[j] = rotate2D(sin, cos, p[j], i == 0 ? 1 : 0, i == 2 ? 1 : 2, i); |
---|
59 | } |
---|
60 | } |
---|
61 | return new Orientation(p[0], p[1], p[2]); |
---|
62 | } |
---|
63 | |
---|
64 | final Point3d get(int i) { |
---|
65 | switch (i) { |
---|
66 | case 0: return x; |
---|
67 | case 1: return y; |
---|
68 | case 2: return z; |
---|
69 | } |
---|
70 | assert false; |
---|
71 | return null; |
---|
72 | } |
---|
73 | |
---|
74 | |
---|
75 | @Override |
---|
76 | public final String toString() { |
---|
77 | return x + " | " + y + " | " + z; |
---|
78 | } |
---|
79 | } |
---|