ICP with Point3d

Comparison/src/main/java/cz/fidentis/analyst/icp/Icp.java

@@ -7,6 +7,7 @@ import cz.fidentis.analyst.visitors.mesh.HausdorffDistance;
 
 import javax.vecmath.Matrix4d;
 import javax.vecmath.Point3d;
+
 import javax.vecmath.Vector3d;
 import java.util.ArrayList;
 import java.util.Collections;

Comparison/src/main/java/cz/fidentis/analyst/icp/Quaternion.java

@@ -128,6 +128,7 @@ public final class Quaternion {
      * @return a rotation matrix (4x4)
      */
     public Matrix4d toMatrix(){
+<<<<<<< HEAD
         double xx = x * x;
         double xy = x * y;
         double xz = x * z;
@@ -142,6 +143,22 @@ public final class Quaternion {
                 2 * ( xy - zw ), 1 - 2 * ( xx + zz ), 2 * ( yz + xw ), 0,
                 2 * ( xz + yw ), 2 * ( yz - xw ), 1 - 2 * ( xx + yy ), 0,
                 0, 0,0, 1);
+=======
+      double xx = x * x;
+      double xy = x * y;
+      double xz = x * z;
+      double xw = x * w;
+      double yy = y * y;
+      double yz = y * z;
+      double yw = y * w;
+      double zz = z * z;
+      double zw = z * w;
+
+      return new Matrix4d(1 - 2 * ( yy + zz ),2 * ( xy + zw), 2 * ( xz - yw),0,
+              2 * ( xy - zw ), 1 - 2 * ( xx + zz ), 2 * ( yz + xw ), 0,
+              2 * ( xz + yw ), 2 * ( yz - xw ), 1 - 2 * ( xx + yy ), 0,
+              0, 0,0, 1);
+>>>>>>> 1d9816fc2b5d38ad345c195f2913fb8adf5a5de1
     }
 
 }