bundles/common/org.eclipse.apogy.common.topology/src/org/eclipse/apogy/common/topology/util/NodeRelativePoseListener.java

/*******************************************************************************
 * 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,
<<<<<<< HEAD
 *     Sebastien Gemme
 * 
=======
 *     Sebastien Gemme  - initial API and implementation
 *     
>>>>>>> refs/heads/eclipse_pa
 * SPDX-License-Identifier: EPL-1.0
 *     
 *******************************************************************************/
package org.eclipse.apogy.common.topology.util;

import java.util.ArrayList;
import java.util.List;

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

import org.eclipse.apogy.common.topology.ApogyCommonTopologyFacade;
import org.eclipse.apogy.common.topology.ApogyCommonTopologyPackage;
import org.eclipse.apogy.common.topology.Node;
import org.eclipse.apogy.common.topology.PositionNode;
import org.eclipse.apogy.common.topology.RotationNode;
import org.eclipse.apogy.common.topology.TransformNode;
import org.eclipse.emf.common.notify.Adapter;
import org.eclipse.emf.common.notify.Notification;
import org.eclipse.emf.common.notify.impl.AdapterImpl;

public class NodeRelativePoseListener {
	private Adapter adapter = null;
	private final List<Node> nodes = new ArrayList<Node>();

	private final double minimumDistanceDeltaMeters = 1E-6;
	private final double minimumOrientationDeltaRad = 0.01;

	private Matrix4d previousPose = null;
	private Matrix4d newPose = null;
	private Node fromNode;
	private Node toNode;

	/**
	 * Method called when a change in relative pose is detected. Sub-classes should
	 * override this method.
	 * 
	 * @param newPose The new pose. Can be null if to or from node is null.
	 */
	public void relativePoseChanged(Matrix4d newPose) {
	}

	public Node getFromNode() {
		return this.fromNode;
	}

	public void setFromNode(Node fromNode) {
		if (this.fromNode != fromNode) {
			this.fromNode = fromNode;
			updateNodeList();
			relativePoseChanged(computeNewPose());
		}
	}

	public Node getToNode() {
		return this.toNode;
	}

	public void setToNode(Node toNode) {
		if (this.toNode != toNode) {
			this.toNode = toNode;
			updateNodeList();
			relativePoseChanged(computeNewPose());
		}
	}

	public Matrix4d getNewPose() {
		return this.newPose;
	}

	/**
	 * Dispose of this NodeRelativePoseListener. The adapter is removed from all
	 * node and the adapter is then set to null. Should be called when the
	 * NodeRelativePoseListener is no longer needed.
	 */
	public void dispose() {
		unregisterAdapter();
		this.fromNode = null;
		this.toNode = null;
		this.adapter = null;
	}

	private boolean hasPoseChanged(Matrix4d newPose, Matrix4d newPreviousPose) {
		boolean result = false;
		if (this.previousPose == null) {
			if (newPose != null)
				this.previousPose = new Matrix4d(newPose);
			result = true;
		} else if (newPose != null && this.previousPose != null) {
			if (!result) {
				// First checks is translation has occured.
				Vector3d previousPosition = new Vector3d();
				Vector3d newPosition = new Vector3d();

				this.previousPose.get(previousPosition);
				newPose.get(newPosition);

				newPosition.sub(previousPosition);
				if (newPosition.length() > this.minimumDistanceDeltaMeters) {
					result = true;
				}
			}

			if (!result) {
				// Then checks if rotation has occured.
				Matrix3d previousRot = new Matrix3d();
				Matrix3d newRot = new Matrix3d();

				this.previousPose.get(previousRot);
				newPose.get(newRot);

				Vector3d previousOrientation = new Vector3d(1, 0, 0);
				Vector3d newOrientation = new Vector3d(1, 0, 0);

				previousRot.transform(previousOrientation);
				newRot.transform(newOrientation);

				if (Math.abs(previousOrientation.angle(newOrientation)) > this.minimumOrientationDeltaRad) {
					result = true;
				}
			}
		}

		if (newPose != null) {
			this.previousPose = new Matrix4d(newPose);
		} else {
			this.previousPose = new Matrix4d();
			this.previousPose.setIdentity();
		}

		return result;
	}

	private void updatePose(Matrix4d newPose) {
		if (hasPoseChanged(newPose, this.previousPose)) {
			this.newPose = newPose;
			relativePoseChanged(newPose);
		}
	}

	private void updateNodeList() {
		// Unregister from previous nodes.
		unregisterAdapter();

		// Clears list of nodes.
		this.nodes.clear();

		// If both from and to Node are defined.
		if (this.fromNode != null && this.toNode != null) {
			Node currentNode = this.fromNode;
			while (currentNode != null) {
				if (currentNode instanceof PositionNode || currentNode instanceof RotationNode) {
					if (!this.nodes.contains(currentNode))
						this.nodes.add(currentNode);
				}

				currentNode = currentNode.getParent();
			}

			// Gets the lineage of to node.
			currentNode = this.toNode;
			while (currentNode != null) {
				if (currentNode instanceof PositionNode || currentNode instanceof RotationNode) {
					if (!this.nodes.contains(currentNode))
						this.nodes.add(currentNode);
				}

				currentNode = currentNode.getParent();
			}
		}
		// Register to new nodes.
		registerAdapter();
	}

	private void registerAdapter() {
		for (Node node : this.nodes) {
			node.eAdapters().add(getAdapter());
		}
	}

	private void unregisterAdapter() {
		for (Node node : this.nodes) {
			node.eAdapters().remove(getAdapter());
		}
	}

	private Adapter getAdapter() {
		if (this.adapter == null) {
			this.adapter = new AdapterImpl() {
				@Override
				public void notifyChanged(Notification msg) {
					if (msg.getNotifier() instanceof TransformNode) {
						int featureId = msg.getFeatureID(TransformNode.class);
						if (featureId == ApogyCommonTopologyPackage.TRANSFORM_NODE__POSITION
								|| featureId == ApogyCommonTopologyPackage.TRANSFORM_NODE__ROTATION_MATRIX) {
							updatePose(computeNewPose());
						} else if (featureId == ApogyCommonTopologyPackage.TRANSFORM_NODE__PARENT) {
							updateNodeList();
							updatePose(computeNewPose());
						}
					} else if (msg.getNotifier() instanceof PositionNode) {
						int featureId = msg.getFeatureID(PositionNode.class);
						if (featureId == ApogyCommonTopologyPackage.POSITION_NODE__POSITION) {
							updatePose(computeNewPose());
						} else if (featureId == ApogyCommonTopologyPackage.POSITION_NODE__PARENT) {
							updateNodeList();
							updatePose(computeNewPose());
						}
					} else if (msg.getNotifier() instanceof RotationNode) {
						int featureId = msg.getFeatureID(RotationNode.class);
						if (featureId == ApogyCommonTopologyPackage.ROTATION_NODE__ROTATION_MATRIX) {
							updatePose(computeNewPose());
						} else if (featureId == ApogyCommonTopologyPackage.ROTATION_NODE__PARENT) {
							updateNodeList();
							updatePose(computeNewPose());
						}
					}
				}
			};
		}
		return this.adapter;
	}

	protected Matrix4d computeNewPose() {
		if (this.fromNode != null && this.toNode != null) {
			return ApogyCommonTopologyFacade.INSTANCE.expressInFrame(this.fromNode, this.toNode);
		} else {
			return null;
		}
	}
}