Create DCM from euler angles
        :param euler: array [roll, pitch, yaw] in rad
        :returns: 3x3 dcm array
542319
def _euler_to_dcm(self, euler):
        
        assert(len(euler) == 3)
        phi = euler[0]
        theta = euler[1]
        psi = euler[2]
        dcm = np.zeros([3, 3])
        c_phi = np.cos(phi)
        s_phi = np.sin(phi)
        c_theta = np.cos(theta)
        s_theta = np.sin(theta)
        c_psi = np.cos(psi)
        s_psi = np.sin(psi)

        dcm[0][0] = c_theta * c_psi
        dcm[0][1] = -c_phi * s_psi + s_phi * s_theta * c_psi
        dcm[0][2] = s_phi * s_psi + c_phi * s_theta * c_psi

        dcm[1][0] = c_theta * s_psi
        dcm[1][1] = c_phi * c_psi + s_phi * s_theta * s_psi
        dcm[1][2] = -s_phi * c_psi + c_phi * s_theta * s_psi

        dcm[2][0] = -s_theta
        dcm[2][1] = s_phi * c_theta
        dcm[2][2] = c_phi * c_theta
        return dcm
962294
def get_orientation_radians(self):
        

        raw = self._get_raw_data('fusionPoseValid', 'fusionPose')

        if raw is not None:
            raw['roll'] = raw.pop('x')
            raw['pitch'] = raw.pop('y')
            raw['yaw'] = raw.pop('z')
            self._last_orientation = raw

        return deepcopy(self._last_orientation)
538829
def _add_torus_extrude_lines(
        self, irad, orad, lcar=None, R=numpy.eye(3), x0=numpy.array([0.0, 0.0, 0.0])
    ):
        
        self.add_comment("Torus")

        # Add circle
        x0t = numpy.dot(R, numpy.array([0.0, orad, 0.0]))
        # Get circles in y-z plane
        Rc = numpy.array([[0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [1.0, 0.0, 0.0]])
        c = self.add_circle(x0 + x0t, irad, lcar=lcar, R=numpy.dot(R, Rc))

        rot_axis = [0.0, 0.0, 1.0]
        rot_axis = numpy.dot(R, rot_axis)
        point_on_rot_axis = [0.0, 0.0, 0.0]
        point_on_rot_axis = numpy.dot(R, point_on_rot_axis) + x0

        # Form the torus by extruding the circle three times by 2/3*pi. This
        # works around the inability of Gmsh to extrude by pi or more. The
        # Extrude() macro returns an array; the first [0] entry in the array is
        # the entity that has been extruded at the far end. This can be used
        # for the following Extrude() step.  The second [1] entry of the array
        # is the surface that was created by the extrusion.
        previous = c.line_loop.lines
        angle = "2*Pi/3"
        all_surfaces = []
        for i in range(3):
            self.add_comment("Round no. {}".format(i + 1))
            for k, p in enumerate(previous):
                # ts1[] = Extrude {{0,0,1}, {0,0,0}, 2*Pi/3}{Line{tc1};};
                # ...
                top, surf, _ = self.extrude(
                    p,
                    rotation_axis=rot_axis,
                    point_on_axis=point_on_rot_axis,
                    angle=angle,
                )
                all_surfaces.append(surf)
                previous[k] = top

        # compound_surface = CompoundSurface(all_surfaces)

        surface_loop = self.add_surface_loop(all_surfaces)
        vol = self.add_volume(surface_loop)

        # The newline at the end is essential:
        # If a GEO file doesn't end in a newline, Gmsh will report a syntax
        # error.
        self.add_comment("\n")
        return vol
359614
def get_angles(self) -> Tuple[List[float], List[float]]:
        
        stacked_params = np.hstack((self.betas, self.gammas))
        vqe = VQE(self.minimizer, minimizer_args=self.minimizer_args,
                  minimizer_kwargs=self.minimizer_kwargs)
        cost_ham = reduce(lambda x, y: x + y, self.cost_ham)
        # maximizing the cost function!
        param_prog = self.get_parameterized_program()
        result = vqe.vqe_run(param_prog, cost_ham, stacked_params, qc=self.qc,
                             **self.vqe_options)
        self.result = result
        betas = result.x[:self.steps]
        gammas = result.x[self.steps:]
        return betas, gammas
230124
function generateArcPath (x, y, radius, startAngle, endAngle) {
  var start = polarToCartesian(x, y, radius, startAngle);
  var end = polarToCartesian(x, y, radius, endAngle);
  var arcSweep = endAngle - startAngle <= 180 ? '0' : '1';
  return [
    'M', start.x, start.y,
    'A', radius, radius, 0, arcSweep, 0, end.x, end.y
  ].join(' ');
}
255685
def euler_number(labels, indexes=None):
    
    if indexes is None:
        labels = labels != 0
        indexes = np.array([1],dtype=np.int32)
    elif getattr(indexes,'__getitem__',False):
        indexes = np.array(indexes,dtype=np.int32)
    else:
        indexes = np.array([indexes],dtype=np.int32)
    fix = fixup_scipy_ndimage_result
    #
    # The algorithm here is from the following reference:
    # S.B. Gray, "Local Properties of Binary Images in Two Dimensions",
    # IEEE Transactions on Computers, Vol c-20 # 5 p 551, May 1971
    #
    # The general idea is that crossings into objects can be measured locally
    # through counting numbers of patterns resulting in crossings. There
    # are three sets that are applicable in Euler Numbers:
    # Q1: 1 0  0 1  0 0  0 0 (or more simply, 1 bit per quad)
    #     0 0  0 0  1 0  0 1
    #
    # Q3: 0 1  1 0  1 1  1 1 (or 3 bits per quad)
    #     1 1  1 1  1 0  0 1
    #
    # QD: 1 0  0 1
    #     0 1  1 0
    #
    # and the Euler number = W of an object is
    #
    # 4W = n(Q1) - n(Q3) - 2n(QD) (equation 34)
    # W  = (n(Q1) - n(Q3) - 2n(QD))/4
    #
    # We shift the label matrix to make matrices, padded by zeros on the
    # sides for each of the four positions of the quad:
    # I00 I01
    # I10 I11
    # 
    # We can then assign each bitquad to a label based on the value
    # of the label at one of the "on" bits. For example, the first pattern
    # of Q1 has the label I00 because that bit is on. It's truth value is
    # I00 != I01 and I00 != I02 and I00 != I03.
    #
    I_shape = (labels.shape[0]+3,labels.shape[1]+3)
    I00 = np.zeros(I_shape,int)
    I01 = np.zeros(I_shape,int)
    I10 = np.zeros(I_shape,int)
    I11 = np.zeros(I_shape,int)
    slice_00 = [slice(1,labels.shape[0]+1),slice(1,labels.shape[1]+1)]
    slice_01 = [slice(1,labels.shape[0]+1),slice(0,labels.shape[1])]
    slice_10 = [slice(labels.shape[0]),slice(1,labels.shape[1]+1)]
    slice_11 = [slice(0,labels.shape[0]),slice(0,labels.shape[1])]
    I00[slice_00] = labels
    I01[slice_01] = labels
    I10[slice_10] = labels
    I11[slice_11] = labels
    #
    # There are 6 binary comparisons among the four bits
    #
    EQ00_01 = I00 == I01;              EQ01_00 = EQ00_01
    EQ00_10 = I00 == I10;              EQ10_00 = EQ00_10
    EQ00_11 = I00 == I11;              EQ11_00 = EQ00_11
    EQ01_10 = I01 == I10;              EQ10_01 = EQ01_10
    EQ01_11 = I01 == I11;              EQ11_01 = EQ01_11
    EQ10_11 = I10 == I11;              EQ11_10 = EQ10_11
    NE00_01 = np.logical_not(EQ00_01); NE01_00 = NE00_01
    NE00_10 = np.logical_not(EQ00_10); NE10_00 = NE00_10
    NE00_11 = np.logical_not(EQ00_11); NE11_00 = NE00_11
    NE01_10 = np.logical_not(EQ01_10); NE10_01 = NE01_10
    NE01_11 = np.logical_not(EQ01_11); NE11_01 = NE01_11
    NE10_11 = np.logical_not(EQ10_11); NE11_10 = NE10_11
    #
    # Q1: 1 0 
    #     0 0
    Q1_condition = (NE00_01 & NE00_10 & NE00_11).astype(int)
    #     0 1
    #     0 0
    Q1_condition[slice_00] += (NE01_00 & NE01_10 & NE01_11)[slice_01]
    #     0 0
    #     1 0
    Q1_condition[slice_00] += (NE10_00 & NE10_01 & NE10_11)[slice_10]
    #     0 0
    #     0 1
    Q1_condition[slice_00] += (NE11_00 & NE11_01 & NE11_10)[slice_11]
    Q1 = fix(scind.sum(Q1_condition, I00, indexes))
    #
    # Q3: 1 1
    #     1 0
    Q3_condition = (EQ00_10 & EQ00_01 & NE00_11).astype(int)
    #     0 1
    #     1 1
    Q3_condition[slice_00] += (NE11_00 & EQ11_10 & EQ11_01)[slice_11]
    #     1 0
    #     1 1
    Q3_condition += (NE00_01 & EQ00_10 & EQ00_11)
    #     1 1
    #     0 1
    Q3_condition += (NE00_10 & EQ00_01 & EQ00_11)
    Q3 = fix(scind.sum(Q3_condition, I00, indexes))
    # QD: 1 0
    #     0 1
    QD_condition = (NE00_01 & NE00_10 & EQ00_11).astype(int)
    #     0 1
    #     1 0
    QD_condition[slice_00] += (NE01_00 & NE01_11 & EQ01_10)[slice_01]
    QD  = fix(scind.sum(QD_condition, I00, indexes))
    W = (Q1 - Q3 - 2*QD).astype(float)/4.0
    if indexes is None:
        return W[0]
    return W
361093
def q(self):
        
        if self._q is None:
            if self._euler is not None:
                # get q from euler
                self._q = self._euler_to_q(self.euler)
            elif self._dcm is not None:
                # get q from DCM
                self._q = self._dcm_to_q(self.dcm)
        return self._q
1355080
def _euler_to_q(self, euler):
        
        assert(len(euler) == 3)
        phi = euler[0]
        theta = euler[1]
        psi = euler[2]
        c_phi_2 = np.cos(phi / 2)
        s_phi_2 = np.sin(phi / 2)
        c_theta_2 = np.cos(theta / 2)
        s_theta_2 = np.sin(theta / 2)
        c_psi_2 = np.cos(psi / 2)
        s_psi_2 = np.sin(psi / 2)
        q = np.zeros(4)
        q[0] = (c_phi_2 * c_theta_2 * c_psi_2 +
                s_phi_2 * s_theta_2 * s_psi_2)
        q[1] = (s_phi_2 * c_theta_2 * c_psi_2 -
                c_phi_2 * s_theta_2 * s_psi_2)
        q[2] = (c_phi_2 * s_theta_2 * c_psi_2 +
                s_phi_2 * c_theta_2 * s_psi_2)
        q[3] = (c_phi_2 * c_theta_2 * s_psi_2 -
                s_phi_2 * s_theta_2 * c_psi_2)
        return q
278737
def rotation(angle):
    """"""
    return N.array([[N.cos(angle),-N.sin(angle),0],
        [N.sin(angle), N.cos(angle),0],
        [0           , 0           ,1]])
736779
def get_nodal_planes(self):
        
        # Convert reference frame to NED
        self.tensor, self.tensor_sigma = self._to_ned()
        self.ref_frame = 'NED'
        # Eigenvalue decomposition
        # Tensor
        _, evect = utils.eigendecompose(self.tensor)
        # Rotation matrix
        _, rot_vec = utils.eigendecompose(np.matrix([[0., 0., -1],
                                                     [0., 0., 0.],
                                                     [-1., 0., 0.]]))
        rotation_matrix = (np.matrix(evect * rot_vec.T)).T
        if np.linalg.det(rotation_matrix) < 0.:
            rotation_matrix *= -1.
        flip_dc = np.matrix([[0., 0., -1.],
                             [0., -1., 0.],
                             [-1., 0., 0.]])
        rotation_matrices = sorted(
            [rotation_matrix, flip_dc * rotation_matrix],
            cmp=cmp_mat)
        nodal_planes = GCMTNodalPlanes()
        dip, strike, rake = [(180. / pi) * angle
                             for angle in utils.matrix_to_euler(rotation_matrices[0])]
        # 1st Nodal Plane
        nodal_planes.nodal_plane_1 = {'strike': strike % 360,
                                      'dip': dip,
                                      'rake': -rake}

        # 2nd Nodal Plane
        dip, strike, rake = [(180. / pi) * angle
                             for angle in utils.matrix_to_euler(rotation_matrices[1])]
        nodal_planes.nodal_plane_2 = {'strike': strike % 360.,
                                      'dip': dip,
                                      'rake': -rake}
        return nodal_planes
273930
def build_rotation(vec3):
        
        if not isinstance(vec3, Vector3):
            raise ValueError("rotAmt must be a Vector3")
        return Matrix4.x_rotate(vec3.x) * Matrix4.y_rotate(vec3.y) * Matrix4.z_rotate(vec3.z)
291984
def odometry2Pose3D(odom):
    '''
    

    '''
    pose = Pose3d()
    ori = odom.pose.pose.orientation

    pose.x = odom.pose.pose.position.x
    pose.y = odom.pose.pose.position.y
    pose.z = odom.pose.pose.position.z
    #pose.h = odom.pose.pose.position.h
    pose.yaw = quat2Yaw(ori.w, ori.x, ori.y, ori.z)
    pose.pitch = quat2Pitch(ori.w, ori.x, ori.y, ori.z)
    pose.roll = quat2Roll(ori.w, ori.x, ori.y, ori.z)
    pose.q = [ori.w, ori.x, ori.y, ori.z]
    pose.timeStamp = odom.header.stamp.secs + (odom.header.stamp.nsecs *1e-9)

    return pose
150635
def euler(self, euler):
        
        assert(len(euler) == 3)
        self._euler = np.array(euler)

        # mark other representations as outdated, will get generated on next
        # read
        self._q = None
        self._dcm = None
57091
def unroll_angles(A,sign):
    
    n = np.array([0,0,0])
    P = np.zeros(np.shape(A))
    P[0]=A[0]
    for i in range(1,len(A)):
        n = n+((A[i]-A[i-1]+0.5*sign*np.pi)*sign<0)*np.ones(3)*2.*np.pi
        P[i] = A[i]+sign*n
    return P
1134884
def make_pinwheel(radial_std, tangential_std, num_classes, num_per_class, rate,
                  rs=npr.RandomState(0)):
    """"""
    rads = np.linspace(0, 2*np.pi, num_classes, endpoint=False)

    features = rs.randn(num_classes*num_per_class, 2) \
        * np.array([radial_std, tangential_std])
    features[:, 0] += 1
    labels = np.repeat(np.arange(num_classes), num_per_class)

    angles = rads[labels] + rate * np.exp(features[:,0])
    rotations = np.stack([np.cos(angles), -np.sin(angles), np.sin(angles), np.cos(angles)])
    rotations = np.reshape(rotations.T, (-1, 2, 2))

    return np.einsum('ti,tij->tj', features, rotations)
955675
def simxGetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, operationMode):
    '''
    
    '''
    eulerAngles = (ct.c_float*3)()
    ret = c_GetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, eulerAngles, operationMode)
    arr = []
    for i in range(3):
        arr.append(eulerAngles[i])
    return ret, arr
90274
def euler_angles_and_eigenframes(self):
        ''''''
        signs = np.array([[1, 1, 1], [-1, 1, 1], [1, -1, 1],
                          [1, 1, -1], [-1, -1, 1], [-1, 1, -1],
                          [1, -1, -1], [-1, -1, -1]])
        eulangs = []
        eigframes = []
        for i, sign in enumerate(signs):
            eigframe = np.dot(self.eigvecs, np.diag(sign))
            if np.linalg.det(eigframe) > 1e-4:
                eigframes.append(np.array(eigframe))
                eulangs.append(np.array(
                    transformations.euler_from_matrix(eigframe, axes='szyz')))

        self.eigframes = np.array(eigframes)
        # The sign has to be inverted to be consistent with ORCA and EasySpin.
        self.eulangs = -np.array(eulangs)