Allow to define rotations with sliders

testExampleOfRotation.py

# save in a len=3 buffer the last moved sliders
# generate transform for slider combination according to dictionary consulted by the buffer
# calculate transform in bufferOrder and set sliders



import math
import numpy as np

#source https://github.com/matthew-brett/transforms3d/blob/52321496a0d98f1f698fd3ed81f680d740202553/transforms3d/_gohlketransforms.py#L1680

# epsilon for testing whether a number is close to zero
_EPS = np.finfo(float).eps * 4.0

# axis sequences for Euler angles
_NEXT_AXIS = [1, 2, 0, 1]

xyzd = {"0":"x","1":"y","2":"z","3":"u"}
d012 = {"x":"0","y":"1","z":"2","u":"3"}

# map axes strings to/from tuples of inner axis, parity, repetition, frame
_AXES2TUPLE = {
    'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),
    'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),
    'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),
    'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),
    'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),
    'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),
    'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),
    'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}

_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())

def euler_matrix(ai, aj, ak, axes='sxyz'):
    """Return homogeneous rotation matrix from Euler angles and axis sequence.
    ai, aj, ak : Euler's roll, pitch and yaw angles
    axes : One of 24 axis sequences as string or encoded tuple
    >>> R = euler_matrix(1, 2, 3, 'syxz')
    >>> np.allclose(np.sum(R[0]), -1.34786452)
    True
    >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1))
    >>> np.allclose(np.sum(R[0]), -0.383436184)
    True
    >>> ai, aj, ak = (4*math.pi) * (np.random.random(3) - 0.5)
    >>> for axes in _AXES2TUPLE.keys():
    ...    R = euler_matrix(ai, aj, ak, axes)
    >>> for axes in _TUPLE2AXES.keys():
    ...    R = euler_matrix(ai, aj, ak, axes)
    """
    try:
        firstaxis, parity, repetition, frame = _AXES2TUPLE[axes]
    except (AttributeError, KeyError):
        _TUPLE2AXES[axes]  # validation
        firstaxis, parity, repetition, frame = axes
    #
    i = firstaxis
    j = _NEXT_AXIS[i+parity]
    k = _NEXT_AXIS[i-parity+1]
    #
    if frame:
        ai, ak = ak, ai
    if parity:
        ai, aj, ak = -ai, -aj, -ak
    #
    si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak)
    ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak)
    cc, cs = ci*ck, ci*sk
    sc, ss = si*ck, si*sk
    #
    M = np.identity(4)
    if repetition:
        M[i, i] = cj
        M[i, j] = sj*si
        M[i, k] = sj*ci
        M[j, i] = sj*sk
        M[j, j] = -cj*ss+cc
        M[j, k] = -cj*cs-sc
        M[k, i] = -sj*ck
        M[k, j] = cj*sc+cs
        M[k, k] = cj*cc-ss
    else:
        M[i, i] = cj*ck
        M[i, j] = sj*sc-cs
        M[i, k] = sj*cc+ss
        M[j, i] = cj*sk
        M[j, j] = sj*ss+cc
        M[j, k] = sj*cs-sc
        M[k, i] = -sj
        M[k, j] = cj*si
        M[k, k] = cj*ci
    return M


def euler_from_matrix(matrix, axes='sxyz'):
    """Return Euler angles from rotation matrix for specified axis sequence.
    axes : One of 24 axis sequences as string or encoded tuple
    Note that many Euler angle triplets can describe one matrix.
    >>> R0 = euler_matrix(1, 2, 3, 'syxz')
    >>> al, be, ga = euler_from_matrix(R0, 'syxz')
    >>> R1 = euler_matrix(al, be, ga, 'syxz')
    >>> np.allclose(R0, R1)
    True

    angles = (math.pi/2,math.pi/4,-math.pi/6)
    mydict = {'syxz': (1, 1, 0, 0)}
    for axes in mydict.keys():
        R0 = euler_matrix(axes=axes, *angles)
        R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes))
        print(euler_from_matrix(R0, axes))
        if not np.allclose(R0, R1): print(axes, "failed")
    """
    try:
        firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
    except (AttributeError, KeyError):
        _TUPLE2AXES[axes]  # validation
        firstaxis, parity, repetition, frame = axes
    #
    i = firstaxis
    j = _NEXT_AXIS[i+parity]
    k = _NEXT_AXIS[i-parity+1]
    #
    M = np.array(matrix, dtype=np.float64, copy=True)[:3, :3]
    #print(M)
    #scales = np.linalg.norm(M,axis=1)
    #M[:,0] = M[:,0]/scales[0]
    #M[:,1] = M[:,1]/scales[1]
    #M[:,2] = M[:,2]/scales[2]
    if repetition:
        sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k])
        if sy > _EPS:
            ax = math.atan2( M[i, j],  M[i, k])
            ay = math.atan2( sy,       M[i, i])
            az = math.atan2( M[j, i], -M[k, i])
        else:
            ax = math.atan2(-M[j, k],  M[j, j])
            ay = math.atan2( sy,       M[i, i])
            az = 0.0
    else:
        cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i])
        if cy > _EPS:
            ax = math.atan2( M[k, j],  M[k, k])
            ay = math.atan2(-M[k, i],  cy)
            az = math.atan2( M[j, i],  M[i, i])
        else:
            ax = math.atan2(-M[j, k],  M[j, j])
            ay = math.atan2(-M[k, i],  cy)
            az = 0.0
    #
    if parity:
        ax, ay, az = -ax, -ay, -az
    if frame:
        ax, az = az, ax
    return ax, ay, az



transformNode = slicer.mrmlScene.AddNewNodeByClass('vtkMRMLTransformNode')
# transformNode = getNode('Transform')

def rotationDegFromBufferOrder(R,buffer_post_reversed):
    '''
    Concert a rotation matrix into the Mayavi/Vtk rotation paramaters (pitch, roll, yaw).
    The buffer order indicates the transforms are applied in index-crescent order, postmultiply order
    '''
    #
    r_abc = np.array(euler_from_matrix(R,'s'+buffer_post_reversed))*180/math.pi
    #
    return r_abc


def bufferPostToPre(buffer_post):
    """The buffer (x3,y2,z1) is decoded so that last transforms are indexed first (z1,y2,x3)"""
    decoded = ""
    for i in range(len(buffer_post)):
        decoded += xyzd[str(buffer_post[i])]
    return decoded[::-1]


def bufferPreToPost(buffer_pre):
    """The buffer_pre (z1,y2,x3) is encoded so that transforms are applied in index-crescent order, 
    postmultiply order (x3,y2,z1)"""
    encoded = []
    for i in range(len(buffer_pre)):
        encoded.insert(0,int(d012[buffer_pre[i]]))
    return encoded



transformObserver = 0
# Create widget
widget = qt.QFrame()
layout = qt.QFormLayout()
widget.setLayout(layout)
axisSliderWidgets = []
#means transforms are applyied in index-decrescent order
buffer_post = []
for i in range(3):
    axisSliderWidget = ctk.ctkSliderWidget()
    axisSliderWidget.singleStep = 1.0
    if i==1:
        axisSliderWidget.minimum = -90
        axisSliderWidget.maximum = 90
    else:
        axisSliderWidget.minimum = -180
        axisSliderWidget.maximum = 180
    axisSliderWidget.value = 0
    axisSliderWidget.tracking = False
    axisSliderWidget.objectName = str(i)
    layout.addRow(f"Axis {i+1}: ", axisSliderWidget)
    axisSliderWidgets.append(axisSliderWidget)

buffer_post.append(3)
buffer_post.append(3)
buffer_post.append(3)

axisSliderWidgets[0].value = 0
axisSliderWidgets[1].value = 0
axisSliderWidgets[2].value = 0

deltaAngle=[0,0,0]



def updateTransformFromWidget(value,widget):
    global transformObserver,buffer_post,deltaAngle
    #
    print("start")
    widgetObjectName = widget.objectName
    if (int(widgetObjectName) != buffer_post[0]):
        if int(widgetObjectName) == buffer_post[1]:
            aux = buffer_post[0]
            buffer_post[0] = buffer_post[1]
            buffer_post[1] = aux
        else:
            #FIFO
            buffer_post.insert(0,int(widgetObjectName))
            buffer_post.pop()
            #
            buffer_pre = bufferPostToPre(buffer_post)
            #print(buffer_pre)
            if buffer_pre[1] == "u":
                for axes in _AXES2TUPLE.keys():
                    print(axes)
                    if ((buffer_pre[-1]==axes[1]) and (axes[0]=='s')):
                        buffer_pre = axes[1:4]
                        break
                #
                buffer_post = bufferPreToPost(buffer_pre)
    #
    print("buffer_post")
    print(buffer_post)
    transform = vtk.vtkTransform()
    transform.PostMultiply()
    #
    #correct angles:
    eulerMat = euler_matrix(
        vtk.vtkMath.RadiansFromDegrees(axisSliderWidgets[1].value),
        vtk.vtkMath.RadiansFromDegrees(axisSliderWidgets[0].value),
        vtk.vtkMath.RadiansFromDegrees(axisSliderWidgets[2].value),
        axes='syxz'
    )
    convertedAnglesDeg_pre = rotationDegFromBufferOrder(
        eulerMat,
        bufferPostToPre(buffer_post)
    )
    print("convertedAnglesDeg_pre")
    print(convertedAnglesDeg_pre)
    #anglesOrig = np.array([
    #    axisSliderWidgets[0].value,
    #    axisSliderWidgets[1].value,
    #    axisSliderWidgets[2].value,
    #])
    #deltaAngle = anglesOrig - convertedAnglesDeg_pre
    #
    convertedAnglesDeg_post = convertedAnglesDeg_pre[::-1]
    if buffer_post[-1]!=3:
        if buffer_post[-1]==0:
            transform.RotateX(convertedAnglesDeg_post[-1])
        elif buffer_post[-1]==1:
            transform.RotateY(convertedAnglesDeg_post[-1])
        elif buffer_post[-1]==2:
            transform.RotateZ(convertedAnglesDeg_post[-1])
    #
    if buffer_post[-2]!=3:
        axisOfRotation = [int(buffer_post[-2]==0),int(buffer_post[-2]==1),int(buffer_post[-2]==2),0]
        #transform.GetMatrix().MultiplyPoint(axisOfRotation,axisOfRotation)
        transform.RotateWXYZ(convertedAnglesDeg_post[-2],axisOfRotation[0],axisOfRotation[1],axisOfRotation[2])
    #
    if buffer_post[-3]!=3:
        axisOfRotation = [int(buffer_post[-3]==0),int(buffer_post[-3]==1),int(buffer_post[-3]==2),0]
        #transform.GetMatrix().MultiplyPoint(axisOfRotation,axisOfRotation)
        transform.RotateWXYZ(convertedAnglesDeg_post[-3],axisOfRotation[0],axisOfRotation[1],axisOfRotation[2])
    #
    #print(np.linalg.det(arrayFromVTKMatrix(transform.GetMatrix())))
    #
    transformNode.RemoveObserver(transformObserver)
    #print(arrayFromVTKMatrix(transform.GetMatrix())[:3,:3])
    print(
        rotationDegFromBufferOrder(
            arrayFromVTKMatrix(transform.GetMatrix())[:3,:3],
            bufferPostToPre(buffer_post)
        )
    )
    print()
    transformNode.SetMatrixTransformToParent(transform.GetMatrix())
    transformObserver = transformNode.AddObserver(slicer.vtkMRMLTransformableNode.TransformModifiedEvent, updateWidgetFromTransform)

def updateWidgetFromTransform(caller, event):
    global deltaAngle,buffer,angleBuffer
    #
    #convertedAnglesDeg_post_reversed = rotationDegFromBufferOrder(
    #    arrayFromVTKMatrix(caller.GetMatrixTransformToParent()),
    #    bufferPostToPre(buffer)
    #)
    for i in range(3):
        axisSliderWidget = axisSliderWidgets[i]
        wasBlocked = axisSliderWidget.blockSignals(True)
        if buffer[0]==i:
            axisSliderWidget.minimum = -90
            axisSliderWidget.maximum = 90
        else:
            axisSliderWidget.minimum = -180
            axisSliderWidget.maximum = 180
        axisSliderWidget.value = angleBuffer[i] + deltaAngle[i]
        axisSliderWidget.blockSignals(wasBlocked)

def resetTransform():
    transformNode.SetMatrixTransformToParent(vtk.vtkMatrix4x4())

for i in range(3):
    axisSliderWidgets[i].valueChanged.connect(
        lambda value,slider=axisSliderWidgets[i]: updateTransformFromWidget(value,slider)
    )

resetButton = qt.QPushButton("Reset")
layout.addWidget(resetButton)
resetButton.connect("clicked()", resetTransform)

widget.show()

transformObserver = transformNode.AddObserver(slicer.vtkMRMLTransformableNode.TransformModifiedEvent, updateWidgetFromTransform)

updateTransformFromWidget(0,axisSliderWidgets[0])

# transformNode.RemoveObserver(transformObserver)

# rotationDegFromBufferOrder(euler_matrix(math.pi/6,math.pi/4,math.pi/8,'syxz'),bufferPostToPre(buffer_post))
# 






###### HERE IT STARTS THE CODE FOR TESTING

#Test
# useful link
# https://en.wikipedia.org/wiki/Davenport_chained_rotations

m60y_45x_15z_pre = euler_matrix(math.pi/6,math.pi/4,math.pi/8,'syxz')
sliderChangesFILO = buffer_post
sliderChangesOfOrdering_pre = bufferPostToPre(sliderChangesFILO)
anglesDegInPre = rotationDegFromBufferOrder(m60y_45x_15z_pre,sliderChangesOfOrdering_pre)
mustMatchMatrix = euler_matrix(
    vtk.vtkMath.RadiansFromDegrees(anglesDegInPre[0]),
    vtk.vtkMath.RadiansFromDegrees(anglesDegInPre[1]),
    vtk.vtkMath.RadiansFromDegrees(anglesDegInPre[2]),
    's'+sliderChangesOfOrdering_pre
)
np.allclose(m60y_45x_15z_pre, mustMatchMatrix)



transform = vtk.vtkTransform()
transform.PostMultiply()
#
axisOfRotation = [0,1,0,0]
#transform.GetMatrix().MultiplyPoint(axisOfRotation,axisOfRotation)
transform.RotateWXYZ(anglesDegInPre[0],axisOfRotation[0],axisOfRotation[1],axisOfRotation[2])
#
axisOfRotation = [0,0,1,0]
#transform.GetMatrix().MultiplyPoint(axisOfRotation,axisOfRotation)
transform.RotateWXYZ(anglesDegInPre[1],axisOfRotation[0],axisOfRotation[1],axisOfRotation[2])
#
axisOfRotation = [1,0,0,0]
#transform.GetMatrix().MultiplyPoint(axisOfRotation,axisOfRotation)
transform.RotateWXYZ(anglesDegInPre[2],axisOfRotation[0],axisOfRotation[1],axisOfRotation[2])
#
np.allclose(m60y_45x_15z_pre, arrayFromVTKMatrix(transform.GetMatrix()))





# https://en.wikipedia.org/wiki/Davenport_chained_rotations#Conversion_to_extrinsic_rotations
# check davenport theorem

matrix_intrinsic = euler_matrix(math.pi/6,math.pi/4,math.pi/8,'syxz')
matrix_extrinsic = euler_matrix(math.pi/8,math.pi/4,math.pi/6,'rzxy')
np.allclose(matrix_extrinsic, matrix_extrinsic)

  File "/var/folders/9b/mhf79phj3ss95qk4p1_vw6gh0000gn/T/xpython_71059/1097426148.py", line 319, in updateWidgetFromTransform
    if buffer[0]==i:
NameError: name 'buffer' is not defined

what's wrong?