Check four 3D circular cones intersection

conesIntersection.py

import ecos
import sys
import os
import numpy as np
from scipy.sparse import csc_matrix

_stdout = sys.stdout


def perpendicular_vector(v):
    index = abs(v).argmin()
    if index == 0:
        ret = np.array((0, -v[2], v[1]))
    elif index == 1:
        ret = np.array((-v[2], 0, v[0]))
    else:
        ret = np.array((-v[1], v[0], 0))
    return ret / np.linalg.norm(ret)


def is_four_cones_intersect(a1, a2, a3, a4, n1, n2, n3, n4, uf):
    """
    Input format
    a: cone apex in R^3
    n: unit normal direction in S^2
    uf: friction coefficient in R
    """

    o1 = perpendicular_vector(n1)
    o2 = perpendicular_vector(n2)
    o3 = perpendicular_vector(n3)
    o4 = perpendicular_vector(n4)

    t1 = np.cross(n1, o1)
    t2 = np.cross(n2, o2)
    t3 = np.cross(n3, o3)
    t4 = np.cross(n4, o4)

    R1 = np.matrix((n1, o1, t1))
    R2 = np.matrix((n2, o2, t2))
    R3 = np.matrix((n3, o3, t3))
    R4 = np.matrix((n4, o4, t4))

    c = np.zeros(3)

    # G = np.zeros((12, 3))
    G = np.vstack((R1, R2, R3, R4))
    G[0::3] *= uf
    G *= -1

    # h = np.zeros(12)
    h = np.hstack((R1.dot(a1), R2.dot(a2), R3.dot(a3), R4.dot(a4)))
    h = np.asarray(h).squeeze()
    h *= -1

    dims = {
        'l': 0,
        'q': [3] * 4,
    }

    with open(os.devnull, 'w') as sys.stdout:
        solution = ecos.solve(c, csc_matrix(G), h, dims)
    sys.stdout = _stdout
    return solution['info']['exitFlag'] == 0


def is_four_double_sided_cones_intersect(a1, a2, a3, a4, n1, n2, n3, n4, uf):
    return is_four_cones_intersect(
        a1, a2, a3, a4, n1, n2, n3, n4, uf) or is_four_cones_intersect(
        a1, a2, a3, a4, n1, n2, n3, -n4, uf) or is_four_cones_intersect(
        a1, a2, a3, a4, n1, n2, -n3, n4, uf) or is_four_cones_intersect(
        a1, a2, a3, a4, n1, n2, -n3, -n4, uf) or is_four_cones_intersect(
        a1, a2, a3, a4, n1, -n2, n3, n4, uf) or is_four_cones_intersect(
        a1, a2, a3, a4, n1, -n2, n3, -n4, uf) or is_four_cones_intersect(
        a1, a2, a3, a4, n1, -n2, -n3, n4, uf) or is_four_cones_intersect(
        a1, a2, a3, a4, n1, -n2, -n3, -n4, uf) or is_four_cones_intersect(
        a1, a2, a3, a4, -n1, n2, n3, n4, uf) or is_four_cones_intersect(
        a1, a2, a3, a4, -n1, n2, n3, -n4, uf) or is_four_cones_intersect(
        a1, a2, a3, a4, -n1, n2, -n3, n4, uf) or is_four_cones_intersect(
        a1, a2, a3, a4, -n1, n2, -n3, -n4, uf) or is_four_cones_intersect(
        a1, a2, a3, a4, -n1, -n2, n3, n4, uf) or is_four_cones_intersect(
        a1, a2, a3, a4, -n1, -n2, n3, -n4, uf) or is_four_cones_intersect(
        a1, a2, a3, a4, -n1, -n2, -n3, n4, uf) or is_four_cones_intersect(
        a1, a2, a3, a4, -n1, -n2, -n3, -n4, uf)

# Example
u_friction = 1

a1 = np.array([0, 0, 0])
a2 = np.array([0, 0, 0])
a3 = np.array([0, 0, 1])
a4 = np.array([0, 0, 0])

n1 = np.array([1, 0, 0])
n2 = np.array([0, 1, 0])
n3 = np.array([0, 0, 1])
n4 = np.array([0.5773502691896257, 0.5773502691896257, 0.5773502691896257])

is_intersect = is_four_cones_intersect(a1, a2, a3, a4, n1, n2, n3, n4, u_friction)
print(is_intersect)
is_intersect2 = is_four_double_sided_cones_intersect(a1, a2, a3, a4, n1, n2, n3, n4, u_friction)
print(is_intersect2)

Depends on https://github.com/embotech/ecos-python.

How do you set the aperture angle of the cone?