/*******************************************************************************
 * Copyright (c) 2012, 2013 Association for Decentralized Information Management in
 * Industry THTH ry.
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the Eclipse Public License v1.0
 * which accompanies this distribution, and is available at
 * http://www.eclipse.org/legal/epl-v10.html
 *
 * Contributors:
 *     VTT Technical Research Centre of Finland - initial API and implementation
 *******************************************************************************/
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);
	}
}