bundles/core/org.eclipse.apogy.core.environment.earth.surface/src/org/eclipse/apogy/core/environment/earth/surface/converters/PositionedToGeographicalCoordinatesConverter.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.core.environment.earth.surface.converters;

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

import org.eclipse.apogy.common.converters.IConverter;
import org.eclipse.apogy.common.math.ApogyCommonMathFacade;
import org.eclipse.apogy.core.Positioned;
import org.eclipse.apogy.core.environment.ApogyEnvironment;
import org.eclipse.apogy.core.environment.earth.GeographicCoordinates;
import org.eclipse.apogy.core.environment.earth.surface.EarthSurfaceWorksite;
import org.eclipse.apogy.core.invocator.InvocatorSession;
import org.eclipse.emf.ecore.EObject;

public class PositionedToGeographicalCoordinatesConverter implements IConverter {

	@Override
	public Class<?> getOutputType() {
		return GeographicCoordinates.class;
	}

	@Override
	public Class<?> getInputType() {
		return Positioned.class;
	}

	@Override
	public boolean canConvert(Object input) {
		if (input instanceof Positioned) {
			Positioned positioned = (Positioned) input;

			if (positioned.getPose() != null) {
				EarthSurfaceWorksite worksite = getEarthSurfaceWorksite(positioned);
				if (worksite != null) {
					return true;
				}
			}
		}

		return false;
	}

	@Override
	public Object convert(Object input) throws Exception {
		if (canConvert(input)) {
			Positioned positioned = (Positioned) input;
			Matrix4d matrix = positioned.getPose().asMatrix4d();
			Vector3d position = new Vector3d();
			matrix.get(position);

			EarthSurfaceWorksite worksite = getEarthSurfaceWorksite(positioned);
			return worksite.convertToGeographicCoordinates(ApogyCommonMathFacade.INSTANCE.createTuple3d(position));
		} else {
			return null;
		}
	}

	protected EarthSurfaceWorksite getEarthSurfaceWorksite(Positioned positioned) {
		EarthSurfaceWorksite earthSurfaceWorksite = null;
		EObject container = positioned.eContainer();

		while (earthSurfaceWorksite == null && container != null) {
			if (container instanceof EarthSurfaceWorksite) {
				earthSurfaceWorksite = (EarthSurfaceWorksite) container;
			} else if (container instanceof InvocatorSession) {
				InvocatorSession session = (InvocatorSession) container;
				if (session.getEnvironment() instanceof ApogyEnvironment) {
					ApogyEnvironment se = (ApogyEnvironment) session.getEnvironment();
					if (se.getActiveWorksite() instanceof EarthSurfaceWorksite) {
						earthSurfaceWorksite = (EarthSurfaceWorksite) se.getActiveWorksite();
					}
				}
			} else {
				container = container.eContainer();
			}
		}

		return earthSurfaceWorksite;
	}
}