bundles/addons/org.eclipse.apogy.addons.sensors.pose/src-gen-custom/org/eclipse/apogy/addons/sensors/pose/impl/SimulatedOrientationSensorCustomImpl.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.slf4j.Logger;
import org.slf4j.LoggerFactory;

public class SimulatedOrientationSensorCustomImpl extends SimulatedOrientationSensorImpl {

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

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

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

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

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

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

		t.start();
	}

} // SimulatedOrientationSensorImpl