3D framework (Simca 2012)

[simantics/3d.git] / org.simantics.g3d / src / org / simantics / g3d / tools / NodeTools.java

diff --git a/org.simantics.g3d/src/org/simantics/g3d/tools/NodeTools.java b/org.simantics.g3d/src/org/simantics/g3d/tools/NodeTools.java
new file mode 100644 (file)
index 0000000..233fbac
--- /dev/null
+++ b/org.simantics.g3d/src/org/simantics/g3d/tools/NodeTools.java
@@ -0,0 +1,125 @@
+package org.simantics.g3d.tools;\r
+\r
+import javax.vecmath.Matrix4d;\r
+import javax.vecmath.Quat4d;\r
+import javax.vecmath.Vector3d;\r
+\r
+import org.simantics.g3d.math.MathTools;\r
+import org.simantics.g3d.scenegraph.IG3DNode;\r
+\r
+public class NodeTools {\r
+\r
+       public static Vector3d getWorldPosition(IG3DNode node, Vector3d localPosition) {\r
+               Vector3d v = new Vector3d(localPosition);\r
+               MathTools.rotate(node.getOrientation(), v, v);\r
+               v.add(node.getPosition());\r
+               \r
+               IG3DNode parent = (IG3DNode)node.getParent();\r
+               if (parent == null)\r
+                       return v;\r
+               return getWorldPosition(parent,v);\r
+       }\r
+       \r
+       public static Vector3d getWorldPosition(IG3DNode node) {\r
+               IG3DNode parent = (IG3DNode)node.getParent();\r
+               if (parent == null)\r
+                       return node.getPosition();\r
+               return NodeTools.getWorldPosition(parent,new Vector3d(node.getPosition()));\r
+       }\r
+       \r
+       public static Quat4d getWorldOrientation(IG3DNode node, Quat4d localOrientation) {\r
+               Quat4d q = new Quat4d();\r
+               q.set(node.getOrientation());\r
+               Quat4d q2 = new Quat4d();\r
+               q2.set(localOrientation);\r
+               q.mul(q2);\r
+               \r
+               IG3DNode parent = (IG3DNode)node.getParent();\r
+               if (parent == null)\r
+                       return q;\r
+               return getWorldOrientation(parent,q);\r
+       }\r
+       \r
+       public static Quat4d getWorldOrientation(IG3DNode node) {\r
+               IG3DNode parent = (IG3DNode)node.getParent();\r
+               if (parent == null)\r
+                       return node.getOrientation();\r
+               return NodeTools.getWorldOrientation(parent, node.getOrientation());\r
+       }\r
+       \r
+       \r
+       public static Vector3d getLocalPosition(IG3DNode node, Vector3d worldCoord)  {\r
+       \r
+               IG3DNode parent = (IG3DNode)node.getParent();\r
+        if (parent == null) {// this is a root node ( has no transformation) \r
+               return worldCoord;\r
+        }\r
+\r
+        Vector3d local = getLocalPosition(parent,worldCoord);\r
+        local.sub(node.getPosition());\r
+        \r
+        Quat4d q = new Quat4d();\r
+        q.set(node.getOrientation());\r
+        q.inverse();\r
+        MathTools.rotate(q, local, local);\r
+        \r
+        return local;\r
+    }\r
+       \r
+       public static Quat4d getLocalOrientation(IG3DNode node, Quat4d worldRot)  {\r
+\r
+               IG3DNode parent = (IG3DNode) node.getParent();\r
+               if (parent == null) // this is a rootnode ( has no transformation)\r
+                       return worldRot;\r
+               Quat4d local = getLocalOrientation(parent, worldRot);\r
+               Quat4d q = new Quat4d();\r
+               q.set(node.getOrientation());\r
+               q.inverse();\r
+               Quat4d q2 = new Quat4d();\r
+               q2.set(local);\r
+               q.mul(q2);\r
+               local.set(q);\r
+\r
+               return local;\r
+       }\r
+       \r
+       public static Matrix4d getWorldTransformation(IG3DNode node) {\r
+               Vector3d pos = node.getWorldPosition();\r
+               Quat4d q = node.getWorldOrientation();\r
+               \r
+               Matrix4d m1 = new Matrix4d();\r
+               Matrix4d m2 = new Matrix4d();\r
+               m1.set(pos);\r
+               m2.set(q);\r
+               m1.mul(m2);\r
+               return m1;\r
+       }\r
+       \r
+       public static Matrix4d getWorldOrientationMat(IG3DNode node) {\r
+               Quat4d q = node.getWorldOrientation();\r
+               \r
+               Matrix4d m2 = new Matrix4d();\r
+               m2.set(q);\r
+               return m2;\r
+       }\r
+       \r
+       public static Vector3d getPosition(IG3DNode relative, IG3DNode node) {\r
+               Vector3d wp = getWorldPosition(node);\r
+               return getLocalPosition(relative, wp);\r
+       }\r
+       \r
+       public static Quat4d getOrientation(IG3DNode relative, IG3DNode node) {\r
+               Quat4d wo = getWorldOrientation(node);\r
+               return getLocalOrientation(relative, wo);\r
+       }\r
+       \r
+       public static void setPosition(IG3DNode relative, IG3DNode node, Vector3d position) {\r
+               Vector3d wp = getWorldPosition(relative,position);\r
+               node.setWorldPosition(wp);\r
+       }\r
+       \r
+       public static void setOrientation(IG3DNode relative, IG3DNode node, Quat4d orientation) {\r
+               Quat4d wo = getWorldOrientation(relative,orientation);\r
+               node.setWorldOrientation(wo);\r
+       }\r
+}\r