From e5e6b19ed9b33ffc7099b3bb8ca62e2db77e3a98 Mon Sep 17 00:00:00 2001
From: Pilar <pilar_monsalvete@yahoo.es>
Date: Tue, 30 Mar 2021 17:57:28 -0400
Subject: [PATCH] storey division working, code needs to be cleaned

---
 helpers/geometry_helper.py | 38 +++++++++++++++++++++++---------------
 1 file changed, 23 insertions(+), 15 deletions(-)

diff --git a/helpers/geometry_helper.py b/helpers/geometry_helper.py
index 0029a16a..10cf3e11 100644
--- a/helpers/geometry_helper.py
+++ b/helpers/geometry_helper.py
@@ -110,19 +110,22 @@ class GeometryHelper:
     return points
 
   @staticmethod
-  def _segment_list_to_point_cloud(segment_list):
-    point_list = np.asarray(segment_list[0])
-    for segment in segment_list:
-      for new_point in segment:
-        found = False
-        for point in point_list:
-          same_point = np.allclose(new_point, point)
-          if same_point:
-            found = True
-            break
-        if not found:
-          point_list = np.concatenate((point_list, [new_point]))
-    return point_list
+  def segment_list_to_point_cloud(lines):
+    line_points = [lines[0][0], lines[0][1]]
+    lines.remove(lines[0])
+    while len(lines) > 1:
+      i = 0
+      for line in lines:
+        i += 1
+        if GeometryHelper.distance_between_points(line[0], line_points[len(line_points) - 1]) < 1e-8:
+          line_points.append(line[1])
+          lines.pop(i - 1)
+          break
+        elif GeometryHelper.distance_between_points(line[1], line_points[len(line_points) - 1]) < 1e-8:
+          line_points.append(line[0])
+          lines.pop(i - 1)
+          break
+    return line_points
 
   @staticmethod
   def _point_cloud_to_mesh(point_list, normal_list):
@@ -187,7 +190,7 @@ class GeometryHelper:
         mesh_final.append(mesh)
         break
       else:
-        points = GeometryHelper._segment_list_to_point_cloud(mesh_1_segments)
+        points = GeometryHelper.segment_list_to_point_cloud(mesh_1_segments)
         points_normals = [[None] * 3] * len(points)
         for j in range(0, len(points_normals)):
           points_normals[j] = normal_opp[i]
@@ -256,6 +259,11 @@ class GeometryHelper:
     if np.linalg.norm(vec_1) == 0 or np.linalg.norm(vec_2) == 0:
       sys.stderr.write("Warning: impossible to calculate angle between planes' normal. Return 0\n")
       return 0
-    alpha = math.acos(np.dot(vec_1, vec_2) / np.linalg.norm(vec_1) / np.linalg.norm(vec_2))
+    cosine = np.dot(vec_1, vec_2) / np.linalg.norm(vec_1) / np.linalg.norm(vec_2)
+    if cosine > 1 and cosine-1 < 1e-5:
+      cosine = 1
+    elif cosine < -1 and cosine+1 > -1e-5:
+      cosine = -1
+    alpha = math.acos(cosine)
     return alpha