def translate(self, chainpos, angle, axis):
"""Eigentlich eine transrotate funktion
Zuerst wird translatiert dann rotiert
"""
if chainpos == 1:
RotMat = tr.rotation_matrix(angle, axis)
oldori = self.orients[0]
newori = np.dot(RotMat,oldori)
self.orients[0] = newori
return self
if chainpos > 1:
point = self.roots[chainpos-1] # hier ändern
RotMat = tr.rotation_matrix(angle, axis, point)
oldroot = self.roots[chainpos-1].toggle_trans()
newroot = np.dot(RotMat,oldroot)[:3,:1] # mal länge
oldori = self.orients[chainpos-1]()
newori = np.dot(tr.unit_vector(RotMat),oldori)
self.roots[chainpos-1] = newroot
self.orients[chainpos] = newori
return self
