dual_quaternion.py 文件源码

def __mul__(self, other):
    """ Dual quaternion multiplication.

    The multiplication with a scalar returns the dual quaternion with all
    elements multiplied by the scalar.

    The multiplication of two dual quaternions dq1 and dq2 as:
    q1_rot * q2_rot + epsilon * (q1_rot * q2_trans + q1_trans * q2_rot),
    where dq1 and dq2 are defined as:
    dq1 = q1_rot + epsilon * q1_trans,
    dq2 = q2_rot + epsilon * q2_trans.
    """
    if isinstance(other, DualQuaternion):
      rotational_part = self.q_rot * other.q_rot
      translational_part = (self.q_rot * other.q_dual +
                            self.q_dual * other.q_rot)
      return DualQuaternion(rotational_part.copy(), translational_part.copy())
    elif isinstance(other, Number):
      dq = self.dq.copy()
      dq_out = dq * np.float64(other)
      return DualQuaternion.from_vector(dq_out)
    else:
      assert False, ("Multiplication is only defined for scalars or dual " "quaternions.")