%begin; !Rubik: an exercise in 3d->3d and 3d->2d transformations and more

%endoflist
%include "mach:extl1.imp"
%list

!!%externallongrealfnspec sqrt(%longreal r)

%integerfn int(%real r)
  %result = intpt(0.5+r) %unless r<0
  %result = -intpt(0.5-r)
%end

%realfn minus(%real r)
  %result=-r
%end

%record(*)%name nil
%recordformat r3(%real x,y,z)
%recordformat r2(%real x,y)
%recordformat r3r3(%real xx,yx,zx,xy,yy,zy,xz,yz,zz)
%recordformat r3r2(%real xx,yx,xy,yy,xz,yz)

%routine transform(%record(r3)%name in,out,%record(r3r3)%name f)
! 3d -> 3d transformation       out = f(in)
%real x,y,z
  x = f_xx*in_x + f_xy*in_y + f_xz*in_z
  y = f_yx*in_x + f_yy*in_y + f_yz*in_z
  z = f_zx*in_x + f_zy*in_y + f_zz*in_z
  out_x = x; out_y = y; out_z = z
%end

%routine project(%record(r3)%name in,%record(r2)%name out,
  %record(r3r2)%name f)
! 3d -> 2d transformation   out = f(in)
  out_x = f_xx*in_x + f_xy*in_y + f_xz*in_z
  out_y = f_yx*in_x + f_yy*in_y + f_yz*in_z
%end

!%realfn distance(%record(r3)%name a,b)
!! scalar distance between two points A and B in 3d space
!  %result = {sqrt(} (a_x-b_x)\2 + (a_y-b_y)\2 + (a_z-b_z)\2 {)}
!%end

%routine p3(%record(r3)%name p,%real x,y,z)
  p_x=x; p_y=y; p_z=z
%end

%routine p2(%record(r2)%name p,%real x,y)
  p_x=x; p_y=y
%end

%routine form r3r3 matrix(%record(r3r3)%name m,%record(r3)%name i,j,k)
  m_xx = i_x; m_yx = i_y; m_zx = i_z
  m_xy = j_x; m_yy = j_y; m_zy = j_z
  m_xz = k_x; m_yz = k_y; m_zz = k_z
%end

%routine form r3r2 matrix(%record(r3r2)%name m,%record(r2)%name i,j,k)
  m_xx = i_x; m_yx = i_y
  m_xy = j_x; m_yy = j_y
  m_xz = k_x; m_yz = k_y
%end

!%realfn determinant(%record(r3r3)%name m)
!%real a,b,c
!
!%realfn d(%real xx,xy,yx,yy)
!  %result = xx*yy-xy*yx
!%end
!
!  a = d(m_yy,m_zy,m_yz,m_zz)
!  b = d(m_yx,m_zx,m_yz,m_zz)
!  c = d(m_yx,m_zx,m_yy,m_zy)
!  %result = m_xx*a - m_xy*b + m_xz*c
!%end

%recordformat squaref(%record(squaref)%name next,dnext,%integer colour,
                      %record(r3)middle,corner1,corner2)
%recordformat piecef(%record(piecef)%name next,%record(squaref)%namec
                     firstsquare,%record(r3)middle)
%recordformat pref(%record(piecef)%name ref)

%record(piecef)%array piece(-13:13)
%record(squaref)%array square(1:54)
%record(pref)%array cube(-13:13)

%record(r3)pi,pj,pk,mi,mj,mk; !(pos and neg)unit vectors spanning r3
%record(r3r3)pr,pl,pu,pd,pf,pb; !Rotation matrices
%record(r3r2)proj
%record(r3)observer
%record(r2)iproj,jproj,kproj

%routine realign point(%record(r3)%name p)
! Eliminate rounding errors incurred by repeated transformations:
! We assume point P should be at a point in r3 which
! is on a 0.05 unit grid.
  p_x = int(p_x*20.0)/20.0
  p_y = int(p_y*20.0)/20.0
  p_z = int(p_z*20.0)/20.0
%end

%routine realign cube
%record(piecef)%name p
%record(r3)%name m
%record(squaref)%name s
%integer i,j,k,x,y,z
  cube(i)_ref == nil %for i = -13,1,13
  %for i=-1,1,1 %cycle
  %for j=-1,1,1 %cycle
  %for k=-1,1,1 %cycle
    p == piece(i+j*3+k*9); m == p_middle
    realign point(m)
    x = int(m_x); y = int(m_y); z = int(m_z)
    cube(x+y*3+z*9)_ref == p
    s == p_firstsquare
    %whilenot addr(s)=0 %cycle
      realign point(s_middle)
      realign point(s_corner1)
      realign point(s_corner2)
      s == s_next
    %repeat
  %repeat
  %repeat
  %repeat
%end

%record(piecef)%map cubelet(%integer i,j,k)
  %result == cube(i+j*3+k*9)_ref
%end

%record(r3r3)%name rot
%record(piecef)%name face

%routine form face(%integer f)
%integer i,j,k,i1,j1,k1,i2,j2,k2
%record(piecef)%name c
  i1=-1; j1=-1; k1=-1; i2=1; j2=1; k2=1
  face == nil; rot == nil
  f=f&95
  %if f='L' %start
    j2=-1; rot == pl
  %finishelseif f='R' %start
    j1=1;  rot == pr
  %finishelseif f='F' %start
    i1=1;  rot == pf
  %finishelseif f='B' %start
    i2=-1; rot == pb
  %finishelseif f='U' %start
    k1=1;  rot == pu
  %finishelseif f='D' %start
    k2=-1; rot == pd
  %finishelsereturn
  %for i = i1,1,i2 %cycle
    %for j =j1,1,j2 %cycle
      %for k=k1,1,k2 %cycle
        c == cubelet(i,j,k); c_next == face; face == c
      %repeat
    %repeat
  %repeat
%end

%record(squaref)%map newsquare(%record(squaref)%name link)
%record(squaref)%name this
%owninteger sq=0
  sq = sq+1; this == square(sq)
  this = 0; this_next == link; %result==this
%end

%routine showpoint(%record(r3)%name r)
  printsymbol('('); print(r_x,0,1); printsymbol('|')
  print(r_y,0,1); printsymbol('|'); print(r_z,0,1); printsymbol(')')
%end

%routine show everything

  %routine showsquares(%record(squaref)%name s)
  %conststring(7)%array cname(0:7)=%C
  "black  ", "red    ", "green  ", "yellow ",
  "blue   ", "magenta", "cyan   ", "white  "
    %whilenot addr(s)=0 %cycle
      printstring(cname(s_colour)); space; showpoint(s_middle)
      space; showpoint(s_corner1); space; showpoint(s_corner2)
      newline; s == s_next
    %repeat
  %end

%integer i,j,k
%integer x,y,z
%record(piecef)%name p

  %for i=-1,1,1 %cycle
  %for j=-1,1,1 %cycle
  %for k=-1,1,1 %cycle
    p == piece(i+j*3+k*9)
    printstring("Piece "); write(i,1); write(j,1); write(k,1)
    printstring(" is at "); showpoint(p_middle); newline
    showsquares(p_firstsquare)
  %repeat
  %repeat
  %repeat
  %for i=-1,1,1 %cycle
  %for j=-1,1,1 %cycle
  %for k=-1,1,1 %cycle
    %for x=-1,1,1 %cycle
    %for y=-1,1,1 %cycle
    %for z=-1,1,1 %cycle
      %if i#x %and j#y %and k#z %start
      %if addr(cubelet(i,j,k))=addr(cubelet(x,y,z)) %start
        printstring("Cubelets "); write(i,1); write(j,1); write(k,1)
        printstring(" and ");     write(x,1); write(y,1); write(z,1)
        printstring(" refer to the same piece."); newline
      %finish
      %finish
    %repeat
    %repeat
    %repeat
  %repeat
  %repeat
  %repeat
%end

%routine display cube
%record(squaref)%name s
%constinteger maxint=(-1)>>1,minint=\maxint
%owninteger left=0,right=1023,bot=0,top=1023
%integer i,j,k
%record(r3)c3,c4
%record(r2)p1,p2,p3,p4
%recordformat i2(%integer x,y)
%record(i2)q1,q2,q3,q4

  %routine mirror(%record(r3)%name point,centre,image)
    image_x = centre_x*2-point_x
    image_y = centre_y*2-point_y
    image_z = centre_z*2-point_z
  %end

  %routine scale(%record(r2)%name i,%record(i2)%name j)
  %constinteger fx=60,fy=60,mx=344,my=256
  %integer x,y
    x = int(i_x*fx+mx); j_x = x
    left = x %if x<left; right=x %if x>right
    y = int(i_y*fy+my); j_y = y
    bot = y %if y<bot; top = y %if y>top
  %end

  %routine add squares(%record(squaref)%name f,%integer x,y,z)
  %record(r3)%name r
  %integer a,b,c
    %whilenot addr(f)=0 %cycle
      r == f_middle
      a = int(r_x*2); b = int(r_y*2); c = int(r_z*2)
      %if (x=0 %or x=a) %and (y=0 %or y=b) %and (z=0 %or z=c) %start
        f_dnext == s %and s == f
      %finish
      f == f_next
    %repeat
  %end

!  %record(squaref)%map sort(%record(squaref)%name z)
!  %record(squaref)%name a,b,c
!  %real dif
!  %integer num=0
!    a == nil; b == nil
!    %cycle
!      %exitif addr(z)=0; c == z
!      z == c_dnext; c_dnext == a; a == c
!num=num+1
!hhh:  %exitif addr(z)=0; c == z
!      z == c_dnext; c_dnext == b; b == c
!num=num+1
!    %repeat
!    %result == a %if addr(b)=0
!    a == sort(a)
!    b == sort(b)
!    z == nil
!    num = -1
!zz: num = num+1
!      %if addr(a)=0 %start
!        %if addr(b)=0 %start
!          %result==z
!        %finish
!        ->bb
!      %finish
!      ->aa %if addr(b)=0
!      dif = distance(a_middle,observer)-distance(b_middle,observer)
!      ->aa %if dif<0
!bb:   c == b; b == b_dnext; b_dnext == z; z == c; ->zz
!aa:   c == a; a == a_dnext; a_dnext == z; z == c; ->zz
!  %end

  s == nil
  %for j=-1,1,1 %cycle
    add squares(cubelet(1,j,k)_firstsquare,3,0,0) %for k=-1,1,1
  %repeat
  %for i=-1,1,1 %cycle
    add squares(cubelet(i,1,k)_firstsquare,0,3,0) %for k=-1,1,1
  %repeat
  %for i=-1,1,1 %cycle
    add squares(cubelet(i,j,1)_firstsquare,0,0,3) %for j=-1,1,1
  %repeat
  %for j=-1,1,1 %cycle
    add squares(cubelet(-1,j,k)_firstsquare,-3,0,0) %for k=-1,1,1
  %repeat
  %for i=-1,1,1 %cycle
    add squares(cubelet(i,-1,k)_firstsquare,0,-3,0) %for k=-1,1,1
  %repeat
  %for i=-1,1,1 %cycle
    add squares(cubelet(i,j,-1)_firstsquare,0,0,-3) %for j=-1,1,1
  %repeat
!  %for i=-1,1,1 %cycle
!  %for j=-1,1,1 %cycle
!  %for k=-1,1,1 %cycle
!    add squares(cubelet(i,j,k)_firstsquare)
!  %repeat
!  %repeat
!  %repeat
!  s == sort(s)
  colour(black)
  fill(left,bot,right,top)
  left = maxint; bot = maxint; right = minint; top = minint
  %whilenot addr(s)=0 %cycle
    colour(s_colour)
    mirror(s_corner1,s_middle,c3); mirror(s_corner2,s_middle,c4)
    project(s_corner1,p1,proj); scale(p1,q1)
    project(s_corner2,p2,proj); scale(p2,q2)
    project(c3,p3,proj); scale(p3,q3)
    project(c4,p4,proj); scale(p4,q4)
    triangle(q1_x,q1_y,q2_x,q2_y,q3_x,q3_y); triangle(q1_x,q1_y,q4_x,q4_y,q3_x,q3_y)
    s == s_dnext
  %repeat
%end

%routine initialise faces(%integer c1,c2)
%integer j,k

%routine do(%record(squaref)%name c,%real x,y,z,%integer col)
%constreal co=0.45; !corner offset
  %while c_colour#0 %cycle
    c == c_next
    %if addr(c)=0 %start
      printstring("** Off squares list **"); newline
      show everything; %stop
    %finish
  %repeat
  c_colour = col
  p3(c_middle,x,y,z)
  p3(c_corner1,x,y-co,z+co)
  p3(c_corner2,x,y+co,z+co)
%end

  %for j = -1,1,1 %cycle
    %for k = -1,1,1 %cycle
      do(cubelet(1,j,k)_firstsquare,1.5,j,k,c1)
      do(cubelet(-1,j,k)_firstsquare,minus(1.5),j,k,c2)
    %repeat
  %repeat
%end

%routine rotate piece(%record(piecef)%name p)
%record(squaref)%name s
  transform(p_middle,p_middle,rot)
  s == p_firstsquare
  %whilenot addr(s)=0 %cycle
    transform(s_middle,s_middle,rot)
    transform(s_corner1,s_corner1,rot)
    transform(s_corner2,s_corner2,rot)
    s == s_next
  %repeat
%end

%routine rotate face
%record(piecef)%name p
  p == face
  %whilenot addr(p)=0 %cycle
    rotate piece(p); p == p_next
  %repeat
  realign cube
%end

%routine rotate cube
%integer i,j,k
  %for i=-1,1,1 %cycle
  %for j=-1,1,1 %cycle
  %for k=-1,1,1 %cycle
    rotate piece(piece(i+j*3+k*9))
  %repeat
  %repeat
  %repeat
  realign cube
%end

%integer i,j,k,l
%record(squaref)%name head
%record(piecef)%name p
%real temp

nil == record(0)

! Set up spanning vectors

temp = minus(1.0)
p3(pi,1.0,0,0); p3(mi,temp,0,0)
p3(pj,0,1.0,0); p3(mj,0,temp,0)
p3(pk,0,0,1.0); p3(mk,0,0,temp)

p2(kproj,0,1.0); p2(jproj,1.0,0)
temp = minus(0.4); p2(iproj,temp,temp)

p3(observer,3.0,3.0,3.0)

! Set up rotation matrices

form r3r3 matrix(pr,pk,pj,mi)
form r3r3 matrix(pl,mk,pj,pi)
form r3r3 matrix(pf,pi,mk,pj)
form r3r3 matrix(pb,pi,pk,mj)
form r3r3 matrix(pu,mj,pi,pk)
form r3r3 matrix(pd,pj,mi,pk)

form r3r2 matrix(proj,iproj,jproj,kproj)

! Set up cube array, giving each cubelet the right number of squares

%for i=-1,1,1 %cycle
%for j=-1,1,1 %cycle
%for k=-1,1,1 %cycle
  l = i+j*3+k*9; p == piece(l); p = 0
  p3(p_middle,i,j,k)
  cube(l)_ref == p; head == nil
  head == newsquare(head) %for l = 1,1,|i|+|j|+|k|
  p_firstsquare == head
%repeat
%repeat
%repeat

! Now we must initialise the squares.  We do this by initialising one
! face only, then rotating the whole cube round so that it presents a
! different face to the initialising tool.

initialise faces(red,magenta)
rot == pu; rotate cube
initialise faces(white,yellow)
rot == pr; rotate cube
initialise faces(blue,green)

newline; printstring("Moves are R r L l U u D d F f B b"); newline
printstring("space or RETURN to redraw without moving"); newline
printstring("! after move to rotate whole cube rather than face"); newline
prompt("Move:")
%cycle
  readsymbol(i)
  %if i=' ' %or i=nl %start
    display cube
  %finishelsestart
    rot == nil
    form face(i); %continueif addr(rot)=0
    %if nextsymbol='!' %start
      rotate cube
      rotate cube %and rotate cube %if i&32=0
    %finishelsestart
      rotate face
      rotate face %and rotate face %if i&32=0
    %finish
  %finish
%repeat

%endofprogram
