def grid(self, rotation: R = None, offset: np.array = None) -> np.array:
""" Return the grid points, possibly rotated and offset """
position = self.position
if rotation is not None:
# Rotate from body to grid
position = rotation.apply(position, inverse=True)
if offset is not None: position = np.add(position, offset)
return position
def integrate(self, force: np.array, rotation: R = None) -> np.array:
""" Return surface integral over force """
surface = self.surface
if rotation is not None:
surface = rotation.apply(surface, inverse=True)
# integral of dot product
return \
np.sum(surface[:,0] * force[:,0]) + \
np.sum(surface[:,1] * force[:,1]) + \
np.sum(surface[:,2] * force[:,2])
def cgoCylinder3(p0=[0,0,0],v0=[1,0,0],inputAng=0, L=20, radius=0.5 ,color=[1,0,0] ,name=''):
p0 = np.array(p0)
V0 = np.array(v0)
r = R.from_euler('z', inputAng, degrees=True)
p1 = np.around(R.apply(r,V0), decimals=6)*L+p0
PP0 = list(p0) ; PP1=list(p1)
obj = [cgo.CYLINDER] + PP0 + PP1 + [radius] + color + color
if not name:
name = cmd.get_unused_name('cylinderObj')
cmd.load_cgo(obj, name)
return name,obj, p1
def get_xdot_xddot(yawvel, yawacc, R, zdot, zddot):
""" Return (xdot, xddot) in world frame (same frame as args?)
for Euler ZYX
"""
# Solve for angvel z using three constraints
# (1) x2 / x1 = tan(yaw)
# (2) x dot z = 0
# (3) x dot x = 1
x = R.apply(e1)
z = R.apply(e3)
x_xy_norm2 = x[0] ** 2 + x[1] ** 2
A = np.zeros((3, 3))
A[0, :] = np.array((-x[1], x[0], 0))
A[1, :] = z
A[2, :] = x
Ainv = np.linalg.inv(A)
b = np.array((yawvel * x_xy_norm2, -x.dot(zdot), 0))
xdot = Ainv.dot(b)
if False:
yaw = np.arctan2(x[1], x[0])
x_C = np.array(( np.cos(yaw), np.sin(yaw), 0))
y_C = np.array((-np.sin(yaw), np.cos(yaw), 0))
xdot_ana = (np.eye(3) - np.outer(x, x)).dot(-yawvel * np.cross(x_C, z) + np.cross(y_C, zdot)) / np.linalg.norm(np.cross(y_C, z))
assert np.allclose(xdot_ana, xdot)
# Solve for xddot using the same three constraints
# It turns out that the A matrix is the same.
b20 = yawacc * x_xy_norm2 + 2 * yawvel * (x[0] * xdot[0] + x[1] * xdot[1])
b2 = np.array((b20, -x.dot(zddot) -2 * xdot.dot(zdot), -xdot.dot(xdot)))
xddot = Ainv.dot(b2)
return (xdot, xddot)
def get_vecs_convex(vecs):
"""
for a set of vectors (which assumed to cover less the pi along any direction)
produces set of vectors which is verteces of convex figure, incapsulating all other vectors on the sphere
------
Params:
vecs - a set of vectos in the form of ... x 3 array
return:
cvec - mean unit vector = SUM vecs / || SUM vecs ||
r1, r2 - two orthogonal vectors, which will define axis dirrection on the sphere surface, at which the convex figure will be searched
quat - quaternion, which puts cvec in the equatorial plane along the shortest trajectory
vecs - a set of vectors, which points to the vertexes of the convex hull on the sphere surface, r, d - quasi cartesian coordinates of the vecteces
it should be noted that convex hull is expected to be alongated along equator after quaternion rotation
"""
cvec = vecs.sum(axis=0)
cvec = cvec / sqrt(np.sum(cvec**2.))
vrot = np.cross(np.array([0., 0., 1.]), cvec)
vrot = vrot / np.sqrt(np.sum(vrot**2.))
#vrot2 = np.cross(vrot, cvec)
#vrot2 = vrot2/sqrt(np.sum(vrot2**2))
alpha = pi / 2. - np.arccos(cvec[2])
quat = Rotation([
vrot[0] * sin(alpha / 2.), vrot[1] * sin(alpha / 2.),
vrot[2] * sin(alpha / 2.),
cos(alpha / 2.)
])
r1 = np.array([0, 0, 1])
r2 = np.cross(quat.apply(cvec), r1)
vecn = quat.apply(vecs) - quat.apply(cvec)
l, b = np.sum(quat.apply(vecs) * r2, axis=1), vecn[:, 2]
ch = ConvexHull(np.array([l, b]).T)
r, d = l[ch.vertices], b[ch.vertices]
return cvec, r1, r2, quat, vecs[ch.vertices], r, d
def state_deriv(t, x):
c = self.a * self.alpha
ang_vel_mat = np.array([
[ 0, x[6], -x[5], x[4]],
[-x[6], 0, x[4], x[5]],
[ x[5], -x[4], 0, x[6]],
[-x[4], -x[5], -x[6], 0]
])
orientation = Rotation(x[7:11])
net_force = orientation.apply([0, 0, self.alpha * np.sum(x[0:4])])
return np.array([
0, 0, 0, 0, # Motor torque derivatives
(x[1] - x[3])*c/self.principal_moments[0], # Angular accel x
(x[2] - x[0])*c/self.principal_moments[1], # Angular accel y
(x[0] + x[2] - x[1] - x[3])/self.principal_moments[2] # Angular accel z
] + list(0.5 * ang_vel_mat @ orientation.as_quat()) # Quaternion derivative
+ list([0, 0, -9.8] + net_force/self.mass) # Acceleration
)
def rotate(self, rotation: Rotation) -> Snap:
"""Rotate snapshot.
Parameters
----------
rotation
The rotation as a scipy.spatial.transform.Rotation object.
Returns
-------
Snap
The rotated Snap. Note that the rotation operation is
in-place.
"""
for arr in self._rotation_required():
if arr in self.loaded_arrays():
self._arrays[arr] = rotation.apply(self._arrays[arr])
self._rotation = rotation
return self
def diagonal_plot(self, offset, r: Rotation, draw_3d=True, slice_size=300):
"""
:param offset: point through which the cutting plane passes
:param r: scipy Rotation object
:param draw_3d: Draw 3d plot where the slice coordinates are more readable
:return:
"""
axes = [range(-slice_size, slice_size) for _ in range(2)]
shape = [len(r) for r in axes]
grid = np.meshgrid(*axes, indexing='xy')
grid = np.stack([*grid, np.ones(shape)], axis=-1).reshape(-1, 3)
grid_rotated = r.apply(grid) + offset
grid_rotated = grid_rotated[..., ::-1].T
img = scipy.ndimage.map_coordinates(self.snapshot,
grid_rotated,
order=0).reshape(shape)
single_2d_plot(img)
def draw_cut_on3d(x, y, z, colors):
v, f = make_mesh(self.snapshot, 350, 2)
window_upper = 1000
window_lower = 0
# trying to map density to opacity/color
def intensity_func(x, y, z):
x = x.astype(int)
y = y.astype(int)
z = z.astype(int)
res = self.snapshot[z, y, x]
res[np.where(res < window_lower)] = window_lower
res[np.where(res > window_upper)] = window_upper
return res
def plotly_triangular_mesh(vertices,
faces,
intensities=intensity_func,
opacity=1,
colorscale="Viridis",
showscale=False,
reversescale=False,
plot_edges=False):
x, y, z = vertices.T
I, J, K = faces.T
# setting intensity func
if hasattr(intensities, '__call__'):
intensity = intensities(
x, y, z
) # the intensities are computed here via the passed function,
# that returns a list of vertices intensities
elif isinstance(intensities, (list, np.ndarray)):
intensity = intensities # intensities are given in a list
else:
raise ValueError(
"intensities can be either a function or a list, np.array"
)
mesh = dict(type='mesh3d',
x=x,
y=y,
z=z,
colorscale=colorscale,
opacity=opacity,
reversescale=reversescale,
intensity=intensity,
i=I,
j=J,
k=K,
name='',
showscale=showscale)
if showscale is True:
mesh.update(
colorbar=dict(thickness=20, ticklen=4, len=0.75))
if plot_edges is False: # the triangle sides are not plotted
return [mesh]
pl_BrBG = 'Viridis'
import plotly.graph_objects as go
axis = dict(showbackground=True,
backgroundcolor="rgb(230, 230,230)",
gridcolor="rgb(255, 255, 255)",
zerolinecolor="rgb(255, 255, 255)")
noaxis = dict(visible=False)
layout = dict(title='Isosurface in volumetric data',
font=dict(family='Balto'),
showlegend=False,
width=800,
height=800,
scene=dict(xaxis=axis,
yaxis=axis,
zaxis=axis,
aspectratio=dict(x=1, y=1, z=1)))
fig = go.Figure()
data = plotly_triangular_mesh(v,
f,
opacity=0.8,
colorscale=pl_BrBG,
showscale=True)
fig.add_trace(data[0])
fig.add_scatter3d(x=x,
y=y,
z=z,
mode='markers',
marker=dict(size=2,
color=colors,
colorscale='Hot'))
fig.layout = layout
iplot(fig, filename='multiple volumes')
def get_cut_coords(grid_rotated):
print(type(grid_rotated))
x = [0, 0, 0, 0, 512, 512, 512, 512]
y = [0, 0, 512, 512, 0, 0, 512, 512]
z = [0, 537, 0, 537, 0, 537, 0, 537]
grid_rotated = grid_rotated[..., ::-1].T
grid_rotated = grid_rotated[..., ::-1].T
grid_rotated = grid_rotated[..., ::-1].T
grid_rotated = grid_rotated[np.arange(0, grid_rotated.shape[0],
500)]
x = np.concatenate((x, grid_rotated[:, 0]))
y = np.concatenate((y, grid_rotated[:, 1]))
z = np.concatenate((z, grid_rotated[:, 2]))
colors = np.ones(8)
colors = np.concatenate((colors, np.zeros(grid_rotated.shape[0])))
return x, y, z, colors
if draw_3d:
x, y, z, colors = get_cut_coords(grid_rotated)
draw_cut_on3d(x, y, z, colors)
def linear_velocity(self, orientation: Rotation, speed: float) -> np.array:
return orientation.apply([speed, 0, 0]) # rotate from +x direction
def alpha_from_zddot(R, zdot, zddot, omega, xddot): # See thesis proposal. x = R.apply(e1) y = R.apply(e2) z = R.apply(e3) return np.cross(z, zddot) + omega.dot(z) * zdot + (y.dot(xddot) - omega.dot(x) * omega.dot(y)) * z
def omega_from_zdot(R, zdot, xdot): # See thesis proposal. y = R.apply(e2) z = R.apply(e3) return np.cross(z, zdot) + y.dot(xdot) * z
def apply_rotation(vector: np.ndarray, matrix: R):
return matrix.apply(vector)
