[MBsysPy][Algebra] Cross product

Numpy.cross is slow

MBsysC/mbs_interface/MBsysPy/mbs_algebra.py

@@ -198,53 +198,44 @@ def vector_diff(v1, v2, vres=None, size_4=True):
 
 
 def cross_product(v1, v2, vres=None, size_4=True):
-    """Compute and return the cross product of 2 vectors with first index is 1.
+    """Compute and return the cross product of 2 vectors.
 
-    The vectors have unused index 0.
+    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.
-    vres : list or numpy.ndarray, optional
-        Vector containing the result if not None.
+    v1 : numpy.ndarray
+        First vector of the cross product.
+    v2 : numpy.ndarray
+        Second vector of the cross product.
+    vres : numpy.ndarray, optional
+        Vector to store the result if not None.
         default: None
     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
     -------
-    v_cross : numpy.ndarray
+    vres : numpy.ndarray
         Cross product between v1 and v2 (v1 x v2).
 
+    Notes
+    -----
+    The function numpy.cross() is not used as it is not optimized for such small
+    vector.
+
     """
-    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 cross product')
-    v_cross = np.cross(v1, v2)
-    if vres is not None:
-        if len(np.shape(vres)) == 1:
-            vres[1:] = v_cross
-        else:
-            if (vres.shape)[0] == 1:
-                vres[0, 1:] = v_cross
-            else:
-                vres[1:, 0] = v_cross
-    else:
-        v_cross = np.append(v_cross.shape[0], v_cross)
-        return v_cross
+    if vres is None:
+        vres = np.zeros(3 + size_4)
+
+    vres[size_4:] = [v1[2] * v2[3] - v1[3] * v2[2],
+                     v1[3] * v2[1] - v1[1] * v2[3],
+                     v1[1] * v2[2] - v1[2] * v2[1]
+                     ]
+
+    return vres
 
 
 def matrix_vector_product(matrix, vector):