Fix: Quick fix to skip inv transform for openings in T

secorolab · Jan 20, 2025 · da71dd6 · da71dd6
1 parent caf89b2
commit da71dd6
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/src/fpm/graph.py b/src/fpm/graph.py
@@ -323,8 +323,8 @@ def get_waypoint_coord(g, point, coordinates_map):
         T = build_transformation_matrix(
             coordinates["x"], coordinates["y"], coordinates["z"], coordinates["alpha"]
         ).astype(float)
-        if not next_pose == 0:
-            if next_pose.count("wall") > 1:
+        if not next_pose == 0 :
+            if next_pose.count("wall") > 1 and ("entryway" not in pose and "window" not in pose):
                 T = np.linalg.pinv(T)
 
         p = np.dot(T, p)