def qrot(q, v):
    """
    Rotate vector(s) v about the rotation described by quaternion(s) q.
    Expects a tensor of shape (*, 4) for q and a tensor of shape (*, 3) for v,
    where * denotes any number of dimensions.
    Returns a tensor of shape (*, 3).
    """
    assert q.shape[-1] == 4
    assert v.shape[-1] == 3
    assert q.shape[:-1] == v.shape[:-1]
    
    original_shape = list(v.shape)
    q = q.view(-1, 4)
    v = v.view(-1, 3)
    
    qvec = q[:, 1:]
    uv = torch.cross(qvec, v, dim=1)
    uuv = torch.cross(qvec, uv, dim=1)
    return (v + 2 * (q[:, :1] * uv + uuv)).view(original_shape) 

def cross_prod(x, y):
    z = torch.cross(x.view(-1, 3), y.view(-1, 3), dim=1).view_as(x)
    return z 

def forward(self, z, pos, batch=None):
        """"""
        edge_index = radius_graph(pos, r=self.cutoff, batch=batch)

        i, j, idx_i, idx_j, idx_k, idx_kj, idx_ji = self.triplets(
            edge_index, num_nodes=z.size(0))

        # Calculate distances.
        dist = (pos[i] - pos[j]).pow(2).sum(dim=-1).sqrt()

        # Calculate angles.
        pos_i = pos[idx_i]
        pos_ji, pos_ki = pos[idx_j] - pos_i, pos[idx_k] - pos_i
        a = (pos_ji * pos_ki).sum(dim=-1)
        b = torch.cross(pos_ji, pos_ki).norm(dim=-1)
        angle = torch.atan2(b, a)

        rbf = self.rbf(dist)
        sbf = self.sbf(dist, angle, idx_kj)

        # Embedding block.
        x = self.emb(z, rbf, i, j)
        P = self.output_blocks[0](x, rbf, i, num_nodes=pos.size(0))

        # Interaction blocks.
        for interaction_block, output_block in zip(self.interaction_blocks,
                                                   self.output_blocks[1:]):
            x = interaction_block(x, rbf, sbf, idx_kj, idx_ji)
            P += output_block(x, rbf, i)

        return P.sum(dim=0) if batch is None else scatter(P, batch, dim=0) 

def get_angle(v1: Tensor, v2: Tensor) -> Tensor:
    return torch.atan2(
        torch.cross(v1, v2, dim=1).norm(p=2, dim=1), (v1 * v2).sum(dim=1)) 

def compute_normal_map(x_img, y_img, z, intrinsics):
    cam_coords = lift(x_img, y_img, z, intrinsics)
    cam_coords = util.lin2img(cam_coords)

    shift_left = cam_coords[:, :, 2:, :]
    shift_right = cam_coords[:, :, :-2, :]

    shift_up = cam_coords[:, :, :, 2:]
    shift_down = cam_coords[:, :, :, :-2]

    diff_hor = F.normalize(shift_right - shift_left, dim=1)[:, :, :, 1:-1]
    diff_ver = F.normalize(shift_up - shift_down, dim=1)[:, :, 1:-1, :]

    cross = torch.cross(diff_hor, diff_ver, dim=1)
    return cross 

def get_normal_map(opt,index,vertices,faces):
	face_vertices = vertices[faces.long()]
	v1,v2,v3 = torch.unbind(face_vertices,dim=1)
	normal = F.normalize(torch.cross(v2-v1,v3-v2),dim=1)
	# face normals towards camera
	normal[normal[:,2]<0] *= -1
	normal_color = (normal+1)/2
	normal_color = torch.cat([torch.zeros(1,3,device=opt.device),normal_color],dim=0)
	normal_color[0] = 0
	normal_map = normal_color[index.long()+1].permute(2,0,1)
	return normal_map 

def get_angles(a, b):
    '''
    calculate the angle between vector a and b
    :param a: Bx3xMxK tensor
    :param b: Bx3xMxK tensor
    :return: Bx1xMxK tensor
    '''
    axb = torch.cross(a, b, dim=1)  # Bx3xMxK
    a_1x3 = a.permute(0, 2, 3, 1).contiguous().unsqueeze(3)  # BxMxKx3 -> BxMxKx1x3
    b_3x1 = b.permute(0, 2, 3, 1).contiguous().unsqueeze(4)  # BxMxKx3 -> BxMxKx3x1
    ab = torch.matmul(a_1x3, b_3x1).squeeze(3).squeeze(3)  # BxMxKx1x1

    angle = torch.atan2(torch.norm(axb, dim=1, keepdim=False), ab).unsqueeze(1)
    return angle 

def qrot(q, v):
    """
    Rotate vector(s) v about the rotation described by quaternion(s) q.
    Expects a tensor of shape (*, 4) for q and a tensor of shape (*, 3) for v,
    where * denotes any number of dimensions.
    Returns a tensor of shape (*, 3).
    """
    assert q.shape[-1] == 4
    assert v.shape[-1] == 3
    assert q.shape[:-1] == v.shape[:-1]

    qvec = q[..., 1:]
    uv = torch.cross(qvec, v, dim=len(q.shape) - 1)
    uuv = torch.cross(qvec, uv, dim=len(q.shape) - 1)
    return v + 2 * (q[..., :1] * uv + uuv) 

def vertex_normals(vertices, faces):
    """
    :param vertices: [batch size, number of vertices, 3]
    :param faces: [batch size, number of faces, 3]
    :return: [batch size, number of vertices, 3]
    """
    assert (vertices.ndimension() == 3)
    assert (faces.ndimension() == 3)
    assert (vertices.shape[0] == faces.shape[0])
    assert (vertices.shape[2] == 3)
    assert (faces.shape[2] == 3)

    bs, nv = vertices.shape[:2]
    bs, nf = faces.shape[:2]
    device = vertices.device
    normals = torch.zeros(bs * nv, 3).to(device)

    faces = faces + (torch.arange(bs, dtype=torch.int32).to(device) * nv)[:, None, None] # expanded faces
    vertices_faces = vertices.reshape((bs * nv, 3))[faces.long()]

    faces = faces.view(-1, 3)
    vertices_faces = vertices_faces.view(-1, 3, 3)

    normals.index_add_(0, faces[:, 1].long(), 
                       torch.cross(vertices_faces[:, 2] - vertices_faces[:, 1], vertices_faces[:, 0] - vertices_faces[:, 1]))
    normals.index_add_(0, faces[:, 2].long(), 
                       torch.cross(vertices_faces[:, 0] - vertices_faces[:, 2], vertices_faces[:, 1] - vertices_faces[:, 2]))
    normals.index_add_(0, faces[:, 0].long(),
                       torch.cross(vertices_faces[:, 1] - vertices_faces[:, 0], vertices_faces[:, 2] - vertices_faces[:, 0]))

    normals = F.normalize(normals, eps=1e-6, dim=1)
    normals = normals.reshape((bs, nv, 3))
    # pytorch only supports long and byte tensors for indexing
    return normals 

def surface_normals(self):
        if self._surface_normals_update:
            v10 = self.face_vertices[:, :, 0] - self.face_vertices[:, :, 1]
            v12 = self.face_vertices[:, :, 2] - self.face_vertices[:, :, 1]
            self._surface_normals = F.normalize(torch.cross(v12, v10), p=2, dim=2, eps=1e-6)
            self._surface_normals_update = False
        return self._surface_normals 

def rot_from_2_vectors(v1, v2):
        """ Returns a Rotation matrix between vectors 'v1' and 'v2'    """
        v1 = v1/torch.norm(v1)
        v2 = v2/torch.norm(v2)
        v = torch.cross(v1, v2)
        cosang = v1.matmul(v2)
        sinang = torch.norm(v)
        Rot = TORCHIEKF.Id3 + TORCHIEKF.skew(v) + \
              TORCHIEKF.skew(v).mm(TORCHIEKF.skew(v))*(1-cosang)/(sinang**2)
        return Rot 

def cross_prod(x, y):
    z = torch.cross(x.view(-1, 3), y.view(-1, 3), dim=1).view_as(x)
    return z 

def s2s2_to_SO3(v1, v2=None):
    '''Normalize 2 3-vectors. Project second to orthogonal component.
    Take cross product for third. Stack to form SO matrix.'''
    if v2 is None:
        assert v1.shape[-1] == 6
        v2 = v1[...,3:]
        v1 = v1[...,0:3]
    u1 = v1
    e1 = u1 / u1.norm(p=2, dim=-1, keepdim=True).clamp(min=1E-5)
    u2 = v2 - (e1 * v2).sum(-1, keepdim=True) * e1
    e2 = u2 / u2.norm(p=2, dim=-1, keepdim=True).clamp(min=1E-5)
    e3 = torch.cross(e1, e2)
    return torch.stack([e1, e2, e3], 1) 

def calculate_normals(points , policy = "upright"):
    if policy is "upright":
        points_temp = F.pad(points, (0, 1, 0, 0), mode="replicate")
        dx = points_temp[:, :, :, :-1] - points_temp[:, :, :, 1:]  # NCHW
        points_temp = F.pad(points, (0, 0, 0, 1), mode="replicate")
        dy = points_temp[:, :, :-1, :] - points_temp[:, :, 1:, :]  # NCHW
        normals = torch.cross(dy,dx)
        #mask = (points[:,2,:,:] == 0).float()
        #normals /= torch.sqrt(torch.sum(normals*normals, 1) + mask)        
        #return normals
        return torch.nn.functional.normalize(normals) 

def nomal_loss(pred, targetN,params,depthI,depthJ):
    depthI = depthI.permute(0, 2, 3, 1)
    depthJ = depthJ.permute(0, 2, 3, 1)

    predN_1 = torch.zeros_like(targetN)
    predN_2 = torch.zeros_like(targetN)

    f = params[:, :, :, 0]
    cx = params[:, :, :, 1]
    cy = params[:, :, :, 2]

    z1 = depthJ - pred
    z1 = torch.squeeze(z1)
    depthJ = torch.squeeze(depthJ)
    predN_1[:, :, :, 0] = ((MatJ - cx) * z1 + depthJ) * 1.0 / f
    predN_1[:, :, :, 1] = (MatI - cy) * z1 * 1.0 / f
    predN_1[:, :, :, 2] = z1

    z2 = depthI - pred
    z2 = torch.squeeze(z2)
    depthI = torch.squeeze(depthI)
    predN_2[:, :, :, 0] = (MatJ - cx) * z2  * 1.0 / f
    predN_2[:, :, :, 1] = ((MatI - cy) * z2 + depthI) * 1.0 / f
    predN_2[:, :, :, 2] = z2

    predN = torch.cross(predN_1, predN_2)
    pred_n = F.normalize(predN)
    pred_n = pred_n.contiguous().view(-1, 3)
    target_n = targetN.contiguous().view(-1, 3)

    loss_function = nn.CosineEmbeddingLoss()
    loss = loss_function(pred_n, target_n, Variable(torch.Tensor(pred_n.size(0)).cuda().fill_(1.0)))
    return loss 

def nomal_loss(pred, targetN,params,depthI,depthJ):
    depthI = depthI.permute(0, 2, 3, 1)
    depthJ = depthJ.permute(0, 2, 3, 1)

    predN_1 = torch.zeros_like(targetN)
    predN_2 = torch.zeros_like(targetN)

    f = params[:, :, :, 0]
    cx = params[:, :, :, 1]
    cy = params[:, :, :, 2]

    z1 = depthJ - pred
    z1 = torch.squeeze(z1)
    depthJ = torch.squeeze(depthJ)
    predN_1[:, :, :, 0] = ((MatJ - cx) * z1 + depthJ) * 1.0 / f
    predN_1[:, :, :, 1] = (MatI - cy) * z1 * 1.0 / f
    predN_1[:, :, :, 2] = z1

    z2 = depthI - pred
    z2 = torch.squeeze(z2)
    depthI = torch.squeeze(depthI)
    predN_2[:, :, :, 0] = (MatJ - cx) * z2  * 1.0 / f
    predN_2[:, :, :, 1] = ((MatI - cy) * z2 + depthI) * 1.0 / f
    predN_2[:, :, :, 2] = z2

    predN = torch.cross(predN_1, predN_2)
    pred_n = F.normalize(predN)
    pred_n = pred_n.contiguous().view(-1, 3)
    target_n = targetN.contiguous().view(-1, 3)

    loss_function = nn.CosineEmbeddingLoss()
    loss = loss_function(pred_n, target_n, Variable(torch.Tensor(pred_n.size(0)).cuda().fill_(1.0)))
    return loss 

def compute_face_normals(self, vertices):
        triangles = vertices[self.faces_torch, :]
        u = triangles[::, 1] - triangles[::, 0]
        v = triangles[::, 2] - triangles[::, 0]
        if self.clockwise:
            n = -torch.cross(u, v)
        else:
            n = torch.cross(u, v)
        l2 = (n ** 2).sum(dim=1)
        norm = l2.sqrt()
        nn = n / norm[:, None]
        return nn 

def qrot(q, v):
    qr = q[None, :].repeat(v.shape[0], 1)
    qvec = qr[:, :-1]
    uv = torch.cross(qvec, v, dim=1)
    uuv = torch.cross(qvec, uv, dim=1)
    return v + 2 * (qr[:, [3]] * uv + uuv) 

def qrot(q, v):
    """
    Rotate vector(s) v about the rotation described by quaternion(s) q.
    Expects a tensor of shape (*, 4) for q and a tensor of shape (*, 3) for v,
    where * denotes any number of dimensions.
    Returns a tensor of shape (*, 3).
    """
    assert q.shape[-1] == 4
    assert v.shape[-1] == 3
    assert q.shape[:-1] == v.shape[:-1]

    qvec = q[..., 1:]
    uv = torch.cross(qvec, v, dim=len(q.shape)-1)
    uuv = torch.cross(qvec, uv, dim=len(q.shape)-1)
    return (v + 2 * (q[..., :1] * uv + uuv)) 

def batch_camera_info(param):
	
	theta = (np.pi*param[:,0]/180.) % 360.
	phi = (np.pi*param[:,1]/180.) % 360.

	camY = param[:,2]*torch.sin(phi)
	temp = param[:,2]*torch.cos(phi)
	
	camX = temp * torch.cos(theta)    
	camZ = temp * torch.sin(theta)    

	cam_pos = torch.cat((camX.unsqueeze(1), camY.unsqueeze(1), camZ.unsqueeze(1)), dim = 1 )    
	
	axisZ = cam_pos.clone()
	axisY = torch.FloatTensor([0,1,0]).cuda().unsqueeze(0).expand(axisZ.shape[0], 3)

	axisX = torch.cross(axisY, axisZ)
	axisY = torch.cross(axisZ, axisX)



	axisX = axisX / torch.sqrt(torch.sum(axisX**2, dim = 1)).unsqueeze(-1)
	axisY = axisY / torch.sqrt(torch.sum(axisY**2, dim = 1)).unsqueeze(-1) 
	axisZ = axisZ / torch.sqrt(torch.sum(axisZ**2, dim = 1)).unsqueeze(-1) 

	cam_mat = torch.cat((axisX.unsqueeze(1), axisY.unsqueeze(1), axisZ.unsqueeze(1)), dim = 1 )
	
	return cam_mat, cam_pos 

def offset_to_normal(offset, x, y, z, location):
    """get normal vector of all triangles"""
    p = offset_to_vertices(offset, x, y, z)
    # get unique triangles from all topologies
    triangles, classes = get_unique_triangles(symmetry=0)

    # get normal vector of each triangle
    # assign a dummy normal vector to the unconnected ones

    # the vertices we care on the specific face
    vertices = []
    if location<6:
        vertices = vertices_on_location()[location]

    normal = []
    if offset.is_cuda:
        dtype = torch.cuda.FloatTensor
    else:
        dtype = torch.FloatTensor
    for tri in triangles:
        # if the triangle doesn't has a line on the face we care about 
        # simply assign a dummy normal vector
        intersection = [xx for xx in vertices if xx in tri]
        #print tri, intersection
        if location < 6 and len(intersection)!=2:
            normal.append(Variable(torch.ones(3, 1).type(dtype)))
        else:
	    ### when inside/outside is considered
            a=tri[0]
            b=tri[1]
            c=tri[2]
            normal.append(torch.cross(torch.cat(p[b])-torch.cat(p[a]), torch.cat(p[c])-torch.cat(p[a])).view(3, 1))
    return torch.cat(normal).view(-1,3) 

def adjoint(A):
    """compute inverse without division by det; ...xv3xc3 input, or array of matrices assumed"""
    AI = np.empty_like(A)
    for i in xrange(3):
        AI[...,i,:] = np.cross(A[...,i-2,:], A[...,i-1,:])
    return AI 

def adjoint_torch(A):
    AI = A.clone()
    for i in xrange(3):
        AI[...,i,:] = torch.cross(A[...,i-2,:], A[...,i-1,:])
    return AI 

def qrot(q, v):
    """
    Rotate vector(s) v about the rotation described by 四元数quaternion(s) q.
    Expects a tensor of shape (*, 4) for q and a tensor of shape (*, 3) for v,
    where * denotes any number of dimensions.
    Returns a tensor of shape (*, 3).
    """
    assert q.shape[-1] == 4
    assert v.shape[-1] == 3
    assert q.shape[:-1] == v.shape[:-1]

    qvec = q[..., 1:]
    uv = torch.cross(qvec, v, dim=len(q.shape) - 1)
    uuv = torch.cross(qvec, uv, dim=len(q.shape) - 1)
    return (v + 2 * (q[..., :1] * uv + uuv)) 

def p2f(points_bxpx3, faces_fx3, cameras):
    # perspective, use just one camera
    camera_rot_bx3x3, camera_pos_bx3, camera_proj_3x1 = cameras
    cameratrans_rot_bx3x3 = camera_rot_bx3x3.permute(0, 2, 1)
    
    # follow pixel2mesh!!!
    # new_p = cam_mat * (old_p - cam_pos)
    points_bxpx3 = points_bxpx3 - camera_pos_bx3.view(-1, 1, 3)
    points_bxpx3 = torch.matmul(points_bxpx3, cameratrans_rot_bx3x3)
    
    camera_proj_bx1x3 = camera_proj_3x1.view(-1, 1, 3)
    xy_bxpx3 = points_bxpx3 * camera_proj_bx1x3
    xy_bxpx2 = xy_bxpx3[:, :, :2] / xy_bxpx3[:, :, 2:3]

    ##########################################################
    # 1 points
    pf0_bxfx3 = points_bxpx3[:, faces_fx3[:, 0], :]
    pf1_bxfx3 = points_bxpx3[:, faces_fx3[:, 1], :]
    pf2_bxfx3 = points_bxpx3[:, faces_fx3[:, 2], :]
    points3d_bxfx9 = torch.cat((pf0_bxfx3, pf1_bxfx3, pf2_bxfx3), dim=2)
    
    xy_f0 = xy_bxpx2[:, faces_fx3[:, 0], :]
    xy_f1 = xy_bxpx2[:, faces_fx3[:, 1], :]
    xy_f2 = xy_bxpx2[:, faces_fx3[:, 2], :]
    points2d_bxfx6 = torch.cat((xy_f0, xy_f1, xy_f2), dim=2)
    
    ######################################################
    # 2 normals
    v01_bxfx3 = pf1_bxfx3 - pf0_bxfx3
    v02_bxfx3 = pf2_bxfx3 - pf0_bxfx3
    
    # bs cannot be 3, if it is 3, we must specify dim
    normal_bxfx3 = torch.cross(v01_bxfx3, v02_bxfx3, dim=2)
    normalz_bxfx1 = normal_bxfx3[:, :, 2:3]
    # normalz_bxfx1 = torch.abs(normalz_bxfx1)
    
    normallen_bxfx1 = torch.sqrt(torch.sum(normal_bxfx3 ** 2, dim=2, keepdim=True))
    normal1_bxfx3 = normal_bxfx3 / (normallen_bxfx1 + 1e-15)
    
    return points3d_bxfx9, points2d_bxfx6, normalz_bxfx1, normal1_bxfx3


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

def compute_frustum_normals(self, corner_coords):
        """
        Computes the normal vectors (pointing inwards) to the 6 planes that bound the viewing frustum

        :param corner_coords: torch tensor of shape (8, 4), coordinates of the corner points of the viewing frustum
        :return: normals: torch tensor of shape (6, 3)
        """

        normals = corner_coords.new(6, 3)

        # compute plane normals
        # front plane
        plane_vec1 = corner_coords[3][:3] - corner_coords[0][:3]
        plane_vec2 = corner_coords[1][:3] - corner_coords[0][:3]
        normals[0] = torch.cross(plane_vec1.view(-1), plane_vec2.view(-1))

        # right side plane
        plane_vec1 = corner_coords[2][:3] - corner_coords[1][:3]
        plane_vec2 = corner_coords[5][:3] - corner_coords[1][:3]
        normals[1] = torch.cross(plane_vec1.view(-1), plane_vec2.view(-1))

        # roof plane
        plane_vec1 = corner_coords[3][:3] - corner_coords[2][:3]
        plane_vec2 = corner_coords[6][:3] - corner_coords[2][:3]
        normals[2] = torch.cross(plane_vec1.view(-1), plane_vec2.view(-1))

        # left side plane
        plane_vec1 = corner_coords[0][:3] - corner_coords[3][:3]
        plane_vec2 = corner_coords[7][:3] - corner_coords[3][:3]
        normals[3] = torch.cross(plane_vec1.view(-1), plane_vec2.view(-1))

        # bottom plane
        plane_vec1 = corner_coords[1][:3] - corner_coords[0][:3]
        plane_vec2 = corner_coords[4][:3] - corner_coords[0][:3]
        normals[4] = torch.cross(plane_vec1.view(-1), plane_vec2.view(-1))

        # back plane
        plane_vec1 = corner_coords[6][:3] - corner_coords[5][:3]
        plane_vec2 = corner_coords[4][:3] - corner_coords[5][:3]
        normals[5] = torch.cross(plane_vec1.view(-1), plane_vec2.view(-1))

        return normals 

def depthToNormal(depthmap,
                  device,
                  K,
                  thres,
                  img_size
                 ):
    # default
    K_torch = torch.tensor([K[0], K[1], K[2], K[3]]).to(device)

    C, H, W = depthmap.size()
    assert( C==1 and H == img_size[0] and W == img_size[1])
    
    depthmap = torch.reshape(depthmap, [H,W]) #resize to H W 
    
    X_grid, Y_grid = torch.meshgrid( [torch.arange(H, out=torch.FloatTensor().to(device)), torch.arange(W, out=torch.FloatTensor().to(device))] )
    
    X = (X_grid - K_torch[2]) * depthmap / K_torch[0] 
    Y = (Y_grid - K_torch[3]) * depthmap / K_torch[1]
    
    DepthPoints = torch.stack([X, Y, depthmap], dim=2) # all 3D point 
    
    delta_right = DepthPoints[2:,1:-1,:] - DepthPoints[1:-1,1:-1,:]
    delta_down  = DepthPoints[1:-1,2:,:] - DepthPoints[1:-1,1:-1,:]
    
    delta_left = DepthPoints[0:-2,1:-1,:] - DepthPoints[1:-1,1:-1,:]
    delta_up   = DepthPoints[1:-1,0:-2,:] - DepthPoints[1:-1,1:-1,:]
    
    
    normal_crop1 = torch.cross(delta_down, delta_right)
    normal_crop1 = F.normalize(normal_crop1, p=2, dim=2)
    
    normal_crop2 = torch.cross(delta_up,   delta_left )
    normal_crop2 = F.normalize(normal_crop2, p=2, dim=2)
    
    normal_crop  = normal_crop1 + normal_crop2
    normal_crop  = F.normalize(normal_crop, p=2, dim=2)
    
    
    normal = torch.zeros(H,W,3).to(device)
    normal[1:-1,1:-1,:] = normal_crop
    
    confidence_map_crop = torch.ones(H-2,W-2).to(device)
    
    delta_right_norm = torch.norm(delta_right, p=2, dim=2)
    delta_down_norm  = torch.norm(delta_down, p=2, dim=2)
    confidence_map_crop[ delta_right_norm > thres] = 0.0
    confidence_map_crop[ delta_down_norm > thres] = 0.0
    
    confidence_map = torch.zeros(H,W)
    confidence_map[1:-1,1:-1] = confidence_map_crop
    confidence_map[depthmap == 0] = 0
    
    # change to CHW
    normal = normal.permute(2, 0, 1)
    
    # return the  normal [3,H,W] and confidence map
    return normal, confidence_map 

def ray_triangle_intersection(ray_near, ray_dir, v123):
    """
    Möller–Trumbore intersection algorithm in pure python
    Based on http://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
    """
    v1, v2, v3 = v123
    eps = 0.000001
    edge1 = v2 - v1
    edge2 = v3 - v1
    pvec = np.cross(ray_dir, edge2)
    det = edge1.dot(pvec)
    retval = None
    ret = True
    if abs(det) < eps:
        ret = False
    inv_det = 1.0 / det
    tvec = ray_near - v1
    u = tvec.dot(pvec) * inv_det
    if u < 0.0 or u > 1.0:
        ret = False
    qvec = np.cross(tvec, edge1)
    v = ray_dir.dot(qvec) * inv_det
    if v < 0.0 or u + v > 1.0:
        ret = False

    t = edge2.dot(qvec) * inv_det
    if t < eps:
        ret = False
    retval = t
    if ret:
        print("v0", v1)
        print("v1", v2)
        print("v2", v3)
        print("edge1", edge1)
        print("edge2", edge2)
        print("det", det)
        print("tvec", tvec)
        print("u", u)
        print("v", v)
        print("qvec", qvec)
        print("pvec", pvec)

    return ret, retval


# Full batch mode 

def batch_mesh_contains_point(ray_origin, obj_triangles, tol_thresh=0.000001):
    """
    Args:
    ray_origin: (batch_size x 3)
    obj_triangles: (batch_size, triangle_nb, vertex_nb=3, vertex_coords=3)
    tol_thresh: To determine if ray and triangle are //
    Returns:
    intersections: (batch_size, triangle_nb) 1 if the point intersects, else 0
    """
    # Get vertex positions of mesh triangles
    v0, v1, v2 = obj_triangles[:, :, 0], obj_triangles[:, :, 1], obj_triangles[:, :, 2]
    batch_size = obj_triangles.shape[0]

    # Get edges of triangle mesh
    v0v1 = v1 - v0
    v0v2 = v2 - v0

    direction = torch.Tensor([0.4395064455, 0.617598629942, 0.652231566745]).cuda()
    batch_direction = direction.view(1, 1, 3).expand(batch_size, v0v2.shape[1], 3)
    pvec = torch.cross(batch_direction, v0v2, dim=2)
    batch_points_size = batch_size * v0v1.shape[1]
    dets = torch.bmm(
        v0v1.view(batch_points_size, 1, 3), pvec.view(batch_points_size, 3, 1)
    ).view(batch_size, v0v1.shape[1])

    # Detect if ray is // to triangle
    parallel = abs(dets) < tol_thresh
    invdet = 1 / (dets + 0.1 * tol_thresh)
    tvec = ray_origin.unsqueeze(1) - v0
    u_val = (
        torch.bmm(
            tvec.view(batch_size * tvec.shape[1], 1, 3),
            pvec.view(batch_size * tvec.shape[1], 3, 1),
        ).view(batch_size, tvec.shape[1])
        * invdet
    )

    # Detect if intersection is inside triangle
    u_correct = (u_val > 0) * (u_val < 1)
    qvec = torch.cross(tvec, v0v1, dim=2)
    v_val = (
        torch.bmm(
            batch_direction.view(batch_size * qvec.shape[1], 1, 3),
            qvec.view(batch_size * qvec.shape[1], 3, 1),
        ).view(batch_size, qvec.shape[1])
        * invdet
    )
    v_correct = (v_val > 0) * (u_val + v_val < 1)
    t = (
        torch.bmm(
            v0v2.view(batch_size * qvec.shape[1], 1, 3),
            qvec.view(batch_size * qvec.shape[1], 3, 1),
        ).view(batch_size, qvec.shape[1])
        * invdet
    )

    # Detect if intersection occurs in correct half-line of ray
    t_pos = t >= tol_thresh
    intersections = v_correct * u_correct * ~parallel * t_pos
    return intersections 

def canonicalise_orientation(skel_desc, skel):
    """Rotate the skeleton into a canonical orientation.

    This is achieved by aligning the plane formed by the left shoulder, right shoulder,
    and pelvis joints with the XY plane. The root joint is positioned at the origin.
    The direction from the pelvis to the midpoint of the soldiers is aligned
    with the negative Y direction. "Forwards" for the skeleton corresponds to
    the negative Z direction.

    Args:
        skel_desc (SkeletonDesc): The skeleton description
        skel (torch.Tensor): The skeleton

    Returns:
        The re-oriented skeleton
    """
    skel = ensure_homogeneous(skel, d=3)

    cart_skel = homogeneous_to_cartesian(skel)
    cart_skel = cart_skel - cart_skel[skel_desc.root_joint_id]
    rshoulder = cart_skel[skel_desc.joint_names.index('right_shoulder')]
    lshoulder = cart_skel[skel_desc.joint_names.index('left_shoulder')]
    pelvis = cart_skel[skel_desc.joint_names.index('pelvis')]

    v1 = rshoulder - pelvis
    v2 = lshoulder - pelvis
    forward = torch.cross(v1, v2)
    forward = forward / forward.norm(2)

    up = 0.5 * (v1 + v2)
    up = up / up.norm(2)

    right = torch.cross(forward, up)
    right = right / right.norm(2)

    up = torch.cross(forward, right)

    look_at = skel.new([
        [right[0], up[0], forward[0], 0],
        [right[1], up[1], forward[1], 0],
        [right[2], up[2], forward[2], 0],
        [0, 0, 0, 1],
    ])

    return torch.matmul(ensure_homogeneous(cart_skel, d=3), look_at)