Python numpy.cross() Examples

The following are 30 code examples for showing how to use numpy.cross(). These examples are extracted from open source projects. You can vote up the ones you like or vote down the ones you don't like, and go to the original project or source file by following the links above each example.

You may check out the related API usage on the sidebar.

You may also want to check out all available functions/classes of the module numpy , or try the search function .

Example 1
Project: neuropythy   Author: noahbenson   File: util.py    License: GNU Affero General Public License v3.0 7 votes vote down vote up
def vector_angle(u, v, direction=None):
    '''
    vector_angle(u, v) yields the angle between the two vectors u and v. The optional argument 
    direction is by default None, which specifies that the smallest possible angle between the
    vectors be reported; if the vectors u and v are 2D vectors and direction parameters True and
    False specify the clockwise or counter-clockwise directions, respectively; if the vectors are
    3D vectors, then direction may be a 3D point that is not in the plane containing u, v, and the
    origin, and it specifies around which direction (u x v or v x u) the the counter-clockwise angle
    from u to v should be reported (the cross product vector that has a positive dot product with
    the direction argument is used as the rotation axis).
    '''
    if direction is None:
        return np.arccos(vector_angle_cos(u, v))
    elif direction is True:
        return np.arctan2(v[1], v[0]) - np.arctan2(u[1], u[0])
    elif direction is False:
        return np.arctan2(u[1], u[0]) - np.arctan2(v[1], v[0])
    else:
        axis1 = normalize(u)
        axis2 = normalize(np.cross(u, v))
        if np.dot(axis2, direction) < 0:
            axis2 = -axis2
        return np.arctan2(np.dot(axis2, v), np.dot(axis1, v)) 
Example 2
Project: StructEngPy   Author: zhuoju36   File: csys.py    License: MIT License 6 votes vote down vote up
def __init__(self,origin, pt1, pt2, name=None):
        """
        origin: 3x1 vector
        pt1: 3x1 vector
        pt2: 3x1 vector
        """
        self.__origin=origin    
        vec1 = np.array([pt1[0] - origin[0] , pt1[1] - origin[1] , pt1[2] - origin[2]])
        vec2 = np.array([pt2[0] - origin[0] , pt2[1] - origin[1] , pt2[2] - origin[2]])
        cos = np.dot(vec1, vec2)/np.linalg.norm(vec1)/np.linalg.norm(vec2)
        if  cos == 1 or cos == -1:
            raise Exception("Three points should not in a line!!")        
        self.__x = vec1/np.linalg.norm(vec1)
        z = np.cross(vec1, vec2)
        self.__z = z/np.linalg.norm(z)
        self.__y = np.cross(self.z, self.x)
        self.__name=uuid.uuid1() if name==None else name 
Example 3
Project: StructEngPy   Author: zhuoju36   File: csys.py    License: MIT License 6 votes vote down vote up
def set_by_3pts(self,origin, pt1, pt2):
        """
        origin: tuple 3
        pt1: tuple 3
        pt2: tuple 3
        """
        self.origin=origin    
        vec1 = np.array([pt1[0] - origin[0] , pt1[1] - origin[1] , pt1[2] - origin[2]])
        vec2 = np.array([pt2[0] - origin[0] , pt2[1] - origin[1] , pt2[2] - origin[2]])
        cos = np.dot(vec1, vec2)/np.linalg.norm(vec1)/np.linalg.norm(vec2)
        if  cos == 1 or cos == -1:
            raise Exception("Three points should not in a line!!")        
        self.x = vec1/np.linalg.norm(vec1)
        z = np.cross(vec1, vec2)
        self.z = z/np.linalg.norm(z)
        self.y = np.cross(self.z, self.x) 
Example 4
Project: neuropythy   Author: noahbenson   File: retinotopy.py    License: GNU Affero General Public License v3.0 6 votes vote down vote up
def _retinotopic_field_sign_triangles(m, retinotopy):
    t = m.tess if isinstance(m, geo.Mesh) or isinstance(m, geo.Topology) else m
    # get the polar angle and eccen data as a complex number in degrees
    if pimms.is_str(retinotopy):
        (x,y) = as_retinotopy(retinotopy_data(m, retinotopy), 'geographical')
    elif retinotopy is Ellipsis:
        (x,y) = as_retinotopy(retinotopy_data(m, 'any'),      'geographical')
    else:
        (x,y) = as_retinotopy(retinotopy,                     'geographical')
    # Okay, now we want to make some coordinates...
    coords = np.asarray([x, y])
    us = coords[:, t.indexed_faces[1]] - coords[:, t.indexed_faces[0]]
    vs = coords[:, t.indexed_faces[2]] - coords[:, t.indexed_faces[0]]
    (us,vs) = [np.concatenate((xs, np.full((1, t.face_count), 0.0))) for xs in [us,vs]]
    xs = np.cross(us, vs, axis=0)[2]
    xs[np.isclose(xs, 0)] = 0
    return np.sign(xs) 
Example 5
Project: DOTA_models   Author: ringringyi   File: rotation_utils.py    License: Apache License 2.0 6 votes vote down vote up
def rotate_camera_to_point_at(up_from, lookat_from, up_to, lookat_to):
  inputs = [up_from, lookat_from, up_to, lookat_to]
  for i in range(4):
    inputs[i] = normalize(np.array(inputs[i]).reshape((-1,)))
  up_from, lookat_from, up_to, lookat_to = inputs
  r1 = r_between(lookat_from, lookat_to)

  new_x = np.dot(r1, np.array([1, 0, 0]).reshape((-1, 1))).reshape((-1))
  to_x = normalize(np.cross(lookat_to, up_to))
  angle = np.arccos(np.dot(new_x, to_x))
  if angle > ANGLE_EPS:
    if angle < np.pi - ANGLE_EPS:
      ax = normalize(np.cross(new_x, to_x))
      flip = np.dot(lookat_to, ax)
      if flip > 0:
        r2 = get_r_matrix(lookat_to, angle)
      elif flip < 0:
        r2 = get_r_matrix(lookat_to, -1. * angle)
    else:
      # Angle of rotation is too close to 180 degrees, direction of rotation
      # does not matter.
      r2 = get_r_matrix(lookat_to, angle)
  else:
    r2 = np.eye(3)
  return np.dot(r2, r1) 
Example 6
Project: PyOptiX   Author: ozen   File: common.py    License: MIT License 6 votes vote down vote up
def calculate_camera_variables(eye, lookat, up, fov, aspect_ratio, fov_is_vertical=False):
    import numpy as np
    import math

    W = np.array(lookat) - np.array(eye)
    wlen = np.linalg.norm(W)
    U = np.cross(W, np.array(up))
    U /= np.linalg.norm(U)
    V = np.cross(U, W)
    V /= np.linalg.norm(V)

    if fov_is_vertical:
        vlen = wlen * math.tan(0.5 * fov * math.pi / 180.0)
        V *= vlen
        ulen = vlen * aspect_ratio
        U *= ulen
    else:
        ulen = wlen * math.tan(0.5 * fov * math.pi / 180.0)
        U *= ulen
        vlen = ulen * aspect_ratio
        V *= vlen

    return U, V, W 
Example 7
Project: pymesh   Author: taxpon   File: base.py    License: MIT License 6 votes vote down vote up
def update_normals(self):
        v0 = self.vectors[:, 0, :3]
        v1 = self.vectors[:, 1, :3]
        v2 = self.vectors[:, 2, :3]
        _normals = numpy.cross(v1 - v0, v2 - v0)

        for i in range(len(_normals)):
            norm = numpy.linalg.norm(_normals[i])
            if norm != 0:
                _normals[i] /= numpy.linalg.norm(_normals[i])

        self.normals[:] = _normals
        return self

    #####################################################################
    # Analyze functions
    # 
Example 8
Project: pyscf   Author: pyscf   File: pywannier90.py    License: Apache License 2.0 6 votes vote down vote up
def transform(x_vec, z_vec):
    '''
    Construct a transformation matrix to transform r_vec to the new coordinate system defined by x_vec and z_vec
    '''

    x_vec = x_vec/np.linalg.norm(np.asarray(x_vec))
    z_vec = z_vec/np.linalg.norm(np.asarray(z_vec))
    assert x_vec.dot(z_vec) == 0
    y_vec = np.cross(x_vec,z_vec)
    new = np.asarray([x_vec, y_vec, z_vec])
    original = np.asarray([[1,0,0],[0,1,0],[0,0,1]])

    tran_matrix = np.empty([3,3])
    for row in range(3):
        for col in range(3):
            tran_matrix[row,col] = np.cos(angle(original[row],new[col]))

    return tran_matrix.T 
Example 9
Project: pyscf   Author: pyscf   File: mesh_affine_equ.py    License: Apache License 2.0 6 votes vote down vote up
def __init__(self, **kw):
    """  
      Constructor of affine, equidistant 3d mesh class
      ucell : unit cell vectors (in coordinate space)
      Ecut  : Energy cutoff to parametrize the discretization 
    """
    from scipy.fftpack import next_fast_len
    
    self.ucell = kw['ucell'] if 'ucell' in kw else 30.0*np.eye(3) # Not even unit cells vectors are required by default
    self.Ecut = Ecut = kw['Ecut'] if 'Ecut' in kw else 50.0 # 50.0 Hartree by default
    luc = np.sqrt(np.einsum('ix,ix->i', self.ucell, self.ucell))
    self.shape = nn = np.array([next_fast_len( int(np.rint(l * np.sqrt(Ecut)/2))) for l in luc], dtype=int)
    self.size  = np.prod(self.shape)
    gc = self.ucell/(nn) # This is probable the best for finite systems, for PBC use nn, not (nn-1)
    self.dv = np.abs(np.dot(gc[0], np.cross(gc[1], gc[2] )))
    rr = [np.array([gc[i]*j for j in range(nn[i])]) for i in range(3)]
    self.rr = rr
    self.origin = kw['origin'] if 'origin' in kw else np.zeros(3) 
Example 10
Project: me-ica   Author: ME-ICA   File: dicomwrappers.py    License: GNU Lesser General Public License v2.1 6 votes vote down vote up
def slice_normal(self):
        #The std_slice_normal comes from the cross product of the directions 
        #in the ImageOrientationPatient
        std_slice_normal = super(SiemensWrapper, self).slice_normal
        csa_slice_normal = csar.get_slice_normal(self.csa_header)
        if std_slice_normal is None and csa_slice_normal is None:
            return None
        elif std_slice_normal is None:
            return np.array(csa_slice_normal)
        elif csa_slice_normal is None:
            return std_slice_normal
        else:
            #Make sure the two normals are very close to parallel unit vectors
            dot_prod = np.dot(csa_slice_normal, std_slice_normal)
            assert np.allclose(np.fabs(dot_prod), 1.0, atol=1e-5)
            #Use the slice normal computed with the cross product as it will 
            #always be the most orthogonal, but take the sign from the CSA
            #slice normal
            if dot_prod < 0:
                return -std_slice_normal
            else:
                return std_slice_normal 
Example 11
Project: simnibs   Author: simnibs   File: coil_numpy.py    License: GNU General Public License v3.0 6 votes vote down vote up
def _calculate_dadt_ccd(msh, ccd_file, coil_matrix, didt, geo_fn):
    """ auxiliary function to calculate the dA/dt field from a ccd file """
    # read ccd file
    d_position, d_moment = read_ccd(ccd_file)
    # transfrom positions to mm
    d_position *= 1e3
    # add a column to the position in order to apply the transformation matrix
    d_position = np.hstack([d_position, np.ones((d_position.shape[0], 1))])
    d_position = coil_matrix.dot(d_position.T).T[:, :3]
    # rotate the moment
    d_moment = coil_matrix[:3, :3].dot(d_moment.T).T
    A = np.zeros((msh.nodes.nr, 3), dtype=float)
    for p, m in zip(d_position, d_moment):
        # get distance of point to dipole, transform back to meters
        r = (msh.nodes.node_coord - p) * 1e-3
        A += 1e-7 * didt * np.cross(m, r) / (np.linalg.norm(r, axis=1)[:, None] ** 3)
    node_data = mesh_io.NodeData(A)

    if geo_fn is not None:
        mesh_io.write_geo_spheres(d_position, geo_fn,
                               np.linalg.norm(d_moment, axis=1),
                               'coil_dipoles')

    return node_data 
Example 12
Project: simnibs   Author: simnibs   File: test_fem.py    License: GNU General Public License v3.0 6 votes vote down vote up
def tms_sphere(sphere3_msh):
    m = sphere3_msh.crop_mesh(elm_type=4)
    dipole_pos = np.array([0., 0., 300])
    dipole_moment = np.array([1., 0., 0.])
    didt = 1e6
    r = (m.nodes.node_coord - dipole_pos) * 1e-3
    dAdt = 1e-7 * didt * np.cross(dipole_moment, r) / (np.linalg.norm(r, axis=1)[:, None] ** 3)
    dAdt = mesh_io.NodeData(dAdt, mesh=m)
    dAdt.field_name = 'dAdt'
    dAdt.mesh = m
    pos = m.elements_baricenters().value
    E_analytical = analytical_solutions.tms_E_field(dipole_pos * 1e-3,
                                                    dipole_moment, didt,
                                                    pos * 1e-3)
    cond = mesh_io.ElementData(np.ones(m.elm.nr))
    cond.mesh = m
    return m, cond, dAdt, E_analytical 
Example 13
Project: DIB-R   Author: nv-tlabs   File: utils_perspective.py    License: MIT License 6 votes vote down vote up
def camera_info(param):
    theta = np.deg2rad(param[0])
    phi = np.deg2rad(param[1])

    camY = param[3] * np.sin(phi)
    temp = param[3] * np.cos(phi)
    camX = temp * np.cos(theta)    
    camZ = temp * np.sin(theta)        
    cam_pos = np.array([camX, camY, camZ])        

    axisZ = cam_pos.copy()
    axisY = np.array([0, 1, 0], dtype=np.float32)
    axisX = np.cross(axisY, axisZ)
    axisY = np.cross(axisZ, axisX)
    
    # cam_mat = np.array([axisX, axisY, axisZ])
    cam_mat = np.array([unit(axisX), unit(axisY), unit(axisZ)])
    
    return cam_mat, cam_pos


##################################################### 
Example 14
Project: rai-python   Author: MarcToussaint   File: transformations.py    License: MIT License 6 votes vote down vote up
def shear_matrix(angle, direction, point, normal):
    """Return matrix to shear by angle along direction vector on shear plane.
    The shear plane is defined by a point and normal vector. The direction
    vector must be orthogonal to the plane's normal vector.
    A point P is transformed by the shear matrix into P" such that
    the vector P-P" is parallel to the direction vector and its extent is
    given by the angle of P-P'-P", where P' is the orthogonal projection
    of P onto the shear plane.
    >>> angle = (random.random() - 0.5) * 4*math.pi
    >>> direct = numpy.random.random(3) - 0.5
    >>> point = numpy.random.random(3) - 0.5
    >>> normal = numpy.cross(direct, numpy.random.random(3))
    >>> S = shear_matrix(angle, direct, point, normal)
    >>> numpy.allclose(1.0, numpy.linalg.det(S))
    True
    """
    normal = unit_vector(normal[:3])
    direction = unit_vector(direction[:3])
    if abs(numpy.dot(normal, direction)) > 1e-6:
        raise ValueError("direction and normal vectors are not orthogonal")
    angle = math.tan(angle)
    M = numpy.identity(4)
    M[:3, :3] += angle * numpy.outer(direction, normal)
    M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction
    return M 
Example 15
Project: connecting_the_dots   Author: autonomousvision   File: geometry.py    License: MIT License 6 votes vote down vote up
def mesh_adjust_winding_order(verts, faces, normals):
  n0 = normals[faces[:,0]]
  n1 = normals[faces[:,1]]
  n2 = normals[faces[:,2]]
  fnormals = (n0 + n1 + n2) / 3

  v0 = verts[faces[:,0]]
  v1 = verts[faces[:,1]]
  v2 = verts[faces[:,2]]

  e0 = v1 - v0
  e1 = v2 - v0
  fn = np.cross(e0, e1)

  dot = np.sum(fnormals * fn, axis=1)
  ma = dot < 0

  nfaces = faces.copy()
  nfaces[ma,1], nfaces[ma,2] = nfaces[ma,2], nfaces[ma,1]

  return nfaces 
Example 16
Project: 2D-Motion-Retargeting   Author: ChrisWu1997   File: motion.py    License: MIT License 6 votes vote down vote up
def get_local3d(motion3d, angles=None):
    """
    Get the unit vectors for local rectangular coordinates for given 3D motion
    :param motion3d: numpy array. 3D motion from 3D joints positions, shape (nr_joints, 3, nr_frames).
    :param angles: tuple of length 3. Rotation angles around each axis.
    :return: numpy array. unit vectors for local rectangular coordinates's , shape (3, 3).
    """
    # 2 RightArm 5 LeftArm 9 RightUpLeg 12 LeftUpLeg
    horizontal = (motion3d[2] - motion3d[5] + motion3d[9] - motion3d[12]) / 2
    horizontal = np.mean(horizontal, axis=1)
    horizontal = horizontal / np.linalg.norm(horizontal)
    local_z = np.array([0, 0, 1])
    local_y = np.cross(horizontal, local_z)  # bugs!!!, horizontal and local_Z may not be perpendicular
    local_y = local_y / np.linalg.norm(local_y)
    local_x = np.cross(local_y, local_z)
    local = np.stack([local_x, local_y, local_z], axis=0)

    if angles is not None:
        local = rotate_coordinates(local, angles)

    return local 
Example 17
Project: Parsing-R-CNN   Author: soeaver   File: densepose_methods.py    License: MIT License 6 votes vote down vote up
def barycentric_coordinates_exists(self, P0, P1, P2, P):
        u = P1 - P0
        v = P2 - P0
        w = P - P0

        vCrossW = np.cross(v, w)
        vCrossU = np.cross(v, u)
        if np.dot(vCrossW, vCrossU) < 0:
            return False

        uCrossW = np.cross(u, w)
        uCrossV = np.cross(u, v)

        if np.dot(uCrossW, uCrossV) < 0:
            return False

        denom = np.sqrt((uCrossV ** 2).sum())
        r = np.sqrt((vCrossW ** 2).sum()) / denom
        t = np.sqrt((uCrossW ** 2).sum()) / denom

        return (r <= 1) & (t <= 1) & (r + t <= 1) 
Example 18
Project: laplacian-meshes   Author: bmershon   File: PolyMesh.py    License: GNU General Public License v3.0 6 votes vote down vote up
def updateNormalBuffer(self):
        #First compute cross products of all face triangles
        V1 = self.VPos[self.ITris[:, 1], :] - self.VPos[self.ITris[:, 0], :]
        V2 = self.VPos[self.ITris[:, 2], :] - self.VPos[self.ITris[:, 0], :]
        FNormals = np.cross(V1, V2)
        FAreas = np.reshape(np.sqrt(np.sum(FNormals**2, 1)), (FNormals.shape[0], 1))
        FAreas[FAreas == 0] = 1
        FNormals = FNormals/FAreas
        self.FNormals = FNormals
        self.FCentroid = 0*FNormals
        self.VNormals = 0*self.VPos
        VAreas = np.zeros((self.VPos.shape[0], 1))
        for k in range(3):
            self.VNormals[self.ITris[:, k], :] += FAreas*FNormals
            VAreas[self.ITris[:, k]] += FAreas
            self.FCentroid += self.VPos[self.ITris[:, k], :]
        self.FCentroid /= 3
        #Normalize normals
        VAreas[VAreas == 0] = 1
        self.VNormals = self.VNormals / VAreas
    
    #buffersOnly: True if there is no mesh structure other than VPos
    #VColors and ITris 
Example 19
Project: neuropythy   Author: noahbenson   File: util.py    License: GNU Affero General Public License v3.0 5 votes vote down vote up
def alignment_matrix_3D(u, v):
    '''
    alignment_matrix_3D(u, v) yields a 3x3 numpy array that rotates the vector u to the vector v
    around the origin.
    '''
    # normalize both vectors:
    u = normalize(u)
    v = normalize(v)
    # get the cross product of u cross v
    w = np.cross(u, v)
    # the angle we need to rotate
    th = vector_angle(u, v)
    # we rotate around this vector by the angle between them
    return rotation_matrix_3D(w, th) 
Example 20
Project: neuropythy   Author: noahbenson   File: util.py    License: GNU Affero General Public License v3.0 5 votes vote down vote up
def triangle_normal(a,b,c):
    '''
    triangle_normal(a, b, c) yields the normal vector of the triangle whose vertices are given by
      the points a, b, and c. If the points are 2D points, then 3D normal vectors are still yielded,
      that are always (0,0,1) or (0,0,-1). This function auto-threads over matrices, in which case
      they must be in equivalent orientations, and the result is returned in whatever orientation
      they are given in. In some cases, the intended orientation of the matrices is ambiguous (e.g.,
      if a, b, and c are 2 x 3 matrices), in which case the matrix is always assumed to be given in
      (dims x vertices) orientation.
    '''
    (a,b,c) = [np.asarray(x) for x in (a,b,c)]
    if len(a.shape) == 1 and len(b.shape) == 1 and len(c.shape) == 1:
        return triangle_normal(*[np.transpose([x]) for x in (a,b,c)])[:,0]
    (a,b,c) = [np.transpose([x]) if len(x.shape) == 1 else x for x in (a,b,c)]
    # find a required number of dimensions, if possible
    if a.shape[0] in (2,3):
        dims = a.shape[0]
        tx = True
    else:
        dims = a.shape[1]
        (a,b,c) = [x.T for x in (a,b,c)]
        tx = False
    n = (a.shape[1] if a.shape[1] != 1 else b.shape[1] if b.shape[1] != 1 else
         c.shape[1] if c.shape[1] != 1 else 1)
    if dims == 2:
        (a,b,c) = [np.vstack((x, np.zeros((1,n)))) for x in (a,b,c)]
    ab = normalize(b - a)
    ac = normalize(c - a)
    res = np.cross(ab, ac, axisa=0, axisb=0)
    return res.T if tx else res 
Example 21
Project: fullrmc   Author: bachiraoun   File: Collection.py    License: GNU Affero General Public License v3.0 5 votes vote down vote up
def get_random_perpendicular_vector(vector):
    """
    Get random normalized perpendicular vector to a given vector.

    :Parameters:
        #. vector (numpy.ndarray, list, set, tuple): Given vector to compute
           a random perpendicular vector to it.

    :Returns:
        #. perpVector (numpy.ndarray): Perpendicular vector of type
           fullrmc.Globals.FLOAT_TYPE
    """
    vectorNorm = np.linalg.norm(vector)
    assert vectorNorm, LOGGER.error("vector returned 0 norm")
    # easy cases
    if np.abs(vector[0])<PRECISION:
        return np.array([1,0,0], dtype=FLOAT_TYPE)
    elif np.abs(vector[1])<PRECISION:
        return np.array([0,1,0], dtype=FLOAT_TYPE)
    elif np.abs(vector[2])<PRECISION:
        return np.array([0,0,1], dtype=FLOAT_TYPE)
    # generate random vector
    randVect = 1-2*np.random.random(3)
    randvect = np.array([vector[idx]*randVect[idx] for idx in range(3)])
    # get perpendicular vector
    perpVector = np.cross(randvect,vector)
    # normalize, coerce and return
    return np.array(perpVector/np.linalg.norm(perpVector), dtype=FLOAT_TYPE) 
Example 22
Project: fullrmc   Author: bachiraoun   File: Collection.py    License: GNU Affero General Public License v3.0 5 votes vote down vote up
def get_orientation_matrix(arrayAxis, alignToAxis):
    """
    Get the rotation matrix that aligns arrayAxis to alignToAxis

    :Parameters:
        #. arrayAxis (list, tuple, numpy.ndarray): xyzArray axis.
        #. alignToAxis (list, tuple, numpy.ndarray): The axis to align to.
    """
    # normalize alignToAxis
    alignToAxisNorm = np.linalg.norm(alignToAxis)
    assert alignToAxisNorm>0, LOGGER.error("alignToAxis returned 0 norm")
    alignToAxis = np.array(alignToAxis, dtype=FLOAT_TYPE)/alignToAxisNorm
    # normalize arrayAxis
    arrayAxisNorm = np.linalg.norm(arrayAxis)
    assert arrayAxisNorm>0, LOGGER.error("arrayAxis returned 0 norm")
    arrayAxis = np.array(arrayAxis, dtype=FLOAT_TYPE)/arrayAxisNorm
    # calculate rotationAngle
    dotProduct = np.dot(arrayAxis, alignToAxis)
    if np.abs(dotProduct-1) <= PRECISION :
        rotationAngle = 0
    elif np.abs(dotProduct+1) <= PRECISION :
        rotationAngle = PI
    else:
        rotationAngle = np.arccos( dotProduct )
    if np.isnan(rotationAngle) or np.abs(rotationAngle) <= PRECISION :
        return np.array([[1.,0.,0.],[0.,1.,0.],[0.,0.,1.]]).astype(FLOAT_TYPE)
    # calculate rotation axis.
    if np.abs(rotationAngle-PI) <= PRECISION:
        rotationAxis = get_random_perpendicular_vector(arrayAxis)
    else:
        rotationAxis = np.cross(alignToAxis, arrayAxis)
    #rotationAxis /= np.linalg.norm(rotationAxis)
    # calculate rotation matrix
    return get_rotation_matrix(rotationAxis, rotationAngle) 
Example 23
Project: fullrmc   Author: bachiraoun   File: Collection.py    License: GNU Affero General Public License v3.0 5 votes vote down vote up
def orient(xyzArray, arrayAxis, alignToAxis):
    """
    Rotates xyzArray using the rotation matrix that rotates and aligns
    arrayAxis to alignToAXis.

    :Parameters:
        #. xyzArray (numpy.ndarray): The xyz (N,3) array to rotate.
        #. arrayAxis (list, tuple, numpy.ndarray): xyzArray axis.
        #. alignToAxis (list, tuple, numpy.ndarray): The axis to align to.
    """
    # normalize alignToAxis
    alignToAxisNorm = np.linalg.norm(alignToAxis)
    assert alignToAxisNorm>0, LOGGER.error("alignToAxis returned 0 norm")
    alignToAxis = np.array(alignToAxis, dtype=FLOAT_TYPE)/alignToAxisNorm
    # normalize arrayAxis
    arrayAxisNorm = np.linalg.norm(arrayAxis)
    assert arrayAxisNorm>0, LOGGER.error("arrayAxis returned 0 norm")
    arrayAxis = np.array(arrayAxis, dtype=FLOAT_TYPE)/arrayAxisNorm
    # calculate rotationAngle
    dotProduct = np.dot(arrayAxis, alignToAxis)
    if np.abs(dotProduct-1) <= PRECISION :
        rotationAngle = 0
    elif np.abs(dotProduct+1) <= PRECISION :
        rotationAngle = PI
    else:
        rotationAngle = np.arccos( dotProduct )
    if np.isnan(rotationAngle) or np.abs(rotationAngle) <= PRECISION :
        return xyzArray
    # calculate rotation axis.
    if np.abs(rotationAngle-PI) <= PRECISION:
        rotationAxis = get_random_perpendicular_vector(arrayAxis)
    else:
        rotationAxis = np.cross(alignToAxis, arrayAxis)
    #rotationAxis /= np.linalg.norm(rotationAxis)
    # calculate rotation matrix
    rotationMatrix = get_rotation_matrix(rotationAxis, rotationAngle)
    # rotate and return
    return rotate(xyzArray , rotationMatrix) 
Example 24
Project: DOTA_models   Author: ringringyi   File: swiftshader_renderer.py    License: Apache License 2.0 5 votes vote down vote up
def sample_points_on_faces(vs, fs, rng, n_samples_per_face):
  idx = np.repeat(np.arange(fs.shape[0]), n_samples_per_face)
  
  r = rng.rand(idx.size, 2)
  r1 = r[:,:1]; r2 = r[:,1:]; sqrt_r1 = np.sqrt(r1);
  
  v1 = vs[fs[idx, 0], :]; v2 = vs[fs[idx, 1], :]; v3 = vs[fs[idx, 2], :];
  pts = (1-sqrt_r1)*v1 + sqrt_r1*(1-r2)*v2 + sqrt_r1*r2*v3
  
  v1 = vs[fs[:,0], :]; v2 = vs[fs[:, 1], :]; v3 = vs[fs[:, 2], :];
  ar = 0.5*np.sqrt(np.sum(np.cross(v1-v3, v2-v3)**2, 1))
  
  return pts, ar, idx 
Example 25
Project: DOTA_models   Author: ringringyi   File: rotation_utils.py    License: Apache License 2.0 5 votes vote down vote up
def r_between(v_from_, v_to_):
  v_from = normalize(v_from_)
  v_to = normalize(v_to_)
  ax = normalize(np.cross(v_from, v_to))
  angle = np.arccos(np.dot(v_from, v_to))
  return get_r_matrix(ax, angle) 
Example 26
Project: robosuite   Author: StanfordVL   File: baxter_peg_in_hole.py    License: MIT License 5 votes vote down vote up
def _compute_orientation(self):
        """
        Helper function to return the relative positions between the hole and the peg.
        In particular, the intersection of the line defined by the peg and the plane
        defined by the hole is computed; the parallel distance, perpendicular distance,
        and angle are returned.
        """
        cyl_mat = self.sim.data.body_xmat[self.cyl_body_id]
        cyl_mat.shape = (3, 3)
        cyl_pos = self.sim.data.body_xpos[self.cyl_body_id]

        hole_pos = self.sim.data.body_xpos[self.hole_body_id]
        hole_mat = self.sim.data.body_xmat[self.hole_body_id]
        hole_mat.shape = (3, 3)

        v = cyl_mat @ np.array([0, 0, 1])
        v = v / np.linalg.norm(v)
        center = hole_pos + hole_mat @ np.array([0.1, 0, 0])

        t = (center - cyl_pos) @ v / (np.linalg.norm(v) ** 2)
        d = np.linalg.norm(np.cross(v, cyl_pos - center)) / np.linalg.norm(v)

        hole_normal = hole_mat @ np.array([0, 0, 1])
        return (
            t,
            d,
            abs(
                np.dot(hole_normal, v) / np.linalg.norm(hole_normal) / np.linalg.norm(v)
            ),
        ) 
Example 27
Project: robosuite   Author: StanfordVL   File: transform_utils.py    License: MIT License 5 votes vote down vote up
def get_pose_error(target_pose, current_pose):
    """
    Computes the error corresponding to target pose - current pose as a 6-dim vector.
    The first 3 components correspond to translational error while the last 3 components
    correspond to the rotational error.

    Args:
        target_pose: a 4x4 homogenous matrix for the target pose
        current_pose: a 4x4 homogenous matrix for the current pose

    Returns:
        A 6-dim numpy array for the pose error.
    """
    error = np.zeros(6)

    # compute translational error
    target_pos = target_pose[:3, 3]
    current_pos = current_pose[:3, 3]
    pos_err = target_pos - current_pos

    # compute rotational error
    r1 = current_pose[:3, 0]
    r2 = current_pose[:3, 1]
    r3 = current_pose[:3, 2]
    r1d = target_pose[:3, 0]
    r2d = target_pose[:3, 1]
    r3d = target_pose[:3, 2]
    rot_err = 0.5 * (np.cross(r1, r1d) + np.cross(r2, r2d) + np.cross(r3, r3d))

    error[:3] = pos_err
    error[3:] = rot_err
    return error 
Example 28
Project: OpenCV-Computer-Vision-Projects-with-Python   Author: PacktPublishing   File: chapter2.py    License: MIT License 5 votes vote down vote up
def angle(v1, v2):
    """ returns the angle (in radians) between two array-like vectors using the
        cross-product method, which is more accurate for small angles than the
        dot-product-acos method."""
    return np.arctan2(np.linalg.norm(np.cross(v1,v2)), np.dot(v1,v2)) 
Example 29
Project: Modeling-Cloth   Author: the3dadvantage   File: ModelingCloth.py    License: MIT License 5 votes vote down vote up
def tri_normals_in_place(object, tri_co):    
    """Takes N x 3 x 3 set of 3d triangles and 
    returns non-unit normals and origins"""
    object.origins = tri_co[:,0]
    object.cross_vecs = tri_co[:,1:] - object.origins[:, nax]
    object.normals = np.cross(object.cross_vecs[:,0], object.cross_vecs[:,1])
    object.nor_dots = np.einsum("ij, ij->i", object.normals, object.normals)
    object.normals /= np.sqrt(object.nor_dots)[:, nax] 
Example 30
Project: Modeling-Cloth   Author: the3dadvantage   File: ModelingCloth.py    License: MIT License 5 votes vote down vote up
def get_tri_normals(tr_co):
    """Takes N x 3 x 3 set of 3d triangles and 
    returns non-unit normals and origins"""
    origins = tr_co[:,0]
    cross_vecs = tr_co[:,1:] - origins[:, nax]
    return cross_vecs, np.cross(cross_vecs[:,0], cross_vecs[:,1]), origins