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
