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)