From 329f8e35c53271af34a0d49f0b9176fe21b2217c Mon Sep 17 00:00:00 2001
From: CagtayFabry <43667554+CagtayFabry@users.noreply.github.com>
Date: Tue, 28 Apr 2020 15:00:05 +0200
Subject: [PATCH] correct array layout during normalization and interpolation
 (#25)

* correct array layout during normalization and interpolation
* remove duplicate checks in check_coordinate_system
* Add test case for normalization during LCS.__init__

Co-authored-by: Hirthammer <volker.hirthammer@bam.de>
---
 tests/test_transformations.py | 36 +++++++++++++++++++++++++++++++----
 weldx/transformations.py      | 20 +++++++++----------
 weldx/utility.py              |  2 +-
 3 files changed, 42 insertions(+), 16 deletions(-)

diff --git a/tests/test_transformations.py b/tests/test_transformations.py
index df2e70696..71607bc28 100644
--- a/tests/test_transformations.py
+++ b/tests/test_transformations.py
@@ -8,6 +8,7 @@
 import pytest
 import random
 import math
+from copy import deepcopy
 from typing import Union, List, Any
 
 
@@ -444,12 +445,8 @@ def check_coordinate_system(
     check_coordinate_system_orientation(
         cs_p.orientation, orientation_expected, positive_orientation_expected
     )
-    check_coordinate_system_orientation(
-        cs_p.orientation, orientation_expected, positive_orientation_expected
-    )
 
     assert np.allclose(cs_p.coordinates.values, coordinates_expected, atol=1e-9)
-    assert np.allclose(cs_p.coordinates.values, coordinates_expected, atol=1e-9)
 
 
 def test_coordinate_system_init():
@@ -554,6 +551,34 @@ def test_coordinate_system_init():
     )
     check_coordinate_system(lcs, orientation_exp, coordinates_exp, True, time_exp)
 
+    # matrix normalization ----------------------
+
+    # no time dependency
+    orientation_exp = tf.rotation_matrix_z(np.pi / 3)
+    orientation_fix_2 = deepcopy(orientation_exp)
+    orientation_fix_2[:, 0] *= 10
+    orientation_fix_2[:, 1] *= 3
+    orientation_fix_2[:, 2] *= 4
+
+    lcs = tf.LocalCoordinateSystem(
+        orientation=orientation_fix_2, coordinates=coordinates_fix
+    )
+
+    check_coordinate_system(lcs, orientation_exp, coordinates_fix, True)
+
+    # time dependent
+    orientation_exp = tf.rotation_matrix_z(np.pi / 3 * ut.to_float_array([1, 2, 4]))
+    orientation_tdp_2 = deepcopy(orientation_exp)
+    orientation_tdp_2[:, :, 0] *= 10
+    orientation_tdp_2[:, :, 1] *= 3
+    orientation_tdp_2[:, :, 2] *= 4
+
+    lcs = tf.LocalCoordinateSystem(
+        orientation=orientation_tdp_2, coordinates=coordinates_fix, time=time_0
+    )
+
+    check_coordinate_system(lcs, orientation_exp, coordinates_fix, True, time_0)
+
     # exceptions --------------------------------
     # invalid inputs
     with pytest.raises(Exception):
@@ -573,6 +598,9 @@ def test_coordinate_system_init():
     # TODO: implement
 
 
+test_coordinate_system_init()
+
+
 def test_coordinate_system_factories():
     """
     Test construction of coordinate system class.
diff --git a/weldx/transformations.py b/weldx/transformations.py
index 83a976763..f96d4a35b 100644
--- a/weldx/transformations.py
+++ b/weldx/transformations.py
@@ -59,19 +59,17 @@ def scale_matrix(scale_x, scale_y, scale_z):
     return np.diag([scale_x, scale_y, scale_z]).astype(float)
 
 
-def normalize(vec):
+def normalize(a):
     """
-    Normalize a vector.
+    Normalize (l2 norm) an ndarray along the last dimension.
 
-    :param vec: Vector
-    :return: Normalized vector
+    :param a: data in ndarray
+    :return: Normalized ndarray
     """
-    norm = np.linalg.norm(vec, axis=(-1))
+    norm = np.linalg.norm(a, axis=(-1), keepdims=True)
     if not np.all(norm):
-        raise ValueError("Vector length is 0.")
-    if vec.ndim > 1:
-        return vec / norm[..., np.newaxis]
-    return vec / norm
+        raise ValueError("Length 0 encountered during normalization.")
+    return a / norm
 
 
 def orientation_point_plane_containing_origin(point, p_a, p_b):
@@ -277,8 +275,8 @@ def __init__(
             orientation = xr.apply_ufunc(
                 normalize,
                 orientation,
-                input_core_dims=[["c", "v"]],
-                output_core_dims=[["c", "v"]],
+                input_core_dims=[["c"]],
+                output_core_dims=[["c"]],
             )
 
             # unify time axis
diff --git a/weldx/utility.py b/weldx/utility.py
index 8c3471da2..36b5eb7a8 100644
--- a/weldx/utility.py
+++ b/weldx/utility.py
@@ -413,7 +413,7 @@ def xr_interp_orientation_in_time(
         ]
 
         # interpolate rotations in the intersecting time range
-        rotations_key = Rot.from_matrix(dsx.data)
+        rotations_key = Rot.from_matrix(dsx.transpose(..., "c", "v").data)
         times_key = dsx.time.astype(np.int64)
         rotations_interp = Slerp(times_key, rotations_key)(
             times_intersect.astype(np.int64)