Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Check four 3D circular cones intersection
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)
@potpath

This comment has been minimized.

Copy link
Owner Author

@potpath potpath commented Jul 15, 2015

@hale2bopp

This comment has been minimized.

Copy link

@hale2bopp hale2bopp commented Nov 25, 2016

How do you set the aperture angle of the cone?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment