bundles/common/org.eclipse.apogy.common.topology/src-gen-custom/org/eclipse/apogy/common/topology/impl/ApogyCommonTopologyFacadeCustomImpl.java - gerrit/apogy/apogy - Git at Google

 /*******************************************************************************
  * Copyright (c) 2018 Agence spatiale canadienne / Canadian Space Agency
  * 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:
  *     Pierre Allard,
  *     Regent L'Archeveque,
  *     Sebastien Gemme  - initial API and implementation
  *
  * SPDX-License-Identifier: EPL-1.0
  *
  *******************************************************************************/
 package org.eclipse.apogy.common.topology.impl;

 import java.util.ArrayDeque;
 import java.util.ArrayList;
 import java.util.Collection;
 import java.util.HashSet;
 import java.util.Iterator;
 import java.util.List;
 import java.util.Queue;
 import java.util.Set;
 import java.util.Stack;

 import javax.vecmath.Matrix3d;
 import javax.vecmath.Matrix4d;
 import javax.vecmath.Vector3d;

 import org.eclipse.apogy.common.math.ApogyCommonMathFacade;
 import org.eclipse.apogy.common.math.ApogyCommonMathFactory;
 import org.eclipse.apogy.common.math.GeometricUtils;
 import org.eclipse.apogy.common.math.Matrix3x3;
 import org.eclipse.apogy.common.math.Tuple3d;
 import org.eclipse.apogy.common.topology.AbstractNodeVisitor;
 import org.eclipse.apogy.common.topology.AggregateContentNode;
 import org.eclipse.apogy.common.topology.ApogyCommonTopologyFacade;
 import org.eclipse.apogy.common.topology.ApogyCommonTopologyFactory;
 import org.eclipse.apogy.common.topology.AttachedViewPoint;
 import org.eclipse.apogy.common.topology.ContentNode;
 import org.eclipse.apogy.common.topology.GroupNode;
 import org.eclipse.apogy.common.topology.INodeVisitor;
 import org.eclipse.apogy.common.topology.Link;
 import org.eclipse.apogy.common.topology.Node;
 import org.eclipse.apogy.common.topology.NodeFilter;
 import org.eclipse.apogy.common.topology.NodePath;
 import org.eclipse.apogy.common.topology.PickAndPlaceNode;
 import org.eclipse.apogy.common.topology.PositionNode;
 import org.eclipse.apogy.common.topology.ReferencedContentNode;
 import org.eclipse.apogy.common.topology.RotationNode;
 import org.eclipse.apogy.common.topology.TransformNode;
 import org.eclipse.emf.common.util.BasicEList;
 import org.eclipse.emf.common.util.EList;
 import org.eclipse.emf.ecore.EClass;

 public class ApogyCommonTopologyFacadeCustomImpl extends ApogyCommonTopologyFacadeImpl {

 	private static final String NULL_NODE_ID_STRING = "NULL";

 	public <T> ContentNode<T> createContentNode(T content) {
 		return createAggregateContentNode(content);
 	}

 	@Override
 	public PositionNode createPositionNode(double x, double y, double z) {
 		PositionNode node = ApogyCommonTopologyFactory.eINSTANCE.createPositionNode();

 		Tuple3d position = ApogyCommonMathFactory.eINSTANCE.createTuple3d();

 		node.setPosition(position);

 		node.getPosition().setX(x);
 		node.getPosition().setY(y);
 		node.getPosition().setZ(z);

 		return node;
 	}

 	@Override
 	public RotationNode createRotationNodeXYZ(double x, double y, double z) {

 		RotationNode node = ApogyCommonTopologyFactory.eINSTANCE.createRotationNode();

 		Matrix3d rotationMatrix = GeometricUtils.packXYZ(x, y, z);

 		Matrix3x3 matrix = ApogyCommonMathFacade.INSTANCE.createMatrix3x3(rotationMatrix);

 		node.setRotationMatrix(matrix);

 		return node;

 	}

 	@Override
 	public Matrix4d expressRootInNodeFrame(Node node) {

 		Matrix4d mat = expressNodeInRootFrame(node);
 		mat.invert();

 		return mat;

 	}

 	public Matrix4d expressInFrame(Node sourceFrame, Node targetFrame) {
 		Matrix4d sourceFrameToRoot = expressNodeInRootFrame(sourceFrame);
 		Matrix4d rootToTargetFrame = expressNodeInRootFrame(targetFrame);
 		rootToTargetFrame.invert();
 		rootToTargetFrame.mul(sourceFrameToRoot);

 		return rootToTargetFrame;
 	}

 	@Override
 	public RotationNode createRotationNodeYZX(double x, double y, double z) {
 		RotationNode node = ApogyCommonTopologyFactory.eINSTANCE.createRotationNode();

 		Matrix3d rotationMatrix = GeometricUtils.packYZX(x, y, z);

 		Matrix3x3 matrix = ApogyCommonMathFacade.INSTANCE.createMatrix3x3(rotationMatrix);

 		node.setRotationMatrix(matrix);

 		return node;
 	}

 	@Override
 	public RotationNode createRotationNodeZYX(double x, double y, double z) {
 		RotationNode node = ApogyCommonTopologyFactory.eINSTANCE.createRotationNode();

 		Matrix3d rotationMatrix = GeometricUtils.packZYX(x, y, z);

 		Matrix3x3 matrix = ApogyCommonMathFacade.INSTANCE.createMatrix3x3(rotationMatrix);

 		node.setRotationMatrix(matrix);

 		return node;
 	}

 	public TransformNode createTransformNodeXYZ(double tx, double ty, double tz, double rx, double ry, double rz) {

 		RotationNode rotationNode = createRotationNodeXYZ(rx, ry, rz);

 		Tuple3d translation = ApogyCommonMathFactory.eINSTANCE.createTuple3d();
 		translation.setX(tx);
 		translation.setY(ty);
 		translation.setZ(tz);

 		TransformNode trNode = ApogyCommonTopologyFactory.eINSTANCE.createTransformNode();
 		trNode.setRotationMatrix(rotationNode.getRotationMatrix());
 		trNode.setPosition(translation);

 		return trNode;

 	}

 	public TransformNode createTransformNodeYZX(double tx, double ty, double tz, double rx, double ry, double rz) {
 		RotationNode rotationNode = createRotationNodeYZX(rx, ry, rz);

 		Tuple3d translation = ApogyCommonMathFactory.eINSTANCE.createTuple3d();
 		translation.setX(tx);
 		translation.setY(ty);
 		translation.setZ(tz);

 		TransformNode trNode = ApogyCommonTopologyFactory.eINSTANCE.createTransformNode();
 		trNode.setRotationMatrix(rotationNode.getRotationMatrix());
 		trNode.setPosition(translation);

 		return trNode;
 	}

 	public TransformNode createTransformNodeZYX(double tx, double ty, double tz, double rx, double ry, double rz) {
 		RotationNode rotationNode = createRotationNodeZYX(rx, ry, rz);

 		Tuple3d translation = ApogyCommonMathFactory.eINSTANCE.createTuple3d();
 		translation.setX(tx);
 		translation.setY(ty);
 		translation.setZ(tz);

 		TransformNode trNode = ApogyCommonTopologyFactory.eINSTANCE.createTransformNode();
 		trNode.setRotationMatrix(rotationNode.getRotationMatrix());
 		trNode.setPosition(translation);

 		return trNode;
 	}

 	@Override
 	public TransformNode createTransformNode(Matrix4d matrix) {

 		TransformNode tn = ApogyCommonTopologyFactory.eINSTANCE.createTransformNode();

 		tn.setTransformation(matrix);

 		return tn;
 	}

 	@Override
 	public PickAndPlaceNode createPickAndPlaceNode(Matrix4d matrix) {

 		PickAndPlaceNode pnpn = ApogyCommonTopologyFactory.eINSTANCE.createPickAndPlaceNode();

 		pnpn.setTransformation(matrix);

 		return pnpn;
 	}

 	public AttachedViewPoint createAttachedViewPoint(Node attachmentNode) {
 		AttachedViewPoint attachedViewPoint = ApogyCommonTopologyFactory.eINSTANCE.createAttachedViewPoint();

 		if (attachmentNode != null) {
 			Node root = ApogyCommonTopologyFacade.INSTANCE.findRoot(attachmentNode);
 			NodePath nodePath = ApogyCommonTopologyFacade.INSTANCE.createNodePath(root, attachmentNode);
 			attachedViewPoint.setNodePath(nodePath);
 		}

 		return attachedViewPoint;
 	}

 	@Override
 	public void printTopology(Node node) {
 		System.out.println("graph G {");

 		printTopology(node, null);

 		System.out.println("}");
 	}

 	public Collection<Node> filter(NodeFilter filter, Collection<Node> nodes) {
 		List<Node> matches = new ArrayList<Node>();

 		for (Node node : nodes) {
 			if (filter.matches(node)) {
 				matches.add(node);
 			}
 		}

 		return matches;
 	}

 	@Override
 	public <T> ReferencedContentNode<T> createReferencedContentNode(T content) {
 		ReferencedContentNode<T> contentNode = ApogyCommonTopologyFactory.eINSTANCE.createReferencedContentNode();
 		contentNode.setContent(content);

 		return contentNode;
 	}

 	@Override
 	public <T> AggregateContentNode<T> createAggregateContentNode(T content) {
 		AggregateContentNode<T> contentNode = ApogyCommonTopologyFactory.eINSTANCE.createAggregateContentNode();
 		contentNode.setContent(content);

 		return contentNode;
 	}

 	@Override
 	public Link createLink(Node node) {
 		Link link = ApogyCommonTopologyFactory.eINSTANCE.createLink();
 		link.setNode(node);

 		return link;
 	}

 	@Override
 	public EList<Node> findNodesByDescription(String description, Node topology) {

 		final String fDescription = description;

 		final EList<Node> matches = new BasicEList<Node>();

 		INodeVisitor visitor = new AbstractNodeVisitor() {

 			@Override
 			public void visit(Node node) {
 				// Special case: description is null
 				if (node.getDescription() == null && fDescription == null) {
 					matches.add(node);
 				} else if (node.getDescription() != null && node.getDescription().equals(fDescription)) {
 					matches.add(node);
 				}
 			}
 		};

 		topology.accept(visitor);

 		return matches;

 	}

 	@Override
 	public EList<Node> findNodesByID(String id, Node topology) {
 		final String fId = id;

 		final EList<Node> matches = new BasicEList<Node>();

 		INodeVisitor visitor = new AbstractNodeVisitor() {

 			@Override
 			public void visit(Node node) {
 				// Special case: ID is null
 				if (node.getNodeId() == null && fId == null) {
 					matches.add(node);
 				} else if (node.getNodeId() != null && node.getNodeId().equals(fId)) {
 					matches.add(node);
 				}
 			}
 		};

 		topology.accept(visitor);

 		return matches;
 	}

 	@Override
 	public EList<Node> findNodesByType(final EClass clazz, Node topology) {
 		final EList<Node> matches = new BasicEList<Node>();

 		INodeVisitor visitor = new AbstractNodeVisitor() {
 			@Override
 			public void visit(Node node) {
 				if (clazz.isSuperTypeOf(node.eClass())) {
 					matches.add(node);
 				}
 			}
 		};

 		topology.accept(visitor);

 		return matches;
 	}

 	@Override
 	public Node findRoot(Node node) {
 		Node current = node;
 		Node root = node;

 		while (current != null) {
 			root = current;
 			current = current.getParent();
 		}

 		return root;
 	}

 	public boolean doNodesShareTopologyTree(Node node1, Node node2) {
 		Node root1 = findRoot(node1);
 		Node root2 = findRoot(node2);
 		return root1 == root2;
 	}

 	public double getEuclideanDistance(Node fromNode, Node toNode) {
 		if (fromNode == toNode) {
 			return 0.0;
 		} else {
 			if (doNodesShareTopologyTree(fromNode, toNode)) {
 				Matrix4d m = expressInFrame(fromNode, toNode);
 				Vector3d v = new Vector3d();
 				m.get(v);
 				return v.length();
 			} else {
 				return Double.NaN;
 			}
 		}
 	}

 	public double getGeodesicDistance(Node fromNode, Node toNode) {
 		if (fromNode == toNode) {
 			return 0.0;
 		} else {
 			if (doNodesShareTopologyTree(fromNode, toNode)) {
 				List<Node> nodePath = findNodePath(fromNode, toNode);

 				double distance = 0.0;
 				Node currentNode = null;
 				Node previousNode = null;

 				for (int i = 0; i < nodePath.size(); i++) {
 					currentNode = nodePath.get(i);
 					if (previousNode != null && currentNode != null) {
 						distance += getEuclideanDistance(previousNode, currentNode);
 					}
 					previousNode = currentNode;
 				}

 				return distance;
 			} else {
 				return Double.NaN;
 			}
 		}
 	}

 	public List<Node> findNodePath(Node fromNode, Node toNode) {
 		if (doNodesShareTopologyTree(fromNode, toNode)) {
 			// Return path with from node if from and to are the same node.
 			if (fromNode == toNode) {
 				List<Node> nodePath = new ArrayList<Node>();
 				nodePath.add(fromNode);
 				return nodePath;
 			}

 			// Traverses the topology from fromNode to root.
 			Node current = fromNode;
 			List<Node> fromToRootList = new ArrayList<Node>();
 			while (current != null) {
 				fromToRootList.add(current);

 				// Move up the tree.
 				current = current.getParent();
 			}

 			// Traverses the topology from toNode to root.
 			current = toNode;
 			List<Node> toToRootList = new ArrayList<Node>();
 			while (current != null) {
 				toToRootList.add(current);

 				// Move up the tree.
 				current = current.getParent();
 			}

 			// Finds the intersection of both path.
 			Node intersectionNode = null;
 			Iterator<Node> iterator = fromToRootList.iterator();
 			while (intersectionNode == null && iterator.hasNext()) {
 				Node node = iterator.next();

 				if (toToRootList.contains(node)) {
 					intersectionNode = node;
 				}
 			}

 			// If no intersection is found.
 			if (intersectionNode == null) {
 				return new ArrayList<Node>();
 			} else {
 				List<Node> nodePath = new ArrayList<Node>();

 				// Gets the from items up to intersectionNode.
 				int index = 0;
 				boolean stop = false;
 				while (!stop && index < fromToRootList.size()) {
 					Node node = fromToRootList.get(index);

 					if (node == intersectionNode) {
 						stop = true;
 					} else {
 						nodePath.add(node);
 					}
 					index++;
 				}

 				// Gets the to items from intersectionNode to to.
 				index = toToRootList.indexOf(intersectionNode);
 				while (index >= 0) {
 					Node node = toToRootList.get(index);
 					nodePath.add(node);
 					index--;
 				}

 				return nodePath;
 			}
 		} else {
 			return new ArrayList<Node>();
 		}
 	}

 	public NodePath createNodePath(Node fromNode, Node toNode) {
 		NodePath nodePath = null;

 		// Traverses the topology from toNode to fromNode.
 		Node current = toNode;
 		List<Node> toToFromList = new ArrayList<Node>();
 		toToFromList.add(current);

 		while (current != null && current != fromNode) {
 			current = current.getParent();

 			if (current != null) {
 				toToFromList.add(current);
 			}
 		}

 		// Check if the last item in the list is the fromNode
 		if (toToFromList.size() > 0) {
 			if (toToFromList.get(toToFromList.size() - 1) == fromNode) {
 				nodePath = ApogyCommonTopologyFactory.eINSTANCE.createNodePath();

 				for (int i = toToFromList.size() - 1; i >= 0; i--) {
 					Node node = toToFromList.get(i);

 					if (node.getNodeId() == null) {
 						nodePath.getNodeIds().add(NULL_NODE_ID_STRING);
 					} else {
 						nodePath.getNodeIds().add(node.getNodeId());
 					}
 				}
 			} else {
 				return null;
 			}
 		} else {
 			// Returns an empty path.
 			nodePath = ApogyCommonTopologyFactory.eINSTANCE.createNodePath();
 		}

 		return nodePath;
 	}

 	public Node resolveNode(Node root, NodePath nodePath) {
 		// Do a Breadth-First search of the topology tree.
 		Node result = null;

 		int level = 0;
 		Set<Node> s = new HashSet<Node>();
 		Queue<Node> q = new ArrayDeque<Node>();

 		s.add(root);
 		q.add(root);
 		Node current = null;
 		while (!q.isEmpty() && result == null && level < nodePath.getNodeIds().size()) {
 			current = q.poll();
 			String nodeId = current.getNodeId();
 			if (nodeId == null)
 				nodeId = NULL_NODE_ID_STRING;

 			if (nodeId.compareTo(nodePath.getNodeIds().get(level)) == 0) {
 				if (current instanceof GroupNode) {
 					level++;

 					GroupNode groupNode = (GroupNode) current;
 					for (Node child : groupNode.getChildren()) {
 						// Check if the child has an ID that fits the NodePath.
 						nodeId = child.getNodeId();
 						if (nodeId == null)
 							nodeId = NULL_NODE_ID_STRING;

 						if (nodeId.compareTo(nodePath.getNodeIds().get(level)) == 0) {
 							// Check to see if we have reached the end of the
 							// search.
 							if (level == (nodePath.getNodeIds().size() - 1)) {
 								return child;
 							}

 							if (!s.contains(child)) {
 								s.add(child);
 								q.add(child);
 							}
 						}
 					}
 				}
 			}
 		}

 		return null;

 	}

 	private void printTopology(Node currentNode, Node parentNode) {
 		if (parentNode == null && !(currentNode instanceof GroupNode)) {
 			System.out.println(currentNode.getNodeId());
 		} else if (parentNode != null) {
 			System.out.println("\"" + parentNode.getNodeId() + "\" -- \"" + currentNode.getNodeId() + "\"");
 		}

 		if (currentNode instanceof GroupNode) {
 			GroupNode groupNode = (GroupNode) currentNode;

 			for (Node node : groupNode.getChildren()) {
 				printTopology(node, currentNode);
 			}
 		}
 	}

 	@Override
 	public Matrix4d expressNodeInRootFrame(Node node) {
 		List<Matrix4d> matrices = computeMatrixList(node);

 		return computeMatrix(matrices, true);
 	}

 	private List<Matrix4d> computeMatrixList(Node node) {
 		Matrix4d transformationMatrix = new Matrix4d();
 		transformationMatrix.setIdentity();

 		Stack<Matrix4d> matrices = new Stack<Matrix4d>();
 		Node currentNode = node;

 		// We get the parent of the node until we get to the root,
 		// i.e. when the parent is null.
 		while (currentNode != null) {
 			Matrix4d nodeMatrix = null;
 			if (currentNode instanceof TransformNode) {
 				TransformNode tnode = (TransformNode) currentNode;
 				nodeMatrix = tnode.asMatrix4d();
 			} else if (currentNode instanceof PositionNode) {
 				PositionNode pNode = (PositionNode) currentNode;
 				nodeMatrix = new Matrix4d();
 				nodeMatrix.setIdentity();
 				nodeMatrix.setTranslation(new Vector3d(pNode.getPosition().asTuple3d()));
 			} else if (currentNode instanceof RotationNode) {
 				RotationNode rNode = (RotationNode) currentNode;
 				nodeMatrix = new Matrix4d();
 				nodeMatrix.setIdentity();
 				nodeMatrix.setRotation(rNode.getRotationMatrix().asMatrix3d());
 			}

 			if (nodeMatrix != null) {
 				matrices.push(nodeMatrix);
 			}
 			currentNode = currentNode.getParent();
 		}

 		return matrices;
 	}

 	private Matrix4d computeMatrix(List<Matrix4d> matrices, boolean reverse) {
 		Matrix4d result = new Matrix4d();
 		result.setIdentity();

 		while (!matrices.isEmpty()) {
 			Matrix4d matrix = null;
 			if (reverse) {
 				matrix = matrices.remove(matrices.size() - 1);
 			} else {
 				matrix = matrices.remove(0);
 			}

 			result.mul(matrix);
 		}

 		return result;
 	}

 } // ApogyCommonTopologyFacadeImpl
	/*******************************************************************************
	* Copyright (c) 2018 Agence spatiale canadienne / Canadian Space Agency
	* 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:
	* Pierre Allard,
	* Regent L'Archeveque,
	* Sebastien Gemme - initial API and implementation
	*
	* SPDX-License-Identifier: EPL-1.0
	*
	*******************************************************************************/
	package org.eclipse.apogy.common.topology.impl;

	import java.util.ArrayDeque;
	import java.util.ArrayList;
	import java.util.Collection;
	import java.util.HashSet;
	import java.util.Iterator;
	import java.util.List;
	import java.util.Queue;
	import java.util.Set;
	import java.util.Stack;

	import javax.vecmath.Matrix3d;
	import javax.vecmath.Matrix4d;
	import javax.vecmath.Vector3d;

	import org.eclipse.apogy.common.math.ApogyCommonMathFacade;
	import org.eclipse.apogy.common.math.ApogyCommonMathFactory;
	import org.eclipse.apogy.common.math.GeometricUtils;
	import org.eclipse.apogy.common.math.Matrix3x3;
	import org.eclipse.apogy.common.math.Tuple3d;
	import org.eclipse.apogy.common.topology.AbstractNodeVisitor;
	import org.eclipse.apogy.common.topology.AggregateContentNode;
	import org.eclipse.apogy.common.topology.ApogyCommonTopologyFacade;
	import org.eclipse.apogy.common.topology.ApogyCommonTopologyFactory;
	import org.eclipse.apogy.common.topology.AttachedViewPoint;
	import org.eclipse.apogy.common.topology.ContentNode;
	import org.eclipse.apogy.common.topology.GroupNode;
	import org.eclipse.apogy.common.topology.INodeVisitor;
	import org.eclipse.apogy.common.topology.Link;
	import org.eclipse.apogy.common.topology.Node;
	import org.eclipse.apogy.common.topology.NodeFilter;
	import org.eclipse.apogy.common.topology.NodePath;
	import org.eclipse.apogy.common.topology.PickAndPlaceNode;
	import org.eclipse.apogy.common.topology.PositionNode;
	import org.eclipse.apogy.common.topology.ReferencedContentNode;
	import org.eclipse.apogy.common.topology.RotationNode;
	import org.eclipse.apogy.common.topology.TransformNode;
	import org.eclipse.emf.common.util.BasicEList;
	import org.eclipse.emf.common.util.EList;
	import org.eclipse.emf.ecore.EClass;

	public class ApogyCommonTopologyFacadeCustomImpl extends ApogyCommonTopologyFacadeImpl {

	private static final String NULL_NODE_ID_STRING = "NULL";

	public <T> ContentNode<T> createContentNode(T content) {
	return createAggregateContentNode(content);
	}

	@Override
	public PositionNode createPositionNode(double x, double y, double z) {
	PositionNode node = ApogyCommonTopologyFactory.eINSTANCE.createPositionNode();

	Tuple3d position = ApogyCommonMathFactory.eINSTANCE.createTuple3d();

	node.setPosition(position);

	node.getPosition().setX(x);
	node.getPosition().setY(y);
	node.getPosition().setZ(z);

	return node;
	}

	@Override
	public RotationNode createRotationNodeXYZ(double x, double y, double z) {

	RotationNode node = ApogyCommonTopologyFactory.eINSTANCE.createRotationNode();

	Matrix3d rotationMatrix = GeometricUtils.packXYZ(x, y, z);

	Matrix3x3 matrix = ApogyCommonMathFacade.INSTANCE.createMatrix3x3(rotationMatrix);

	node.setRotationMatrix(matrix);

	return node;

	}

	@Override
	public Matrix4d expressRootInNodeFrame(Node node) {

	Matrix4d mat = expressNodeInRootFrame(node);
	mat.invert();

	return mat;

	}

	public Matrix4d expressInFrame(Node sourceFrame, Node targetFrame) {
	Matrix4d sourceFrameToRoot = expressNodeInRootFrame(sourceFrame);
	Matrix4d rootToTargetFrame = expressNodeInRootFrame(targetFrame);
	rootToTargetFrame.invert();
	rootToTargetFrame.mul(sourceFrameToRoot);

	return rootToTargetFrame;
	}

	@Override
	public RotationNode createRotationNodeYZX(double x, double y, double z) {
	RotationNode node = ApogyCommonTopologyFactory.eINSTANCE.createRotationNode();

	Matrix3d rotationMatrix = GeometricUtils.packYZX(x, y, z);

	Matrix3x3 matrix = ApogyCommonMathFacade.INSTANCE.createMatrix3x3(rotationMatrix);

	node.setRotationMatrix(matrix);

	return node;
	}

	@Override
	public RotationNode createRotationNodeZYX(double x, double y, double z) {
	RotationNode node = ApogyCommonTopologyFactory.eINSTANCE.createRotationNode();

	Matrix3d rotationMatrix = GeometricUtils.packZYX(x, y, z);

	Matrix3x3 matrix = ApogyCommonMathFacade.INSTANCE.createMatrix3x3(rotationMatrix);

	node.setRotationMatrix(matrix);

	return node;
	}

	public TransformNode createTransformNodeXYZ(double tx, double ty, double tz, double rx, double ry, double rz) {

	RotationNode rotationNode = createRotationNodeXYZ(rx, ry, rz);

	Tuple3d translation = ApogyCommonMathFactory.eINSTANCE.createTuple3d();
	translation.setX(tx);
	translation.setY(ty);
	translation.setZ(tz);

	TransformNode trNode = ApogyCommonTopologyFactory.eINSTANCE.createTransformNode();
	trNode.setRotationMatrix(rotationNode.getRotationMatrix());
	trNode.setPosition(translation);

	return trNode;

	}

	public TransformNode createTransformNodeYZX(double tx, double ty, double tz, double rx, double ry, double rz) {
	RotationNode rotationNode = createRotationNodeYZX(rx, ry, rz);

	Tuple3d translation = ApogyCommonMathFactory.eINSTANCE.createTuple3d();
	translation.setX(tx);
	translation.setY(ty);
	translation.setZ(tz);

	TransformNode trNode = ApogyCommonTopologyFactory.eINSTANCE.createTransformNode();
	trNode.setRotationMatrix(rotationNode.getRotationMatrix());
	trNode.setPosition(translation);

	return trNode;
	}

	public TransformNode createTransformNodeZYX(double tx, double ty, double tz, double rx, double ry, double rz) {
	RotationNode rotationNode = createRotationNodeZYX(rx, ry, rz);

	Tuple3d translation = ApogyCommonMathFactory.eINSTANCE.createTuple3d();
	translation.setX(tx);
	translation.setY(ty);
	translation.setZ(tz);

	TransformNode trNode = ApogyCommonTopologyFactory.eINSTANCE.createTransformNode();
	trNode.setRotationMatrix(rotationNode.getRotationMatrix());
	trNode.setPosition(translation);

	return trNode;
	}

	@Override
	public TransformNode createTransformNode(Matrix4d matrix) {

	TransformNode tn = ApogyCommonTopologyFactory.eINSTANCE.createTransformNode();

	tn.setTransformation(matrix);

	return tn;
	}

	@Override
	public PickAndPlaceNode createPickAndPlaceNode(Matrix4d matrix) {

	PickAndPlaceNode pnpn = ApogyCommonTopologyFactory.eINSTANCE.createPickAndPlaceNode();

	pnpn.setTransformation(matrix);

	return pnpn;
	}

	public AttachedViewPoint createAttachedViewPoint(Node attachmentNode) {
	AttachedViewPoint attachedViewPoint = ApogyCommonTopologyFactory.eINSTANCE.createAttachedViewPoint();

	if (attachmentNode != null) {
	Node root = ApogyCommonTopologyFacade.INSTANCE.findRoot(attachmentNode);
	NodePath nodePath = ApogyCommonTopologyFacade.INSTANCE.createNodePath(root, attachmentNode);
	attachedViewPoint.setNodePath(nodePath);
	}

	return attachedViewPoint;
	}

	@Override
	public void printTopology(Node node) {
	System.out.println("graph G {");

	printTopology(node, null);

	System.out.println("}");
	}

	public Collection<Node> filter(NodeFilter filter, Collection<Node> nodes) {
	List<Node> matches = new ArrayList<Node>();

	for (Node node : nodes) {
	if (filter.matches(node)) {
	matches.add(node);
	}
	}

	return matches;
	}

	@Override
	public <T> ReferencedContentNode<T> createReferencedContentNode(T content) {
	ReferencedContentNode<T> contentNode = ApogyCommonTopologyFactory.eINSTANCE.createReferencedContentNode();
	contentNode.setContent(content);

	return contentNode;
	}

	@Override
	public <T> AggregateContentNode<T> createAggregateContentNode(T content) {
	AggregateContentNode<T> contentNode = ApogyCommonTopologyFactory.eINSTANCE.createAggregateContentNode();
	contentNode.setContent(content);

	return contentNode;
	}

	@Override
	public Link createLink(Node node) {
	Link link = ApogyCommonTopologyFactory.eINSTANCE.createLink();
	link.setNode(node);

	return link;
	}

	@Override
	public EList<Node> findNodesByDescription(String description, Node topology) {

	final String fDescription = description;

	final EList<Node> matches = new BasicEList<Node>();

	INodeVisitor visitor = new AbstractNodeVisitor() {

	@Override
	public void visit(Node node) {
	// Special case: description is null
	if (node.getDescription() == null && fDescription == null) {
	matches.add(node);
	} else if (node.getDescription() != null && node.getDescription().equals(fDescription)) {
	matches.add(node);
	}
	}
	};

	topology.accept(visitor);

	return matches;

	}

	@Override
	public EList<Node> findNodesByID(String id, Node topology) {
	final String fId = id;

	final EList<Node> matches = new BasicEList<Node>();

	INodeVisitor visitor = new AbstractNodeVisitor() {

	@Override
	public void visit(Node node) {
	// Special case: ID is null
	if (node.getNodeId() == null && fId == null) {
	matches.add(node);
	} else if (node.getNodeId() != null && node.getNodeId().equals(fId)) {
	matches.add(node);
	}
	}
	};

	topology.accept(visitor);

	return matches;
	}

	@Override
	public EList<Node> findNodesByType(final EClass clazz, Node topology) {
	final EList<Node> matches = new BasicEList<Node>();

	INodeVisitor visitor = new AbstractNodeVisitor() {
	@Override
	public void visit(Node node) {
	if (clazz.isSuperTypeOf(node.eClass())) {
	matches.add(node);
	}
	}
	};

	topology.accept(visitor);

	return matches;
	}

	@Override
	public Node findRoot(Node node) {
	Node current = node;
	Node root = node;

	while (current != null) {
	root = current;
	current = current.getParent();
	}

	return root;
	}

	public boolean doNodesShareTopologyTree(Node node1, Node node2) {
	Node root1 = findRoot(node1);
	Node root2 = findRoot(node2);
	return root1 == root2;
	}

	public double getEuclideanDistance(Node fromNode, Node toNode) {
	if (fromNode == toNode) {
	return 0.0;
	} else {
	if (doNodesShareTopologyTree(fromNode, toNode)) {
	Matrix4d m = expressInFrame(fromNode, toNode);
	Vector3d v = new Vector3d();
	m.get(v);
	return v.length();
	} else {
	return Double.NaN;
	}
	}
	}

	public double getGeodesicDistance(Node fromNode, Node toNode) {
	if (fromNode == toNode) {
	return 0.0;
	} else {
	if (doNodesShareTopologyTree(fromNode, toNode)) {
	List<Node> nodePath = findNodePath(fromNode, toNode);

	double distance = 0.0;
	Node currentNode = null;
	Node previousNode = null;

	for (int i = 0; i < nodePath.size(); i++) {
	currentNode = nodePath.get(i);
	if (previousNode != null && currentNode != null) {
	distance += getEuclideanDistance(previousNode, currentNode);
	}
	previousNode = currentNode;
	}

	return distance;
	} else {
	return Double.NaN;
	}
	}
	}

	public List<Node> findNodePath(Node fromNode, Node toNode) {
	if (doNodesShareTopologyTree(fromNode, toNode)) {
	// Return path with from node if from and to are the same node.
	if (fromNode == toNode) {
	List<Node> nodePath = new ArrayList<Node>();
	nodePath.add(fromNode);
	return nodePath;
	}

	// Traverses the topology from fromNode to root.
	Node current = fromNode;
	List<Node> fromToRootList = new ArrayList<Node>();
	while (current != null) {
	fromToRootList.add(current);

	// Move up the tree.
	current = current.getParent();
	}

	// Traverses the topology from toNode to root.
	current = toNode;
	List<Node> toToRootList = new ArrayList<Node>();
	while (current != null) {
	toToRootList.add(current);

	// Move up the tree.
	current = current.getParent();
	}

	// Finds the intersection of both path.
	Node intersectionNode = null;
	Iterator<Node> iterator = fromToRootList.iterator();
	while (intersectionNode == null && iterator.hasNext()) {
	Node node = iterator.next();

	if (toToRootList.contains(node)) {
	intersectionNode = node;
	}
	}

	// If no intersection is found.
	if (intersectionNode == null) {
	return new ArrayList<Node>();
	} else {
	List<Node> nodePath = new ArrayList<Node>();

	// Gets the from items up to intersectionNode.
	int index = 0;
	boolean stop = false;
	while (!stop && index < fromToRootList.size()) {
	Node node = fromToRootList.get(index);

	if (node == intersectionNode) {
	stop = true;
	} else {
	nodePath.add(node);
	}
	index++;
	}

	// Gets the to items from intersectionNode to to.
	index = toToRootList.indexOf(intersectionNode);
	while (index >= 0) {
	Node node = toToRootList.get(index);
	nodePath.add(node);
	index--;
	}

	return nodePath;
	}
	} else {
	return new ArrayList<Node>();
	}
	}

	public NodePath createNodePath(Node fromNode, Node toNode) {
	NodePath nodePath = null;

	// Traverses the topology from toNode to fromNode.
	Node current = toNode;
	List<Node> toToFromList = new ArrayList<Node>();
	toToFromList.add(current);

	while (current != null && current != fromNode) {
	current = current.getParent();

	if (current != null) {
	toToFromList.add(current);
	}
	}

	// Check if the last item in the list is the fromNode
	if (toToFromList.size() > 0) {
	if (toToFromList.get(toToFromList.size() - 1) == fromNode) {
	nodePath = ApogyCommonTopologyFactory.eINSTANCE.createNodePath();

	for (int i = toToFromList.size() - 1; i >= 0; i--) {
	Node node = toToFromList.get(i);

	if (node.getNodeId() == null) {
	nodePath.getNodeIds().add(NULL_NODE_ID_STRING);
	} else {
	nodePath.getNodeIds().add(node.getNodeId());
	}
	}
	} else {
	return null;
	}
	} else {
	// Returns an empty path.
	nodePath = ApogyCommonTopologyFactory.eINSTANCE.createNodePath();
	}

	return nodePath;
	}

	public Node resolveNode(Node root, NodePath nodePath) {
	// Do a Breadth-First search of the topology tree.
	Node result = null;

	int level = 0;
	Set<Node> s = new HashSet<Node>();
	Queue<Node> q = new ArrayDeque<Node>();

	s.add(root);
	q.add(root);
	Node current = null;
	while (!q.isEmpty() && result == null && level < nodePath.getNodeIds().size()) {
	current = q.poll();
	String nodeId = current.getNodeId();
	if (nodeId == null)
	nodeId = NULL_NODE_ID_STRING;

	if (nodeId.compareTo(nodePath.getNodeIds().get(level)) == 0) {
	if (current instanceof GroupNode) {
	level++;

	GroupNode groupNode = (GroupNode) current;
	for (Node child : groupNode.getChildren()) {
	// Check if the child has an ID that fits the NodePath.
	nodeId = child.getNodeId();
	if (nodeId == null)
	nodeId = NULL_NODE_ID_STRING;

	if (nodeId.compareTo(nodePath.getNodeIds().get(level)) == 0) {
	// Check to see if we have reached the end of the
	// search.
	if (level == (nodePath.getNodeIds().size() - 1)) {
	return child;
	}

	if (!s.contains(child)) {
	s.add(child);
	q.add(child);
	}
	}
	}
	}
	}
	}

	return null;

	}

	private void printTopology(Node currentNode, Node parentNode) {
	if (parentNode == null && !(currentNode instanceof GroupNode)) {
	System.out.println(currentNode.getNodeId());
	} else if (parentNode != null) {
	System.out.println("\"" + parentNode.getNodeId() + "\" -- \"" + currentNode.getNodeId() + "\"");
	}

	if (currentNode instanceof GroupNode) {
	GroupNode groupNode = (GroupNode) currentNode;

	for (Node node : groupNode.getChildren()) {
	printTopology(node, currentNode);
	}
	}
	}

	@Override
	public Matrix4d expressNodeInRootFrame(Node node) {
	List<Matrix4d> matrices = computeMatrixList(node);

	return computeMatrix(matrices, true);
	}

	private List<Matrix4d> computeMatrixList(Node node) {
	Matrix4d transformationMatrix = new Matrix4d();
	transformationMatrix.setIdentity();

	Stack<Matrix4d> matrices = new Stack<Matrix4d>();
	Node currentNode = node;

	// We get the parent of the node until we get to the root,
	// i.e. when the parent is null.
	while (currentNode != null) {
	Matrix4d nodeMatrix = null;
	if (currentNode instanceof TransformNode) {
	TransformNode tnode = (TransformNode) currentNode;
	nodeMatrix = tnode.asMatrix4d();
	} else if (currentNode instanceof PositionNode) {
	PositionNode pNode = (PositionNode) currentNode;
	nodeMatrix = new Matrix4d();
	nodeMatrix.setIdentity();
	nodeMatrix.setTranslation(new Vector3d(pNode.getPosition().asTuple3d()));
	} else if (currentNode instanceof RotationNode) {
	RotationNode rNode = (RotationNode) currentNode;
	nodeMatrix = new Matrix4d();
	nodeMatrix.setIdentity();
	nodeMatrix.setRotation(rNode.getRotationMatrix().asMatrix3d());
	}

	if (nodeMatrix != null) {
	matrices.push(nodeMatrix);
	}
	currentNode = currentNode.getParent();
	}

	return matrices;
	}

	private Matrix4d computeMatrix(List<Matrix4d> matrices, boolean reverse) {
	Matrix4d result = new Matrix4d();
	result.setIdentity();

	while (!matrices.isEmpty()) {
	Matrix4d matrix = null;
	if (reverse) {
	matrix = matrices.remove(matrices.size() - 1);
	} else {
	matrix = matrices.remove(0);
	}

	result.mul(matrix);
	}

	return result;
	}

	} // ApogyCommonTopologyFacadeImpl