#!/usr/bin/python
#import timeit, sys
import mirge.forgi.threedee.utilities.cytvec as ftuc
import mirge.forgi.utilities.debug as fud
import numpy as np
import math as m
import numpy.linalg as nl
import numpy.testing as nt
import random as rand
#import scipy as sp
null_array = np.array([0., 0., 0.])
x_array = np.array([1., 0., 0.])
y_array = np.array([0., 1., 0.])
z_array = np.array([0., 0., 1.])
# identity matrix
identity_matrix = np.array([x_array, y_array, z_array])
standard_basis = np.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
tau = 2 * m.pi
def get_inter_distances(vecs):
'''
Calculate all of the distances between the points of vecs.
'''
distances = []
for i in range(len(vecs)):
for j in range(i+1, len(vecs)):
distances += [vec_distance(vecs[i], vecs[j])]
return distances
def get_random_vector(mult=1.):
return np.array([mult * rand.uniform(-1, 1), mult * rand.uniform(-1, 1), mult * rand.uniform(-1, 1)])
def get_orthogonal_unit_vector(vec):
'''
Return a vector orthogonal to vec.
'''
vec2 = get_non_colinear_unit_vector(vec)
vec3 = np.cross(vec, vec2)
return normalize(vec3)
def get_random_vector_pair(angle=rand.uniform(0, m.pi)):
vec1 = get_random_vector()
vec2 = get_non_colinear_unit_vector(vec1)
rot_vec = np.cross(vec1, vec2)
rotmat = rotation_matrix(rot_vec, angle)
vec2 = np.dot(rotmat, vec1)
return (vec1, vec2)
def get_double_alignment_matrix(vp1, vp2):
'''
Align two sets of two vectors onto each other.
@param vp1: A pair of two vectors.
@param vp2: Another pair of two vectors.
'''
angle1 = vec_angle(vp1[0], vp1[1])
angle2 = vec_angle(vp2[0], vp2[1])
nt.assert_allclose(angle1, angle2, rtol=1e-7, atol=1e-7)
# Align the first two segments
mat1 = get_alignment_matrix(vp1[0], vp2[0])
# See where the second segment of the second set ends up
# after the first alignment
new_vp2_1 = np.dot(mat1, vp2[1])
comp1 = np.cross(vp1[1], vp1[0])
#comp1 = np.cross(vp1[0], vp1[1])
comp2 = np.cross(vp1[0], comp1) # should be along the plane of vp1[0] and vp1[1]
basis1 = create_orthonormal_basis(normalize(vp1[0]), normalize(comp2))
rej2 = change_basis(new_vp2_1, basis1, standard_basis)
angle = m.atan2(rej2[2], rej2[1])
mat2 = rotation_matrix(vp1[0], angle)
#return np.dot(mat1, mat2)
return np.dot(mat2, mat1)
def get_alignment_matrix(vec1, vec2):
'''
Return a rotation matrix that will align vec1 along vec2.
@param vec1: The target vector.
@param vec2: The vector to be aligned.
'''
comp = np.cross(vec1, vec2)
angle = vec_angle(vec1, vec2)
return rotation_matrix(comp, angle)
def get_non_colinear_unit_vector(vec):
'''
Get a unit vector that does not lie on the line defined by vec.
This is done by creating a vector along the least represented axis in vec.
@param vec: The vector under consideration.
@return: A vector along an axis.
'''
absvec = [abs(v) for v in vec]
m = min(absvec)
ind = absvec.index(m)
unit = [0., 0., 0.]
unit[ind] = 1.
return np.array(unit)
def create_orthonormal_basis1(vec1, vec2=None, vec3=None):
'''
Create an orthonormal basis using the provided vectors.
If more than one is provided, it must be orthogonal to
the others.
'''
if vec2 == None:
vec2 = get_non_colinear_unit_vector(vec1)
#else:
# nt.assert_allclose(np.dot(vec2, vec1), 0., rtol=1e-7, atol=1e-7)
vec1 = normalize(np.array(vec1))
vec2 = normalize(np.array(vec2))
if vec3 == None:
vec3 = np.cross(vec1, vec2)
vec3 = normalize(vec3)
return np.array([vec1, vec2, vec3])
def create_orthonormal_basis(vec1, vec2=None, vec3=None):
'''
Create an orthonormal basis using the provided vectors.
If more than one is provided, it must be orthogonal to
the others.
'''
if vec2 == None:
vec2 = get_non_colinear_unit_vector(vec1)
vec2 = np.cross(vec1, vec2)
#else:
# nt.assert_allclose(np.dot(vec2, vec1), 0., rtol=1e-7, atol=1e-7)
vec1 /= magnitude(vec1)
vec2 /= magnitude(vec2)
if vec3 == None:
vec3 = np.cross(vec1, vec2)
vec3 /= magnitude(vec3)
return np.array([vec1, vec2, vec3])
def time_cob1():
vec1 = get_random_vector()
vec2 = get_random_vector()
basis = create_orthonormal_basis1(vec1, vec2)
def time_cob2():
vec1 = get_random_vector()
vec2 = get_random_vector()
basis = create_orthonormal_basis(vec1, vec2)
def time_cob():
t1 = timeit.Timer("time_cob1()", "from forgi.utilities.vector import time_cob1")
t2 = timeit.Timer("time_cob2()", "from forgi.utilities.vector import time_cob2")
print t1.repeat(number=10000)
print t2.repeat(number=10000)
def spherical_cartesian_to_polar(vec):
'''
Return a parameterization of a vector of 3 coordinates:
x = r sin u cos v
y = r sin u sin v
z = r cos u
0 <= u <= pi
-pi <= v <= pi
Where u is the polar angle and v is the azimuth angle.
@param vec: A vector of 3 cartesian coordinates.
@return: (r, u, v)
'''
r = magnitude(vec)
u = m.acos(vec[2] / r)
v = m.atan2(vec[1], vec[0])
nt.assert_allclose(vec[0], r * m.sin(u) * m.cos(v), rtol=1e-7, atol=1e-7)
return (r, u, v)
def spherical_polar_to_cartesian(vec):
'''
Convert spherical polar coordinates to cartesian coordinates:
See the definition of spherical_cartesian_to_polar.
@param vec: A vector of the 3 polar coordinates (r, u, v)
@return: (x, y, z)
'''
(r, u, v) = vec
x = r * m.sin(u) * m.cos(v)
y = r * m.sin(u) * m.sin(v)
z = r * m.cos(u)
return [x, y, z]
def get_standard_basis(dim):
'''
Get a standard basis for the given dimension.
For 2D, this equals [[1.,0.],[0.,1.]]
@param dim: The dimension of the vector space.
@return: A vector of vectors that constitute the standard basis.
'''
standard_basis = [[0. for j in range(dim)] for i in range(dim)]
for i in range(dim):
standard_basis[i][i] = 1.
standard_basis = np.array(standard_basis)
return standard_basis
def change_basis(coords, new_basis, old_basis):
'''
Change the basis of coordinates to a new basis. In a regular structure
we have the coordinates in the regular cartesian coordinate system. For helix-helix
orientations, however, we want to express the coordinates in a coordinate system
defined by the first helix.
The new basis will consist of the axis of the first helix, one of its twist elements
(the one closest to the second vector) and a third vector orthogonal to the previous
two.
# http://tutorial.math.lamar.edu/Classes/LinAlg/ChangeOfBasis.aspx
@param coords: The coordinates to transform (array of n elements).
@param new_basis: The new basis vectors (n x n matrix)
@param old_basis: The old basis for the coordinates(n x n matrix)
@return: The new coordinates according to the new basis
'''
#assert(len(coords) == len(new_basis))
#assert(len(new_basis) == len(old_basis))
dim = len(coords)
#print "coords:", coords
standard_coords = np.dot(old_basis.transpose(), coords)
'''
#print "standard_coords:", standard_coords
standard_to_new = inv(new_basis.transpose())
#print "standard_to_new:", standard_to_new
new_coords = np.dot(standard_to_new, standard_coords)
print "new_coords:", new_coords
'''
new_coords = nl.solve(new_basis.transpose(), standard_coords)
#print "new_coords1:", new_coords1
return new_coords
def change_basis1(coords, new_basis, old_basis):
'''
'''
dim = len(coords)
standard_coords = np.dot(old_basis.transpose(), coords)
standard_to_new = nl.inv(new_basis.transpose())
new_coords = np.dot(standard_to_new, standard_coords)
return new_coords
def change_basis2(coords, new_basis, old_basis):
'''
'''
dim = len(coords)
standard_coords = np.dot(old_basis.T, coords)
new_coords = nl.solve(new_basis.T, standard_coords)
return new_coords
def change_basis1_benchmark():
coords = get_random_vector(10.)
basis1 = np.array([get_random_vector(10.) for i in range(3)])
basis2 = np.array([get_random_vector(10.) for i in range(3)])
nc = change_basis1(coords, basis1, basis2)
def change_basis2_benchmark():
coords = get_random_vector(10.)
basis1 = np.array([get_random_vector(10.) for i in range(3)])
basis2 = np.array([get_random_vector(10.) for i in range(3)])
nc = change_basis2(coords, basis1, basis2)
def time_basis1():
t1 = timeit.Timer("change_basis1_benchmark()","from forgi.utilities.vector import change_basis1_benchmark")
print t1.repeat(number=10000)
def time_basis2():
t2 = timeit.Timer("change_basis2_benchmark()","from forgi.utilities.vector import change_basis2_benchmark")
print t2.repeat(number=10000)
def vector_rejection(a, b):
'''
Return the vector rejection of a from b. In other words, return the orthogonal
projection of a onto the plane orthogonal to b.
@param a: The vector to be projected.
@param b: The vector defining the normal of the plane.
@return: The rejection of the vector a from b. (a - (np.dot(a, b) / np.dot(b, b)) * b)
'''
n = np.dot(a, b)
d = np.dot(b, b)
return a - (n / d) * b
def rotation_matrix_weave(axis, theta, mat = None):
'''
Calculate the rotation matrix for a rotation of theta degrees around axis.
Implemented in C++ using the weave module. Runs approximately 6x faster than
the numpy version below if no mat is passed in and around 20x faster if mat is
passed in.
Thanks to unutbu on StackOverflow
http://stackoverflow.com/questions/6802577/python-rotation-of-3d-vector
@param axis: The axis around which to rotate
@param theta: The angle of rotation
@return: A matrix which can be used to perform the given rotation. The coordinates
need only be multiplied by the matrix.
'''
if mat == None:
mat = np.eye(3,3)
support = "#include <math.h>"
code = """
double x = sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]);
double a = cos(theta / 2.0);
double b = -(axis[0] / x) * sin(theta / 2.0);
double c = -(axis[1] / x) * sin(theta / 2.0);
double d = -(axis[2] / x) * sin(theta / 2.0);
mat[0] = a*a + b*b - c*c - d*d;
mat[1] = 2 * (b*c - a*d);
mat[2] = 2 * (b*d + a*c);
mat[3*1 + 0] = 2*(b*c+a*d);
mat[3*1 + 1] = a*a+c*c-b*b-d*d;
mat[3*1 + 2] = 2*(c*d-a*b);
mat[3*2 + 0] = 2*(b*d-a*c);
mat[3*2 + 1] = 2*(c*d+a*b);
mat[3*2 + 2] = a*a+d*d-b*b-c*c;
"""
import scipy as sp
sp.weave.inline(code, ['axis', 'theta', 'mat'], support_code = support, libraries = ['m'])
return mat
def vector_set_rmsd(set1, set2):
'''
Calculate the rmsd between two sets of vectors.
@param set1: A matrix
@param set2: Another matrix.
@return: The rmsd between the rows of the matrix.
'''
rmsd = 0
count = 0
for i in range(len(set1)):
rmsd += magnitude(set2[i] - set1[i]) ** 2
count += 1
rmsd /= count
return m.sqrt(rmsd)
def rotation_matrix(axis, theta):
'''
Calculate the rotation matrix for a rotation of theta degrees around axis.
Thanks to unutbu on StackOverflow
http://stackoverflow.com/questions/6802577/python-rotation-of-3d-vector
@param axis: The axis around which to rotate
@param theta: The angle of rotation
@return: A matrix which can be used to perform the given rotation. The coordinates
need only be multiplied by the matrix.
'''
axis = axis/m.sqrt(np.dot(axis, axis))
a = m.cos(theta/2)
b, c, d = -axis*m.sin(theta/2)
return np.array([[a*a+b*b-c*c-d*d, 2*(b*c-a*d), 2*(b*d+a*c)],
[2*(b*c+a*d), a*a+c*c-b*b-d*d, 2*(c*d-a*b)],
[2*(b*d-a*c), 2*(c*d+a*b), a*a+d*d-b*b-c*c]])
def get_vector_centroid(crds1):
'''
Find the centroid of a set of vectors.
@param crds: A matrix containing all of the vectors.
@return: The centroid of the rows of the matrix crds.
'''
centroid1 = np.array([0., 0., 0.])
for i in range(len(crds1)):
centroid1 += crds1[i]
centroid1 /= float(len(crds1))
for i in centroid1:
if m.isnan(i):
raise Exception('nan encountered')
return centroid1
def center_on_centroid(crds1):
centroid1 = get_vector_centroid(crds1)
crds = []
for i in range(len(crds1)):
crds += [crds1[i] - centroid1]
return np.array(crds)
def magnitude(vec):
'''
Return the magnitude of a vector (|V|).
@param vec: The vector in question.
@return: The magnitude of the vector.
'''
return ftuc.magnitude(vec)
#return m.sqrt(np.dot(vec, vec))
def time_mag1():
vec1 = get_random_vector()
return m.sqrt(np.dot(vec1, vec1))
def time_mag2():
vec1 = get_random_vector()
return m.sqrt(np.dot(vec1, vec1))
def time_mag():
t1 = timeit.Timer("time_mag1()", "from forgi.utilities.vector import time_mag1")
t2 = timeit.Timer("time_mag2()", "from forgi.utilities.vector import time_mag2")
print t1.repeat(number=10000)
print t2.repeat(number=10000)
def normalize(vec):
'''
Normalize a vector so that its magnitude becomes 1.0 while
its direction remains the same.
@param vec: The vector in question.
@return: A normalized version of the vector.
'''
return vec / magnitude(vec)
def vec_angle(vec1, vec2):
'''
Get the angle between two vectors using the identity:
A * B = |A||B| cos t
Where A and B are two vectors and t is the angle between them.
@param vec1: The first vector (A)
@param vec2: The second vector (B)
@return: The angle between the two vectors.
'''
vec1n = normalize(vec1)
vec2n = normalize(vec2)
d = np.dot(vec1n, vec2n)
# this shouldn't happen, but sometimes it does, presumably because
# of rounding errors
if d >= 1.:
d = 1.
if d <= -1.:
d = -1.
angle = m.acos(d)
return angle
def vec_dot(a, b):
return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
'''
def cross(a, b):
c = [a[1]*b[2] - a[2]*b[1],
a[2]*b[0] - a[0]*b[2],
a[0]*b[1] - a[1]*b[0]]
return c
'''
def vec_distance(vec1, vec2):
return ftuc.vec_distance(vec1, vec2)
#return m.sqrt(np.dot(vec2 - vec1, vec2 - vec1))
def line_segment_distance(s1_p0, s1_p1, s2_p0, s2_p1):
'''
Calculate the two points on each of the segments that are closest to
each other. The first segment is defined as p1->p2 and the second as
p3->p4.
Code shamelessly translated from:
http://softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm#dist3D_Segment_to_Segment
@param s1_p0: The start of the first segment
@param s1_p1: The end of the first segment
@param s2_p0: The start of the second segment
@param s2_p1: The end of the second segment
@return: A tuple of points (i1,i2) containing the point i1 on s1
closest to the point i2 on segment s2
'''
u = s1_p1 - s1_p0
v = s2_p1 - s2_p0
w = s1_p0 - s2_p0
a = np.dot(u,u) # always >= 0
b = np.dot(u,v)
c = np.dot(v,v) # always >= 0
d = np.dot(u,w)
e = np.dot(v,w)
D = a*c - b*b # always >= 0
sD = D # sc = sN / sD, default sD = D >= 0
tD = D # tc = tN / tD, default tD = D >= 0
SMALL_NUM = 0.000001
# compute the line parameters of the two closest points
if (D < SMALL_NUM): # the lines are almost parallel
sN = 0.0 # force using point P0 on segment S1
sD = 1.0 # to prevent possible division by 0.0 later
tN = e
tD = c
else: # get the closest points on the infinite lines
sN = (b*e - c*d)
tN = (a*e - b*d)
if (sN < 0.0): # sc < 0 => the s=0 edge is visible
sN = 0.0
tN = e
tD = c
elif (sN > sD): # sc > 1 => the s=1 edge is visible
sN = sD
tN = e + b
tD = c
if (tN < 0.0): # tc < 0 => the t=0 edge is visible
tN = 0.0
# recompute sc for this edge
if (-d < 0.0):
sN = 0.0
elif (-d > a):
sN = sD
else:
sN = -d
sD = a
elif (tN > tD): # tc > 1 => the t=1 edge is visible
tN = tD
# recompute sc for this edge
if ((-d + b) < 0.0):
sN = 0
elif ((-d + b) > a):
sN = sD
else:
sN = (-d + b)
sD = a
# finally do the division to get sc and tc
sc = 0.0 if abs(sN) < SMALL_NUM else sN / sD
tc = 0.0 if abs(tN) < SMALL_NUM else tN / tD
# get the difference of the two closest points
dP = w + (sc * u) - (tc * v) # = S1(sc) - S2(tc)
return (s1_p0 + sc * u, s2_p0 + tc * v)
def closest_point_on_seg(seg_a, seg_b, circ_pos):
'''
Closest point between a line segment and a point.
Lifted from:
http://doswa.com/2009/07/13/circle-segment-intersectioncollision.html
'''
seg_v = seg_b - seg_a
pt_v = circ_pos - seg_a
mag = m.sqrt(sum(seg_v * seg_v))
if m <= 0:
raise ValueError, "Invalid segment length"
seg_v_unit = seg_v / mag
proj = pt_v.dot(seg_v_unit)
if proj <= 0:
return seg_a.copy()
if proj >= mag:
return seg_b.copy()
proj_v = seg_v_unit * proj
closest = proj_v + seg_a
return closest
def segment_circle(seg_a, seg_b, circ_pos, circ_rad):
'''
Something. Lifted from:
http://doswa.com/2009/07/13/circle-segment-intersectioncollision.html
'''
closest = closest_point_on_seg(seg_a, seg_b, circ_pos)
dist_v = circ_pos - closest
mag = m.sqrt(sum(dist_v * dist_v))
if m > circ_rad:
return vec(0, 0)
if mag(dist_v) <= 0:
raise ValueError, "Circle's center is exactly on segment"
offset = dist_v / mag(dist_v) * (circ_rad - mag(dist_v))
return offset
def cylinder_line_intersection(cyl, line, r):
'''
Get the points of intersection between a line and a cylinder.
If they do not intersect, return an empty list. If the line
touches the cylinder, then return a 2 point list with two identical points.
If the line crosses the cylinder, then return a list of 2 points.
'''
cyl = np.array(cyl)
line = np.array(line)
cyl_vec = cyl[1] - cyl[0]
line_vec = line[1] - line[0]
cyl_basis = create_orthonormal_basis(cyl_vec)
line_t = change_basis(line.T, cyl_basis, standard_basis).T
cyl_t = change_basis(cyl.T, cyl_basis, standard_basis).T
cyl_vec_t = cyl_t[1] - cyl_t[0]
line_vec_t = line_t[1] - line_t[0]
line_vec_t_normed = normalize(line_vec_t)
# get the point on the line closest to the
# center of the cylinder
p = closest_point_on_seg(line_t[0][1:], line_t[1][1:],
np.array(cyl_t[0][1:]))
if line_vec_t[1] == 0:
return []
# Figure out the x position by determining how far along
# the y-coordinate of the segment the closest point is
x = (line_t[0][0] +
(line_vec_t[0] *
(p[0] - line_t[0][1]) / line_vec_t[1]))
v = p - cyl_t[0][1:]
o = m.sqrt(sum(v * v))
p = [x, p[0], p[1]]
if o > r:
# no intersection
return []
t = m.sqrt(r ** 2 - o ** 2)
i1 = p - t * line_vec_t_normed
i2 = p + t * line_vec_t_normed
endpoints_t = [i1, i2]
endpoints_t = sorted(endpoints_t, key=lambda x: x[0])
line_t = sorted(line_t, key=lambda x: x[0])
cyl_t = sorted(cyl_t, key=lambda x: x[0])
# the start point will be the highest of the top of the cylinder
# the end of the line, and the point where the line intersects
# the surface of the cylinder
start_points = np.array([cyl_t[0][0], line_t[0][0], endpoints_t[0][0]])
# the end point will be the lowest of the... " " "
end_points = np.array([cyl_t[1][0], line_t[1][0], endpoints_t[1][0]])
start_points = sorted(start_points)
end_points = sorted(end_points)
start_param = (start_points[-1] - p[0]) / line_vec_t_normed[0]
end_param = (end_points[0] - p[0]) / line_vec_t_normed[0]
endpoints_t[0] = p + start_param * line_vec_t_normed
endpoints_t[1] = p + end_param * line_vec_t_normed
'''
endpoints_t[0] = [start_points[-1],
endpoints_t[
endpoints_t[1][0] = end_points[0]
real_start = start_points[-1]
real_end = end_points[0]
'''
intersects_t = np.array(endpoints_t)
if intersects_t[0][0] > intersects_t[1][0]:
# degenerate case, the line is to the side of the cylinder
return []
return change_basis(intersects_t.T, standard_basis, cyl_basis).T
def pin_fits_two_cyl(cyl1, cyl2, cyl_width):
'''
If we create a cone that starts at one end of cylinder1, does it
enclose cylinder2?
This function approximates an answer by projecting a circle on
the plane normal to the axis of cylinder1.
@param cyl1: The coordinates of cylinder1
@param cyl2: The coordinates of cylinder2
@param cyl_width: The widths of the two cylinders
@return: True or False
'''
basis = create_orthonormal_basis(cyl1[1] - cyl1[0])
cyl2 = np.array([cyl2[0] - cyl1[1], cyl2[1] - cyl1[1]])
cyl2_t = change_basis(cyl2.T, basis, standard_basis).T
cone_width = cyl_width
cyl1_len = magnitude(cyl1[1] - cyl1[0])
# the cone expands
cone_width_cyl2_start = cyl_width * (cyl1_len + cyl2_t[0][0]) / cyl1_len
cone_width_cyl2_end = cyl_width * (cyl1_len + cyl2_t[1][0]) / cyl1_len
cyl2_start_offset = m.sqrt(cyl2_t[0][1] ** 2 + cyl2_t[0][2] ** 2)
cyl2_end_offset = m.sqrt(cyl2_t[1][1] ** 2 + cyl2_t[1][2] ** 2)
if cyl2_start_offset > cone_width_cyl2_start:
return False
if cyl2_end_offset > cone_width_cyl2_end:
return False
return True
def point_in_cylinder(pt1, pt2, r, testpt):
'''
Determine if testpt is within a cylinder of radius r.
Translated from:
http://www.flipcode.com/archives/Fast_Point-In-Cylinder_Test.shtml
@param pt1: The start of the cylinder axis.
@param pt2: The end of the cylinder axis.
@param r: The radius of the cylinder.
@param testpt: The point we are testing.
@return: True if the point is within the cylinder, False otherwise.
'''
d = pt2 - pt1
lengthsq = np.dot(d, d)
radius_sq = r * r
pd = testpt - pt1
dot = np.dot(pd, d)
if dot < 0. or dot > lengthsq:
# beyond the edges of the cylinder
return False
else:
dsq = np.dot(pd, pd) - (dot * dot) / lengthsq
if (dsq > radius_sq):
return False
else:
return True
