source: java/main/src/main/java/com/framsticks/util/math/Orientation.java @ 193

Last change on this file since 193 was 193, checked in by Maciej Komosinski, 10 years ago

Set svn:eol-style native for all textual files

  • Property svn:eol-style set to native
File size: 1.8 KB
Line 
1package com.framsticks.util.math;
2
3/**
4 * @author Piotr Sniegowski
5 */
6public 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}
Note: See TracBrowser for help on using the repository browser.