def unitize_arr(arr):
'''
Normalize array row-wise
'''
narr = np.zeros((arr.shape[0], 3), dtype=np.float64)
for i in np.arange(0, arr.shape[0]):
narr[i] = arr[i, :] / vecnorm(arr[i, :])
return narr
def rotate_vec2vec(v1, v2):
'''
Rotate vector v1 --> v2 and return the transformation matrix R that
achieves this
'''
n = np.cross(v1, v2)
sinv = vecnorm(n)
cosv = np.dot(v1, v2)
R = np.eye(3) + skew(n) + np.matmul(skew(n), skew(n)) * (1 - cosv) / (sinv
**2)
return R
def define_coil_orientation(loc, rot, n):
'''
Construct the coil orientation matrix to be used by simnibs
loc -- center of coil
rot -- vector pointing in handle direction (tangent to surface)
n -- normal vector
'''
y = rot / vecnorm(rot)
z = n / vecnorm(n)
x = np.cross(y, z)
c = loc
matsimnibs = np.zeros((4, 4), dtype=np.float64)
matsimnibs[:3, 0] = x
matsimnibs[:3, 1] = y
matsimnibs[:3, 2] = z
matsimnibs[:3, 3] = c
matsimnibs[3, 3] = 1
return matsimnibs
def _construct_local_quadric(self, p, tol=1e-3):
'''
Given a single point construct a local quadric
surface on a given mesh
'''
# Get local neighbourhood
neighbours_ind = np.where(vecnorm(self.coords - p, axis=1) < tol)
neighbours = self.coords[neighbours_ind]
# Calculate normals
normals = geolib.get_normals(self.nodes[neighbours_ind], self.nodes,
self.coords, self.trigs)
# Usage average of normals for alignment
n = normals / vecnorm(normals)
# Make transformation matrix
z = np.array([0, 0, 1])
R = np.eye(4)
R[:3, :3] = geolib.rotate_vec2vec(n, z)
T = np.eye(4)
T[:3, 3] = -p
affine = R @ T
# Create inverse rotation
iR = R
iR[:3, :3] = iR[:3, :3].T
# Create inverse translation
iT = T
iT[:3, 3] = -T[:3, 3]
i_affine = iT @ iR
# Perform quadratic fitting
r_neighbours = geolib.affine(affine, neighbours)
C = geolib.quad_fit(r_neighbours[:, :2], r_neighbours[:, 2])
return C, affine, i_affine
def _initialize(self, centroid, span):
'''
Construct quadratic basis and rotation at centroid point
to use for sampling
'''
v = geolib.closest_point2surf(centroid, self.coords)
C, R, iR = self._construct_local_quadric(v, 0.75 * span)
# Calculate neighbours, rotate to flatten on XY plane
neighbours_ind = np.where(vecnorm(self.coords - v, axis=1) < span)
neighbours = self.coords[neighbours_ind]
r_neighbours = geolib.affine(R, neighbours)
minarr = np.min(r_neighbours, axis=0)
maxarr = np.max(r_neighbours, axis=0)
bounds = np.c_[minarr.T, maxarr.T]
return C, iR, bounds
def _construct_sample(self, x, y):
'''
Given a sampling point, estimate local geometry to
get accurate normals/curvatures
'''
pp = geolib.map_param_2_surf(x, y, self.C)[np.newaxis, :]
p = geolib.affine(self.iR, pp)
v = geolib.closest_point2surf(p, self.coords)
C, _, iR = self._construct_local_quadric(v, self.geo_radius)
_, _, n = geolib.compute_principal_dir(0, 0, C)
# Map normal to coordinate space
n_r = iR[:3, :3] @ n
n_r = n_r / vecnorm(n_r)
# Push sample out by set distance
sample = v + (n_r * self.distance)
return sample, iR, C, n
def compute_principal_dir(x, y, C):
'''
Compute the principal direction of a quadratic surface of form:
S: f(x,y) = a + bx + cy + dxy + ex^2 + fy^2
Using the second fundamental form basis matrix eigendecomposition method
x -- scalar x input
y -- scalar y input
C -- scalar quadratic vector (a,b,c,d,e,f)
V[:,0] -- major principal direction
V[:,1] -- minor principal direction
n -- normal to surface S at point (x,y)
'''
# Compute partial first and second derivatives
r_x = np.array([1, 0, 2 * C[4] * x + C[1] + C[3] * y])
r_y = np.array([0, 1, 2 * C[5] * y + C[2] + C[3] * x])
r_xx = np.array([0, 0, 2 * C[4]])
r_yy = np.array([0, 0, 2 * C[5]])
r_xy = np.array([0, 0, C[3]])
# Compute surface point normal
r_x_cross_y = np.cross(r_x, r_y)
n = r_x_cross_y / vecnorm(r_x_cross_y)
# Compute second fundamental form constants
L = np.dot(r_xx, n)
M = np.dot(r_xy, n)
N = np.dot(r_yy, n)
# Form basis matrix
P = np.array([[L, M], [M, N]])
# Eigendecomposition, then convert into 3D vector
_, V = np.linalg.eig(P)
V = np.concatenate((V, np.zeros((1, 2))), axis=0)
return V[:, 0], V[:, 1], n
def closest_point2surf(p, coords):
ind = np.argmin(vecnorm(coords - p, axis=1))
return coords[ind, :]
def ray_interception(pn, pf, coords, trigs, epsilon=1e-6):
'''
Compute interception point and distance of ray to mesh defined
by a set of vertex coordinates and triangles. Yields minimum coordinate
of ray.
Algorithm vectorized and
adapted from http://geomalgorithms.com/a06-_intersect-2.html
Arguments:
pn, pf Points to define ray (near, far)
coords Array of vertex coordinates defining mesh
trigs Array of vertex IDs defining mesh triangles
Returns:
p_I Minimum Point of intersection
ray_len Length of line from pn to p_I
min_trig Triangle ID that contains the shortest
length intersection
'''
# Get coordinates for triangles
V0 = coords[trigs[:, 0], :]
V1 = coords[trigs[:, 1], :]
V2 = coords[trigs[:, 2], :]
# Calculate triangle plane
u = V1 - V0
v = V2 - V0
n = np.cross(u, v)
# Remove all degenerate triangles
valid_verts = np.where(vecnorm(n, axis=1) > epsilon)
u = u[valid_verts]
v = v[valid_verts]
n = n[valid_verts]
# Check if ray is in plane w/triangle and if ray is moving toward triangle
r_denom = (n * (pf - pn)).sum(axis=1)
r_numer = (n * (V0[valid_verts] - pn)).sum(axis=1)
r = r_numer / r_denom
ray_valid = np.where((np.abs(r_denom) > epsilon) & (r > 0))
u = u[ray_valid]
v = v[ray_valid]
n = n[ray_valid]
# Solve for intersection point of ray to plane
i_p = pn - (r[:, np.newaxis][ray_valid] * (pn - pf))
# Check whether the point of intersection lies within the triangle
w = i_p - V0[valid_verts][ray_valid]
s, t = compute_parameteric_coordinates(u, v, w)
s_conditional = ((s > 0) & (s < 1))
t_conditional = ((t > 0) & (t < 1))
within_trig = np.where(s_conditional & t_conditional & ((s + t) < 1))
# Get minimizing triangle identity if a triangle is identified
if len(within_trig[0]) > 0:
argmin_r = np.argmin(r[ray_valid][within_trig])
trig_ids = np.arange(
0, trigs.shape[0])[valid_verts][ray_valid][within_trig]
min_trig = trig_ids[argmin_r]
# Compute distance from ray origin to triangle
p_I = i_p[within_trig][argmin_r]
ray_len = vecnorm(p_I - pn)
# Return point of intersection point, ray length, and triangle with minimum
return (p_I, ray_len, min_trig)
else:
return (None, None, None)