bundles/addons/org.eclipse.apogy.addons.vehicle/src-gen-custom/org/eclipse/apogy/addons/vehicle/impl/VehiclePathPlannerToolCustomImpl.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 - initial API and implementation
 *     
 * SPDX-License-Identifier: EPL-1.0
 *     
 *******************************************************************************/
package org.eclipse.apogy.addons.vehicle.impl;

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

import org.eclipse.apogy.common.math.ApogyCommonMathFacade;
import org.eclipse.apogy.common.math.Tuple3d;
import org.eclipse.apogy.common.topology.Node;
import org.eclipse.apogy.common.topology.ui.NodeSelection;
import org.eclipse.apogy.core.ApogySystemApiAdapter;
import org.eclipse.apogy.core.invocator.VariableImplementation;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

public class VehiclePathPlannerToolCustomImpl extends VehiclePathPlannerToolImpl {

	private static final Logger Logger = LoggerFactory.getLogger(VehiclePathPlannerToolImpl.class);

	@Override
	public boolean isFromNodeLock() {
		return true;
	}

	@Override
	public void selectionChanged(NodeSelection nodeSelection) {
		if (!isDisposed()) {
			Node node = nodeSelection.getSelectedNode();

			Tuple3d relativePosition = null;
			if (nodeSelection.getRelativeIntersectionPoint() != null) {
				relativePosition = ApogyCommonMathFacade.INSTANCE
						.createTuple3d(nodeSelection.getRelativeIntersectionPoint());
			}

			Tuple3d normal = null;
			if (nodeSelection.getAbsoluteIntersectionNormal() != null) {
				normal = ApogyCommonMathFacade.INSTANCE.createTuple3d(nodeSelection.getAbsoluteIntersectionNormal().x,
						nodeSelection.getAbsoluteIntersectionNormal().y,
						nodeSelection.getAbsoluteIntersectionNormal().z);
			}

			// Always update to Node. From node is the vehicle.
			updateToNode(node, relativePosition, normal);
		}
	};

	@Override
	public Tuple3d getFromAbsolutePosition() {
		if (getVariable() != null) {
			try {
				// Resolve the variable so as to get is pose provider.
				VariableImplementation variableImplementation = getVariable().getEnvironment().getActiveContext()
						.getVariableImplementationsList().getVariableImplementation(getVariable());
				if (variableImplementation.getAdapterInstance() instanceof ApogySystemApiAdapter) {
					ApogySystemApiAdapter apogySystemApiAdapter = (ApogySystemApiAdapter) variableImplementation
							.getAdapterInstance();

					Matrix4d m = null;
					if (apogySystemApiAdapter.getPoseTransform() != null) {
						m = apogySystemApiAdapter.getPoseTransform().asMatrix4d();
					} else {
						m = new Matrix4d();
						m.setIdentity();
					}
					Vector3d v = new Vector3d();
					m.get(v);

					return ApogyCommonMathFacade.INSTANCE.createTuple3d(v);
				}
			} catch (Throwable t) {
				Logger.error(t.getMessage(), t);
			}
		}
		return super.getFromAbsolutePosition();
	}
} // VehiclePathPlannerToolImpl