<html><head><meta name="color-scheme" content="light dark"></head><body><pre style="word-wrap: break-word; white-space: pre-wrap;">""" Script used to transform probe STLs from CAD into the probe tip coordinate system. """

from mayavi import mlab

# To access any VTK object, we use 'tvtk', which is a Python wrapping of
# VTK replacing C++ setters and getters by Python properties and
# converting numpy arrays to VTK arrays when setting data.
from tvtk.api import tvtk
from tvtk.common import configure_input_data, configure_input
from mayavi.filters.transform_data import TransformData
import math
import numpy as np
import stl

def get_xyzrpw(T):
    x = T[0,3]
    y = T[1,3]
    z = T[2,3]
    w = math.atan2(T[1,0], T[0,0])
    p = math.atan2(-T[2,0], math.sqrt(T[2,1]**2+T[2,2]**2))
    r = math.atan2(T[2,1], T[2,2])
    return x, y, z, r, p, w

v = mlab.figure()

# filename2 = "/home/morrile2/Documents/MULTIS Data/MULTIS_invitro/Probe STLS/9L4.stl"
filename2 = "/home/morrile2/Documents/MULTIS Data/MULTIS_invitro/Probe STLS/14L5.stl"

# #Transformation for 9L4 Probe T_Global_9L4
# T = np.array([-0.0126419467, -0.9999122962, 0.00394728293, 0.00030525*1000, 0.01730525858, -0.0041657951, -0.9998415745, -0.05709375*1000, 0.9997703282, -0.0125716352, 0.0173564046, 0.0005895*1000, 0, 0, 0, 1])
# T = T.reshape(4, -1)

#Transformation for 14L5 Probe T_Global_14L5
T = np.array([0.00279193729, -0.9999728356, 0.00682151652, -0.000451*1000, 0.0203363336, -0.0067633557, -0.9997703189, -0.057441*1000, 0.9997892971, 0.00293002067, 0.02031689807, -0.00008*1000, 0, 0, 0, 1])
T = T.reshape(4, -1)

# print(np.allclose(np.linalg.det(T[0:3,0:3]), 1))

stl_file = stl.Mesh.from_file(filename2, calculate_normals=True)
stl_file.transform(np.linalg.inv(T))
stl_file.save(filename2[0:-3]+'transformed.stl')

reader3 = tvtk.STLReader()
reader3.file_name=file_name=filename2[0:-3]+'transformed.stl'
reader3.update()


x,y,z,r,p,w = get_xyzrpw(T)
print(x,y,z,r,p,w)
print(math.degrees(r), math.degrees(p), math.degrees(w))
reader2 = tvtk.STLReader()
reader2.file_name=file_name=filename2
reader2.update()

# t = tvtk.Transform()
# tf = tvtk.TransformFilter()
# tf.transform = t
# # t.rotate_z(180)
#
# # t.rotate_y(np.degrees(p))
# # t.rotate_z(np.degrees(w))
# # t.translate((x*1000, y*1000, z*1000))
# configure_input_data(tf, reader2.output)
# tf.update()
#
#
# t1 = tvtk.Transform()
# tf2 = tvtk.TransformFilter()
# tf2.transform = t1
# t1.rotate_z(np.degrees(w))
# configure_input_data(tf2, tf.output)
# tf2.update()
# #
# t2 = tvtk.Transform()
# tf3 = tvtk.TransformFilter()
# tf3.transform = t2
# t2.rotate_y(np.degrees(p))
# configure_input_data(tf3, tf2.output)
# tf3.update()
# #
# t3 = tvtk.Transform()
# tf4 = tvtk.TransformFilter()
# tf4.transform = t3
# t3.rotate_x(np.degrees(r))
# # t3.translate((x*1000, y*1000, z*1000))
# configure_input_data(tf4, tf3.output)
# tf4.update()
#
# mapper2 = tvtk.PolyDataMapper()
# configure_input(mapper2, tf4.output)
# mapper2.update()
#
# # print(tf.output)
# writer = tvtk.STLWriter()
# writer.file_name=file_name=filename2[0:-3]+'transformed.stl'
# # writer.SetInputConnection(mapper2)
# configure_input_data(writer, tf.output)
# writer.write()

#Transformed
mapper2 = tvtk.PolyDataMapper()
configure_input(mapper2, reader3.output)
mapper2.update()

p = tvtk.Property(opacity=0.2, color = (1,0,0))
actor2 = tvtk.Actor(mapper=mapper2, property = p)

v.scene.add_actor(actor2)

#Original
mapper2 = tvtk.PolyDataMapper()
configure_input(mapper2, reader2.output)
mapper2.update()

p = tvtk.Property(opacity=0.2)
actor2 = tvtk.Actor(mapper=mapper2, property = p)


v.scene.add_actor(actor2)

# filename2 = filename2[0:-3]+'transformed.stl'
# reader2 = tvtk.STLReader()
# reader2.file_name=file_name=filename2
# reader2.update()
#
# # x axis translate
# t = tvtk.Transform()
# tf = tvtk.TransformFilter()
# tf.transform = t
# # t.rotate_x(90.0)
# t.rotate_z(180.0)
# # t.translate((57.78, 0, 0))
# configure_input_data(tf, reader2.output)
#
# mapper2 = tvtk.PolyDataMapper()
# configure_input(mapper2, tf)
# mapper2.update()
#
# p = tvtk.Property(opacity=0.2, color=(1, 0, 0))
# actor2 = tvtk.Actor(mapper=mapper2, property = p, orientation=[0, 0, 0])
#
# writer = tvtk.STLWriter()
# writer.file_name=file_name=filename2[0:-3]+'transformed2.stl'
# configure_input_data(writer, tf.output)
# writer.write()
#
# v.scene.add_actor(actor2)
#
#
# # y axis translate
#
# t = tvtk.Transform()
# tf = tvtk.TransformFilter()
# tf.transform = t
# # t.rotate_x(90.0)
# # t.rotate_z(90.0)
# t.translate((0, 57.78, 0))
# configure_input_data(tf, reader2.output)
#
# mapper2 = tvtk.PolyDataMapper()
# configure_input(mapper2, tf)
#
# p = tvtk.Property(opacity=0.2, color=(0, 1, 0))
# actor2 = tvtk.Actor(mapper=mapper2, property = p)
#
#
# v.scene.add_actor(actor2)
#
# # z axis translate
# t = tvtk.Transform()
# tf = tvtk.TransformFilter()
# tf.transform = t
# # t.rotate_x(90.0)
# # t.rotate_z(90.0)
# t.translate((0, 0, 57.78))
# configure_input_data(tf, reader2.output)
#
# mapper2 = tvtk.PolyDataMapper()
# configure_input(mapper2, tf)
#
# p = tvtk.Property(opacity=0.2, color=(0, 0, 1))
# actor2 = tvtk.Actor(mapper=mapper2, property = p)
#
#
# v.scene.add_actor(actor2)


mlab.points3d(0,0,0, 1, scale_factor=5, figure=v)
# mlab.points3d(0,0,0, 1, scale_factor=5, figure=v)

mlab.show()

# # Create a rendering window and renderer
# ren = vtk.vtkRenderer()
# renWin = vtk.vtkRenderWindow()
# renWin.AddRenderer(ren)
#
# # Create a renderwindowinteractor
# iren = vtk.vtkRenderWindowInteractor()
# iren.SetRenderWindow(renWin)
#
# # Assign actor to the renderer
# ren.AddActor(actor)
#
# # Enable user interface interactor
# iren.Initialize()
# renWin.Render()
# iren.Start()</pre></body></html>