bundles/addons/org.eclipse.apogy.addons.sensors.pose/src-gen-custom/org/eclipse/apogy/addons/sensors/pose/impl/SimulatedPoseSensorCustomImpl.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.sensors.pose.impl;

import javax.vecmath.Matrix3d;

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.Tuple3d;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

public class SimulatedPoseSensorCustomImpl extends SimulatedPoseSensorImpl {

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

	protected SimulatedPoseSensorCustomImpl() {
		super();
		initialize();
	}

	private void initialize() {
		if (getRotationMatrix() == null)
			setRotationMatrix(ApogyCommonMathFactory.eINSTANCE.createMatrix3x3());
		if (getPosition() == null)
			setPosition(ApogyCommonMathFactory.eINSTANCE.createTuple3d());

		Thread t = new Thread() {
			@Override
			public void run() {
				while (true) {
					try {
						// Update rotation.
						Matrix3d oldRotation = getRotationMatrix().asMatrix3d();
						Matrix3d newRotation = GeometricUtils.packXYZ(getUpdatePeriod() * getXAngularVelocity(),
								getUpdatePeriod() * getYAngularVelocity(), getUpdatePeriod() * getZAngularVelocity());

						oldRotation.mul(newRotation);
						setRotationMatrix(ApogyCommonMathFacade.INSTANCE.createMatrix3x3(oldRotation));

						// Update position.
						Tuple3d oldPosition = getPosition();
						Tuple3d newPosition = ApogyCommonMathFactory.eINSTANCE.createTuple3d();

						newPosition.setX(oldPosition.getX() + getXVelocity() * getUpdatePeriod());
						newPosition.setY(oldPosition.getY() + getYVelocity() * getUpdatePeriod());
						newPosition.setZ(oldPosition.getZ() + getZVelocity() * getUpdatePeriod());
						setPosition(newPosition);

						long delay = Math.round(getUpdatePeriod() * 1000.0);
						Thread.sleep(delay);
					} catch (Exception e) {
						Logger.error(e.getMessage(), e);
					}
				}
			}
		};

		t.start();
	}

} // SimulatedPoseSensorImpl