# Python numpy.cross() Examples

The following are 30 code examples for showing how to use numpy.cross(). These examples are extracted from open source projects. You can vote up the ones you like or vote down the ones you don't like, and go to the original project or source file by following the links above each example.

You may check out the related API usage on the sidebar.

You may also want to check out all available functions/classes of the module , or try the search function .

Example 1
```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)) ```
Example 2
```def __init__(self,origin, pt1, pt2, name=None):
"""
origin: 3x1 vector
pt1: 3x1 vector
pt2: 3x1 vector
"""
self.__origin=origin
vec1 = np.array([pt1[0] - origin[0] , pt1[1] - origin[1] , pt1[2] - origin[2]])
vec2 = np.array([pt2[0] - origin[0] , pt2[1] - origin[1] , pt2[2] - origin[2]])
cos = np.dot(vec1, vec2)/np.linalg.norm(vec1)/np.linalg.norm(vec2)
if  cos == 1 or cos == -1:
raise Exception("Three points should not in a line!!")
self.__x = vec1/np.linalg.norm(vec1)
z = np.cross(vec1, vec2)
self.__z = z/np.linalg.norm(z)
self.__y = np.cross(self.z, self.x)
self.__name=uuid.uuid1() if name==None else name ```
Example 3
```def set_by_3pts(self,origin, pt1, pt2):
"""
origin: tuple 3
pt1: tuple 3
pt2: tuple 3
"""
self.origin=origin
vec1 = np.array([pt1[0] - origin[0] , pt1[1] - origin[1] , pt1[2] - origin[2]])
vec2 = np.array([pt2[0] - origin[0] , pt2[1] - origin[1] , pt2[2] - origin[2]])
cos = np.dot(vec1, vec2)/np.linalg.norm(vec1)/np.linalg.norm(vec2)
if  cos == 1 or cos == -1:
raise Exception("Three points should not in a line!!")
self.x = vec1/np.linalg.norm(vec1)
z = np.cross(vec1, vec2)
self.z = z/np.linalg.norm(z)
self.y = np.cross(self.z, self.x) ```
Example 4
```def _retinotopic_field_sign_triangles(m, retinotopy):
t = m.tess if isinstance(m, geo.Mesh) or isinstance(m, geo.Topology) else m
# get the polar angle and eccen data as a complex number in degrees
if pimms.is_str(retinotopy):
(x,y) = as_retinotopy(retinotopy_data(m, retinotopy), 'geographical')
elif retinotopy is Ellipsis:
(x,y) = as_retinotopy(retinotopy_data(m, 'any'),      'geographical')
else:
(x,y) = as_retinotopy(retinotopy,                     'geographical')
# Okay, now we want to make some coordinates...
coords = np.asarray([x, y])
us = coords[:, t.indexed_faces[1]] - coords[:, t.indexed_faces[0]]
vs = coords[:, t.indexed_faces[2]] - coords[:, t.indexed_faces[0]]
(us,vs) = [np.concatenate((xs, np.full((1, t.face_count), 0.0))) for xs in [us,vs]]
xs = np.cross(us, vs, axis=0)[2]
xs[np.isclose(xs, 0)] = 0
return np.sign(xs) ```
Example 5
```def rotate_camera_to_point_at(up_from, lookat_from, up_to, lookat_to):
inputs = [up_from, lookat_from, up_to, lookat_to]
for i in range(4):
inputs[i] = normalize(np.array(inputs[i]).reshape((-1,)))
up_from, lookat_from, up_to, lookat_to = inputs
r1 = r_between(lookat_from, lookat_to)

new_x = np.dot(r1, np.array([1, 0, 0]).reshape((-1, 1))).reshape((-1))
to_x = normalize(np.cross(lookat_to, up_to))
angle = np.arccos(np.dot(new_x, to_x))
if angle > ANGLE_EPS:
if angle < np.pi - ANGLE_EPS:
ax = normalize(np.cross(new_x, to_x))
flip = np.dot(lookat_to, ax)
if flip > 0:
r2 = get_r_matrix(lookat_to, angle)
elif flip < 0:
r2 = get_r_matrix(lookat_to, -1. * angle)
else:
# Angle of rotation is too close to 180 degrees, direction of rotation
# does not matter.
r2 = get_r_matrix(lookat_to, angle)
else:
r2 = np.eye(3)
return np.dot(r2, r1) ```
Example 6
```def calculate_camera_variables(eye, lookat, up, fov, aspect_ratio, fov_is_vertical=False):
import numpy as np
import math

W = np.array(lookat) - np.array(eye)
wlen = np.linalg.norm(W)
U = np.cross(W, np.array(up))
U /= np.linalg.norm(U)
V = np.cross(U, W)
V /= np.linalg.norm(V)

if fov_is_vertical:
vlen = wlen * math.tan(0.5 * fov * math.pi / 180.0)
V *= vlen
ulen = vlen * aspect_ratio
U *= ulen
else:
ulen = wlen * math.tan(0.5 * fov * math.pi / 180.0)
U *= ulen
vlen = ulen * aspect_ratio
V *= vlen

return U, V, W ```
Example 7
```def update_normals(self):
v0 = self.vectors[:, 0, :3]
v1 = self.vectors[:, 1, :3]
v2 = self.vectors[:, 2, :3]
_normals = numpy.cross(v1 - v0, v2 - v0)

for i in range(len(_normals)):
norm = numpy.linalg.norm(_normals[i])
if norm != 0:
_normals[i] /= numpy.linalg.norm(_normals[i])

self.normals[:] = _normals
return self

#####################################################################
# Analyze functions
# ```
Example 8
```def transform(x_vec, z_vec):
'''
Construct a transformation matrix to transform r_vec to the new coordinate system defined by x_vec and z_vec
'''

x_vec = x_vec/np.linalg.norm(np.asarray(x_vec))
z_vec = z_vec/np.linalg.norm(np.asarray(z_vec))
assert x_vec.dot(z_vec) == 0
y_vec = np.cross(x_vec,z_vec)
new = np.asarray([x_vec, y_vec, z_vec])
original = np.asarray([[1,0,0],[0,1,0],[0,0,1]])

tran_matrix = np.empty([3,3])
for row in range(3):
for col in range(3):
tran_matrix[row,col] = np.cos(angle(original[row],new[col]))

return tran_matrix.T ```
Example 9
```def __init__(self, **kw):
"""
Constructor of affine, equidistant 3d mesh class
ucell : unit cell vectors (in coordinate space)
Ecut  : Energy cutoff to parametrize the discretization
"""
from scipy.fftpack import next_fast_len

self.ucell = kw['ucell'] if 'ucell' in kw else 30.0*np.eye(3) # Not even unit cells vectors are required by default
self.Ecut = Ecut = kw['Ecut'] if 'Ecut' in kw else 50.0 # 50.0 Hartree by default
luc = np.sqrt(np.einsum('ix,ix->i', self.ucell, self.ucell))
self.shape = nn = np.array([next_fast_len( int(np.rint(l * np.sqrt(Ecut)/2))) for l in luc], dtype=int)
self.size  = np.prod(self.shape)
gc = self.ucell/(nn) # This is probable the best for finite systems, for PBC use nn, not (nn-1)
self.dv = np.abs(np.dot(gc[0], np.cross(gc[1], gc[2] )))
rr = [np.array([gc[i]*j for j in range(nn[i])]) for i in range(3)]
self.rr = rr
self.origin = kw['origin'] if 'origin' in kw else np.zeros(3) ```
Example 10
```def slice_normal(self):
#The std_slice_normal comes from the cross product of the directions
#in the ImageOrientationPatient
std_slice_normal = super(SiemensWrapper, self).slice_normal
if std_slice_normal is None and csa_slice_normal is None:
return None
elif std_slice_normal is None:
return np.array(csa_slice_normal)
elif csa_slice_normal is None:
return std_slice_normal
else:
#Make sure the two normals are very close to parallel unit vectors
dot_prod = np.dot(csa_slice_normal, std_slice_normal)
assert np.allclose(np.fabs(dot_prod), 1.0, atol=1e-5)
#Use the slice normal computed with the cross product as it will
#always be the most orthogonal, but take the sign from the CSA
#slice normal
if dot_prod < 0:
return -std_slice_normal
else:
return std_slice_normal ```
Example 11
```def _calculate_dadt_ccd(msh, ccd_file, coil_matrix, didt, geo_fn):
""" auxiliary function to calculate the dA/dt field from a ccd file """
# transfrom positions to mm
d_position *= 1e3
# add a column to the position in order to apply the transformation matrix
d_position = np.hstack([d_position, np.ones((d_position.shape[0], 1))])
d_position = coil_matrix.dot(d_position.T).T[:, :3]
# rotate the moment
d_moment = coil_matrix[:3, :3].dot(d_moment.T).T
A = np.zeros((msh.nodes.nr, 3), dtype=float)
for p, m in zip(d_position, d_moment):
# get distance of point to dipole, transform back to meters
r = (msh.nodes.node_coord - p) * 1e-3
A += 1e-7 * didt * np.cross(m, r) / (np.linalg.norm(r, axis=1)[:, None] ** 3)
node_data = mesh_io.NodeData(A)

if geo_fn is not None:
mesh_io.write_geo_spheres(d_position, geo_fn,
np.linalg.norm(d_moment, axis=1),
'coil_dipoles')

return node_data ```
Example 12
```def tms_sphere(sphere3_msh):
m = sphere3_msh.crop_mesh(elm_type=4)
dipole_pos = np.array([0., 0., 300])
dipole_moment = np.array([1., 0., 0.])
didt = 1e6
r = (m.nodes.node_coord - dipole_pos) * 1e-3
dAdt = 1e-7 * didt * np.cross(dipole_moment, r) / (np.linalg.norm(r, axis=1)[:, None] ** 3)
pos = m.elements_baricenters().value
E_analytical = analytical_solutions.tms_E_field(dipole_pos * 1e-3,
dipole_moment, didt,
pos * 1e-3)
cond = mesh_io.ElementData(np.ones(m.elm.nr))
cond.mesh = m
return m, cond, dAdt, E_analytical ```
Example 13
```def camera_info(param):

camY = param[3] * np.sin(phi)
temp = param[3] * np.cos(phi)
camX = temp * np.cos(theta)
camZ = temp * np.sin(theta)
cam_pos = np.array([camX, camY, camZ])

axisZ = cam_pos.copy()
axisY = np.array([0, 1, 0], dtype=np.float32)
axisX = np.cross(axisY, axisZ)
axisY = np.cross(axisZ, axisX)

# cam_mat = np.array([axisX, axisY, axisZ])
cam_mat = np.array([unit(axisX), unit(axisY), unit(axisZ)])

return cam_mat, cam_pos

##################################################### ```
Example 14
```def shear_matrix(angle, direction, point, normal):
"""Return matrix to shear by angle along direction vector on shear plane.
The shear plane is defined by a point and normal vector. The direction
vector must be orthogonal to the plane's normal vector.
A point P is transformed by the shear matrix into P" such that
the vector P-P" is parallel to the direction vector and its extent is
given by the angle of P-P'-P", where P' is the orthogonal projection
of P onto the shear plane.
>>> angle = (random.random() - 0.5) * 4*math.pi
>>> direct = numpy.random.random(3) - 0.5
>>> point = numpy.random.random(3) - 0.5
>>> normal = numpy.cross(direct, numpy.random.random(3))
>>> S = shear_matrix(angle, direct, point, normal)
>>> numpy.allclose(1.0, numpy.linalg.det(S))
True
"""
normal = unit_vector(normal[:3])
direction = unit_vector(direction[:3])
if abs(numpy.dot(normal, direction)) > 1e-6:
raise ValueError("direction and normal vectors are not orthogonal")
angle = math.tan(angle)
M = numpy.identity(4)
M[:3, :3] += angle * numpy.outer(direction, normal)
M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction
return M ```
Example 15
```def mesh_adjust_winding_order(verts, faces, normals):
n0 = normals[faces[:,0]]
n1 = normals[faces[:,1]]
n2 = normals[faces[:,2]]
fnormals = (n0 + n1 + n2) / 3

v0 = verts[faces[:,0]]
v1 = verts[faces[:,1]]
v2 = verts[faces[:,2]]

e0 = v1 - v0
e1 = v2 - v0
fn = np.cross(e0, e1)

dot = np.sum(fnormals * fn, axis=1)
ma = dot < 0

nfaces = faces.copy()
nfaces[ma,1], nfaces[ma,2] = nfaces[ma,2], nfaces[ma,1]

return nfaces ```
Example 16
```def get_local3d(motion3d, angles=None):
"""
Get the unit vectors for local rectangular coordinates for given 3D motion
:param motion3d: numpy array. 3D motion from 3D joints positions, shape (nr_joints, 3, nr_frames).
:param angles: tuple of length 3. Rotation angles around each axis.
:return: numpy array. unit vectors for local rectangular coordinates's , shape (3, 3).
"""
# 2 RightArm 5 LeftArm 9 RightUpLeg 12 LeftUpLeg
horizontal = (motion3d[2] - motion3d[5] + motion3d[9] - motion3d[12]) / 2
horizontal = np.mean(horizontal, axis=1)
horizontal = horizontal / np.linalg.norm(horizontal)
local_z = np.array([0, 0, 1])
local_y = np.cross(horizontal, local_z)  # bugs!!!, horizontal and local_Z may not be perpendicular
local_y = local_y / np.linalg.norm(local_y)
local_x = np.cross(local_y, local_z)
local = np.stack([local_x, local_y, local_z], axis=0)

if angles is not None:
local = rotate_coordinates(local, angles)

return local ```
Example 17
```def barycentric_coordinates_exists(self, P0, P1, P2, P):
u = P1 - P0
v = P2 - P0
w = P - P0

vCrossW = np.cross(v, w)
vCrossU = np.cross(v, u)
if np.dot(vCrossW, vCrossU) < 0:
return False

uCrossW = np.cross(u, w)
uCrossV = np.cross(u, v)

if np.dot(uCrossW, uCrossV) < 0:
return False

denom = np.sqrt((uCrossV ** 2).sum())
r = np.sqrt((vCrossW ** 2).sum()) / denom
t = np.sqrt((uCrossW ** 2).sum()) / denom

return (r <= 1) & (t <= 1) & (r + t <= 1) ```
Example 18
```def updateNormalBuffer(self):
#First compute cross products of all face triangles
V1 = self.VPos[self.ITris[:, 1], :] - self.VPos[self.ITris[:, 0], :]
V2 = self.VPos[self.ITris[:, 2], :] - self.VPos[self.ITris[:, 0], :]
FNormals = np.cross(V1, V2)
FAreas = np.reshape(np.sqrt(np.sum(FNormals**2, 1)), (FNormals.shape[0], 1))
FAreas[FAreas == 0] = 1
FNormals = FNormals/FAreas
self.FNormals = FNormals
self.FCentroid = 0*FNormals
self.VNormals = 0*self.VPos
VAreas = np.zeros((self.VPos.shape[0], 1))
for k in range(3):
self.VNormals[self.ITris[:, k], :] += FAreas*FNormals
VAreas[self.ITris[:, k]] += FAreas
self.FCentroid += self.VPos[self.ITris[:, k], :]
self.FCentroid /= 3
#Normalize normals
VAreas[VAreas == 0] = 1
self.VNormals = self.VNormals / VAreas

#buffersOnly: True if there is no mesh structure other than VPos
#VColors and ITris ```
Example 19
```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) ```
Example 20
```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 ```
Example 21
```def get_random_perpendicular_vector(vector):
"""
Get random normalized perpendicular vector to a given vector.

:Parameters:
#. vector (numpy.ndarray, list, set, tuple): Given vector to compute
a random perpendicular vector to it.

:Returns:
#. perpVector (numpy.ndarray): Perpendicular vector of type
fullrmc.Globals.FLOAT_TYPE
"""
vectorNorm = np.linalg.norm(vector)
assert vectorNorm, LOGGER.error("vector returned 0 norm")
# easy cases
if np.abs(vector[0])<PRECISION:
return np.array([1,0,0], dtype=FLOAT_TYPE)
elif np.abs(vector[1])<PRECISION:
return np.array([0,1,0], dtype=FLOAT_TYPE)
elif np.abs(vector[2])<PRECISION:
return np.array([0,0,1], dtype=FLOAT_TYPE)
# generate random vector
randVect = 1-2*np.random.random(3)
randvect = np.array([vector[idx]*randVect[idx] for idx in range(3)])
# get perpendicular vector
perpVector = np.cross(randvect,vector)
# normalize, coerce and return
return np.array(perpVector/np.linalg.norm(perpVector), dtype=FLOAT_TYPE) ```
Example 22
```def get_orientation_matrix(arrayAxis, alignToAxis):
"""
Get the rotation matrix that aligns arrayAxis to alignToAxis

:Parameters:
#. arrayAxis (list, tuple, numpy.ndarray): xyzArray axis.
#. alignToAxis (list, tuple, numpy.ndarray): The axis to align to.
"""
# normalize alignToAxis
alignToAxisNorm = np.linalg.norm(alignToAxis)
assert alignToAxisNorm>0, LOGGER.error("alignToAxis returned 0 norm")
alignToAxis = np.array(alignToAxis, dtype=FLOAT_TYPE)/alignToAxisNorm
# normalize arrayAxis
arrayAxisNorm = np.linalg.norm(arrayAxis)
assert arrayAxisNorm>0, LOGGER.error("arrayAxis returned 0 norm")
arrayAxis = np.array(arrayAxis, dtype=FLOAT_TYPE)/arrayAxisNorm
# calculate rotationAngle
dotProduct = np.dot(arrayAxis, alignToAxis)
if np.abs(dotProduct-1) <= PRECISION :
rotationAngle = 0
elif np.abs(dotProduct+1) <= PRECISION :
rotationAngle = PI
else:
rotationAngle = np.arccos( dotProduct )
if np.isnan(rotationAngle) or np.abs(rotationAngle) <= PRECISION :
return np.array([[1.,0.,0.],[0.,1.,0.],[0.,0.,1.]]).astype(FLOAT_TYPE)
# calculate rotation axis.
if np.abs(rotationAngle-PI) <= PRECISION:
rotationAxis = get_random_perpendicular_vector(arrayAxis)
else:
rotationAxis = np.cross(alignToAxis, arrayAxis)
#rotationAxis /= np.linalg.norm(rotationAxis)
# calculate rotation matrix
return get_rotation_matrix(rotationAxis, rotationAngle) ```
Example 23
```def orient(xyzArray, arrayAxis, alignToAxis):
"""
Rotates xyzArray using the rotation matrix that rotates and aligns
arrayAxis to alignToAXis.

:Parameters:
#. xyzArray (numpy.ndarray): The xyz (N,3) array to rotate.
#. arrayAxis (list, tuple, numpy.ndarray): xyzArray axis.
#. alignToAxis (list, tuple, numpy.ndarray): The axis to align to.
"""
# normalize alignToAxis
alignToAxisNorm = np.linalg.norm(alignToAxis)
assert alignToAxisNorm>0, LOGGER.error("alignToAxis returned 0 norm")
alignToAxis = np.array(alignToAxis, dtype=FLOAT_TYPE)/alignToAxisNorm
# normalize arrayAxis
arrayAxisNorm = np.linalg.norm(arrayAxis)
assert arrayAxisNorm>0, LOGGER.error("arrayAxis returned 0 norm")
arrayAxis = np.array(arrayAxis, dtype=FLOAT_TYPE)/arrayAxisNorm
# calculate rotationAngle
dotProduct = np.dot(arrayAxis, alignToAxis)
if np.abs(dotProduct-1) <= PRECISION :
rotationAngle = 0
elif np.abs(dotProduct+1) <= PRECISION :
rotationAngle = PI
else:
rotationAngle = np.arccos( dotProduct )
if np.isnan(rotationAngle) or np.abs(rotationAngle) <= PRECISION :
return xyzArray
# calculate rotation axis.
if np.abs(rotationAngle-PI) <= PRECISION:
rotationAxis = get_random_perpendicular_vector(arrayAxis)
else:
rotationAxis = np.cross(alignToAxis, arrayAxis)
#rotationAxis /= np.linalg.norm(rotationAxis)
# calculate rotation matrix
rotationMatrix = get_rotation_matrix(rotationAxis, rotationAngle)
# rotate and return
return rotate(xyzArray , rotationMatrix) ```
Example 24
```def sample_points_on_faces(vs, fs, rng, n_samples_per_face):
idx = np.repeat(np.arange(fs.shape[0]), n_samples_per_face)

r = rng.rand(idx.size, 2)
r1 = r[:,:1]; r2 = r[:,1:]; sqrt_r1 = np.sqrt(r1);

v1 = vs[fs[idx, 0], :]; v2 = vs[fs[idx, 1], :]; v3 = vs[fs[idx, 2], :];
pts = (1-sqrt_r1)*v1 + sqrt_r1*(1-r2)*v2 + sqrt_r1*r2*v3

v1 = vs[fs[:,0], :]; v2 = vs[fs[:, 1], :]; v3 = vs[fs[:, 2], :];
ar = 0.5*np.sqrt(np.sum(np.cross(v1-v3, v2-v3)**2, 1))

return pts, ar, idx ```
Example 25
```def r_between(v_from_, v_to_):
v_from = normalize(v_from_)
v_to = normalize(v_to_)
ax = normalize(np.cross(v_from, v_to))
angle = np.arccos(np.dot(v_from, v_to))
return get_r_matrix(ax, angle) ```
Example 26
```def _compute_orientation(self):
"""
Helper function to return the relative positions between the hole and the peg.
In particular, the intersection of the line defined by the peg and the plane
defined by the hole is computed; the parallel distance, perpendicular distance,
and angle are returned.
"""
cyl_mat = self.sim.data.body_xmat[self.cyl_body_id]
cyl_mat.shape = (3, 3)
cyl_pos = self.sim.data.body_xpos[self.cyl_body_id]

hole_pos = self.sim.data.body_xpos[self.hole_body_id]
hole_mat = self.sim.data.body_xmat[self.hole_body_id]
hole_mat.shape = (3, 3)

v = cyl_mat @ np.array([0, 0, 1])
v = v / np.linalg.norm(v)
center = hole_pos + hole_mat @ np.array([0.1, 0, 0])

t = (center - cyl_pos) @ v / (np.linalg.norm(v) ** 2)
d = np.linalg.norm(np.cross(v, cyl_pos - center)) / np.linalg.norm(v)

hole_normal = hole_mat @ np.array([0, 0, 1])
return (
t,
d,
abs(
np.dot(hole_normal, v) / np.linalg.norm(hole_normal) / np.linalg.norm(v)
),
) ```
Example 27
```def get_pose_error(target_pose, current_pose):
"""
Computes the error corresponding to target pose - current pose as a 6-dim vector.
The first 3 components correspond to translational error while the last 3 components
correspond to the rotational error.

Args:
target_pose: a 4x4 homogenous matrix for the target pose
current_pose: a 4x4 homogenous matrix for the current pose

Returns:
A 6-dim numpy array for the pose error.
"""
error = np.zeros(6)

# compute translational error
target_pos = target_pose[:3, 3]
current_pos = current_pose[:3, 3]
pos_err = target_pos - current_pos

# compute rotational error
r1 = current_pose[:3, 0]
r2 = current_pose[:3, 1]
r3 = current_pose[:3, 2]
r1d = target_pose[:3, 0]
r2d = target_pose[:3, 1]
r3d = target_pose[:3, 2]
rot_err = 0.5 * (np.cross(r1, r1d) + np.cross(r2, r2d) + np.cross(r3, r3d))

error[:3] = pos_err
error[3:] = rot_err
return error ```
Example 28
```def angle(v1, v2):
""" returns the angle (in radians) between two array-like vectors using the
cross-product method, which is more accurate for small angles than the
dot-product-acos method."""
return np.arctan2(np.linalg.norm(np.cross(v1,v2)), np.dot(v1,v2)) ```
Example 29
```def tri_normals_in_place(object, tri_co):
"""Takes N x 3 x 3 set of 3d triangles and
returns non-unit normals and origins"""
object.origins = tri_co[:,0]
object.cross_vecs = tri_co[:,1:] - object.origins[:, nax]
object.normals = np.cross(object.cross_vecs[:,0], object.cross_vecs[:,1])
object.nor_dots = np.einsum("ij, ij->i", object.normals, object.normals)
object.normals /= np.sqrt(object.nor_dots)[:, nax] ```
Example 30
```def get_tri_normals(tr_co):