diff --git a/Comparison/src/main/java/cz/fidentis/analyst/icp/Icp.java b/Comparison/src/main/java/cz/fidentis/analyst/icp/Icp.java
new file mode 100644
index 0000000000000000000000000000000000000000..e6c0547da27d5e30a2e59c4484980a5c1fc93379
--- /dev/null
+++ b/Comparison/src/main/java/cz/fidentis/analyst/icp/Icp.java
@@ -0,0 +1,304 @@
+package cz.fidentis.analyst.icp;
+
+import cz.fidentis.analyst.mesh.core.MeshFacet;
+import cz.fidentis.analyst.mesh.core.MeshPoint;
+import cz.fidentis.analyst.visitors.mesh.HausdorffDistance;
+
+import javax.vecmath.Matrix4d;
+import javax.vecmath.Quat4d;
+import javax.vecmath.Vector3d;
+import java.util.ArrayList;
+import java.util.List;
+
+public class Icp {
+
+ public List<IcpTransformation> initIcp(MeshFacet mainFacet, MeshFacet comparedFacet, int maxIteration, boolean scale, double e){
+
+ HausdorffDistance hausdorffDist = new HausdorffDistance(mainFacet, HausdorffDistance.Strategy.POINT_TO_POINT, false, false);
+
+ List<IcpTransformation> trans = new ArrayList<>(maxIteration);
+
+ int currentIteration = 0;
+ IcpTransformation transformation = null;
+
+ double prevMeanD = Double.POSITIVE_INFINITY; // mean distance of previous transformation
+
+ while ((currentIteration <= maxIteration) &&
+ (Double.isInfinite(prevMeanD) || Math.abs(prevMeanD - transformation.getMeanD()) > e )){
+
+ hausdorffDist.visitMeshFacet(comparedFacet);
+ List<Double> distances = hausdorffDist.getDistances().get(comparedFacet);
+ List<Vector3d> nearestPoints = hausdorffDist.getNearestPoints().get(comparedFacet);
+
+ if(transformation != null){
+ prevMeanD = transformation.getMeanD();
+ }
+
+ transformation = computeIcpTransformation(nearestPoints, distances, comparedFacet.getVertices(),scale);
+ trans.add(transformation);
+ applyTransformation(comparedFacet.getVertices(), transformation, scale);
+ currentIteration ++;
+ }
+
+ return trans;
+ }
+
+ /**
+ *
+ * @param nearestPoints
+ * @param comparedPoints
+ * @return List with two points. The first one represents center of main facet
+ * and the second one represents center of compared facet.
+ */
+ private List<Vector3d> computeCenterBothFacets(List<Vector3d> nearestPoints, List<MeshPoint> comparedPoints) {
+ double xN = 0;
+ double yN = 0;
+ double zN = 0;
+
+ double xC = 0;
+ double yC = 0;
+ double zC = 0;
+
+ int countOfNotNullPoints = 0;
+ List<Vector3d> result = new ArrayList<>(2);
+
+ for (int i = 0; i < nearestPoints.size(); i++) {
+
+ if(nearestPoints.get(i) == null){
+ continue;
+ }
+
+ xN += nearestPoints.get(i).x;
+ yN += nearestPoints.get(i).y;
+ zN += nearestPoints.get(i).z;
+
+ xC += comparedPoints.get(i).getPosition().x;
+ yC += comparedPoints.get(i).getPosition().y;
+ zC += comparedPoints.get(i).getPosition().z;
+
+ countOfNotNullPoints ++;
+ }
+ result.add(new Vector3d(xN/countOfNotNullPoints,yN/countOfNotNullPoints,zN/countOfNotNullPoints));
+ result.add(new Vector3d(xC/countOfNotNullPoints,yC/countOfNotNullPoints,zC/countOfNotNullPoints));
+ return result;
+ }
+
+
+ private Vector3d relativeCoordinate(Vector3d p, Vector3d center) {
+ return new Vector3d(p.x - center.x,p.y - center.y, p.z - center.z);
+ }
+
+ private Matrix4d sumMatrixComp(Vector3d p) {
+ return new Matrix4d(0, -p.x, -p.y, -p.z,
+ p.x, 0, p.z, -p.y,
+ p.y, -p.z, 0, p.x,
+ p.z, p.y, -p.x,0 );
+ }
+
+ private Matrix4d sumMatrixMain(Vector3d p) {
+ return new Matrix4d(0, -p.x, -p.y, -p.z,
+ p.x, 0, -p.z, p.y,
+ p.y, p.z, 0, -p.x,
+ p.z, -p.y, p.x, 0);
+ }
+
+ public Matrix4d pointToMatrix(Vector3d p){
+ return new Matrix4d(p.x, p.y, p.z, 1,
+ 0, 0, 0, 0, 0, 0,
+ 0,0, 0, 0, 0, 0);
+ }
+
+ public IcpTransformation computeIcpTransformation(List<Vector3d> nearestPoints, List<Double> distances, List<MeshPoint>comparedPoints, boolean scale) {
+ double x, y, z;
+ double meanX = 0;
+ double meanY = 0;
+ double meanZ = 0;
+ double meanD = 0;
+
+ int countOfNotNullPoints = 0;
+
+ List<Vector3d>centers = computeCenterBothFacets(nearestPoints, comparedPoints);
+ Vector3d mainCenter = centers.get(0);
+ Vector3d comparedCenter = centers.get(1);
+
+ Matrix4d sumMatrix = new Matrix4d();
+ Matrix4d multipleMatrix = new Matrix4d();
+
+ // List<Vector3d> near = new ArrayList<>(comparedPoints.size());
+ List<Vector3d> compared = new ArrayList<>(comparedPoints.size());
+
+
+ for(int i = 0; i < comparedPoints.size(); i++) {
+
+ if (comparedPoints.get(i) == null){
+ continue;
+ }
+
+ // START computing Translation coordinates
+ x = nearestPoints.get(i).x - comparedPoints.get(i).getPosition().x;
+ y = nearestPoints.get(i).y - comparedPoints.get(i).getPosition().y;
+ z = nearestPoints.get(i).z - comparedPoints.get(i).getPosition().z;
+
+ meanX += x;
+ meanY += y;
+ meanZ += z;
+ meanD += distances.get(i);
+ //near.add(nearestPoints.get(i));
+ compared.add(comparedPoints.get(i).getPosition());
+ // END computing Translation coordinates
+
+ // START computing Rotation parameters
+
+ // set the value of multipleMatrix to transpose of sumMatrix of relative coordinates compared point
+ multipleMatrix.transpose(sumMatrixComp(relativeCoordinate(comparedPoints.get(i).getPosition(),comparedCenter)));
+
+ // set the value of multipleMatrix to the result of multiplying itself with sumMatrix of relative coordinates main point
+ multipleMatrix.mul(sumMatrixMain(relativeCoordinate(nearestPoints.get(i), mainCenter)));
+
+ // set the value of sumMatrix to sum of itself and multipleMatrix
+ sumMatrix.add(multipleMatrix);
+ // END computing Rotation parameters
+
+
+ // START computing Scale parameters
+
+ countOfNotNullPoints ++;
+
+ }
+
+ meanD /= countOfNotNullPoints;
+ meanX /= countOfNotNullPoints;
+ meanY /= countOfNotNullPoints;
+ meanZ /= countOfNotNullPoints;
+ // end computing translation parameter
+/*
+ //computing ROTATION parameter
+ Vector3d mainCenter = computeCenterV(nearestPoints);
+ Vector3d comparedCenter = computeCenterV(compared);
+
+ List<Vector3d> relativeM = relativeCord(nearestPoints, mainCenter);
+ List<Vector3d> relativeC = relativeCord(compared, comparedCenter);
+
+
+
+ for(int i = 0; i < relativeC.size(); i++) {
+ multipleMatrix.transpose(sumMatrixComp(relativeC.get(i)));
+ multipleMatrix.mul(sumMatrixMain(relativeM.get(i)));
+ sumMatrix.add(multipleMatrix);
+ }
+ */
+
+
+ Quat4d rotation = new Quat4d();
+ sumMatrix.setRotation(rotation);
+ rotation.normalize();
+ // end computing rotation parameter
+
+
+ //computing SCALE parameter
+ double sxUp = 0;
+ double scaleFactor = 0;
+ double sxDown = 0;
+
+ if (scale) {
+ Matrix4d rotationMatrix = new Matrix4d();
+ rotationMatrix.set(rotation);
+ Matrix4d matrixPoint, matrixPointCompare;
+
+ for (int i = 0; i < nearestPoints.size(); i++) {
+ if(nearestPoints.get(i) == null) {
+ continue;
+ }
+ matrixPoint = pointToMatrix(relativeCoordinate(nearestPoints.get(i), mainCenter));
+ matrixPointCompare = pointToMatrix(relativeCoordinate(comparedPoints.get(i).getPosition(),comparedCenter));
+ matrixPointCompare.mul(rotationMatrix);
+
+ matrixPoint.transpose();
+ matrixPoint.mul(matrixPointCompare);
+ sxUp += matrixPoint.getElement(0, 0);
+
+ matrixPointCompare.transpose();
+ matrixPointCompare.mul(matrixPointCompare);
+ sxDown += matrixPointCompare.getElement(0,0);
+ }
+
+ scaleFactor = sxUp / sxDown;
+
+ } // end computing scale parameter
+
+ return new IcpTransformation(new Vector3d(meanX, meanY, meanZ), rotation, scaleFactor, meanD, null);
+ }
+
+ private Vector3d multiplyVectorByNumber(Vector3d vector, double number) {
+ return new Vector3d(vector.x * number, vector.y * number, vector.z * number);
+ }
+
+
+ public void applyTransformation(List<MeshPoint> comparedPoints, IcpTransformation transformation, boolean scale) {
+ Vector3d meshPointPosition;
+ Quat4d rotationCopy = new Quat4d();
+ Quat4d conjugateRotation = new Quat4d();
+
+ for (MeshPoint comparedPoint : comparedPoints) {
+ meshPointPosition = comparedPoint.getPosition();
+
+ Quat4d point = new Quat4d(meshPointPosition.x, meshPointPosition.y, meshPointPosition.z, 1);
+
+ if (comparedPoints.size() > 1) {
+ conjugateRotation.conjugate(transformation.getRotation());
+ rotationCopy.mul(point,conjugateRotation);
+ rotationCopy.mul(transformation.getRotation());
+ } else {
+ rotationCopy = point;
+ }
+
+ if(scale && !Double.isNaN(transformation.getScaleFactor())) {
+ meshPointPosition.set(rotationCopy.x * transformation.getScaleFactor() + transformation.getTranslation().x,
+ rotationCopy.y * transformation.getScaleFactor() + transformation.getTranslation().y,
+ rotationCopy.z * transformation.getScaleFactor() + transformation.getTranslation().z);
+ } else {
+ meshPointPosition.set(rotationCopy.x+ transformation.getTranslation().x,
+ rotationCopy.y + transformation.getTranslation().y,
+ rotationCopy.z + transformation.getTranslation().z);
+ }
+ }
+ }
+
+ public IcpTransformation computeFinalTransformation(List<IcpTransformation> transformations, boolean scale) {
+ double scaleFactor = 1;
+ Vector3d translation = new Vector3d();
+ Quat4d rotation = new Quat4d();
+
+ for(int i = 0; i < transformations.size(); i++) {
+ IcpTransformation trans = transformations.get(i);
+
+ if(scale){
+ scaleFactor = trans.getScaleFactor();
+ }
+
+ if(i == 0){
+ translation = trans.getTranslation();
+ }
+
+ if(i > 0 && scale) {
+ translation = multiplyVectorByNumber(translation, scaleFactor);
+ }
+ Quat4d conjugateRotation = trans.getRotation();
+ Quat4d tmp = new Quat4d(translation.x, translation.y, translation.z, 1 );
+
+ tmp.mul(trans.getRotation());
+ conjugateRotation.conjugate();
+ tmp.mul(conjugateRotation);
+
+ translation.x = tmp.x + trans.getTranslation().x;
+ translation.y = tmp.y + trans.getTranslation().y;
+ translation.z = tmp.z + trans.getTranslation().z;
+
+ rotation.mul(trans.getRotation());
+
+ }
+
+ return new IcpTransformation(translation,rotation, scaleFactor, 0, null);
+ }
+
+}
diff --git a/Comparison/src/main/java/cz/fidentis/analyst/icp/IcpTransformation.java b/Comparison/src/main/java/cz/fidentis/analyst/icp/IcpTransformation.java
new file mode 100644
index 0000000000000000000000000000000000000000..c46898aeb34ef2f17d19dabddf78da62bf965285
--- /dev/null
+++ b/Comparison/src/main/java/cz/fidentis/analyst/icp/IcpTransformation.java
@@ -0,0 +1,42 @@
+package cz.fidentis.analyst.icp;
+
+import javax.vecmath.Matrix4d;
+import javax.vecmath.Quat4d;
+import javax.vecmath.Vector3d;
+
+public class IcpTransformation {
+
+ private Quat4d rotation;
+ private double scaleFactor;
+ private Vector3d translation;
+ private Matrix4d transformationMatrix;
+ private double meanD;
+
+ public IcpTransformation(Vector3d translation, Quat4d rotation, double scaleFactor,double meanD , Matrix4d transformationMatrix) {
+ this.translation = translation;
+ this.scaleFactor = scaleFactor;
+ this.rotation = rotation;
+ this.meanD = meanD;
+ this.transformationMatrix = transformationMatrix;
+ }
+
+ public Quat4d getRotation() {
+ return rotation;
+ }
+
+ public double getScaleFactor() {
+ return scaleFactor;
+ }
+
+ public Vector3d getTranslation() {
+ return translation;
+ }
+
+ public Matrix4d getTransformationMatrix() {
+ return transformationMatrix;
+ }
+
+ public double getMeanD() {
+ return meanD;
+ }
+}