X-Git-Url: https://gerrit.simantics.org/r/gitweb?a=blobdiff_plain;f=org.simantics.g3d%2Fsrc%2Forg%2Fsimantics%2Fg3d%2Ftools%2FNodeTools.java;fp=org.simantics.g3d%2Fsrc%2Forg%2Fsimantics%2Fg3d%2Ftools%2FNodeTools.java;h=233fbacd60199de85c13e430c60e7c268fe2cbbb;hb=87b3241ec277ba3d8e414b26186a032c9cdcaeed;hp=0000000000000000000000000000000000000000;hpb=1f0bcd66274375f2278d2e6c486cb28257a5f7b2;p=simantics%2F3d.git

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