u

from ./roto_translation import *

__VERSION__="1.0.0 - 2021 Dec 03"

__DESCRIPTION__="""

__DESCRIPTION__="""

not vectorized rototranslation library

M.Maris - V0.0 - 2018 May 03 -

"""

import numpy as np

def transform(point, TransformArray):

  p = np.array([0,0,0,1])

  for i in range (0,len(point)-1):

      p[i] = point[i]

  p=np.dot(TransformArray,np.transpose(p))

  for i in range (0,len(point)-1):

      point[i]=p[i]

  return point

def roto_trans_matrix(rotation, translation):

  xC, xS = trig(rotation[0])

  yC, yS = trig(rotation[1])

  zC, zS = trig(rotation[2])

  dX = translation[0]

  dY = translation[1]

  dZ = translation[2]

  Translate_matrix = np.array([[1, 0, 0, dX],

                               [0, 1, 0, dY],

                               [0, 0, 1, dZ],

                               [0, 0, 0, 1]])

  Rotate_X_matrix = np.array([[1, 0, 0, 0],

                              [0, xC, -xS, 0],

                              [0, xS, xC, 0],

                              [0, 0, 0, 1]])

  Rotate_Y_matrix = np.array([[yC, 0, yS, 0],

                              [0, 1, 0, 0],

                              [-yS, 0, yC, 0],

                              [0, 0, 0, 1]])

  Rotate_Z_matrix = np.array([[zC, -zS, 0, 0],

                              [zS, zC, 0, 0],

                              [0, 0, 1, 0],

                              [0, 0, 0, 1]])

  return np.dot(Rotate_Z_matrix,np.dot(Rotate_Y_matrix,np.dot(Rotate_X_matrix,Translate_matrix)))