{{ message }}

Instantly share code, notes, and snippets.

# potpath/conesIntersection.py

Last active Nov 25, 2016
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 commented Jul 15, 2015

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

### hale2bopp commented Nov 25, 2016

 How do you set the aperture angle of the cone?