####################################################################################################
# neuropythy/geometry/util.py
# This file defines common rotation functions that are useful with cortical mesh spheres, such as
# those produced with FreeSurfer.
# By Noah C. Benson
import numpy as np
import math, six
from ..util import (czdivide, zinv)
def normalize(u):
'''
normalize(u) yields a vetor with the same direction as u but unit length, or, if u has zero
length, yields u.
'''
u = np.asarray(u)
unorm = np.sqrt(np.sum(u**2, axis=0))
z = np.isclose(unorm, 0)
c = np.logical_not(z) / (unorm + z)
return u * c
def vector_angle_cos(u, v):
'''
vector_angle_cos(u, v) yields the cosine of the angle between the two vectors u and v. If u
or v (or both) is a (d x n) matrix of n vectors, the result will be a length n vector of the
cosines.
'''
u = np.asarray(u)
v = np.asarray(v)
return (u * v).sum(0) / np.sqrt((u ** 2).sum(0) * (v ** 2).sum(0))
def vector_angle(u, v, direction=None):
'''
vector_angle(u, v) yields the angle between the two vectors u and v. The optional argument
direction is by default None, which specifies that the smallest possible angle between the
vectors be reported; if the vectors u and v are 2D vectors and direction parameters True and
False specify the clockwise or counter-clockwise directions, respectively; if the vectors are
3D vectors, then direction may be a 3D point that is not in the plane containing u, v, and the
origin, and it specifies around which direction (u x v or v x u) the the counter-clockwise angle
from u to v should be reported (the cross product vector that has a positive dot product with
the direction argument is used as the rotation axis).
'''
if direction is None:
return np.arccos(vector_angle_cos(u, v))
elif direction is True:
return np.arctan2(v[1], v[0]) - np.arctan2(u[1], u[0])
elif direction is False:
return np.arctan2(u[1], u[0]) - np.arctan2(v[1], v[0])
else:
axis1 = normalize(u)
axis2 = normalize(np.cross(u, v))
if np.dot(axis2, direction) < 0:
axis2 = -axis2
return np.arctan2(np.dot(axis2, v), np.dot(axis1, v))
def spherical_distance(pt0, pt1):
'''
spherical_distance(a, b) yields the angular distance between points a and b, both of which
should be expressed in spherical coordinates as (longitude, latitude).
If a and/or b are (2 x n) matrices, then the calculation is performed over all columns.
The spherical_distance function uses the Haversine formula; accordingly it may suffer from
rounding errors in the case of nearly antipodal points.
'''
dtheta = pt1[0] - pt0[0]
dphi = pt1[1] - pt0[1]
a = np.sin(dphi/2)**2 + np.cos(pt0[1]) * np.cos(pt1[1]) * np.sin(dtheta/2)**2
return 2 * np.arcsin(np.sqrt(a))
def rotation_matrix_3D(u, th):
"""
rotation_matrix_3D(u, t) yields a 3D numpy matrix that rotates any vector about the axis u
t radians counter-clockwise.
"""
# normalize the axis:
u = normalize(u)
# We use the Euler-Rodrigues formula;
# see https://en.wikipedia.org/wiki/Euler-Rodrigues_formula
a = math.cos(0.5 * th)
s = math.sin(0.5 * th)
(b, c, d) = -s * u
(a2, b2, c2, d2) = (a*a, b*b, c*c, d*d)
(bc, ad, ac, ab, bd, cd) = (b*c, a*d, a*c, a*b, b*d, c*d)
return np.array([[a2 + b2 - c2 - d2, 2*(bc + ad), 2*(bd - ac)],
[2*(bc - ad), a2 + c2 - b2 - d2, 2*(cd + ab)],
[2*(bd + ac), 2*(cd - ab), a2 + d2 - b2 - c2]])
def rotation_matrix_2D(th):
'''
rotation_matrix_2D(th) yields a 2D numpy rotation matrix th radians counter-clockwise about the
origin.
'''
s = np.sin(th)
c = np.cos(th)
return np.array([[c, -s], [s, c]])
# construct a rotation matrix of vector u to vector b around center
def alignment_matrix_3D(u, v):
'''
alignment_matrix_3D(u, v) yields a 3x3 numpy array that rotates the vector u to the vector v
around the origin.
'''
# normalize both vectors:
u = normalize(u)
v = normalize(v)
# get the cross product of u cross v
w = np.cross(u, v)
# the angle we need to rotate
th = vector_angle(u, v)
# we rotate around this vector by the angle between them
return rotation_matrix_3D(w, th)
def alignment_matrix_2D(u, v):
'''
alignment_matrix_2D(u, v) yields a 2x2 numpy array that rotates the vector u to the vector v
around the origin.
'''
return rotation_matrix_2D(vector_angle(u, v, direction=True))
def point_on_line(ab, c, atol=1e-8):
'''
point_on_line((a,b), c) yields True if point x is on line (a,b) and False otherwise.
'''
(a,b) = ab
abc = [np.asarray(u) for u in (a,b,c)]
if any(len(u.shape) == 2 for u in abc): (a,b,c) = [np.reshape(u,(len(u),-1)) for u in abc]
else: (a,b,c) = abc
vca = a - c
vcb = b - c
uba = czdivide(vba, np.sqrt(np.sum(vba**2, axis=0)))
uca = czdivide(vca, np.sqrt(np.sum(vca**2, axis=0)))
return (np.isclose(np.sqrt(np.sum(vca**2, axis=0)), 0, atol=atol) |
np.isclose(np.sqrt(np.sum(vcb**2, axis=0)), 0, atol=atol) |
np.isclose(np.abs(np.sum(uba*uca, axis=0)), 1, atol=atol))
def point_on_segment(ac, b, atol=1e-8):
'''
point_on_segment((a,b), c) yields True if point x is on segment (a,b) and False otherwise. Note
that this differs from point_in_segment in that a point that if c is equal to a or b it is
considered 'on' but not 'in' the segment.
The option atol can be given and is used only to test for difference from 0; by default it is
1e-8.
'''
(a,c) = ac
abc = [np.asarray(u) for u in (a,b,c)]
if any(len(u.shape) > 1 for u in abc): (a,b,c) = [np.reshape(u,(len(u),-1)) for u in abc]
else: (a,b,c) = abc
vab = b - a
vbc = c - b
vac = c - a
dab = np.sqrt(np.sum(vab**2, axis=0))
dbc = np.sqrt(np.sum(vbc**2, axis=0))
dac = np.sqrt(np.sum(vac**2, axis=0))
return np.isclose(dab + dbc - dac, 0, atol=atol)
def point_in_segment(ac, b, atol=1e-8):
'''
point_in_segment((a,b), c) yields True if point x is in segment (a,b) and False otherwise. Note
that this differs from point_on_segment in that a point that if c is equal to a or b it is
considered 'on' but not 'in' the segment.
The option atol can be given and is used only to test for difference from 0; by default it is
1e-8.
'''
(a,c) = ac
abc = [np.asarray(u) for u in (a,b,c)]
if any(len(u.shape) > 1 for u in abc): (a,b,c) = [np.reshape(u,(len(u),-1)) for u in abc]
else: (a,b,c) = abc
vab = b - a
vbc = c - b
vac = c - a
dab = np.sqrt(np.sum(vab**2, axis=0))
dbc = np.sqrt(np.sum(vbc**2, axis=0))
dac = np.sqrt(np.sum(vac**2, axis=0))
return (np.isclose(dab + dbc - dac, 0, atol=atol) &
~np.isclose(dac - dab, 0, atol=atol) &
~np.isclose(dac - dbc, 0, atol=atol))
def lines_colinear(ab, cd, atol=1e-8):
'''
liness_colinear((a, b), (c, d)) yields True if the lines containing points (a,b) and points (c,d)
are colinear and false otherwise. All of a, b, c, and d must be (x,y) coordinates or 2xN (x,y)
coordinate matrices, or (x,y,z) or 3xN matrices.
'''
# simple check: a and b must be on (c,d)
return point_on_line(ab, cd[0], atol=atol) & point_on_line(ab, cd[1], atol=atol)
def segments_colinear(ab, cd, atol=1e-8):
'''
segments_colinear_2D((a, b), (c, d)) yields True if either a or b is on the line segment (c,d) or
if c or d is on the line segment (a,b) and the lines are colinear; otherwise yields False. All
of a, b, c, and d must be (x,y) coordinates or 2xN (x,y) coordinate matrices, or (x,y,z) or 3xN
matrices.
'''
(a,b) = ab
(c,d) = cd
ss = [point_on_segment(ab, c, atol=atol), point_on_segment(ab, d, atol=atol),
point_on_segment(cd, a, atol=atol), point_on_segment(cd, b, atol=atol)]
return np.sum(ss, axis=0) > 1
def points_close(a,b, atol=1e-8):
'''
points_close(a,b) yields True if points a and b are close to each other and False otherwise.
'''
(a,b) = [np.asarray(u) for u in (a,b)]
if len(a.shape) == 2 or len(b.shape) == 2: (a,b) = [np.reshape(u,(len(u),-1)) for u in (a,b)]
return np.isclose(np.sqrt(np.sum((a - b)**2, axis=0)), 0, atol=atol)
def segments_overlapping(ab, cd, atol=1e-8):
'''
segments_overlapping((a, b), (c, d)) yields True if the line segments (a,b) and (c,d) are both
colinear and have a non-finite overlap. If (a,b) and (c,d) touch at a single point, they are not
considered overlapping.
'''
(a,b) = ab
(c,d) = cd
ss = [point_in_segment(ab, c, atol=atol), point_in_segment(ab, d, atol=atol),
point_in_segment(cd, a, atol=atol), point_in_segment(cd, b, atol=atol)]
return (~(points_close(a,b) | points_close(c,d, atol=atol)) &
((np.sum(ss, axis=0) > 1) |
(points_close(a,c, atol=atol) & points_close(b,d, atol=atol)) |
(points_close(a,d, atol=atol) & points_close(b,c, atol=atol))))
def line_intersection_2D(abarg, cdarg, atol=1e-8):
'''
line_intersection((a, b), (c, d)) yields the intersection point between the lines that pass
through the given pairs of points. If any lines are parallel, (numpy.nan, numpy.nan) is
returned; note that a, b, c, and d can all be 2 x n matrices of x and y coordinate row-vectors.
'''
((x1,y1),(x2,y2)) = abarg
((x3,y3),(x4,y4)) = cdarg
dx12 = (x1 - x2)
dx34 = (x3 - x4)
dy12 = (y1 - y2)
dy34 = (y3 - y4)
denom = dx12*dy34 - dy12*dx34
unit = np.isclose(denom, 0, atol=atol)
if unit is True: return (np.nan, np.nan)
denom = unit + denom
q12 = (x1*y2 - y1*x2) / denom
q34 = (x3*y4 - y3*x4) / denom
xi = q12*dx34 - q34*dx12
yi = q12*dy34 - q34*dy12
if unit is False: return (xi, yi)
elif unit is True: return (np.nan, np.nan)
else:
xi = np.asarray(xi)
yi = np.asarray(yi)
xi[unit] = np.nan
yi[unit] = np.nan
return (xi, yi)
def segment_intersection_2D(p12arg, p34arg, atol=1e-8):
'''
segment_intersection((a, b), (c, d)) yields the intersection point between the line segments
that pass from point a to point b and from point c to point d. If there is no intersection
point, then (numpy.nan, numpy.nan) is returned.
'''
(p1,p2) = p12arg
(p3,p4) = p34arg
pi = np.asarray(line_intersection_2D(p12arg, p34arg, atol=atol))
p1 = np.asarray(p1)
p2 = np.asarray(p2)
p3 = np.asarray(p3)
p4 = np.asarray(p4)
u12 = p2 - p1
u34 = p4 - p3
cfn = lambda px,iis: (px if iis is None or len(px.shape) == 1 or px.shape[1] == len(iis) else
px[:,iis])
dfn = lambda a,b: a[0]*b[0] + a[1]*b[1]
sfn = lambda a,b: ((a-b) if len(a.shape) == len(b.shape) else
(np.transpose([a])-b) if len(a.shape) < len(b.shape) else
(a - np.transpose([b])))
fn = lambda px,iis: (1 - ((dfn(cfn(u12,iis), sfn( px, cfn(p1,iis))) > 0) *
(dfn(cfn(u34,iis), sfn( px, cfn(p3,iis))) > 0) *
(dfn(cfn(u12,iis), sfn(cfn(p2,iis), px)) > 0) *
(dfn(cfn(u34,iis), sfn(cfn(p4,iis), px)) > 0)))
if len(pi.shape) == 1:
if not np.isfinite(pi[0]): return (np.nan, np.nan)
bad = fn(pi, None)
return (np.nan, np.nan) if bad else pi
else:
nonpar = np.where(np.isfinite(pi[0]))[0]
bad = fn(cfn(pi, nonpar), nonpar)
(xi,yi) = pi
bad = nonpar[np.where(bad)[0]]
xi[bad] = np.nan
yi[bad] = np.nan
return (xi,yi)
def lines_touch_2D(ab, cd, atol=1e-8):
'''
lines_touch_2D((a,b), (c,d)) is equivalent to lines_colinear((a,b), (c,d)) |
numpy.isfinite(line_intersection_2D((a,b), (c,d))[0])
'''
return (lines_colinear(ab, cd, atol=atol) |
np.isfinite(line_intersection_2D(ab, cd, atol=atol)[0]))
def segments_touch_2D(ab, cd, atol=1e-8):
'''
segmentss_touch_2D((a,b), (c,d)) is equivalent to segments_colinear((a,b), (c,d)) |
numpy.isfinite(segment_intersection_2D((a,b), (c,d))[0])
'''
return (segments_colinear(ab, cd, atol=atol) |
np.isfinite(segment_intersection_2D(ab, cd, atol=atol)[0]))
def line_segment_intersection_2D(p12arg, p34arg, atol=1e-8):
'''
line_segment_intersection((a, b), (c, d)) yields the intersection point between the line
passing through points a and b and the line segment that passes from point c to point d. If
there is no intersection point, then (numpy.nan, numpy.nan) is returned.
'''
(p1,p2) = p12arg
(p3,p4) = p34arg
pi = np.asarray(line_intersection_2D(p12arg, p34arg, atol=atol))
p3 = np.asarray(p3)
u34 = p4 - p3
cfn = lambda px,iis: (px if iis is None or len(px.shape) == 1 or px.shape[1] == len(iis) else
px[:,iis])
dfn = lambda a,b: a[0]*b[0] + a[1]*b[1]
sfn = lambda a,b: ((a-b) if len(a.shape) == len(b.shape) else
(np.transpose([a])-b) if len(a.shape) < len(b.shape) else
(a - np.transpose([b])))
fn = lambda px,iis: (1 - ((dfn(cfn(u34,iis), sfn( px, cfn(p3,iis))) > 0) *
(dfn(cfn(u34,iis), sfn(cfn(p4,iis), px)) > 0)))
if len(pi.shape) == 1:
if not np.isfinite(pi[0]): return (np.nan, np.nan)
bad = fn(pi, None)
return (np.nan, np.nan) if bad else pi
else:
nonpar = np.where(np.isfinite(pi[0]))[0]
bad = fn(cfn(pi, nonpar), nonpar)
(xi,yi) = pi
bad = nonpar[np.where(bad)[0]]
xi[bad] = np.nan
yi[bad] = np.nan
return (xi,yi)
def triangle_area(a,b,c):
'''
triangle_area(a, b, c) yields the area of the triangle whose vertices are given by the points a,
b, and c.
'''
(a,b,c) = [np.asarray(x) for x in (a,b,c)]
sides = np.sqrt(np.sum([(p1 - p2)**2 for (p1,p2) in zip([b,c,a],[c,a,b])], axis=1))
s = 0.5 * np.sum(sides, axis=0)
s = np.clip(s * np.prod(s - sides, axis=0), 0.0, None)
return np.sqrt(s)
def triangle_normal(a,b,c):
'''
triangle_normal(a, b, c) yields the normal vector of the triangle whose vertices are given by
the points a, b, and c. If the points are 2D points, then 3D normal vectors are still yielded,
that are always (0,0,1) or (0,0,-1). This function auto-threads over matrices, in which case
they must be in equivalent orientations, and the result is returned in whatever orientation
they are given in. In some cases, the intended orientation of the matrices is ambiguous (e.g.,
if a, b, and c are 2 x 3 matrices), in which case the matrix is always assumed to be given in
(dims x vertices) orientation.
'''
(a,b,c) = [np.asarray(x) for x in (a,b,c)]
if len(a.shape) == 1 and len(b.shape) == 1 and len(c.shape) == 1:
return triangle_normal(*[np.transpose([x]) for x in (a,b,c)])[:,0]
(a,b,c) = [np.transpose([x]) if len(x.shape) == 1 else x for x in (a,b,c)]
# find a required number of dimensions, if possible
if a.shape[0] in (2,3):
dims = a.shape[0]
tx = True
else:
dims = a.shape[1]
(a,b,c) = [x.T for x in (a,b,c)]
tx = False
n = (a.shape[1] if a.shape[1] != 1 else b.shape[1] if b.shape[1] != 1 else
c.shape[1] if c.shape[1] != 1 else 1)
if dims == 2:
(a,b,c) = [np.vstack((x, np.zeros((1,n)))) for x in (a,b,c)]
ab = normalize(b - a)
ac = normalize(c - a)
res = np.cross(ab, ac, axisa=0, axisb=0)
return res.T if tx else res
def cartesian_to_barycentric_3D(tri, xy):
'''
cartesian_to_barycentric_3D(tri,xy) is identical to cartesian_to_barycentric_2D(tri,xy) except
it works on 3D data. Note that if tri is a 3 x 3 x n, a 3 x n x 3 or an n x 3 x 3 matrix, the
first dimension must always be the triangle vertices and the second 3-sized dimension must be
the (x,y,z) coordinates.
'''
xy = np.asarray(xy)
tri = np.asarray(tri)
if len(xy.shape) == 1:
return cartesian_to_barycentric_3D(np.transpose(np.asarray([tri]), (1,2,0)),
np.asarray([xy]).T)[:,0]
xy = xy if xy.shape[0] == 3 else xy.T
if tri.shape[0] == 3:
tri = tri if tri.shape[1] == 3 else np.transpose(tri, (0,2,1))
elif tri.shape[1] == 3:
tri = tri.T if tri.shape[0] == 3 else np.transpose(tri, (1,2,0))
elif tri.shape[2] == 3:
tri = np.transpose(tri, (2,1,0) if tri.shape[1] == 3 else (2,0,1))
if tri.shape[0] != 3 or tri.shape[1] != 3:
raise ValueError('Triangle array did not have dimensions of sizes 3 and 3')
if xy.shape[0] != 3:
raise ValueError('coordinate matrix did not have a dimension of size 3')
if tri.shape[2] != xy.shape[1]:
raise ValueError('number of triangles and coordinates must match')
# The algorithm here is borrowed from this stack-exchange post:
# http://gamedev.stackexchange.com/questions/23743
# in which it is attributed to Christer Ericson's book Real-Time Collision Detection.
v0 = tri[1] - tri[0]
v1 = tri[2] - tri[0]
v2 = xy - tri[0]
d00 = np.sum(v0 * v0, axis=0)
d01 = np.sum(v0 * v1, axis=0)
d11 = np.sum(v1 * v1, axis=0)
d20 = np.sum(v2 * v0, axis=0)
d21 = np.sum(v2 * v1, axis=0)
den = d00*d11 - d01*d01
zero = np.isclose(den, 0)
unit = 1 - zero
den += zero
l2 = unit * (d11 * d20 - d01 * d21) / den
l3 = unit * (d00 * d21 - d01 * d20) / den
return np.asarray([1.0 - l2 - l3, l2])
def cartesian_to_barycentric_2D(tri, xy, atol=1e-8):
'''
cartesian_to_barycentric_2D(tri, xy) yields a (2 x n) barycentric coordinate matrix
(or just a tuple if xy is a single (x, y) coordinate) of the first two barycentric coordinates
for the triangle coordinate array tri. The array tri should be 3 (vertices) x 2 (coordinates) x
n (triangles) unless xy is a tuple, in which case it should be a (3 x 2) matrix.
'''
xy = np.asarray(xy)
tri = np.asarray(tri)
if len(xy.shape) == 1:
return cartesian_to_barycentric_2D(np.transpose(np.asarray([tri]), (1,2,0)),
np.asarray([xy]).T)[:,0]
xy = xy if xy.shape[0] == 2 else xy.T
if tri.shape[0] == 3:
tri = tri if tri.shape[1] == 2 else np.transpose(tri, (0,2,1))
elif tri.shape[1] == 3:
tri = tri.T if tri.shape[0] == 2 else np.transpose(tri, (1,2,0))
elif tri.shape[2] == 3:
tri = np.transpose(tri, (2,1,0) if tri.shape[1] == 2 else (2,0,1))
if tri.shape[0] != 3 or tri.shape[1] != 2:
raise ValueError('Triangle array did not have dimensions of sizes 3 and 2')
if xy.shape[0] != 2:
raise ValueError('coordinate matrix did not have a dimension of size 2')
if tri.shape[2] != xy.shape[1]:
raise ValueError('number of triangles and coordinates must match')
# Okay, everything's the right shape...
(x,y) = xy
((x1,y1), (x2,y2), (x3,y3)) = tri
x_x3 = x - x3
x1_x3 = x1 - x3
x3_x2 = x3 - x2
y_y3 = y - y3
y1_y3 = y1 - y3
y2_y3 = y2 - y3
num1 = (y2_y3*x_x3 + x3_x2*y_y3)
num2 = (-y1_y3*x_x3 + x1_x3*y_y3)
den = (y2_y3*x1_x3 + x3_x2*y1_y3)
zero = np.isclose(den, 0, atol=atol)
den += zero
unit = 1 - zero
l1 = unit * num1 / den
l2 = unit * num2 / den
return np.asarray((l1, l2))
def barycentric_to_cartesian(tri, bc):
'''
barycentric_to_cartesian(tri, bc) yields the d x n coordinate matrix of the given barycentric
coordinate matrix (also d x n) bc interpolated in the n triangles given in the array tri. See
also cartesian_to_barycentric. If tri and bc represent one triangle and coordinate, then just
the coordinate and not a matrix is returned. The value d, dimensions, must be 2 or 3.
'''
bc = np.asarray(bc)
tri = np.asarray(tri)
if len(bc.shape) == 1:
return barycentric_to_cartesian(np.transpose(np.asarray([tri]), (1,2,0)),
np.asarray([bc]).T)[:,0]
bc = bc if bc.shape[0] == 2 else bc.T
if bc.shape[0] != 2: raise ValueError('barycentric matrix did not have a dimension of size 2')
n = bc.shape[1]
# we know how many bc's there are now; lets reorient tri to match with the last dimension as n
if len(tri.shape) == 2:
tri = np.transpose([tri for _ in range(n)], (1,2,0))
# the possible orientations of tri:
if tri.shape[0] == 3:
if tri.shape[1] in [2,3] and tri.shape[2] == n:
pass # default orientation
elif tri.shape[1] == n and tri.shape[2] in [2,3]:
tri = np.transpose(tri, (0,2,1))
else: raise ValueError('could not deduce triangle dimensions')
elif tri.shape[1] == 3:
if tri.shape[0] in [2,3] and tri.shape[2] == n:
tri = np.transpose(tri, (1,0,2))
elif tri.shape[0] == n and tri.shape[2] in [2,3]:
tri = np.transpose(tri, (1,2,0))
else: raise ValueError('could not deduce triangle dimensions')
elif tri.shape[2] == 3:
if tri.shape[0] in [2,3] and tri.shape[1] == n:
tri = np.transpose(tri, (2,0,1))
elif tri.shape[0] == n and tri.shape[1] in [2,3]:
tri = np.transpose(tri, (2,1,0))
else: raise ValueError('could not deduce triangle dimensions')
else: raise ValueError('At least one dimension of triangles must be 3')
if tri.shape[0] != 3 or (tri.shape[1] not in [2,3]):
raise ValueError('Triangle array did not have dimensions of sizes 3 and (2 or 3)')
if tri.shape[2] != n:
raise ValueError('number of triangles and coordinates must match')
(l1,l2) = bc
(p1, p2, p3) = tri
l3 = (1 - l1 - l2)
return np.asarray([x1*l1 + x2*l2 + x3*l3 for (x1,x2,x3) in zip(p1, p2, p3)])
def triangle_address(fx, pt):
'''
triangle_address(FX, P) yields an address coordinate (t,r) for the point P in the triangle
defined by the (3 x d)-sized coordinate matrix FX, in which each row of the matrix is the
d-dimensional vector representing the respective triangle vertx for triangle [A,B,C]. The
resulting coordinates (t,r) (0 <= t <= 1, 0 <= r <= 1) address the point P such that, if t gives
the fraction of the angle from vector AB to vector AC that is made by the angle between vectors
AB and AP, and r gives the fraction ||AP||/||AR|| where R is the point of intersection between
lines AP and BC. If P is a (d x n)-sized matrix of points, then a (2 x n) matrix of addresses
is returned.
'''
fx = np.asarray(fx)
pt = np.asarray(pt)
# The triangle vectors...
ab = fx[1] - fx[0]
ac = fx[2] - fx[0]
bc = fx[2] - fx[1]
ap = np.asarray([pt_i - a_i for (pt_i, a_i) in zip(pt, fx[0])])
# get the unnormalized distance...
r = np.sqrt((ap ** 2).sum(0))
# now we can find the angle...
unit = 1 - r.astype(bool)
t0 = vector_angle(ab, ac)
t = vector_angle(ap + [ab_i * unit for ab_i in ab], ab)
sint = np.sin(t)
sindt = np.sin(t0 - t)
# finding r0 is tricker--we use this fancy formula based on the law of sines
q0 = np.sqrt((bc ** 2).sum(0)) # B->C distance
beta = vector_angle(-ab, bc) # Angle at B
sinGamma = np.sin(math.pi - beta - t0)
sinBeta = np.sin(beta)
r0 = q0 * sinBeta * sinGamma / (sinBeta * sindt + sinGamma * sint)
return np.asarray([t/t0, r/r0])
def triangle_unaddress(fx, tr):
'''
triangle_unaddress(FX, tr) yields the point P, inside the reference triangle given by the
(3 x d)-sized coordinate matrix FX, that is addressed by the address coordinate tr, which may
either be a 2-d vector or a (2 x n)-sized matrix.
'''
fx = np.asarray(fx)
tr = np.asarray(tr)
# the triangle vectors...
ab = fx[1] - fx[0]
ac = fx[2] - fx[0]
bc = fx[2] - fx[1]
return np.asarray([ax + tr[1]*(abx + tr[0]*bcx) for (ax, bcx, abx) in zip(fx[0], bc, ab)])
def point_in_triangle(tri, pt, atol=1e-13):
tri = np.asarray(tri)
pt = np.asarray(pt)
if len(tri.shape) == 2 and len(pt.shape) == 1:
if len(pt) == 2:
tol = atol
v0 = tri[2] - tri[0]
v1 = tri[1] - tri[0]
v2 = pt - tri[0]
d00 = np.dot(v0, v0)
d01 = np.dot(v0, v1)
d02 = np.dot(v0, v2)
d11 = np.dot(v1, v1)
d12 = np.dot(v1, v2)
invDenom = (d00*d11 - d01*d01)
if np.isclose(invDenom, 0, atol=atol): return False
s = (d11*d02 - d01*d12) / invDenom
if (s + tol) < 0 or (s - tol) >= 1: return False
t = (d00*d12 - d01*d02) / invDenom
#return False if (t + tol) < 0 or (s + t - tol) > 1 else True
s = s + t
tp = np.isclose(t, 0, atol=tol) or t >= 0
tn = np.isclose(s - 1, 0, atol=tol) or s <= 1
return tn and tp
else:
dp1 = np.dot(pt - tri[0], np.cross(tri[0], tri[1] - tri[0]))
dp2 = np.dot(pt - tri[1], np.cross(tri[1], tri[2] - tri[1]))
db3 = np.dot(pt - tri[2], np.cross(tri[2], tri[0] - tri[2]))
return ((dp1 > 0 or np.isclose(dp1, 0, atol=atol)) and
(dp2 > 0 or np.isclose(dp2, 0, atol=atol)) and
(dp3 > 0 or np.isclose(dp3, 0, atol=atol)))
elif len(tri.shape) == 3 and len(pt.shape) == 2:
if len(pt) != len(tri):
raise ValueError('the number of triangles and points must be equal')
if pt.shape[1] == 2:
tol = atol
v0 = tri[:,2] - tri[:,0]
v1 = tri[:,1] - tri[:,0]
v2 = pt - tri[:,0]
d00 = np.sum(v0*v0, axis=1)
d01 = np.sum(v0*v1, axis=1)
d02 = np.sum(v0*v2, axis=1)
d11 = np.sum(v1*v1, axis=1)
d12 = np.sum(v1*v2, axis=1)
invDenom = (d00*d11 - d01*d01)
zeros = np.isclose(invDenom, 0)
invDenom[zeros] = 1.0
s = (d11*d02 - d01*d12) / invDenom
t = (d00*d12 - d01*d02) / invDenom
#return ~((s + tol < 0) | (s - tol >= 1) | (t + tol < 0) | (s + t - tol > 1) | zeros)
#return (~(s + tol < 0) & ~(s - tol >= 1) & ~(t + tol < 0) & ~(s + t - tol > 1) & ~zeros)
#return ((s + tol >= 0) & (s - tol < 1) & (t + tol >= 0) & (s + t - tol <= 1) & ~zeros)
st = s + t
q1 = np.isclose(s, 0, atol=atol) | (s > 0)
q2 = ~np.isclose(s - 1, 0, atol=atol) & (s < 1)
q3 = np.isclose(t, 0, atol=atol) | (t > 0)
q4 = np.isclose(st - 1, 0, atol=atol) | (st < 1)
return q1 * q2 & q3 & q4 & ~zeros
else:
x0 = np.sum((pt - tri[:,0]) * np.cross(tri[:,0], tri[:,1] - tri[:,0], axis=1), axis=1)
x1 = np.sum((pt - tri[:,1]) * np.cross(tri[:,1], tri[:,2] - tri[:,1], axis=1), axis=1)
x2 = np.sum((pt - tri[:,2]) * np.cross(tri[:,2], tri[:,0] - tri[:,2], axis=1), axis=1)
return (((x0 > 0) | np.isclose(x0, 0, atol=atol)) &
((x1 > 0) | np.isclose(x1, 0, atol=atol)) &
((x2 > 0) | np.isclose(x2, 0, atol=atol)))
elif len(tri.shape) == 3 and len(pt.shape) == 1:
return point_in_triangle(tri, np.asarray([pt for _ in tri]))
elif len(tri.shape) == 2 and len(pt.shape) == 1:
return point_in_triangle(np.asarray([tri for _ in pt]), pt)
else:
raise ValueError('triangles and pts do not have parallel shapes')
def det4D(m):
'''
det4D(array) yields the determinate of the given matrix array, which may have more than 2
dimensions, in which case the later dimensions are multiplied and added point-wise.
'''
# I just solved this in Mathematica, copy-pasted, and replaced the string '] m' with ']*m':
# Mathematica code: Det@Table[m[i][j], {i, 0, 3}, {j, 0, 3}]
return (m[0][3]*m[1][2]*m[2][1]*m[3][0] - m[0][2]*m[1][3]*m[2][1]*m[3][0] -
m[0][3]*m[1][1]*m[2][2]*m[3][0] + m[0][1]*m[1][3]*m[2][2]*m[3][0] +
m[0][2]*m[1][1]*m[2][3]*m[3][0] - m[0][1]*m[1][2]*m[2][3]*m[3][0] -
m[0][3]*m[1][2]*m[2][0]*m[3][1] + m[0][2]*m[1][3]*m[2][0]*m[3][1] +
m[0][3]*m[1][0]*m[2][2]*m[3][1] - m[0][0]*m[1][3]*m[2][2]*m[3][1] -
m[0][2]*m[1][0]*m[2][3]*m[3][1] + m[0][0]*m[1][2]*m[2][3]*m[3][1] +
m[0][3]*m[1][1]*m[2][0]*m[3][2] - m[0][1]*m[1][3]*m[2][0]*m[3][2] -
m[0][3]*m[1][0]*m[2][1]*m[3][2] + m[0][0]*m[1][3]*m[2][1]*m[3][2] +
m[0][1]*m[1][0]*m[2][3]*m[3][2] - m[0][0]*m[1][1]*m[2][3]*m[3][2] -
m[0][2]*m[1][1]*m[2][0]*m[3][3] + m[0][1]*m[1][2]*m[2][0]*m[3][3] +
m[0][2]*m[1][0]*m[2][1]*m[3][3] - m[0][0]*m[1][2]*m[2][1]*m[3][3] -
m[0][1]*m[1][0]*m[2][2]*m[3][3] + m[0][0]*m[1][1]*m[2][2]*m[3][3])
def det_4x3(a,b,c,d):
'''
det_4x3(a,b,c,d) yields the determinate of the matrix formed the given rows, which may have
more than 1 dimension, in which case the later dimensions are multiplied and added point-wise.
The point's must be 3D points; the matrix is given a fourth column of 1s and the resulting
determinant is of this matrix.
'''
# I just solved this in Mathematica, copy-pasted, and replaced the string '] m' with ']*m':
# Mathematica code: Det@Table[If[j == 3, 1, i[j]], {i, {a, b, c, d}}, {j, 0, 3}]
return (a[1]*b[2]*c[0] + a[2]*b[0]*c[1] - a[2]*b[1]*c[0] - a[0]*b[2]*c[1] -
a[1]*b[0]*c[2] + a[0]*b[1]*c[2] + a[2]*b[1]*d[0] - a[1]*b[2]*d[0] -
a[2]*c[1]*d[0] + b[2]*c[1]*d[0] + a[1]*c[2]*d[0] - b[1]*c[2]*d[0] -
a[2]*b[0]*d[1] + a[0]*b[2]*d[1] + a[2]*c[0]*d[1] - b[2]*c[0]*d[1] -
a[0]*c[2]*d[1] + b[0]*c[2]*d[1] + a[1]*b[0]*d[2] - a[0]*b[1]*d[2] -
a[1]*c[0]*d[2] + b[1]*c[0]*d[2] + a[0]*c[1]*d[2] - b[0]*c[1]*d[2])
def tetrahedral_barycentric_coordinates(tetra, pt):
'''
tetrahedral_barycentric_coordinates(tetrahedron, point) yields a list of weights for each vertex
in the given tetrahedron in the same order as the vertices given. If all weights are 0, then
the point is not inside the tetrahedron.
'''
# I found a description of this algorithm here (Nov. 2017):
# http://steve.hollasch.net/cgindex/geometry/ptintet.html
tetra = np.asarray(tetra)
pt = np.asarray(pt)
if tetra.shape[0] != 4:
if tetra.shape[1] == 4:
if tetra.shape[0] == 3:
tetra = np.transpose(tetra, (1,0) if len(tetra.shape) == 2 else (1,0,2))
else:
tetra = np.transpose(tetra, (1,2,0))
elif tetra.shape[1] == 3:
tetra = np.transpose(tetra, (2,1,0))
else:
tetra = np.transpose(tetra, (2,0,1))
elif tetra.shape[1] != 3:
tetra = np.transpose(tetra, (0,2,1))
if pt.shape[0] != 3: pt = pt.T
# Okay, calculate the determinants...
d_ = det_4x3(tetra[0], tetra[1], tetra[2], tetra[3])
d0 = det_4x3(pt, tetra[1], tetra[2], tetra[3])
d1 = det_4x3(tetra[0], pt, tetra[2], tetra[3])
d2 = det_4x3(tetra[0], tetra[1], pt, tetra[3])
d3 = det_4x3(tetra[0], tetra[1], tetra[2], pt)
s_ = np.sign(d_)
z_ = np.logical_or(np.any([s_ * si == -1 for si in np.sign([d0,d1,d2,d3])], axis=0),
np.isclose(d_,0))
x_ = np.logical_not(z_)
d_inv = x_ / (x_ * d_ + z_)
return np.asarray([d_inv * dq for dq in (d0,d1,d2,d3)])
def point_in_tetrahedron(tetra, pt):
'''
point_in_tetrahedron(tetrahedron, point) yields True if the given point is in the given
tetrahedron. If either tetrahedron or point (or both) are lists of shapes/points, then this
calculation is automatically threaded over all the given arguments.
'''
bcs = tetrahedral_barycentric_coordinates(tetra, pt)
return np.logical_not(np.all(np.isclose(bcs, 0), axis=0))
def prism_barycentric_coordinates(tri1, tri2, pt):
'''
prism_barycentric_coordinates(tri1, tri2, point) yields a list of weights for each vertex
in the given tetrahedron in the same order as the vertices given. If all weights are 0, then
the point is not inside the tetrahedron. The returned weights are (a,b,d) in a numpy array;
the values a, b, and c are the barycentric coordinates corresponding to the three points of
the triangles (where c = (1 - a - b) and the value d is the fractional distance (in the range
[0,1]) of the point between tri1 (d=0) and tri2 (d=1).
'''
pt = np.asarray(pt)
tri1 = np.asarray(tri1)
tri2 = np.asarray(tri2)
(tri1,tri2) = [
(np.transpose(tri, (1,0) if len(tri.shape) == 2 else (2,0,1)) if tri.shape[0] != 3 else
np.transpose(tri, (0,2,1)) if tri.shape[1] != 3 else
tri)
for tri in (tri1,tri2)]
pt = pt.T if pt.shape[0] != 3 else pt
## if the triangles aren't on coherent sides of each other, something is wrong with this prism
## (for now we don't do this because I'm not convinced it's necessary with cortex)
#faces = np.asarray([tri1, tri2])
#(u1,u2) = np.cross(faces[:,1] - faces[:,0], faces[:,2] - faces[:,0], axis=1)
#(p1,p2) = np.reshape([tri1[0], tri2[0]], (2,1,3,-1))
## make sure all three points from both triangles are on the right side of the other
#sd1to2 = np.sign(np.sum((tri2 - p1) * np.reshape(u1, (1,3,-1)), axis=1))
#sd2to1 = np.sign(np.sum((tri1 - p2) * np.reshape(u2, (1,3,-1)), axis=1))
#bad = np.where(#np.any(sd1to2 == sd2to1, axis=0) |
# (sd1to2[0] != sd1to2[1]) |
# (sd1to2[0] != sd1to2[2]))[0]
# get the individual tetrahedron bc coordinates; we divide the prism up into a few tetrahedrons
((a1,b1,c1),(a2,b2,c2)) = (tri1, tri2)
(ab2,bc2,ca2) = np.mean([(a2,b2), (b2,c2), (c2,a2)], axis=1)
mu1 = np.mean(tri1, axis=0)
tetpts = np.asarray([a1,b1,c1, a2,b2,c2, ab2,bc2,ca2, mu1])
bcs = np.vstack([tetrahedral_barycentric_coordinates(tetpts[tet], pt)
for tet in prism_barycentric_coordinates.tetrahedrons])
# add up the weights on the points...
abc12 = np.zeros([6, bcs.shape[1]])
for (k, ii, wii) in prism_barycentric_coordinates.weight_plan:
for (i,wi) in zip(ii, wii):
abc12[wi] += bcs[i] / k
# okay, now separate into h and ab:
abc = abc12[:3] + abc12[3:]
tot = np.sum(abc, axis=0)
abc[2] = 1 - np.sum(abc12[:3], axis=0)
bad = np.where(~np.isclose(tot, 1))[0]
abc[:,bad] = 0
q = tot[(tot > 1) & ~np.isclose(tot, 1)]
return abc
# meta-data used by the above function:
# order of points is a1, b1, c1, a2, b2, c2, ab2, bc2, ca2, mu1, where 1 is the first triangle,
# 2 is the second trignale, uv1 is the average of points u1 and v1, and mu1 is the average of tri1.
prism_barycentric_coordinates.weight_counts = np.asarray([1,1,1, 1,1,1, 2,2,2, 3])
prism_barycentric_coordinates.weights = {
1: np.reshape(np.arange(6), (6,1)),
2: np.array([[-1,-1],[-1,-1],[-1,-1],[-1,-1],[-1,-1],[-1,-1], # a1-c3 (0-5)
[3,4], [4,5], [5,3]]), #ab2-ca2 (6-8)
3: np.full((10, 3), -1)}
prism_barycentric_coordinates.weights[3][-1] = [0,1,2]
prism_barycentric_coordinates.tetrahedrons = np.array([[0, 1, 6, 9], [1, 2, 7, 9], [2, 0, 8, 9],
[0, 6, 8, 9], [1, 6, 7, 9], [2, 7, 8, 9],
[0, 6, 8, 3], [1, 6, 7, 4], [2, 7, 8, 5],
[6, 7, 8, 9]])
prism_barycentric_coordinates.tetflat = prism_barycentric_coordinates.tetrahedrons.flatten()
prism_barycentric_coordinates.tetflat_weight_count = prism_barycentric_coordinates.weight_counts[
prism_barycentric_coordinates.tetflat]
prism_barycentric_coordinates.weight_plan = tuple(
[(k, ii, v[prism_barycentric_coordinates.tetflat[ii]])
for (k,v) in six.iteritems(prism_barycentric_coordinates.weights)
for ii in np.where(prism_barycentric_coordinates.tetflat_weight_count == k)])
def point_in_prism(tri1, tri2, pt):
'''
point_in_prism(tri1, tri2, pt) yields True if the given point is inside the prism that stretches
between triangle 1 and triangle 2. Will automatically thread over extended dimensions. If
multiple triangles are given, then the vertices must be an earlier dimension than the
coordinates; e.g., a 3 x 3 x n array will be assumed to organized such that element [0,1,k] is
the y coordinate of the first vertex of the k'th triangle.
'''
bcs = prism_barycentric_coordinates(tri1, tri2, pt)
return np.logical_not(np.isclose(np.sum(bcs, axis=0), 0))
