Changeset 78 for java/main/src/main/java/com/framsticks/util/Orientation.java

Timestamp:

01/09/13 00:09:10 (11 years ago)

Author:

psniegowski

Message:

Add f0 parsing and f0->Model transformation.

File:

• java/main/src/main/java/com/framsticks/util/Orientation.java (modified) (1 diff)

java/main/src/main/java/com/framsticks/util/Orientation.java

@@ -9 +9 @@
     public final Point3d z;
 
-    public Orientation() {
-        x = new Point3d();
-        y = new Point3d();
-        z = new Point3d();
-    }
+        public Orientation(Point3d x, Point3d y, Point3d z) {
+                this.x = x;
+                this.y = y;
+                this.z = z;
+        }
+
+        public Orientation() {
+                x = new Point3d(1, 0, 0);
+                y = new Point3d(0, 1, 0);
+                z = new Point3d(0, 0, 1);
+        }
+
+        /** based on c++ void Orient::transform(Pt3D& target,const Pt3D &s) const */
+        public Point3d transform(Point3d source) {
+                Point3d.Builder b = new Point3d.Builder();
+                for (int i = 0; i < 3; ++i) {
+                        double v = 0;
+                        for (int j = 0; j < 3; ++j) {
+                                v += source.get(j) * this.get(j).get(i);
+                        }
+                        b.set(i, v);
+                }
+                return b.build();
+        }
+
+        /** based on c++ void Orient::transform(Orient& target,const Orient& src) const */
+        public Orientation transform(Orientation source) {
+                return new Orientation(transform(source.x), transform(source.y), transform(source.z));
+        }
+
+        protected static Point3d rotate2D(double sin, double cos, Point3d p, int a0, int a1, int im) {
+                Point3d.Builder b = new Point3d.Builder();
+                b.set(a0, cos * p.get(a0) - sin * p.get(a1));
+                b.set(a1, sin * p.get(a0) + cos * p.get(a1));
+                b.set(im, p.get(im));
+                return b.build();
+        }
+
+        /** based on c++ Orient::rotate(const Pt3D &v) */
+        public Orientation rotate(Point3d v) {
+                Point3d[] p = new Point3d[] {x, y, z};
+
+                for (int i = 0; i < 3; ++i) {
+                        double sin = Math.sin(v.get(i));
+                        double cos = Math.cos(v.get(i));
+
+                        for (int j = 0; j < 3; ++j) {
+                                p[j] = rotate2D(sin, cos, p[j], i == 0 ? 1 : 0, i == 2 ? 1 : 2, i);
+                        }
+                }
+                return new Orientation(p[0], p[1], p[2]);
+        }
+
+        final Point3d get(int i) {
+                switch (i) {
+                        case 0: return x;
+                        case 1: return y;
+                        case 2: return z;
+                }
+                assert false;
+                return null;
+        }
 
 
+        @Override
+        public final String toString() {
+                return x + " | " + y + " | " + z;
+        }
 }