Python numpy.cross() Examples

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)) 

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 

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) 

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) 

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) 

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 

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
    # 

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 

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) 

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 

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 

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 

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


##################################################### 

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 

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 

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 

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) 

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 

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) 

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 

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) 

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) 

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) 

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 

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) 

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)
            ),
        ) 

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 

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)) 

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] 

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