![[PukiWiki]](image/pukiwiki.png "[PukiWiki]")
code(python{{ import math from tkinter import * import tkinter.font as tkFont import matrix import copy import threading import re
class Cube:
''' Coordiante-system: Right-handed, Matrices are column-major ones '''
def __init__(self,space):
# set space
self.space=space
self.name=""
#Let these be in World-coordinates (worldview-matrix already applied)
#In right-handed, counter-clockwise order
# original cube
self.org_cube = [
matrix.Vector3D(-0.5,0.5,-0.5),
matrix.Vector3D(0.5,0.5,-0.5),
matrix.Vector3D(0.5,-0.5,-0.5),
matrix.Vector3D(-0.5,-0.5,-0.5),
matrix.Vector3D(-0.5,0.5,0.5),
matrix.Vector3D(0.5,0.5,0.5),
matrix.Vector3D(0.5,-0.5,0.5),
matrix.Vector3D(-0.5,-0.5,0.5)
]
#
# 0------- 1
# 4 ------ 5- |
# | | | |
# | | | |
# | 3--- | 2
# 7--------6
# y
# |
# |
# +------>x
# /
# z
#
# Define the vertices that compose each of the 6 faces. These numbers are
# indices to the vertices list defined above.
self.cubefaces = [(0,4,5,1),(0,1,2,3),(4,0,3,7),(5,4,7,6),(1,5,6,2),(3,2,6,7)]
self.cls = ['red', 'blue', 'yellow', 'green', 'gray', 'white']
#The matrices (Scale, Shear, Rotate, Translate) apply to locally translated cube
self.ang = [0.0, 0.0, 0.0] # phi(x), theta(y), psi(z) of the local cube, apply to the original cube
self.trans = [0.0, 0.0, 0.0] # translation (x, y, z) of the local cube, apply to the original cube
#The Scale Matrix
self.Scale = matrix.Matrix(4, 4)
Scalex = 1.0
Scaley = 1.0
Scalez = 1.0
self.Scale[(0,0)] = Scalex
self.Scale[(1,1)] = Scaley
self.Scale[(2,2)] = Scalez
#The Shear Matrix
self.Shearxy = matrix.Matrix(4, 4)
self.Shearxy[(0,2)] = 0.0
self.Shearxy[(1,2)] = 0.0
self.Shearxz = matrix.Matrix(4, 4)
self.Shearxz[(0,1)] = 0.0
self.Shearxz[(2,1)] = 0.0
self.Shearyz = matrix.Matrix(4, 4)
self.Shearyz[(1,0)] = 0.0
self.Shearyz[(2,0)] = 0.0
self.Shear = self.Shearxy*self.Shearxz*self.Shearyz
#The Rotation Matrices
self.Rotx = matrix.Matrix(4,4)
self.Roty = matrix.Matrix(4,4)
self.Rotz = matrix.Matrix(4,4)
#The Translation Matrix (will contain xoffset, yoffset, zoffset)
self.Tr = matrix.Matrix(4, 4)
self.Tr[(0,3)] = self.trans[0]
self.Tr[(1,3)] = self.trans[1]
self.Tr[(2,3)] = self.trans[2]
# LED-s(+) and a photo-tr(*) on a face
#
# 0----------------1
# | |
# | + |
# | + |
# | |
# | |
# | + |
# | * + |
# | |
# 4----------------5
#
#
self.led_face = [
#0 matrix.Vector3D(-0.5,0.5,-0.5),
matrix.Vector3D(0.3,0.5, 0.4),
#1 matrix.Vector3D(0.5,0.5,-0.5),
matrix.Vector3D(0.4,0.5, -0.3),
#2 matrix.Vector3D(-0.5,0.5,0.5),
matrix.Vector3D(-0.3,0.5,-0.4),
#3 matrix.Vector3D(0.5,0.5,0.5),
matrix.Vector3D(-0.4,0.5,0.3)
]
self.phtr_face = matrix.Vector3D(-0.3,0.5,0.4)
self.face_00=copy.deepcopy([self.led_face,self.phtr_face])
#
self.face_01=self.rotateFaceTo(90.0,0.0,0.0,self.face_00)
self.face_02=self.rotateFaceTo(0.0,-90.0,0.0,self.face_01)
self.face_03=self.rotateFaceTo(0.0,-180.0,0.0,self.face_01)
self.face_04=self.rotateFaceTo(0.0,-270.0,0.0,self.face_01)
self.face_05=self.rotateFaceTo(0.0,0.0,90.0,self.face_04)
self.org_cfaces=[self.face_00,self.face_01,self.face_02,self.face_03,self.face_04,self.face_05]
self.update()
def moveTo(self,x,y,z):
self.trans=[x,y,z]
self.update()
def rotateTo(self,phy,theta,psi):
print("rotateTo(",end="")
print(phy,theta,psi,end="")
print(")")
px=self.get_face_vector(0)
#print("b face_vector(0)=",end="")
#print(px)
px=self.get_face_vector(1)
#print("b face_vector(1)=",end="")
#print(px)
self.ang=[phy,theta,psi]
self.update()
px=self.get_face_vector(0)
#print("a face_vector(0)=",end="")
#print(px)
px=self.get_face_vector(1)
#print("a face_vector(1)=",end="")
#print(px)
def rotateFaceTo(self,phy,theta,psi,orgface):
ang=[phy,theta,psi]
#transform original cube to local cube
#The Rotation Matrices
_Rotx = matrix.Matrix(4,4)
_Roty = matrix.Matrix(4,4)
_Rotz = matrix.Matrix(4,4)
#
_Rotx[(1,1)] = math.cos(math.radians(360.0-ang[0]))
_Rotx[(1,2)] = -math.sin(math.radians(360.0-ang[0]))
_Rotx[(2,1)] = math.sin(math.radians(360.0-ang[0]))
_Rotx[(2,2)] = math.cos(math.radians(360.0-ang[0]))
_Roty[(0,0)] = math.cos(math.radians(360.0-ang[1]))
_Roty[(0,2)] = math.sin(math.radians(360.0-ang[1]))
_Roty[(2,0)] = -math.sin(math.radians(360.0-ang[1]))
_Roty[(2,2)] = math.cos(math.radians(360.0-ang[1]))
_Rotz[(0,0)] = math.cos(math.radians(360.0-ang[2]))
_Rotz[(0,1)] = -math.sin(math.radians(360.0-ang[2]))
_Rotz[(1,0)] = math.sin(math.radians(360.0-ang[2]))
_Rotz[(1,1)] = math.cos(math.radians(360.0-ang[2]))
_Tr = matrix.Matrix(4,4)
_trans = [0.0, 0.0, 0.0]
#The Rotation matrix
_Rot = _Rotx*_Roty*_Rotz
#Translation (just copying)
_Tr[(0,3)] = -_trans[0]
_Tr[(1,3)] = -_trans[1]
_Tr[(2,3)] = -_trans[2]
#The Transformation matrix
_Tsf = _Tr*self.Scale*self.Shear*_Rot
#orgface=[self.led_face,self.phtr_face]
newface=copy.deepcopy(orgface)
xface=newface[0]
for j in range(len(xface)):
v = xface[j]
# Scale, Shear, Rotate the vertex around X axis, then around Y axis, and finally around Z axis and Translate.
r = _Tsf*v
xface[j]=r
v=newface[1]
r = _Tsf*v
newface[1]=r
return newface
def rotate(self,phy,theta,psi):
print(self.get_name(),end="");
print(".rotate(",end="")
print(phy,theta,psi,end="")
print(")")
px=self.get_face_vector(0)
#print("b face_vector(0)=",end="")
#print(px)
px=self.get_face_vector(1)
#print("b face_vector(1)=",end="")
#print(px)
self.ang=[(self.ang[0]+phy)%360.0,(self.ang[1]+theta)%360.0,(self.ang[2]+psi)%360.0]
self.update()
px=self.get_face_vector(0)
#print("a face_vector(0)=",end="")
#print(px)
px=self.get_face_vector(1)
#print("a face_vector(1)=",end="")
#print(px)
def set_name(self,x):
self.name=x
def get_name(self):
return self.name
def get_cube_center(self):
print("get_cube_center ",self.get_name(),end="")
p1=self.cube[0]
p2=self.cube[6]
px=p2+p1
px.x=px.x/2
px.y=px.y/2
px.z=px.z/2
print(px)
return px
def get_face_vector(self,f):
print(self.get_name(),".get_face_vector(",end="")
#print("cube_id")
print(f,end="")
print(")=",end="")
#print(f)
fx=self.cubefaces[f]
fx01=fx[0]
#print("fx01")
#print(fx01)
fx02=fx[2]
#print("fx02")
#print(fx02)
fv01=self.cube[fx01]
#print("fv01")
#print(fv01)
fv02=self.cube[fx02]
cc=self.get_cube_center()
fv01=fv01-cc
fv02=fv02-cc
#print("fv02")
#print(fv02)
r=fv01+fv02
r.x=r.x/2
r.y=r.y/2
r.z=r.z/2
#print("get_face_vector return ",end="")
print(r)
return r
def get_next_place_position(self,f):
print("get_next_place_position ",self.get_name()," f=",end="")
print(f)
nfv=self.get_face_vector(f)
#print("nfv=")
#print(nfv)
#The Scale Matrix
_Scale = matrix.Matrix(4, 4)
Scalex = 2.0
Scaley = 2.0
Scalez = 2.0
_Scale[(0,0)] = Scalex
_Scale[(1,1)] = Scaley
_Scale[(2,2)] = Scalez
v=_Scale*nfv
#print("v=")
#print(v)
cc=self.get_cube_center()
px_x=v.x+cc.x
px_y=v.y+cc.y
px_z=v.z+cc.z
pv=matrix.Vector3D(px_x,px_y,px_z)
print("get_next_place_position return ",end="")
#print("new position=")
print(pv)
return pv
def is_next_door_face(self,my_face,cubex,facex):
print("is_nextdoor_face(",end="")
print(self.get_name())
print(",my_face=",end="")
print(my_face,end="")
print(",cubex=",end="")
print(cubex.get_name(),end="")
print(",facex=",end="")
print(facex,end="")
print(")")
v1=self.get_face_vector(my_face)
tx=matrix.Vector3D(self.trans[0],self.trans[1],self.trans[2])
#print("cube1 position")
#print(tx)
p1=v1+tx
#print("cube1.f position")
#print(p1)
v2=cubex.get_face_vector(facex)
#print("cube2 f position")
#print(v2)
tx2=matrix.Vector3D(cubex.trans[0],cubex.trans[1],cubex.trans[2])
#print("cube2 position")
#print(tx2)
p2=v2+tx2
#print("cube2.f position")
#print(p2)
dx=p1-p2
#print("dx")
#print(dx)
dxx=math.sqrt(dx.x*dx.x+dx.y*dx.y+dx.z*dx.z)
print("dxx=",end="")
print(dxx)
if dxx<0.1:
return True
return False
def rotate_nextdoor_cube_until_match_the_face(self,my_face, next_cube,x_face):
print("rotate_nextdoor_cube_until_match_the_face(",end="")
print(self.get_name(),",",end="")
print("my_face=",end="")
print(my_face,end="")
print(",next_cube=",end="")
print(next_cube.get_name(),end="")
print(",x_face=",end="")
print(x_face,end="")
print(")")
#self.print_cfaces()
#next_cube.print_cfaces()
if self.is_next_door_face( my_face, next_cube, x_face):
return True
dcx=self.get_face_vector(my_face)
if abs(dcx.y) <=0.01 and abs(dcx.z) <=0.01:
next_cube.rotate(0.0,90.0,0.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_next_door_face(my_face, next_cube, x_face):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_next_door_face(my_face, next_cube, x_face):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
if abs(dcx.x) <=0.01 and abs(dcx.z) <=0.01:
next_cube.rotate(90.0,0.0,0.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(90.0,0.0,0.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(90.0,0.0,0.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(90.0,0.0,0.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
if abs(dcx.x) <=0.01 and abs(dcx.y) <=0.01:
next_cube.rotate(90.0,0.0,0.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(90.0,0.0,0.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(90.0,0.0,0.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(90.0,0.0,0.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_next_door_face( my_face, next_cube, x_face):
return True
return False
def is_connect_with_the_direction(self, my_face, cubex, facex, d):
print("is_connect_with_the_direction(",self.get_name(),",",end="")
xface=cubex.cfaces[facex]
print(my_face,end="")
print(",",cubex.get_name(),",",end="")
print(facex,end="")
print(",d=",end="")
print(d,end="")
print(")")
xxface=xface[0]
#print("xxface=")
#print(xxface)
ledx=xxface[d]
print("ledx=",end="")
print(ledx)
#cubex_pos=cubex.get_cube_center()
#ledxx=ledx+cubex_pos
#print("ledxx=",end="")
#print(ledxx)
mfacex=self.cfaces[my_face]
#print("mfacex=",end="")
#print(mfacex)
ptrx=mfacex[1]
print("ptrx=",end="")
print(ptrx)
#mycube_pos=self.get_cube_center()
#ptrxx=ptrx+mycube_pos
#print("ptrxx=",end="")
#print(ptrxx)
#dx=ledxx-ptrxx
dx=ledx-ptrx
print("dx=",end="")
print(dx)
dxx=math.sqrt(dx.x*dx.x+dx.y*dx.y+dx.z*dx.z)
print("dxx=",end="")
print(dxx)
if dxx<0.1:
return True
return False
def rotate_nextdoor_cube_until_match_the_direction(self,my_face, next_cube,x_face,d):
print("rotate_nextdoor_cube_until_match_the_direction(",self.get_name(),end="")
print(",my_face=",end="")
print(my_face,end="")
print(",next_cube=",next_cube.get_name(),end="")
print(",x_face=",end="")
print(x_face,end="")
print(",d=",end="")
print(d,end="")
print(")")
#self.print_cfaces()
#next_cube.print_cfaces()
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
dcx=self.get_face_vector(my_face)
if abs(dcx.y) <=0.01 and abs(dcx.z) <=0.01:
next_cube.rotate(90.0,0.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(90.0,0.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(90.0,0.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(90.0,0.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
if abs(dcx.x) <=0.01 and abs(dcx.z)<=0.01:
next_cube.rotate(0.0,90.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(90.0,0.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(90.0,0.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(90.0,0.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(90.0,0.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
if abs(dcx.x) <=0.01 and abs(dcx.y)<=0.01:
next_cube.rotate(0.0,0.0,90.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,0.0,90.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(90.0,0.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(90.0,0.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(90.0,0.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(90.0,0.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
next_cube.rotate(0.0,90.0,0.0)
if self.is_connect_with_the_direction(my_face, next_cube, x_face,d):
return True
return False
def update(self):
#transform original cube to local cube
self.Rotx[(1,1)] = math.cos(math.radians(360.0-self.ang[0]))
self.Rotx[(1,2)] = -math.sin(math.radians(360.0-self.ang[0]))
self.Rotx[(2,1)] = math.sin(math.radians(360.0-self.ang[0]))
self.Rotx[(2,2)] = math.cos(math.radians(360.0-self.ang[0]))
self.Roty[(0,0)] = math.cos(math.radians(360.0-self.ang[1]))
self.Roty[(0,2)] = math.sin(math.radians(360.0-self.ang[1]))
self.Roty[(2,0)] = -math.sin(math.radians(360.0-self.ang[1]))
self.Roty[(2,2)] = math.cos(math.radians(360.0-self.ang[1]))
self.Rotz[(0,0)] = math.cos(math.radians(360.0-self.ang[2]))
self.Rotz[(0,1)] = -math.sin(math.radians(360.0-self.ang[2]))
self.Rotz[(1,0)] = math.sin(math.radians(360.0-self.ang[2]))
self.Rotz[(1,1)] = math.cos(math.radians(360.0-self.ang[2]))
#The Rotation matrix
self.Rot = self.Rotx*self.Roty*self.Rotz
#Translation (just copying)
self.Tr[(0,3)] = self.trans[0]
self.Tr[(1,3)] = self.trans[1]
self.Tr[(2,3)] = self.trans[2]
#The Transformation matrix
self.Tsf = self.Tr*self.Scale*self.Shear*self.Rot
self.cube=copy.deepcopy(self.org_cube)
for i in range(len(self.cubefaces)):
for j in range(len(self.cubefaces[0])):
v = self.org_cube[self.cubefaces[i][j]]
#print("v[%d,%d]="%(i,j))
#print(v)
# Scale, Shear, Rotate the vertex around X axis, then around Y axis, and finally around Z axis and Translate.
r = self.Tsf*v
#print("r[%d,%d]="%(i,j))
#print(v)
self.cube[self.cubefaces[i][j]]=r
self.cfaces=copy.deepcopy(self.org_cfaces)
#print("update cube ",self.get_name())
for i in range(len(self.cfaces)):
#print("face(",end="")
#print(i,end="")
#print(")")
xface=self.cfaces[i]
xface_led=xface[0]
for j in range(len(xface_led)):
#print("b led(",end="")
#print(j,end="")
#print(")=",end="")
v = xface_led[j]
#print(v)
# Scale, Shear, Rotate the vertex around X axis, then around Y axis, and finally around Z axis and Translate.
r = self.Tsf*v
xface_led[j]=r
#print("a led(",end="")
#print(j,end="")
#print(")=",end="")
#print(r)
v=xface[1]
#print("b ptr()=",end="")
r=self.Tsf*v
xface[1]=r
#print(r)
self.cfaces[i]=xface
def print_cfaces(self):
print("cfaces cube=",self.get_name())
for i in range(len(self.cfaces)):
print("face(",end="")
print(i,end="")
print(")")
xface=self.cfaces[i]
xface_led=xface[0]
for j in range(len(xface_led)):
print(" led(",end="")
print(j,end="")
print(")=",end="")
v = xface_led[j]
print(v)
# Scale, Shear, Rotate the vertex around X axis, then around Y axis, and finally around Z axis and Translate.
v=xface[1]
print("ptr()=",end="")
print(v)
}}
