bundles/common/org.eclipse.apogy.common.geometry.data3d/src-gen-custom/org/eclipse/apogy/common/geometry/data3d/impl/ApogyCommonGeometryData3DFacadeCustomImpl.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.common.geometry.data3d.impl;

import java.util.ArrayList;
import java.util.Comparator;
import java.util.HashMap;
import java.util.LinkedList;
import java.util.List;
import java.util.Map;
import java.util.SortedMap;
import java.util.SortedSet;
import java.util.TreeMap;
import java.util.TreeSet;

import javax.vecmath.Matrix3d;
import javax.vecmath.Matrix4d;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DFactory;
import org.eclipse.apogy.common.geometry.data3d.CartesianCoordinatesMesh;
import org.eclipse.apogy.common.geometry.data3d.CartesianCoordinatesSet;
import org.eclipse.apogy.common.geometry.data3d.CartesianOrientationCoordinates;
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.ColoredCartesianPositionCoordinates;
import org.eclipse.apogy.common.geometry.data3d.DigitalElevationMap;
import org.eclipse.apogy.common.geometry.data3d.Geometry3DUtilities;
import org.eclipse.apogy.common.geometry.data3d.NormalPointCloud;
import org.eclipse.apogy.common.geometry.data3d.Pose;
import org.eclipse.apogy.common.geometry.data3d.RGBAColor;
import org.eclipse.apogy.common.geometry.data3d.SphericalCoordinates;
import org.eclipse.emf.ecore.util.EcoreUtil;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

public class ApogyCommonGeometryData3DFacadeCustomImpl extends ApogyCommonGeometryData3DFacadeImpl {

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


	@Override
	public CartesianPositionCoordinates createCartesianPositionCoordinates(double x, double y, double z) {
		CartesianPositionCoordinates coord = ApogyCommonGeometryData3DFactory.eINSTANCE
				.createCartesianPositionCoordinates();

		coord.setX(x);
		coord.setY(y);
		coord.setZ(z);

		return coord;
	}

	@Override
	public ColoredCartesianPositionCoordinates createColoredCartesianPositionCoordinates(double x, double y, double z,
			short red, short green, short blue) {
		return createColoredCartesianPositionCoordinates(x, y, z, (short) 255, red, green, blue);
	}


	@Override
	public ColoredCartesianPositionCoordinates createColoredCartesianPositionCoordinates(double x, double y, double z,
			short alpha, short red, short green, short blue) {
		ColoredCartesianPositionCoordinates coord = ApogyCommonGeometryData3DFactory.eINSTANCE
				.createColoredCartesianPositionCoordinates();

		coord.setX(x);
		coord.setY(y);
		coord.setZ(z);

		coord.setAlpha(alpha);
		coord.setRed(red);
		coord.setGreen(green);
		coord.setBlue(blue);

		return coord;
	}

	@Override
	public CartesianOrientationCoordinates createCartesianOrientationCoordinates(double xRotation, double yRotation,
			double zRotation) {
		CartesianOrientationCoordinates coord = ApogyCommonGeometryData3DFactory.eINSTANCE
				.createCartesianOrientationCoordinates();

		coord.setXRotation(xRotation);
		coord.setYRotation(yRotation);
		coord.setZRotation(zRotation);

		return coord;
	}


	@Override
	public SphericalCoordinates createSphericalCoordinates(double phi, double theta, double r) {
		SphericalCoordinates coord = ApogyCommonGeometryData3DFactory.eINSTANCE.createSphericalCoordinates();

		coord.setPhi(phi);
		coord.setTheta(theta);
		coord.setR(r);

		return coord;
	}

	@Override
	public Pose createPose(double x, double y, double z, double xRotation, double yRotation, double zRotation) {
		Pose pose = ApogyCommonGeometryData3DFactory.eINSTANCE.createPose();
		pose.setX(x);
		pose.setY(y);
		pose.setZ(z);

		pose.setXRotation(xRotation);
		pose.setYRotation(yRotation);
		pose.setZRotation(zRotation);

		return pose;

	}

	@Override
	public Pose createPose(Pose pose) {
		return createPose(pose.getX(), pose.getY(), pose.getZ(), pose.getXRotation(), pose.getYRotation(),
				pose.getZRotation());
	}


	@Override
	public Pose createPose(CartesianPositionCoordinates position, CartesianOrientationCoordinates orientation) {
		Pose pose = ApogyCommonGeometryData3DFactory.eINSTANCE.createPose();

		pose.setX(position.getX());
		pose.setY(position.getY());
		pose.setZ(position.getZ());

		pose.setXRotation(orientation.getXRotation());
		pose.setYRotation(orientation.getYRotation());
		pose.setZRotation(orientation.getZRotation());

		return pose;
	}

	@Override
	public CartesianPolygon createCartesianPolygon(CartesianPositionCoordinates v1, CartesianPositionCoordinates v2,
			CartesianPositionCoordinates v3) {

		CartesianPolygon polygon = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianPolygon();
		polygon.getVertices().add(v1);
		polygon.getVertices().add(v2);
		polygon.getVertices().add(v3);

		return polygon;
	}

	public CartesianPositionCoordinates createCartesianPositionCoordinates(CartesianPositionCoordinates coordinates) {
		CartesianPositionCoordinates coord = ApogyCommonGeometryData3DFactory.eINSTANCE
				.createCartesianPositionCoordinates();

		coord.setX(coordinates.getX());
		coord.setY(coordinates.getY());
		coord.setZ(coordinates.getZ());

		return coord;
	}

	@Override
	public CartesianOrientationCoordinates createCartesianOrientationCoordinates(
			CartesianOrientationCoordinates coordinates) {
		CartesianOrientationCoordinates coord = ApogyCommonGeometryData3DFactory.eINSTANCE
				.createCartesianOrientationCoordinates();

		coord.setXRotation(coordinates.getXRotation());
		coord.setYRotation(coordinates.getYRotation());
		coord.setZRotation(coordinates.getZRotation());

		return coord;
	}


	@Override
	public CartesianCoordinatesMesh createCartesianCoordinatesMesh(CartesianCoordinatesMesh cartesianCoordinatesMesh) {
		return EcoreUtil.copy(cartesianCoordinatesMesh);
	}

	public CartesianTriangle createCartesianTriangle(CartesianPositionCoordinates v1, CartesianPositionCoordinates v2,
			CartesianPositionCoordinates v3) {
		CartesianTriangle triangle = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianTriangle();
		triangle.getVertices().add(v1);
		triangle.getVertices().add(v2);
		triangle.getVertices().add(v3);

		return triangle;
	}

	public <T extends CartesianPolygon> CartesianTriangle createCartesianTriangle(T polygon)
			throws IllegalArgumentException {
		CartesianTriangle triangle = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianTriangle();

		if (polygon.getVertices().size() != 3) {
			throw new IllegalArgumentException();
		} else {
			triangle.getVertices().addAll(polygon.getVertices());
		}

		return triangle;
	}

	public CartesianCoordinatesSet applyTransform(CartesianCoordinatesSet points, Matrix4d trMatrix) {

		CartesianCoordinatesSet trPoints = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianCoordinatesSet();

		List<CartesianPositionCoordinates> pointList = new ArrayList<CartesianPositionCoordinates>();

		for (CartesianPositionCoordinates point : points.getPoints()) {
			Point3d p = point.asPoint3d();

			trMatrix.transform(p);

			CartesianPositionCoordinates trPoint = createCartesianPositionCoordinates(p.x, p.y, p.z);

			pointList.add(trPoint);
		}

		trPoints.getPoints().addAll(pointList);

		return trPoints;
	}

	public CartesianCoordinatesMesh createCartesianCoordinatesMesh(List<CartesianPolygon> polygons) {
		CartesianCoordinatesMesh mesh = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianCoordinatesMesh();

		TreeMap<CartesianPositionCoordinates, CartesianPositionCoordinates> originalToCopyMap = new TreeMap<CartesianPositionCoordinates, CartesianPositionCoordinates>(
				new Geometry3DUtilities.CartesianPositionCoordinatesDistanceComparator());
		for (CartesianPolygon polygon : polygons) {
			// Creates a new polygon.
			CartesianPolygon polygonCopy = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianPolygon();
			mesh.getPolygons().add(polygonCopy);

			// Copies the vertex and keep a reference to the original.
			for (CartesianPositionCoordinates point : polygon.getVertices()) {
				CartesianPositionCoordinates pointCopy = originalToCopyMap.get(point);

				// If the point original has not been encountered yet.
				if (pointCopy == null) {
					// Create copy of vertex.
					pointCopy = createCartesianPositionCoordinates(point);

					// Adds it to the mesh.
					mesh.getPoints().add(pointCopy);

					// Keep a references to the original.
					originalToCopyMap.put(point, pointCopy);
				}

				// Adds the vertex copy to the polygon copy.
				polygonCopy.getVertices().add(pointCopy);
			}
		}

		return mesh;
	}

	public CartesianTriangularMesh createCartesianTriangularMesh(List<CartesianTriangle> polygons) {
		CartesianTriangularMesh mesh = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianTriangularMesh();

		TreeMap<CartesianPositionCoordinates, CartesianPositionCoordinates> originalToCopyMap = new TreeMap<CartesianPositionCoordinates, CartesianPositionCoordinates>(
				new Geometry3DUtilities.CartesianPositionCoordinatesDistanceComparator());
		for (CartesianPolygon polygon : polygons) {
			// Creates a new polygon.
			CartesianTriangle polygonCopy = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianTriangle();
			mesh.getPolygons().add(polygonCopy);

			// Copies the vertex and keep a reference to the original.
			for (CartesianPositionCoordinates point : polygon.getVertices()) {
				CartesianPositionCoordinates pointCopy = originalToCopyMap.get(point);

				// If the point original has not been encountered yet.
				if (pointCopy == null) {
					// Create copy of vertex.
					pointCopy = createCartesianPositionCoordinates(point);

					// Adds it to the mesh.
					mesh.getPoints().add(pointCopy);

					// Keep a references to the original.
					originalToCopyMap.put(point, pointCopy);
				}

				// Adds the vertex copy to the polygon copy.
				polygonCopy.getVertices().add(pointCopy);
			}
		}

		return mesh;
	}

	public List<CartesianPositionCoordinates> applyTransform(List<CartesianPositionCoordinates> points,
			Matrix4d trMatrix) {
		List<CartesianPositionCoordinates> trPoints = new ArrayList<CartesianPositionCoordinates>(points.size());

		for (CartesianPositionCoordinates point : points) {
			Point3d p = point.asPoint3d();

			trMatrix.transform(p);

			CartesianPositionCoordinates trPoint = createCartesianPositionCoordinates(p.x, p.y, p.z);

			trPoints.add(trPoint);
		}

		return trPoints;
	}

	public CartesianTriangularMesh createTransformedMesh(CartesianTriangularMesh mesh, Matrix4d trMatrix) {
		List<CartesianPositionCoordinates> trPoints = new ArrayList<CartesianPositionCoordinates>();

		Map<CartesianPositionCoordinates, Integer> pointToIdMap = new HashMap<CartesianPositionCoordinates, Integer>();

		int i = 0;

		for (CartesianPositionCoordinates p : mesh.getPoints()) {
			Point3d p3d = p.asPoint3d();

			trMatrix.transform(p3d);

			CartesianPositionCoordinates trP = createCartesianPositionCoordinates(p3d.x, p3d.y, p3d.z);

			trPoints.add(trP);

			pointToIdMap.put(p, i++);
		}

		List<CartesianTriangle> triangles = new ArrayList<CartesianTriangle>();

		for (CartesianTriangle tri : mesh.getPolygons()) {
			CartesianTriangle newTri = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianTriangle();
			for (CartesianPositionCoordinates triPoint : tri.getVertices()) {
				int id = pointToIdMap.get(triPoint);

				CartesianPositionCoordinates newPoint = trPoints.get(id);
				newTri.getVertices().add(newPoint);
			}
			triangles.add(newTri);
		}

		CartesianTriangularMesh trMesh = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianTriangularMesh();

		trMesh.getPoints().addAll(trPoints);
		trMesh.getPolygons().addAll(triangles);

		// We transform the normals.
		if (mesh.getNormals() != null) {
			List<Vector3d> trNormals = new LinkedList<Vector3d>();
			Matrix3d rotation = new Matrix3d();
			trMatrix.getRotationScale(rotation);
			rotation.setScale(1.0);
			for (Vector3d normal : mesh.getNormals()) {
				Vector3d trNormal = new Vector3d(normal);
				rotation.transform(trNormal);
				trNormals.add(trNormal);
			}
			trMesh.setNormals(trNormals);
		}

		return trMesh;
	}

	public void applyTransform(CartesianTriangularMesh mesh, Matrix4d trMatrix) {
		for (CartesianPositionCoordinates p : mesh.getPoints()) {
			Point3d p3d = p.asPoint3d();

			trMatrix.transform(p3d);

			p.setX(p3d.x);
			p.setY(p3d.y);
			p.setZ(p3d.z);
		}
	}

	public NormalPointCloud applyTransform(NormalPointCloud points, Matrix4d trMatrix) {

		if (points.getNormals() != null && points.getNormals().size() != points.getPoints().size()) {
			throw new IllegalArgumentException(
					"Error: normal (" + points.getNormals().size() + ") != points (" + points.getPoints().size() + ")");
		}

		// We extract the rotation component of 'trMatrix', to apply it to the
		// normals.
		Matrix3d rotation = new Matrix3d();
		trMatrix.getRotationScale(rotation);

		rotation.setScale(1.0);

		List<Vector3d> trNormals = null;

		if (points.getNormals() != null) {

			trNormals = new ArrayList<Vector3d>(points.getNormals().size());
		}

		List<Point3d> trPoints = new ArrayList<Point3d>(points.getPoints().size());

		for (int i = 0; i < points.getPoints().size(); i++) {
			Point3d pi = new Point3d(points.getPoints().get(i));

			if (points.getNormals() != null) {

				Vector3d ni = new Vector3d(points.getNormals().get(i));
				rotation.transform(ni);
				trNormals.add(ni);
			}

			trMatrix.transform(pi);
			trPoints.add(pi);
		}

		NormalPointCloud trPointCloud = ApogyCommonGeometryData3DFactory.eINSTANCE.createNormalPointCloud();
		trPointCloud.setNormals(trNormals);
		trPointCloud.setPoints(trPoints);

		return trPointCloud;
	}

	public CartesianCoordinatesMesh createCartesianCoordinatesMesh(CartesianTriangularMesh cartesianTriangularMesh) {
		CartesianCoordinatesMesh ccmesh = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianCoordinatesMesh();

		TreeMap<CartesianPositionCoordinates, CartesianPositionCoordinates> originalToCopyMap = new TreeMap<CartesianPositionCoordinates, CartesianPositionCoordinates>(
				new Geometry3DUtilities.CartesianPositionCoordinatesDistanceComparator());
		for (CartesianTriangle polygon : cartesianTriangularMesh.getPolygons()) {
			// Creates a new polygon.
			CartesianPolygon polygonCopy = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianPolygon();
			ccmesh.getPolygons().add(polygonCopy);

			// Copies the vertex and keep a reference to the original.
			for (CartesianPositionCoordinates point : polygon.getVertices()) {
				CartesianPositionCoordinates pointCopy = originalToCopyMap.get(point);

				// If the point original has not been encountered yet.
				if (pointCopy == null) {
					// Create copy of vertex.
					pointCopy = createCartesianPositionCoordinates(point);

					// Adds it to the mesh.
					ccmesh.getPoints().add(pointCopy);

					// Keep a references to the original.
					originalToCopyMap.put(point, pointCopy);
				}

				// Adds the vertex copy to the polygon copy.
				polygonCopy.getVertices().add(pointCopy);
			}
		}

		return ccmesh;
	}

	public CartesianTriangularMesh createCartesianTriangularMesh(CartesianTriangularMesh cartesianTriangularMesh) {
		CartesianTriangularMesh trimesh = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianTriangularMesh();

		TreeMap<CartesianPositionCoordinates, CartesianPositionCoordinates> originalToCopyMap = new TreeMap<CartesianPositionCoordinates, CartesianPositionCoordinates>(
				new Geometry3DUtilities.CartesianPositionCoordinatesDistanceComparator());

		// Copies all the vertices
		for (CartesianPositionCoordinates point : cartesianTriangularMesh.getPoints()) {
			// Copies the point.
			CartesianPositionCoordinates pointCopy = createCartesianPositionCoordinates(point);

			// Adds the copy to the mesh
			trimesh.getPoints().add(pointCopy);

			// Keep a references to the original.
			originalToCopyMap.put(point, pointCopy);
		}

		// Copies the polygons.
		for (CartesianPolygon polygon : cartesianTriangularMesh.getPolygons()) {
			// Creates a new polygon.
			CartesianTriangle polygonCopy = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianTriangle();

			// Copies the vertex and keep a reference to the original.
			for (CartesianPositionCoordinates point : polygon.getVertices()) {
				CartesianPositionCoordinates pointCopy = originalToCopyMap.get(point);

				// Adds the vertex copy to the polygon copy.
				polygonCopy.getVertices().add(pointCopy);
			}

			// Verify that copied polygons is a triangles
			// Verification is done after the copy, because the copy does could
			// not contain duplicated vertices.
			if (polygonCopy.getVertices().size() != 3) {
				/* Simply skip this polygon */
				/*
				 * YES: It may occur, Specially with experimental meshes generated with FCL
				 * mesher
				 */
				continue;
			}

			// Add polygon to the mesh
			trimesh.getPolygons().add(polygonCopy);
		}

		return trimesh;
	}

	public void updateCartesianCoordinatesSet(CartesianCoordinatesSet cartesianCoordinatesSet, double[][] xyzData) {
		int numberOfPointsInSet = cartesianCoordinatesSet.getPoints().size();

		// If there is more point in the array than the set, create the required
		// additional points.
		// If not, remove the surplus points in the data set.
		if (xyzData.length > numberOfPointsInSet) {
			List<CartesianPositionCoordinates> toAdd = new ArrayList<CartesianPositionCoordinates>();
			for (int j = numberOfPointsInSet; j < xyzData.length; j++) {
				toAdd.add((createCartesianPositionCoordinates(xyzData[j][0], xyzData[j][1], xyzData[j][2])));
			}
			cartesianCoordinatesSet.getPoints().addAll(toAdd);
		} else {
			List<CartesianPositionCoordinates> toRemove = new ArrayList<CartesianPositionCoordinates>();
			for (int j = xyzData.length; j < numberOfPointsInSet; j++) {
				toRemove.add(cartesianCoordinatesSet.getPoints().get(j));
			}
			cartesianCoordinatesSet.getPoints().removeAll(toRemove);
		}

		// Updates the existing points in the CartesianCoordinatesSet.
		int numberOfPointsInArray = xyzData.length;
		for (int i = 0; i < numberOfPointsInArray; i++) {
			cartesianCoordinatesSet.getPoints().get(i).setX(xyzData[i][0]);
			cartesianCoordinatesSet.getPoints().get(i).setY(xyzData[i][1]);
			cartesianCoordinatesSet.getPoints().get(i).setZ(xyzData[i][2]);
		}
	}

	public DigitalElevationMap createDigitalElevationMap(CartesianCoordinatesSet coordinatesSet) {

		// Sort the input point by x coordinates and y coordinates.
		SortedMap<Double, SortedSet<CartesianPositionCoordinates>> map = new TreeMap<Double, SortedSet<CartesianPositionCoordinates>>();

		int yDimension = 0;
		for (CartesianPositionCoordinates p : coordinatesSet.getPoints()) {
			Double x = new Double(p.getX());
			if (!map.containsKey(x)) {
				SortedSet<CartesianPositionCoordinates> s = new TreeSet<CartesianPositionCoordinates>(
						new CartesianPositionCoordinatesYComparator());
				s.add(p);
				map.put(x, s);
			} else {
				SortedSet<CartesianPositionCoordinates> s = map.get(x);
				s.add(p);

				if (s.size() > yDimension)
					yDimension = s.size();
			}
		}

		// Determines the X Dimension.
		int xDimension = map.keySet().size();

		// Creates the DigitalElevationMap.
		DigitalElevationMap result = ApogyCommonGeometryData3DFactory.eINSTANCE.createDigitalElevationMap();
		result.setXDimension(xDimension);
		result.setYDimension(yDimension);

		List<CartesianPositionCoordinates> points = new ArrayList<CartesianPositionCoordinates>();
		for (Double x : map.keySet()) {
			SortedSet<CartesianPositionCoordinates> s = map.get(x);
			for (CartesianPositionCoordinates p : s) {
				CartesianPositionCoordinates copy = createCartesianPositionCoordinates(p);
				points.add(copy);
			}
		}
		result.getPoints().addAll(points);

		return result;
	}

	@Override
	public CartesianTriangularMesh concatenateTriangularMeshes(List<CartesianTriangularMesh> listOfTriangularMeshes) {
		CartesianTriangularMesh contatenatedMesh = ApogyCommonGeometryData3DFactory.eINSTANCE
				.createCartesianTriangularMesh();

		TreeMap<CartesianPositionCoordinates, CartesianPositionCoordinates> originalToCopyMap = new TreeMap<CartesianPositionCoordinates, CartesianPositionCoordinates>(
				new Geometry3DUtilities.CartesianPositionCoordinatesDistanceComparator());

		for (CartesianTriangularMesh triMesh : listOfTriangularMeshes) {

			for (CartesianPolygon polygon : triMesh.getPolygons()) {

				// Creates a new polygon.
				CartesianTriangle polygonCopy = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianTriangle();

				// Copies the vertex and keep a reference to the original.
				for (CartesianPositionCoordinates point : polygon.getVertices()) {
					CartesianPositionCoordinates pointCopy = originalToCopyMap.get(point);

					// If the point original has not been encountered yet.
					if (pointCopy == null) {
						// Create copy of vertex.
						pointCopy = createCartesianPositionCoordinates(point);

						// Adds it to the mesh.
						contatenatedMesh.getPoints().add(pointCopy);

						// Keep a references to the original.
						originalToCopyMap.put(point, pointCopy);
					}

					// Adds the vertex copy to the polygon copy.
					polygonCopy.getVertices().add(pointCopy);
				}

				// Verify that copied polygons is a triangles
				// Verification is done after the copy, because the copy does
				// could
				// not contain duplicated vertices.
				if (polygonCopy.getVertices().size() != 3) {
					/* Simply skip this polygon */
					/*
					 * YES: It may occur, Specially with experimental meshes generated with FCL
					 * mesher
					 */
					continue;
				}

				// Add polygon to the mesh
				contatenatedMesh.getPolygons().add(polygonCopy);
			}
		}
		return contatenatedMesh;
	}

	public CartesianCoordinatesSet generatePointCloud(CartesianTriangularMesh cartesianCoordinatesMesh,
			double resolution) {
		List<CartesianPositionCoordinates> points = new ArrayList<CartesianPositionCoordinates>();
		double pointGenerationResolution = resolution;
		for (CartesianTriangle triangle : cartesianCoordinatesMesh.getPolygons()) {
			try {
				// Gets the normal of the triangle.
				Vector3d normal = triangle.getNormal();

				Point3d p0 = triangle.getVertices().get(0).asPoint3d();
				Point3d p1 = triangle.getVertices().get(1).asPoint3d();
				Point3d p2 = triangle.getVertices().get(2).asPoint3d();

				Vector3d u = new Vector3d(p1);
				u.sub(p0);

				Vector3d p0p2 = new Vector3d(p2);
				p0p2.sub(p0);

				Vector3d v = new Vector3d();
				v.cross(normal, u);

				// Makes v as long as p0p2.
				v.normalize();
				v.scale(p0p2.length());

				double s = 0;
				double t = 0;

				double deltaS = 1.0;
				double deltaT = 1.0;

				if (u.length() <= pointGenerationResolution) {
					deltaS = 0.5;
				} else {
					deltaS = pointGenerationResolution / u.length();
				}

				if (v.length() <= pointGenerationResolution) {
					deltaT = 0.5;
				} else {
					deltaT = pointGenerationResolution / v.length();
				}

				if (deltaS != 0 && deltaT != 0) {
					while (s <= (1.0 + deltaS)) {
						Vector3d vs = new Vector3d(u);
						vs.scale(s);
						t = 0;

						while (t <= (1.0 + deltaT)) {
							Vector3d vt = new Vector3d(v);
							vt.scale(t);
							vt.add(vs);
							vt.add(p0);

							// Finds the projection of vt onto the triangle.
							CartesianPositionCoordinates p = createCartesianPositionCoordinates(vt.getX(), vt.getY(),
									vt.getZ());
							CartesianPositionCoordinates point = Geometry3DUtilities.getProjectionInPolygonPlane(p,
									triangle);

							if (point != null) {
								if (Geometry3DUtilities.isInsidePolygon(point, triangle)) {
									points.add(point);
								}
							}

							t += deltaT;
						}

						s += deltaS;
					}
				}

				// Generates point on the edges.
				points.addAll(generatePointsOnSegment(p0, p1, resolution));
				points.addAll(generatePointsOnSegment(p1, p2, resolution));
				points.addAll(generatePointsOnSegment(p2, p0, resolution));
			} catch (Throwable t) {
				Logger.error(t.getMessage(), t);
			}
		}

		CartesianCoordinatesSet pointCloud = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianCoordinatesSet();
		pointCloud.getPoints().addAll(points);

		Logger.info("Generated <" + pointCloud.getPoints().size() + "> raw points.");
		return pointCloud;

	}

	private List<CartesianPositionCoordinates> generatePointsOnSegment(Point3d p0, Point3d p1, double resolution) {
		List<CartesianPositionCoordinates> points = new ArrayList<CartesianPositionCoordinates>();

		Vector3d v = new Vector3d(p1);
		v.sub(p0);

		double t = 0;
		double deltaT = resolution / v.length();

		while (t <= (1.0 + deltaT)) {
			Vector3d w = new Vector3d(v);
			w.scale(deltaT);
			w.add(p0);

			CartesianPositionCoordinates p = createCartesianPositionCoordinates(w.getX(), w.getY(), w.getZ());
			points.add(p);

			t += deltaT;
		}

		return points;
	}

	private class CartesianPositionCoordinatesYComparator implements Comparator<CartesianPositionCoordinates> {
		public int compare(CartesianPositionCoordinates o1, CartesianPositionCoordinates o2) {
			if (o1.getY() > o2.getY()) {
				return 1;
			} else if (o1.getY() < o2.getY()) {
				return -1;
			} else {
				return 0;
			}
		}
	}

	public CartesianCoordinatesSet createCartesianCoordinatesSet(List<Point3d> points) {

		CartesianCoordinatesSet pointSet = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianCoordinatesSet();

		for (Point3d point : points) {
			CartesianPositionCoordinates pointCoordinates = createCartesianPositionCoordinates(point.x, point.y,
					point.z);

			pointSet.getPoints().add(pointCoordinates);
		}

		return pointSet;
	}

	public RGBAColor createRGBAColor(short alpha, short red, short green, short blue) {
		RGBAColor rgbaColor = ApogyCommonGeometryData3DFactory.eINSTANCE.createRGBAColor();

		rgbaColor.setAlpha(alpha);
		rgbaColor.setRed(red);
		rgbaColor.setGreen(green);
		rgbaColor.setBlue(blue);
		return rgbaColor;
	}
} // ApogyCommonGeometryData3DFacadeImpl