[MBsysPy][Algebra] numpy dot related functions

MBsysC/mbs_interface/MBsysPy/mbs_algebra.py

@@ -73,40 +73,28 @@ def normalize(v, vres=None, size_4=True):
 
     return vres
 
-def scalar_product(v1, v2, size_4=True):
-    """Compute and return the scalar product of 2 vectors with first index is 1.
 
-    The vectors have unused index 0.
+def scalar_product(v1, v2, size_4=True):
+    """Compute and return the scalar product of 2 vectors.
 
+    The shape of the vector differs according to size_4.
 
     Parameters
     ----------
-    v1 : list or numpy.ndarray
-        Vector with first element unused.
-    v2 : list or numpy.ndarray
-        Vector with first element unused.
+    v1 : numpy.ndarray
+        First vector of the product.
+    v2 : numpy.ndarray
+        Second vector of the product.
     size_4 : bool, optional
         True if vectors have a size of 4.
         default: True.
 
-    Raises
-    ------
-    ValueError
-        If v1 or v2 is not a 1-dim array of size 4 if size_4 is True.
-    TypeError
-        If v1 or v2 is not a numpy.ndarray or a list.
-
     Returns
     -------
     float
         Scalar product between v1 and v2.
-
     """
-    v1, mat1 = __get_index_0_matrix__(v1, size_4)
-    v2, mat2 = __get_index_0_matrix__(v2, size_4)
-    if mat1 or mat2:
-        raise ValueError('Vectors must be passed as argument for scalar product')
-    return np.dot(v1, v2)
+    return np.dot(v1[size_4:], v2[size_4:])
 
 
 def vector_sum(v1, v2, vres=None, size_4=True):
@@ -259,24 +247,92 @@ def cross_product(v1, v2, vres=None, size_4=True):
         return v_cross
 
 
+def matrix_vector_product(matrix, vector):
+    """Compute the product of a matrix and a vector with first index is 1.
+
+    Parameters
+    ----------
+    matrix : np.ndarray
+        Matrix of shape (4, 4), the first line and first row are disregarded.
+    vector : np.ndarray
+        Vector of shape (4, ), the first lelement is disregarded.
+
+    Returns
+    -------
+    vector_res : numpy.ndarray
+        Result of the matrix product. The shape of the vector is (4, ). The first
+        element is disregarded.
+
+    See Also
+    --------
+    matrix_product :
+        Slower but more versatile implementation of this functions. It accept both
+        vector or matrix as second parameter and both index 0 or index 1 array.
+    matrix_matrix_product :
+        Fasr implementation restricted to matrix product between 2 matrices with
+        first index unused.
+
+    Notes
+    -----
+    Globally the functions simply calls numpy.dot(matrix[1:, 1:], vector[1:, 1:])
+
+    """
+    vector_res = np.zeros(4)
+    vector_res[1:] = np.dot(matrix[1:, 1:], vector[1:])[:]
+    return vector_res
+
+
+def matrix_matrix_product(matrix_1, matrix_2):
+    """Compute the product of 2 matrices with first index is 1.
+
+    Parameters
+    ----------
+    matrix_1 : np.ndarray
+        Matrix of shape (4, 4), the first line and first row are disregarded.
+    matrix_2 : np.ndarray
+        Matrix of shape (4, 4), the first line and first row are disregarded.
+
+    Returns
+    -------
+    matrix_res : numpy.ndarray
+        Result of the matrix product. The shape of the matrix is (4, 4), the first
+        line and first row are disregarded.
+
+    See Also
+    --------
+    matrix_product :
+        Slower but more versatile implementation of this functions. It accept both
+        vector or matrix as second parameter and both index 0 or index 1 array.
+    matrix_vector_product :
+        Fast implementation restricted to matrix product between a matrix and
+        a vector both with first index unused.
+
+    Notes
+    -----
+    Globally the functions simply calls numpy.dot(matrix_1[1:, 1:], matrix_2[1:, 1:])
+
+    """
+    matrix_res = np.zeros((4, 4))
+    matrix_res[1:, 1:] = np.dot(matrix_1[1:, 1:], matrix_2[1:, 1:])[:, :]
+    return matrix_res
+
+
 def matrix_product(M1, M2, Mres=None, size_4=True):
-    """Compute the product of 2 matrices. The first index is 1.
+    """Compute the product of 2 matrices.
 
-    The second parameter can be a vector.
-    Matrices and vector have unused index 0.
+    The function handle both index 0 and index 1 array according to the parameter
+    'size_4'.
 
 
     Parameters
     ----------
-    M1 : list or numpy.ndarray
-        Matrix with first line and row unused, will be multiplied by M2.
-    M2 : list or numpy.ndarray
-        Matrix with first line and row unused.
-        It could also be a vector with first element unused
-    Mres : list or numpy.ndarray, optional
-        Matrice containing the result if not None.
+    M1 : numpy.ndarray
+        Matrix to be multiplied by M2.
+    M2 : numpy.ndarray
+        Second matrix or vector of the product.
+    Mres : numpy.ndarray, optional
+        Matrice to store the result if not None.
         It must have the right dimensions.
-        If M2 is a vector, Mres also has to be a vector
         default: None
     size_4 : bool, optional
         True if vectors have a size of 4 and matrices are 4x4.
@@ -285,38 +341,49 @@ def matrix_product(M1, M2, Mres=None, size_4=True):
     Raises
     ------
     ValueError
-        If M1 or M2 is not an array of size 4 if size_4 is True.
-    TypeError
-        If M1 or M2 is not a numpy.ndarray or a list.
+        If M1 is not a matrix.
 
     Returns
     -------
-    M_prod : numpy.ndarray
-        Matrix product between M1 and M2 (M . M2) if Mres is None.
-
+    Mres : numpy.ndarray
+        Matrix product between M1 and M2 (M1 . M2). The array is allocated if
+        it was not provided.
+
+    See Also
+    --------
+    matrix_matrix_product :
+        Faster implementation restricted to matrix product between 2 matrices with
+        first index unused.
+    matrix_vector_product :
+        Faster implementation restricted to matrix product between a matrix and
+        a vector both with first index unused.
+
+    Notes
+    -----
+    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)
-    M_prod = np.dot(M1, M2)
-    if Mres is not None:
+
+    if not mat1:
+        raise ValueError('Parameter M1 must be a matrix')
+
+    if Mres is None:
+        size = 4 * size_4 + 3 * (not size_4)
         if mat2:
-            Mres[1:, 1:] = M_prod
+            Mres = np.zeros((size, size))
         else:
-            if len(np.shape(Mres)) == 1:
-                Mres[1:] = M_prod
-            else:
-                if (Mres.shape)[0] == 1:
-                    Mres[0, 1:] = M_prod
-                else:
-                    Mres[1:, 0] = M_prod
+            Mres = np.zeros(size)
+
+    first_index = int(size_4)
+
+    if mat2:
+        Mres[first_index:, first_index:] = np.dot(M1, M2)[:, :]
     else:
-        if mat2:
-            M_prod = np.hstack((np.ones((M_prod.shape[0], 1)) * M_prod.shape[0], M_prod))
-            M_prod = np.vstack((np.ones((1, M_prod.shape[1])) * M_prod.shape[0], M_prod))
-        else:
-            M_prod = np.append(M_prod.shape[0], M_prod)
+        Mres[first_index:] = np.dot(M1, M2)[:]
 
-        return M_prod
+    return Mres
 
 
 def __get_index_0_matrix__(M, size_4):