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; }