class mbVector(scene.visuals.Vector): """ a simple wrapper class which includes the transformation done on the vector """ def __init__(self, view, face_color, state_vec, orient_vec): super(mbVector, self).__init__(10, 10, 0.05, 1., 0.1, 0.25, color=face_color, shading="smooth", parent=view) self.unfreeze() self.n = 0 self.n_max = len(state_vec) self.trafo = MatrixTransform() self.state_vec = state_vec self.orient_vec = orient_vec def _update(self): """ update called for every time-step """ self.n += 1 if self.n > self.n_max - 1: self.n = 0 return # first flip the vector in the x direction, such that further # rotations are handled correctly self.trafo.set_rotation((0, 0, 1, 0, 1, 0, 1, 0, 0)) # calculates the scaling of the object on bases of the orientation # vector scale_x = self.orient_vec[self.n][0] scale_y = self.orient_vec[self.n][1] scale_z = self.orient_vec[self.n][2] scale = np.sqrt(scale_x * scale_x + scale_y * scale_y + scale_z * scale_z) scale = (scale, 1., 1.) # calculates the translation of the object on bases of the state vector dx = self.state_vec[self.n][0] dy = self.state_vec[self.n][1] dz = self.state_vec[self.n][2] # calculates the rotation (orthogonal O3 Trafo) on bases of the # orientation vector) ex, ey, ez = self._get_ortho_base((scale_x, scale_y, scale_z)) new_base = (ex[0], ex[1], ex[2], ey[0], ey[1], ey[2], ez[0], ez[1], ez[2]) self._doTrafo(dx, dy, dz, base=new_base, scale=scale, reset=False) def _doTrafo(self, x=0., y=0., z=0., base=None, scale=None, reset=True): """ doing first the scale then rotation and afterwards translate """ if reset: self.trafo.reset() if scale is not None: self.trafo.scale(scale) if base is not None: self.trafo.mult_rotation(base) self.trafo.translate((x, y, z)) self.transform = self.trafo def _norm(self, n): """ normalizing a vector n = (x,y,z) """ norm = np.linalg.norm(n) if norm > 0: return n / norm def _cross(self, n0, n1): """ doing a crossproduct i.e. n1 x n2 """ return np.cross(n0, n1) def _ortho(self, n): """ finding an arbitrary orthogonal vector to another in 3d """ x, y, z = n if z != 0. and y != 0.: return np.array((0., z, -y)) elif x != 0. and y != 0.: return np.array((y, -x, 0.)) elif x != 0. and z != 0.: return np.array((z, 0., -x)) elif x == 0 and y == 0: return np.array((1., 0., 0.)) elif x == 0 and z == 0: return np.array((1., 0., 0.)) elif y == 0 and z == 0: return np.array((0., 1., 0.)) else: return np.array((0., 0., 0.)) def _get_ortho_base(self, n): """ calc an ortho base for one direction, such that ex is pointing in the end to that direction """ ex = self._norm(n) ey = self._norm(self._ortho(ex)) ez = self._cross(ex, ey) return ex, ey, ez
class Body(): """ binds tranfos to the bodies and the order of doing it """ def __init__(self, state_vec, p): self.state_vec = state_vec self.n = 0 self.n_max = len(state_vec) self.p = p self.radius = p/6.0 self.rot = MatrixTransform() self.v_orient = None self.x = 0. self.y = 0. self.z = 0. #print("-------", self.n_max, self.p) def set_orient(self,v_orient): """ set orientation to v_orient. :param v_orient: new orientation of point object """ self.v_orient = v_orient def _update(self): """ update rotation and then translation """ global OO self.n += 1 if self.n > self.n_max-1: self.n = 0 return dx = self.state_vec[self.n][0] dy = self.state_vec[self.n][1] dz = self.state_vec[self.n][2] if self.v_orient: new_base = self.v_orient[self.n] else: new_base = None self.trafo(dx,dy,dz,base=new_base) def _norm(self, n): """ normalizing a vector n = (x,y,z) """ x, y, z = n norm = np.sqrt(x*x + y*y + z*z) return x/norm, y/norm, z/norm def _cross(self, n0, n1): """ doing a crossproduct i.e. n1 x n2 """ x0, y0, z0 = n0 x1, y1, z1 = n1 x = y0*z1 - z0*y1 y = z0*x1 - x0*z1 z = x0*y1 - y0*x1 return x,y,z def _ortho(self, n): """ finding an arbitrary orthogonal vector to another in 3d """ x,y,z = n if z!=0. and y!=0.: return 0., z, -y elif x!=0. and y!=0.: return y, -x, 0. elif x!=0. and z!=0.: return z, 0., -x else: return x,y,z+1 def _get_ortho_base(self, n): """ calc an ottho base for one direction, such that ex is pointing in the end to that direction """ ex = self._norm(n) ey = self._norm(self._ortho(ex)) ez = self._cross(ex, ey) return ex, ey, ez def trafo(self, x=0.,y=0.,z=0., base=None, scale=None, reset=True): """ doing first the scale then rotation and afterwards translate """ if reset: self.rot.reset() if scale is not None: self.rot.scale(scale) if base is not None: self.rot.mult_rotation(base) self.rot.translate((x,y,z)) self.transform = self.rot