From 1142c5285c6b441158a4e25bb3320c1f2623e2b1 Mon Sep 17 00:00:00 2001
From: Dion Moult <dion@thinkmoult.com>
Date: Wed, 20 Jan 2021 10:07:48 +1100
Subject: [PATCH] Geolocation util can now apply georeferencing to matrices

---
 .../ifcopenshell/util/geolocation.py          | 49 +++++++++++++++++++
 1 file changed, 49 insertions(+)

diff --git a/src/ifcopenshell-python/ifcopenshell/util/geolocation.py b/src/ifcopenshell-python/ifcopenshell/util/geolocation.py
index 4bc3e9c2c1..a3f5d1722d 100644
--- a/src/ifcopenshell-python/ifcopenshell/util/geolocation.py
+++ b/src/ifcopenshell-python/ifcopenshell/util/geolocation.py
@@ -1,4 +1,5 @@
 import math
+import numpy as np
 
 
 def dms2dd(degrees, minutes, seconds, ms=0):
@@ -45,6 +46,54 @@ def enh2xyz(e, n, h, eastings, northings, orthogonal_height, x_axis_abscissa, x_
     return (x, y, z)
 
 
+def local2global(matrix, eastings, northings, orthogonal_height, x_axis_abscissa, x_axis_ordinate, scale=None):
+    if scale is None:
+        scale = 1.0
+    x = np.array([x_axis_abscissa, x_axis_ordinate, 0])
+    x /= np.linalg.norm(x)
+    y = np.cross(np.array([0, 0, 1]), x)
+    intermediate = (
+        np.matrix(
+            [
+                [x[0], y[0], 0, 0],
+                [x[1], y[1], 0, 0],
+                [x[2], y[2], 1, 0],
+                [0, 0, 0, 1],
+            ]
+        )
+        @ matrix
+    )
+    intermediate[0, 3] = (intermediate[0, 3] * scale) + eastings
+    intermediate[1, 3] = (intermediate[1, 3] * scale) + northings
+    intermediate[2, 3] = (intermediate[2, 3] * scale) + orthogonal_height
+    return intermediate
+
+
+def global2local(matrix, eastings, northings, orthogonal_height, x_axis_abscissa, x_axis_ordinate, scale=None):
+    if scale is None:
+        scale = 1.0
+    x = np.array([x_axis_abscissa, x_axis_ordinate, 0])
+    x /= np.linalg.norm(x)
+    y = np.cross(np.array([0, 0, 1]), x)
+    result = matrix.copy()
+    result[0, 3] = (result[0, 3] - eastings) / scale
+    result[1, 3] = (result[1, 3] - northings) / scale
+    result[2, 3] = (result[2, 3] - orthogonal_height) / scale
+    return (
+        np.linalg.inv(
+            np.matrix(
+                [
+                    [x[0], y[0], 0, 0],
+                    [x[1], y[1], 0, 0],
+                    [x[2], y[2], 1, 0],
+                    [0, 0, 0, 1],
+                ]
+            )
+        )
+        @ result
+    )
+
+
 # Used for converting the X and Y vectors of the X Axis in IFC geolocation
 def xy2angle(x, y):
     return math.degrees(math.atan2(y, x))