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

   1 /*******************************************************************************\r
   2  * Copyright (c) 2012, 2013 Association for Decentralized Information Management in\r
   3  * Industry THTH ry.\r
   4  * All rights reserved. This program and the accompanying materials\r
   5  * are made available under the terms of the Eclipse Public License v1.0\r
   6  * which accompanies this distribution, and is available at\r
   7  * http://www.eclipse.org/legal/epl-v10.html\r
   8  *\r
   9  * Contributors:\r
  10  *     VTT Technical Research Centre of Finland - initial API and implementation\r
  11  *******************************************************************************/\r
  12 package org.simantics.g3d.tools;\r
  13 \r
  14 import javax.vecmath.Matrix4d;\r
  15 import javax.vecmath.Quat4d;\r
  16 import javax.vecmath.Vector3d;\r
  17 \r
  18 import org.simantics.g3d.math.MathTools;\r
  19 import org.simantics.g3d.scenegraph.IG3DNode;\r
  20 \r
  21 public class NodeTools {\r
  22 \r
  23         public static Vector3d getWorldPosition(IG3DNode node, Vector3d localPosition) {\r
  24                 Vector3d v = new Vector3d(localPosition);\r
  25                 MathTools.rotate(node.getOrientation(), v, v);\r
  26                 v.add(node.getPosition());\r
  27                 \r
  28                 IG3DNode parent = (IG3DNode)node.getParent();\r
  29                 if (parent == null)\r
  30                         return v;\r
  31                 return getWorldPosition(parent,v);\r
  32         }\r
  33         \r
  34         public static Vector3d getWorldPosition(IG3DNode node) {\r
  35                 IG3DNode parent = (IG3DNode)node.getParent();\r
  36                 if (parent == null)\r
  37                         return node.getPosition();\r
  38                 return NodeTools.getWorldPosition(parent,new Vector3d(node.getPosition()));\r
  39         }\r
  40         \r
  41         public static Quat4d getWorldOrientation(IG3DNode node, Quat4d localOrientation) {\r
  42                 Quat4d q = new Quat4d();\r
  43                 q.set(node.getOrientation());\r
  44                 Quat4d q2 = new Quat4d();\r
  45                 q2.set(localOrientation);\r
  46                 q.mul(q2);\r
  47                 \r
  48                 IG3DNode parent = (IG3DNode)node.getParent();\r
  49                 if (parent == null)\r
  50                         return q;\r
  51                 return getWorldOrientation(parent,q);\r
  52         }\r
  53         \r
  54         public static Quat4d getWorldOrientation(IG3DNode node) {\r
  55                 IG3DNode parent = (IG3DNode)node.getParent();\r
  56                 if (parent == null)\r
  57                         return node.getOrientation();\r
  58                 return NodeTools.getWorldOrientation(parent, node.getOrientation());\r
  59         }\r
  60         \r
  61         \r
  62         public static Vector3d getLocalPosition(IG3DNode node, Vector3d worldCoord)  {\r
  63                 \r
  64                 IG3DNode parent = (IG3DNode)node.getParent();\r
  65                 if (parent == null) {// this is a root node ( has no transformation) \r
  66                         return worldCoord;\r
  67                 }\r
  68 \r
  69                 Vector3d local = getLocalPosition(parent,worldCoord);\r
  70                 local.sub(node.getPosition());\r
  71                 \r
  72                 Quat4d q = new Quat4d();\r
  73                 q.set(node.getOrientation());\r
  74                 q.inverse();\r
  75                 MathTools.rotate(q, local, local);\r
  76                 \r
  77                 return local;\r
  78         }\r
  79         \r
  80         public static Quat4d getLocalOrientation(IG3DNode node, Quat4d worldRot)  {\r
  81 \r
  82                 IG3DNode parent = (IG3DNode) node.getParent();\r
  83                 if (parent == null) // this is a rootnode ( has no transformation)\r
  84                         return worldRot;\r
  85                 Quat4d local = getLocalOrientation(parent, worldRot);\r
  86                 Quat4d q = new Quat4d();\r
  87                 q.set(node.getOrientation());\r
  88                 q.inverse();\r
  89                 Quat4d q2 = new Quat4d();\r
  90                 q2.set(local);\r
  91                 q.mul(q2);\r
  92                 local.set(q);\r
  93 \r
  94                 return local;\r
  95         }\r
  96         \r
  97         public static Matrix4d getWorldTransformation(IG3DNode node) {\r
  98                 Vector3d pos = node.getWorldPosition();\r
  99                 Quat4d q = node.getWorldOrientation();\r
 100                 \r
 101                 Matrix4d m1 = new Matrix4d();\r
 102                 Matrix4d m2 = new Matrix4d();\r
 103                 m1.set(pos);\r
 104                 m2.set(q);\r
 105                 m1.mul(m2);\r
 106                 return m1;\r
 107         }\r
 108         \r
 109         public static Matrix4d getWorldOrientationMat(IG3DNode node) {\r
 110                 Quat4d q = node.getWorldOrientation();\r
 111                 \r
 112                 Matrix4d m2 = new Matrix4d();\r
 113                 m2.set(q);\r
 114                 return m2;\r
 115         }\r
 116         \r
 117         public static Vector3d getPosition(IG3DNode relative, IG3DNode node) {\r
 118                 Vector3d wp = getWorldPosition(node);\r
 119                 return getLocalPosition(relative, wp);\r
 120         }\r
 121         \r
 122         public static Quat4d getOrientation(IG3DNode relative, IG3DNode node) {\r
 123                 Quat4d wo = getWorldOrientation(node);\r
 124                 return getLocalOrientation(relative, wo);\r
 125         }\r
 126         \r
 127         public static void setPosition(IG3DNode relative, IG3DNode node, Vector3d position) {\r
 128                 Vector3d wp = getWorldPosition(relative,position);\r
 129                 node.setWorldPosition(wp);\r
 130         }\r
 131         \r
 132         public static void setOrientation(IG3DNode relative, IG3DNode node, Quat4d orientation) {\r
 133                 Quat4d wo = getWorldOrientation(relative,orientation);\r
 134                 node.setWorldOrientation(wo);\r
 135         }\r
 136 }\r