[MBsysPy][algebra] faster strong matrix product, __get_index_0_matrix__ removed

MBsysC/mbs_interface/MBsysPy/mbs_algebra.py

@@ -348,76 +348,21 @@ def matrix_product(M1, M2, res=None, index_1=True):
     Due to the checks on input parameters this function is slow. Faster implementation,
     each specific to a product (matrix with vector or matrix with matrix) exist.
     """
-    M1, mat1 = __get_index_0_matrix__(M1, size_4)
-    M2, mat2 = __get_index_0_matrix__(M2, size_4)
-
-    if not mat1:
+    if M1.ndim != 2:
         raise ValueError('Parameter M1 must be a matrix')
-
-    if Mres is None:
-        size = 4 * size_4 + 3 * (not size_4)
-        if mat2:
-            Mres = np.zeros((size, size))
-        else:
-            Mres = np.zeros(size)
-
-    first_index = int(size_4)
-
-    if mat2:
-        Mres[first_index:, first_index:] = np.dot(M1, M2)[:, :]
-    else:
-        Mres[first_index:] = np.dot(M1, M2)[:]
-
-    return Mres
-
-
-def __get_index_0_matrix__(M, size_4):
-    """Remove first index and check if right dimensions.
-
-    Parameters
-    ----------
-    M : list or numpy.ndarray
-        Matrix or vector.
-    size_4 : bool, optional
-        True if vectors have a size of 4 and matrices are 4x4.
-        default: True.
-
-    Raises
-    ------
-    ValueError
-        If M is not an array of size 4 if size_4 is True.
-    TypeError
-        If M is not a numpy.ndarray or a list.
-
-    Returns
-    -------
-    M1 : numpy.ndarray
-        M without first index.
-    mat : bool
-        True if M1 is a matrix.
-
-    """
-    mat = True
-    if isinstance(M, (list, np.ndarray)):
-        if len(np.shape(M)) == 1:
-            M = np.array([M], dtype=float)
-        if len(np.shape(M)) == 2:
-            if (M.shape)[0] == 1:
-                M1 = np.array(M[0, 1:], dtype=float)
-                mat = False
-            elif (M.shape)[1] == 1:
-                M1 = np.array(M[1:, 0], dtype=float)
-                mat = False
-            else:
-                M1 = np.array(M[1:, 1:], dtype=float)
+    if M2.ndim not in [1, 2]:
+        raise ValueError('Parameter M2 must be a vector or a matrix.')
+
+    if res is None:
+        size = index_1 + 3
+        if M2.ndim == 2:
+            res = np.zeros((size, size))
+            res[index_1:, index_1:] = np.dot(M1[index_1:size, index_1:size], M2[index_1:size, index_1:size])[:, :]
         else:
-            raise ValueError('Matrices with more than 2 dimensions are not handled')
-        if size_4 and (not np.all(np.array(M1.shape) == 3)):
-            raise ValueError('Matrices must be arrays of size 4 if "size_4" is True')
+            res = np.zeros(size)
+            res[index_1:] = np.dot(M1[index_1:size, index_1:size], M2[index_1:size])[:]
 
-        return M1, mat
-    else:
-        raise TypeError('Matrices must be numpy.ndarray or list')
+    return res
 
 
 def rotation_matrix(rot_type, angle, Rres=None):