private double[] findClosestTranslation(
      Plane3D[] elements_scene,
      Plane3D[] model,
      Plane3D[] modeltr,
      int[] closests,
      double variance) {
    double[] ret = new double[2];
    double total;
    double[] distances;
    double[][] traslations;

    Plane3D element_scene;
    Plane3D element_model;
    Plane3D element_modeltr;
    double angle;
    double dist;
    double total_dist;
    double less_total;
    int len, len2;
    int count, count2;

    len = elements_scene.length;
    len2 = model.length;
    distances = new double[len2];
    traslations = new double[2][len2];
    Point3D projection;

    total = 0;
    for (count = 0; count < len2; count++) {
      element_modeltr = modeltr[count];
      less_total = Double.MAX_VALUE;
      for (count2 = 0; count2 < len; count2++) {
        element_scene = elements_scene[count2];
        angle =
            element_modeltr.vector.getX() * element_scene.vector.getX()
                + element_modeltr.vector.getZ() * element_scene.vector.getZ();
        angle /=
            Math.sqrt(
                    element_modeltr.vector.getX() * element_modeltr.vector.getX()
                        + element_modeltr.vector.getZ() * element_modeltr.vector.getZ())
                * Math.sqrt(
                    element_scene.vector.getX() * element_scene.vector.getX()
                        + element_scene.vector.getZ() * element_scene.vector.getZ());
        angle = Math.acos(angle);

        dist =
            Math.abs(element_scene.pointDistance(element_modeltr.origin))
                + Math.abs(element_modeltr.origin.getDistance(element_scene.origin));

        total_dist = angle + alpha * Math.abs(dist);
        if (total_dist < less_total) {
          less_total = total_dist;
          closests[count] = count2;
        }
      }
      distances[count] = Math.exp(-(less_total * less_total) / (variance));
      total += distances[count];
      element_model = model[count];
      element_scene = elements_scene[closests[count]];
      projection = element_scene.pointProjection(element_model.origin);
      traslations[0][count] = projection.getX() - element_model.origin.getX();
      traslations[1][count] = projection.getZ() - element_model.origin.getZ();
    }

    ret[0] = 0;
    ret[1] = 0;
    if (total != 0)
      for (count = 0; count < len2; count++) {
        distances[count] /= total;
        ret[0] += traslations[0][count] * distances[count];
        ret[1] += traslations[1][count] * distances[count];
      }

    return ret;
  }
  private MyTransform2D findClosestRotation(
      Plane3D[] elements_scene,
      Plane3D[] model,
      Plane3D[] modeltr,
      int[] closests,
      double variance) {
    MyTransform2D ret = new MyTransform2D();
    double angle_res = 0;
    double total = 0;
    double[] distances;
    double[] angles;

    Plane3D element_scene;
    Plane3D element_model;
    Plane3D element_modeltr;
    double angle;
    double dist;
    double total_dist;
    double less_total;
    double less_angle = 100;
    double less_dist = 100;
    int len, len2;
    int count, count2;
    double dist_aux;

    len = elements_scene.length;
    len2 = model.length;
    distances = new double[len2];
    angles = new double[len2];

    for (count = 0; count < len2; count++) {
      element_modeltr = modeltr[count];
      less_total = Double.MAX_VALUE;
      for (count2 = 0; count2 < len; count2++) {
        element_scene = elements_scene[count2];
        angle =
            element_modeltr.vector.getX() * element_scene.vector.getX()
                + element_modeltr.vector.getZ() * element_scene.vector.getZ();
        angle /=
            Math.sqrt(
                    element_modeltr.vector.getX() * element_modeltr.vector.getX()
                        + element_modeltr.vector.getZ() * element_modeltr.vector.getZ())
                * Math.sqrt(
                    element_scene.vector.getX() * element_scene.vector.getX()
                        + element_scene.vector.getZ() * element_scene.vector.getZ());
        angle = Math.acos(angle);

        dist_aux = Math.abs(element_scene.pointDistance(element_modeltr.origin));
        dist = dist_aux + element_scene.origin.getDistance(element_modeltr.origin);

        total_dist = angle + beta * Math.abs(dist);
        if (total_dist < less_total) {
          less_total = total_dist;
          less_angle = angle;
          less_dist = dist;
          closests[count] = count2;
        }
      }
      distances[count] =
          Math.exp(-(less_angle * less_angle) / (variance))
              + beta * Math.exp(-(less_dist * less_dist) / variance);
      total += distances[count];
      element_model = model[count];
      element_scene = elements_scene[closests[count]];
      angle =
          element_model.vector.getX() * element_scene.vector.getX()
              + element_model.vector.getZ() * element_scene.vector.getZ();
      angle /=
          Math.sqrt(
                  element_model.vector.getX() * element_model.vector.getX()
                      + element_model.vector.getZ() * element_model.vector.getZ())
              * Math.sqrt(
                  element_scene.vector.getX() * element_scene.vector.getX()
                      + element_scene.vector.getZ() * element_scene.vector.getZ());
      // degenerate case: by precision errors, angle could be > 1
      if (angle > 1) angle = 0;
      else angle = Math.acos(angle);
      if ((element_model.vector.getZ() * element_scene.vector.getX()
              - element_model.vector.getX() * element_scene.vector.getZ())
          < 0) angle = -angle;
      angles[count] = angle;
    }

    if (total != 0)
      for (count = 0; count < len2; count++) {
        distances[count] /= total;
        angle_res += angles[count] * distances[count];
      }

    ret.setRotation(angle_res);
    return ret;
  }