bundles/addons/org.eclipse.apogy.addons.vehicle/src-gen-custom/org/eclipse/apogy/addons/vehicle/impl/ContactProviderCustomImpl.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 java.util.HashMap;
import java.util.List;
import java.util.Map;

import javax.vecmath.Matrix4d;

import org.eclipse.apogy.addons.vehicle.ClosestNeighbourIteratorProvider;
import org.eclipse.apogy.addons.vehicle.MeshExtent2D;
import org.eclipse.apogy.addons.vehicle.WheelVehicleUtilities;
import org.eclipse.apogy.common.geometry.data3d.CartesianAxis;
import org.eclipse.apogy.common.geometry.data3d.CartesianPolygon;
import org.eclipse.apogy.common.geometry.data3d.CartesianPositionCoordinates;
import org.eclipse.apogy.common.geometry.data3d.CartesianTriangle;
import org.eclipse.apogy.common.geometry.data3d.CartesianTriangularMesh;
import org.eclipse.apogy.common.geometry.data3d.Geometry3DUtilities;
import org.eclipse.apogy.common.math.Matrix4x4;
import org.eclipse.apogy.common.topology.ApogyCommonTopologyFacade;
import org.eclipse.apogy.common.topology.Node;
import org.eclipse.apogy.common.topology.addons.dynamics.PhysicalBody;
import org.jgrapht.graph.DefaultEdge;
import org.jgrapht.traverse.BreadthFirstIterator;

public abstract class ContactProviderCustomImpl extends ContactProviderImpl {

	/*
	 * Stores the last triangle were a contact was made for each mesh and each
	 * wheel. A null triangle indicates that no intersection was found for the
	 * wheel.
	 */
	protected Map<CartesianTriangularMesh, Map<Integer, CartesianTriangle>> meshToWheelToLastTriangleMap = new HashMap<CartesianTriangularMesh, Map<Integer, CartesianTriangle>>();

	protected CartesianPositionCoordinates[] getProjectionAlongAxisOnToPolygon(CartesianAxis axis,
			List<CartesianPositionCoordinates> points, CartesianTriangularMesh mesh) {
		CartesianPositionCoordinates[] intersections = new CartesianPositionCoordinates[points.size()];
		ClosestNeighbourIteratorProvider closestNeighbourIteratorProvider = getVehiclePoseCorrector()
				.getClosestNeighbourIteratorProvider(mesh);
		MeshExtent2D meshExtent2D = getVehiclePoseCorrector().getMeshExtent2D(mesh);

		Map<Integer, CartesianTriangle> wheelToLastTriangleMap = this.meshToWheelToLastTriangleMap.get(mesh);
		if (wheelToLastTriangleMap == null) {
			wheelToLastTriangleMap = new HashMap<Integer, CartesianTriangle>();
			this.meshToWheelToLastTriangleMap.put(mesh, wheelToLastTriangleMap);
		}

		// Process each of wheels.
		for (int i = 0; i < points.size(); i++) {
			CartesianPositionCoordinates point = points.get(i);
			Integer wheelIndex = new Integer(i);

			// If the point falls within the mesh extent.
			if (WheelVehicleUtilities.INSTANCE.isWithin(point.asPoint3d(), meshExtent2D)) {
				CartesianTriangle startTriangle = wheelToLastTriangleMap.get(wheelIndex);

				BreadthFirstIterator<CartesianTriangle, DefaultEdge> it = closestNeighbourIteratorProvider
						.createBreadthFirstIterator(startTriangle);
				CartesianPositionCoordinates projection = null;

				while (it.hasNext() && projection == null) {
					CartesianPolygon polygon = it.next();
					projection = Geometry3DUtilities.getProjectionAlongAxisOnToPolygon(CartesianAxis.Z, point, polygon);

					if (projection != null) {
						wheelToLastTriangleMap.put(wheelIndex, (CartesianTriangle) polygon);
					}
				}
				intersections[i] = projection;
			}
		}

		return intersections;
	}

	/*
	 * Gets the matrix that expresses the pose of a PhysicalBody relative to a
	 * ContentNode.
	 * 
	 * @param originalPose The original, uncorrected pose of the system.
	 * 
	 * @param body The PhysicalBody.
	 * 
	 * @param node The node containing the mesh.
	 * 
	 * @return The matrix.
	 */
	protected Matrix4d getBodyToMeshTransform(Matrix4x4 originalPose, PhysicalBody body, Node node) {
		// Gets the body to System transform
		Matrix4d bodyToSystemTransform = getFootToSystemTransform(body);

		// Gets the body in the world origin.
		Matrix4d bodyToWorldTransform = new Matrix4d(originalPose.asMatrix4d());
		bodyToWorldTransform.mul(bodyToSystemTransform);

		// Gets the mesh to world transform.
		Matrix4d meshToWorldTransform = ApogyCommonTopologyFacade.INSTANCE.expressNodeInRootFrame(node);

		// Gets the bodyToMeshTransform
		meshToWorldTransform.invert();
		bodyToWorldTransform.mul(meshToWorldTransform);

		return bodyToWorldTransform;
	}

	/*
	 * Gets the matrix that expresses the pose of a PhysicalBody relative to the
	 * current System.
	 * 
	 * @param body The PhysicalBody.
	 * 
	 * @return The matrix.
	 */
	protected Matrix4d getFootToSystemTransform(PhysicalBody body) {

		Node root = getVehiclePoseCorrector().getSystemRootNode();
		Matrix4d transform = ApogyCommonTopologyFacade.INSTANCE.expressInFrame(body, root);
		return transform;
	}
} // ContactProviderImpl