[77] | 1 | package com.framsticks.util; |
---|
| 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 | |
---|
[78] | 11 | public Orientation(Point3d x, Point3d y, Point3d z) { |
---|
| 12 | this.x = x; |
---|
| 13 | this.y = y; |
---|
| 14 | this.z = z; |
---|
| 15 | } |
---|
[77] | 16 | |
---|
[78] | 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 | } |
---|
[77] | 22 | |
---|
[78] | 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 | } |
---|
[77] | 79 | } |
---|