import open3d as o3d
import numpy as np
theta = np.pi / 2 #4번 문제는 theta = np.pi / 4 로 변경
T1 = np.array([[np.cos(theta),-np.sin(theta),0,0],
[np.sin(theta),np.cos(theta),0,0],
[0,0,1,0],
[0,0,0,1]
])
T2 = np.array([[1,0,0,0],
[0,np.cos(theta),-np.sin(theta),0],
[0,np.sin(theta),np.cos(theta),0],
[0,0,0,1]
])
T3 = np.array([[np.cos(theta),0,np.sin(theta),0],
[0,1,0,0],
[-np.sin(theta),0,np.cos(theta),0],
[0,0,0,1]
])
X_before = np.array([1,0,0,1])
X1 = np.dot(T1,X_before)
print("X1:\n",np.round(X1))
X2 = np.dot(T2,X1)
print("X2:\n",np.round(X2))
X3 = np.dot(T3,X2)
print("X3:\n",np.round(X3))
print(np.linalg.det(T1))
T1inv = np.linalg.inv(T1)
T2inv = np.linalg.inv(T2)
T3inv = np.linalg.inv(T3)
Tinv = T1inv@T2inv@T3inv
X_inv = np.dot(Tinv,X3)
print("X:\n",np.round(X_inv))
# 점이 작아서 잘 안보임
# pcd= o3d.geometry.PointCloud()
# points= np.array([X_before[:3],X1[:3],X2[:3],X3[:3]])
# pcd.points= o3d.utility.Vector3dVector(points)
X = [X_before[:3], X1[:3], X2[:3], X3[:3], X_inv[:3]]
def mesh_sphere(x):
mesh_sphere= o3d.geometry.TriangleMesh.create_sphere(radius=0.1)
mesh_sphere.translate(x)
mesh_sphere.compute_vertex_normals()
mesh_sphere.paint_uniform_color([1, 0, 0])
return mesh_sphere
mesh_sphere_list = []
for i in X:
mesh_sphere_list.append(mesh_sphere(i))
axis_frame= o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5, origin=[0, 0, 0])
for i in mesh_sphere_list:
o3d.visualization.draw_geometries([i,axis_frame], width=500,height=500)