city_retrofit/helpers/geometry_helper.py

"""
Geometry helper
SPDX - License - Identifier: LGPL - 3.0 - or -later
Copyright © 2020 Project Author Guille Gutierrez guillermo.gutierrezmorote@concordia.ca
Contributors Pilar Monsalvete Álvarez de Uribarri pilar.monsalvete@concordia.ca
"""
import sys
import math
import numpy as np
import open3d as o3d
import requests
from trimesh import Trimesh
from trimesh import intersections
from helpers.configuration_helper import ConfigurationHelper


class GeometryHelper:
  """
  Geometry helper class
  """

  def __init__(self, delta=0, area_delta=0):
    self._delta = delta
    self._area_delta = area_delta

  @property
  def config(self):
    print(f'delta {self._delta} area {self._area_delta}')

  @staticmethod
  def adjacent_locations(location1, location2):
    """
    Determine when two attributes may be adjacent or not based in the dis
    :param location1:
    :param location2:
    :return: Boolean
    """
    max_distance = ConfigurationHelper().max_location_distance_for_shared_walls
    return GeometryHelper.distance_between_points(location1, location2) < max_distance

  def almost_same_area(self, a1, a2):
    """
    Compare two areas and decides if they are almost equal (absolute error under delta)
    :param a1
    :param a2
    :return: Boolean
    """
    if a1 == 0 or a2 == 0:
      return False
    delta = math.fabs(a1 - a2)
    return delta <= self._area_delta

  def almost_equal(self, delta_max, v1, v2):
    """
    Compare two points and decides if they are almost equal (distance under delta_max)
    :param delta_max: maximum distance to be considered same point
    :param v1: [x,y,z]
    :param v2: [x,y,z]
    :return: Boolean
    """
    delta = self.distance_between_points(v1, v2)
    return delta <= delta_max

  def is_almost_same_surface(self, s1, s2):
    """
    Compare two surfaces and decides if they are almost equal (quadratic error under delta)
    :param s1: Surface
    :param s2: Surface
    :return: Boolean
    """

    # delta is grads an need to be converted into radians
    delta = np.rad2deg(self._delta)
    difference = (s1.inclination - s2.inclination) % math.pi
    if abs(difference) > delta:
      return False
    # s1 and s2 are at least almost parallel surfaces
    # calculate distance point to plane using all the vertex
    # select surface1 value for the point (X,Y,Z) where two of the values are 0
    minimum_distance = self._delta + 1
    parametric = s2.polygon.get_parametric()
    n2 = s2.normal
    for point in s1.points:
      distance = abs(
        (point[0] * parametric[0]) + (point[1] * parametric[1]) + (point[2] * parametric[2]) + parametric[3])
      normal_module = math.sqrt(pow(n2[0], 2) + pow(n2[1], 2) + pow(n2[2], 2))

      if normal_module == 0:
        continue
      distance = distance / normal_module
      if distance < minimum_distance:
        minimum_distance = distance
      if minimum_distance <= self._delta:
        break

    if minimum_distance > self._delta or s1.intersect(s2) is None:
      return False
    else:
      return True

  @staticmethod
  def to_points_matrix(points):
    """
    Transform a point vector into a point matrix
    :param points: [x, y, z, x, y, z ...]
    :return: [[x,y,z],[x,y,z]...]
    """
    rows = points.size // 3
    points = points.reshape(rows, 3)
    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

  @staticmethod
  def _point_cloud_to_mesh(point_list, normal_list):
    # todo @Guille: I think this method should be removed (and create_mesh??)
    # Return a mesh composed only by triangles
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(point_list)
    pcd.normals = o3d.utility.Vector3dVector(normal_list)
    distances = pcd.compute_nearest_neighbor_distance()
    avg_dist = np.mean(distances)
    radius = 3 * avg_dist
    bpa_mesh = o3d.geometry.TriangleMesh().create_from_point_cloud_ball_pivoting(
      pcd, o3d.utility.DoubleVector([radius, radius * 2]))
    mesh_result = Trimesh(vertices=np.asarray(bpa_mesh.vertices), faces=np.asarray(bpa_mesh.triangles))
    return mesh_result

  @staticmethod
  def _merge_meshes(mesh1, mesh2):
    v_1 = mesh1.vertices
    f_1 = mesh1.faces
    v_2 = mesh2.vertices
    f_2 = mesh2.faces
    length = len(v_1)
    v_merge = np.concatenate((v_1, v_2))
    f_merge = np.asarray(f_1)

    for item in f_2:
      point1 = item.item(0) + length
      point2 = item.item(1) + length
      point3 = item.item(2) + length
      surface = np.asarray([point1, point2, point3])
      f_merge = np.concatenate((f_merge, [surface]))

    mesh_merge = Trimesh(vertices=v_merge, faces=f_merge)

    return mesh_merge

  @staticmethod
  def divide_mesh_by_plane(mesh, normal_plane, point_plane):
    """
    Divide a mesh by a plane
    :param mesh: Trimesh
    :param normal_plane: [x, y, z]
    :param point_plane: [x, y, z]
    :return: [Trimesh]
    """
    # The first mesh returns the positive side of the plane and the second the negative side.
    # If the plane does not divide the mesh (i.e. it does not touch it or it is coplanar with one or more faces),
    # then it returns only the original mesh.
    normal_plane_opp = [None] * len(normal_plane)
    for i in range(0, len(normal_plane)):
      normal_plane_opp[i] = - normal_plane[i]

    normal = [normal_plane, normal_plane_opp]
    normal_opp = [normal_plane_opp, normal_plane]
    mesh_final = []
    for i in range(0, 2):
      mesh_1 = intersections.slice_mesh_plane(mesh, normal[i], point_plane)
      mesh_1_segments = intersections.mesh_plane(mesh, normal[i], point_plane)
      mesh.difference(mesh_1, engine='blender')
      if len(mesh_1_segments) <= 0 or len(mesh_1.faces) == len(mesh.faces):
        mesh_final.append(mesh)
        break
      else:
        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]
        mesh_2 = GeometryHelper._point_cloud_to_mesh(points, points_normals)
        mesh_final.append(GeometryHelper._merge_meshes(mesh_1, mesh_2))
    return mesh_final

  @staticmethod
  def gml_surface_to_libs(surface):
    if surface == 'WallSurface':
      return 'Wall'
    elif surface == 'GroundSurface':
      return 'Ground'
    else:
      return 'Roof'

  @staticmethod
  def get_location(latitude, longitude):
    url = 'https://nominatim.openstreetmap.org/reverse?lat={latitude}&lon={longitude}&format=json'
    resp = requests.get(url.format(latitude=latitude, longitude=longitude))
    if resp.status_code != 200:
      # This means something went wrong.
      raise Exception('GET /tasks/ {}'.format(resp.status_code))
    else:
      response = resp.json()
      # todo: this is wrong, remove in the future
      city = 'new_york_city'
      country = 'us'
      if 'city' in response['address']:
        city = response['address']['city']
      if 'country_code' in response['address']:
        country = response['address']['country_code']
      return [country, city]

  @staticmethod
  def create_mesh(surfaces):
    point_list = []
    # todo: ensure use almost equal in the points to avoid duplicated points
    normal_list = []
    for surface in surfaces:
      for point in surface.points:
        point_list.append(point)
        normal_list.append(surface.normal)
    pcd = o3d.geometry.PointCloud()
    points = np.asarray(point_list)
    normals = np.asarray(normal_list)
    pcd.points = o3d.utility.Vector3dVector(points)
    pcd.normals = o3d.utility.Vector3dVector(normals)
    alpha = 0.5
    tetra_mesh, pt_map = o3d.geometry.TetraMesh.create_from_point_cloud(pcd)
    mesh = o3d.geometry.TriangleMesh().create_from_point_cloud_alpha_shape(pcd, alpha, tetra_mesh, pt_map)
    mesh.compute_vertex_normals()
    o3d.visualization.draw_geometries([mesh], mesh_show_back_face=True)
    return Trimesh(vertices=np.asarray(mesh.vertices), faces=np.asarray(mesh.triangles))

  @staticmethod
  def distance_between_points(vertex1, vertex2):
    power = 0
    for dimension in range(0, len(vertex1)):
      power += math.pow(vertex2[dimension]-vertex1[dimension], 2)
    distance = math.sqrt(power)
    return distance

  @staticmethod
  def angle_between_vectors(vec_1, vec_2):
    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))
    return alpha