org.simantics.g3d/src/org/simantics/g3d/tools/NodeTools.java

   1 package org.simantics.g3d.tools;\r
   2 \r
   3 import javax.vecmath.Matrix4d;\r
   4 import javax.vecmath.Quat4d;\r
   5 import javax.vecmath.Vector3d;\r
   6 \r
   7 import org.simantics.g3d.math.MathTools;\r
   8 import org.simantics.g3d.scenegraph.IG3DNode;\r
   9 \r
  10 public class NodeTools {\r
  11 \r
  12         public static Vector3d getWorldPosition(IG3DNode node, Vector3d localPosition) {\r
  13                 Vector3d v = new Vector3d(localPosition);\r
  14                 MathTools.rotate(node.getOrientation(), v, v);\r
  15                 v.add(node.getPosition());\r
  16                 \r
  17                 IG3DNode parent = (IG3DNode)node.getParent();\r
  18                 if (parent == null)\r
  19                         return v;\r
  20                 return getWorldPosition(parent,v);\r
  21         }\r
  22         \r
  23         public static Vector3d getWorldPosition(IG3DNode node) {\r
  24                 IG3DNode parent = (IG3DNode)node.getParent();\r
  25                 if (parent == null)\r
  26                         return node.getPosition();\r
  27                 return NodeTools.getWorldPosition(parent,new Vector3d(node.getPosition()));\r
  28         }\r
  29         \r
  30         public static Quat4d getWorldOrientation(IG3DNode node, Quat4d localOrientation) {\r
  31                 Quat4d q = new Quat4d();\r
  32                 q.set(node.getOrientation());\r
  33                 Quat4d q2 = new Quat4d();\r
  34                 q2.set(localOrientation);\r
  35                 q.mul(q2);\r
  36                 \r
  37                 IG3DNode parent = (IG3DNode)node.getParent();\r
  38                 if (parent == null)\r
  39                         return q;\r
  40                 return getWorldOrientation(parent,q);\r
  41         }\r
  42         \r
  43         public static Quat4d getWorldOrientation(IG3DNode node) {\r
  44                 IG3DNode parent = (IG3DNode)node.getParent();\r
  45                 if (parent == null)\r
  46                         return node.getOrientation();\r
  47                 return NodeTools.getWorldOrientation(parent, node.getOrientation());\r
  48         }\r
  49         \r
  50         \r
  51         public static Vector3d getLocalPosition(IG3DNode node, Vector3d worldCoord)  {\r
  52         \r
  53                 IG3DNode parent = (IG3DNode)node.getParent();\r
  54         if (parent == null) {// this is a root node ( has no transformation) \r
  55                 return worldCoord;\r
  56         }\r
  57 \r
  58         Vector3d local = getLocalPosition(parent,worldCoord);\r
  59         local.sub(node.getPosition());\r
  60         \r
  61         Quat4d q = new Quat4d();\r
  62         q.set(node.getOrientation());\r
  63         q.inverse();\r
  64         MathTools.rotate(q, local, local);\r
  65         \r
  66         return local;\r
  67     }\r
  68         \r
  69         public static Quat4d getLocalOrientation(IG3DNode node, Quat4d worldRot)  {\r
  70 \r
  71                 IG3DNode parent = (IG3DNode) node.getParent();\r
  72                 if (parent == null) // this is a rootnode ( has no transformation)\r
  73                         return worldRot;\r
  74                 Quat4d local = getLocalOrientation(parent, worldRot);\r
  75                 Quat4d q = new Quat4d();\r
  76                 q.set(node.getOrientation());\r
  77                 q.inverse();\r
  78                 Quat4d q2 = new Quat4d();\r
  79                 q2.set(local);\r
  80                 q.mul(q2);\r
  81                 local.set(q);\r
  82 \r
  83                 return local;\r
  84         }\r
  85         \r
  86         public static Matrix4d getWorldTransformation(IG3DNode node) {\r
  87                 Vector3d pos = node.getWorldPosition();\r
  88                 Quat4d q = node.getWorldOrientation();\r
  89                 \r
  90                 Matrix4d m1 = new Matrix4d();\r
  91                 Matrix4d m2 = new Matrix4d();\r
  92                 m1.set(pos);\r
  93                 m2.set(q);\r
  94                 m1.mul(m2);\r
  95                 return m1;\r
  96         }\r
  97         \r
  98         public static Matrix4d getWorldOrientationMat(IG3DNode node) {\r
  99                 Quat4d q = node.getWorldOrientation();\r
 100                 \r
 101                 Matrix4d m2 = new Matrix4d();\r
 102                 m2.set(q);\r
 103                 return m2;\r
 104         }\r
 105         \r
 106         public static Vector3d getPosition(IG3DNode relative, IG3DNode node) {\r
 107                 Vector3d wp = getWorldPosition(node);\r
 108                 return getLocalPosition(relative, wp);\r
 109         }\r
 110         \r
 111         public static Quat4d getOrientation(IG3DNode relative, IG3DNode node) {\r
 112                 Quat4d wo = getWorldOrientation(node);\r
 113                 return getLocalOrientation(relative, wo);\r
 114         }\r
 115         \r
 116         public static void setPosition(IG3DNode relative, IG3DNode node, Vector3d position) {\r
 117                 Vector3d wp = getWorldPosition(relative,position);\r
 118                 node.setWorldPosition(wp);\r
 119         }\r
 120         \r
 121         public static void setOrientation(IG3DNode relative, IG3DNode node, Quat4d orientation) {\r
 122                 Quat4d wo = getWorldOrientation(relative,orientation);\r
 123                 node.setWorldOrientation(wo);\r
 124         }\r
 125 }\r