1 package org.simantics.g3d.tools;
\r
3 import javax.vecmath.Matrix4d;
\r
4 import javax.vecmath.Quat4d;
\r
5 import javax.vecmath.Vector3d;
\r
7 import org.simantics.g3d.math.MathTools;
\r
8 import org.simantics.g3d.scenegraph.IG3DNode;
\r
10 public class NodeTools {
\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
17 IG3DNode parent = (IG3DNode)node.getParent();
\r
20 return getWorldPosition(parent,v);
\r
23 public static Vector3d getWorldPosition(IG3DNode node) {
\r
24 IG3DNode parent = (IG3DNode)node.getParent();
\r
26 return node.getPosition();
\r
27 return NodeTools.getWorldPosition(parent,new Vector3d(node.getPosition()));
\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
37 IG3DNode parent = (IG3DNode)node.getParent();
\r
40 return getWorldOrientation(parent,q);
\r
43 public static Quat4d getWorldOrientation(IG3DNode node) {
\r
44 IG3DNode parent = (IG3DNode)node.getParent();
\r
46 return node.getOrientation();
\r
47 return NodeTools.getWorldOrientation(parent, node.getOrientation());
\r
51 public static Vector3d getLocalPosition(IG3DNode node, Vector3d worldCoord) {
\r
53 IG3DNode parent = (IG3DNode)node.getParent();
\r
54 if (parent == null) {// this is a root node ( has no transformation)
\r
58 Vector3d local = getLocalPosition(parent,worldCoord);
\r
59 local.sub(node.getPosition());
\r
61 Quat4d q = new Quat4d();
\r
62 q.set(node.getOrientation());
\r
64 MathTools.rotate(q, local, local);
\r
69 public static Quat4d getLocalOrientation(IG3DNode node, Quat4d worldRot) {
\r
71 IG3DNode parent = (IG3DNode) node.getParent();
\r
72 if (parent == null) // this is a rootnode ( has no transformation)
\r
74 Quat4d local = getLocalOrientation(parent, worldRot);
\r
75 Quat4d q = new Quat4d();
\r
76 q.set(node.getOrientation());
\r
78 Quat4d q2 = new Quat4d();
\r
86 public static Matrix4d getWorldTransformation(IG3DNode node) {
\r
87 Vector3d pos = node.getWorldPosition();
\r
88 Quat4d q = node.getWorldOrientation();
\r
90 Matrix4d m1 = new Matrix4d();
\r
91 Matrix4d m2 = new Matrix4d();
\r
98 public static Matrix4d getWorldOrientationMat(IG3DNode node) {
\r
99 Quat4d q = node.getWorldOrientation();
\r
101 Matrix4d m2 = new Matrix4d();
\r
106 public static Vector3d getPosition(IG3DNode relative, IG3DNode node) {
\r
107 Vector3d wp = getWorldPosition(node);
\r
108 return getLocalPosition(relative, wp);
\r
111 public static Quat4d getOrientation(IG3DNode relative, IG3DNode node) {
\r
112 Quat4d wo = getWorldOrientation(node);
\r
113 return getLocalOrientation(relative, wo);
\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
121 public static void setOrientation(IG3DNode relative, IG3DNode node, Quat4d orientation) {
\r
122 Quat4d wo = getWorldOrientation(relative,orientation);
\r
123 node.setWorldOrientation(wo);
\r