Performs multiplication of the 2 quaterniona arrays p and q
        :param p: array of len 4
        :param q: array of len 4
        :returns: array of len, result of p * q (with p, q quaternions)

def _mul_array(self, p, q):
        """
        Performs multiplication of the 2 quaterniona arrays p and q
        :param p: array of len 4
        :param q: array of len 4
        :returns: array of len, result of p * q (with p, q quaternions)
        """
        assert(len(q) == len(p) == 4)
        p0 = p[0]
        pi = p[1:4]
        q0 = q[0]
        qi = q[1:4]

        res = np.zeros(4)
        res[0] = p0 * q0 - np.dot(pi, qi)
        res[1:4] = p0 * qi + q0 * pi + np.cross(pi, qi)

        return res