Compute the quaternion representing the rotation given by the matrix
483074
def rotation_matrix_to_quaternion(rotation_matrix):
    """"""
    invert = (np.linalg.det(rotation_matrix) < 0)
    if invert:
        factor = -1
    else:
        factor = 1
    c2 = 0.25*(factor*np.trace(rotation_matrix) + 1)
    if c2 < 0:
        #print c2
        c2 = 0.0
    c = np.sqrt(c2)
    r2 = 0.5*(1 + factor*np.diagonal(rotation_matrix)) - c2
    #print "check", r2.sum()+c2
    r = np.zeros(3, float)
    for index, r2_comp in enumerate(r2):
        if r2_comp < 0:
            continue
        else:
            row, col = off_diagonals[index]
            if (rotation_matrix[row, col] - rotation_matrix[col, row] < 0):
                r[index] = -np.sqrt(r2_comp)
            else:
                r[index] = +np.sqrt(r2_comp)
    return factor, np.array([c, r[0], r[1], r[2]], float)
954059
def quat_rotate(rotation, vector):
    

    def quat_mult_point(q, w):
        return (q[3] * w[0] + q[1] * w[2] - q[2] * w[1],
                q[3] * w[1] + q[2] * w[0] - q[0] * w[2],
                q[3] * w[2] + q[0] * w[1] - q[1] * w[0],
                -q[0] * w[0] - q[1] * w[1] - q[2] * w[2])

    q = quat_mult_point(rotation, vector)
    q = tf.transformations.quaternion_multiply(
        q, tf.transformations.quaternion_inverse(rotation))
    return [q[0], q[1], q[2]]
366385
public Matrix4f rotateAroundLocal(Quaternionfc quat, float ox, float oy, float oz, Matrix4f dest) {
        float w2 = quat.w() * quat.w();
        float x2 = quat.x() * quat.x();
        float y2 = quat.y() * quat.y();
        float z2 = quat.z() * quat.z();
        float zw = quat.z() * quat.w();
        float xy = quat.x() * quat.y();
        float xz = quat.x() * quat.z();
        float yw = quat.y() * quat.w();
        float yz = quat.y() * quat.z();
        float xw = quat.x() * quat.w();
        float lm00 = w2 + x2 - z2 - y2;
        float lm01 = xy + zw + zw + xy;
        float lm02 = xz - yw + xz - yw;
        float lm10 = -zw + xy - zw + xy;
        float lm11 = y2 - z2 + w2 - x2;
        float lm12 = yz + yz + xw + xw;
        float lm20 = yw + xz + xz + yw;
        float lm21 = yz + yz - xw - xw;
        float lm22 = z2 - y2 - x2 + w2;
        float tm00 = m00 - ox * m03;
        float tm01 = m01 - oy * m03;
        float tm02 = m02 - oz * m03;
        float tm10 = m10 - ox * m13;
        float tm11 = m11 - oy * m13;
        float tm12 = m12 - oz * m13;
        float tm20 = m20 - ox * m23;
        float tm21 = m21 - oy * m23;
        float tm22 = m22 - oz * m23;
        float tm30 = m30 - ox * m33;
        float tm31 = m31 - oy * m33;
        float tm32 = m32 - oz * m33;
        dest._m00(lm00 * tm00 + lm10 * tm01 + lm20 * tm02 + ox * m03);
        dest._m01(lm01 * tm00 + lm11 * tm01 + lm21 * tm02 + oy * m03);
        dest._m02(lm02 * tm00 + lm12 * tm01 + lm22 * tm02 + oz * m03);
        dest._m03(m03);
        dest._m10(lm00 * tm10 + lm10 * tm11 + lm20 * tm12 + ox * m13);
        dest._m11(lm01 * tm10 + lm11 * tm11 + lm21 * tm12 + oy * m13);
        dest._m12(lm02 * tm10 + lm12 * tm11 + lm22 * tm12 + oz * m13);
        dest._m13(m13);
        dest._m20(lm00 * tm20 + lm10 * tm21 + lm20 * tm22 + ox * m23);
        dest._m21(lm01 * tm20 + lm11 * tm21 + lm21 * tm22 + oy * m23);
        dest._m22(lm02 * tm20 + lm12 * tm21 + lm22 * tm22 + oz * m23);
        dest._m23(m23);
        dest._m30(lm00 * tm30 + lm10 * tm31 + lm20 * tm32 + ox * m33);
        dest._m31(lm01 * tm30 + lm11 * tm31 + lm21 * tm32 + oy * m33);
        dest._m32(lm02 * tm30 + lm12 * tm31 + lm22 * tm32 + oz * m33);
        dest._m33(m33);
        dest._properties(properties & ~(PROPERTY_PERSPECTIVE | PROPERTY_IDENTITY | PROPERTY_TRANSLATION));
        return dest;
    }
377574
def quaternion_product(quat1, quat2):
    """"""
    return np.array([
        quat1[0]*quat2[0] - np.dot(quat1[1:], quat2[1:]),
        quat1[0]*quat2[1] + quat2[0]*quat1[1] + quat1[2]*quat2[3] - quat1[3]*quat2[2],
        quat1[0]*quat2[2] + quat2[0]*quat1[2] + quat1[3]*quat2[1] - quat1[1]*quat2[3],
        quat1[0]*quat2[3] + quat2[0]*quat1[3] + quat1[1]*quat2[2] - quat1[2]*quat2[1]
    ], float)
1185413
def dual_quaternion(self):
        
        qr = self.quaternion
        qd = np.append([0], self.translation / 2.)
        return DualQuaternion(qr, qd)
349882
def _symbolic_rotation_helper():
    """"""
    from sympy import symbols, Matrix

    cpsi, spsi = symbols('cpsi, spsi')
    cth, sth = symbols('cth, sth')
    cphi, sphi = symbols('cphi, sphi')
    Rpsi = Matrix([[cpsi, spsi, 0], [-spsi, cpsi, 0], [0, 0, 1]])
    Rth = Matrix([[1, 0, 0], [0, cth, sth], [0, -sth, cth]])
    Rphi = Matrix([[cphi, sphi, 0], [-sphi, cphi, 0], [0, 0, 1]])

    Q = Rpsi * Rth * Rphi

    epsy, epsz = symbols('epsy, epsz')
    A = Matrix([[1, 0, 0], [0, epsy, 0], [0, 0, epsz]])

    D = Q * A * Q.T

    for i in range(3):
        for j in range(3):
            print('D[%d, %d] = %s' % (i, j, D[i, j]))
1127918
def _dcm_to_q(self, dcm):
        
        assert(isinstance(dcm, Matrix3))
        arr = self._matrix3_to_dcm_array(dcm)
        return super(Quaternion, self)._dcm_to_q(arr)
172713
def copy(self):
        
        return Quaternion(self.w, self.x, self.y, self.z, False)
262638
def euler_angles(self):
        
        q_wxyz = self.quaternion
        q_xyzw = np.roll(q_wxyz, -1)
        return transformations.euler_from_quaternion(q_xyzw)
291456
public void rotate(Quaternion rotation) {
		Transform3D m = new Transform3D();
		m.transform(this.getFirstAxis());
		m.transform(this.getSecondAxis());
		m.transform(this.getThirdAxis());
		this.setFirstAxisExtent(this.getFirstAxis().length());
		this.setSecondAxisExtent(this.getSecondAxis().length());
		this.setThirdAxisExtent(this.getThirdAxis().length());
		this.getFirstAxis().normalize();
		this.getSecondAxis().normalize();
		this.getThirdAxis().normalize();
	}
607226
def quaternion_conjugate(quaternion):
    
    q = np.array(quaternion, dtype=np.float64, copy=True)
    np.negative(q[1:], q[1:])
    return q
1074996
def get_rotations(self):
        """"""
        if self.centrosymmetric:
            return np.vstack((self.rotations, -self.rotations))
        else:
            return self.rotations
605282
function() {

      var w = this['w'];
      var x = this['x'];
      var y = this['y'];
      var z = this['z'];

      var vNorm = Math.sqrt(x * x + y * y + z * z);
      var wExp = Math.exp(w);
      var scale = wExp / vNorm * Math.sin(vNorm);

      if (vNorm === 0) {
        //return new Quaternion(wExp * Math.cos(vNorm), 0, 0, 0);
        return new Quaternion(wExp, 0, 0, 0);
      }

      return new Quaternion(
              wExp * Math.cos(vNorm),
              x * scale,
              y * scale,
              z * scale);
    }
558206
def symbolic_axis_rotation_matrix(axis, symbolic_theta):
    """"""
    [x, y, z] = axis
    c = sympy.cos(symbolic_theta)
    s = sympy.sin(symbolic_theta)
    return sympy.Matrix([
        [x**2 + (1 - x**2) * c, x * y * (1 - c) - z * s, x * z * (1 - c) + y * s],
        [x * y * (1 - c) + z * s, y ** 2 + (1 - y**2) * c, y * z * (1 - c) - x * s],
        [x * z * (1 - c) - y * s, y * z * (1 - c) + x * s, z**2 + (1 - z**2) * c]
    ])
1352597
def symbolic_rotation_matrix(phi, theta, symbolic_psi):
    """"""
    return sympy.Matrix(Rz_matrix(phi)) * sympy.Matrix(Rx_matrix(theta)) * symbolic_Rz_matrix(symbolic_psi)
1177891
def rotate(self, matrix, tol=1e-3):
        
        matrix = SquareTensor(matrix)
        if not matrix.is_rotation(tol):
            raise ValueError("Rotation matrix is not valid.")
        sop = SymmOp.from_rotation_and_translation(matrix,
                                                   [0., 0., 0.])
        return self.transform(sop)
1145612
def rotMatrix2AxisAndAngle(R):
    
    angle = np.arccos((R[0, 0] + R[1, 1] + R[2, 2] - 1)/ 2)
    axis = np.array([
        # x
        (R[2, 1] - R[1, 2])/ np.sqrt((R[2, 1] - R[1, 2])
                                                ** 2 + (R[0, 2] - R[2, 0])**2 + (R[1, 0] - R[0, 1])**2),
        # y
        (R[0, 2] - R[2, 0])/ np.sqrt((R[2, 1] - R[1, 2])
                                                ** 2 + (R[0, 2] - R[2, 0])**2 + (R[1, 0] - R[0, 1])**2),
        # z
        (R[1, 0] - R[0, 1])/ np.sqrt((R[2, 1] - R[1, 2])**2 + 
                                                (R[0, 2] - R[2, 0])**2 + (R[1, 0] - R[0, 1])**2)])
    return axis, angle