terminal glasses (3D)

-- Main program

local p = peripheral.find("openperipheral_bridge")
local s = peripheral.find("openperipheral_sensor")

function vec3d(x,y,z,w)
  if w == nil then w=1 end
  T = {}
  T.x, T.y, T.z, T.w = x,y,z,w
  return T
end

function triangle(a,b,c)
  T = {}
  T[1], T[2], T[3] = a,b,c
  return T
end

function mat4x4()
  M = {}
  for i=1,4 do
    M[i] = {0,0,0,0}
  end
  return M
end

function Vector_Add(vec1,vec2)
  return vec3d(vec1.x + vec2.x , vec1.y + vec2.y, vec1.z + vec2.z)
end

function Vector_Sub(vec1,vec2)
  return vec3d(vec1.x - vec2.x , vec1.y - vec2.y, vec1.z - vec2.z)
end

function Vector_Mul(f,vec1)
  return vec3d(f*vec1.x,f*vec1.y,f*vec1.z)
end

function Vector_Div(vec1,f)
  return vec3d(vec1.x/f,vec1.y/f,vec1.z/f)
end

function Vector_DotProduct(v1,v2)
  return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z
end

function Vector_Length(v)
  return math.sqrt(Vector_DotProduct(v,v))
end

function Vector_Normalise(v)
  local l = Vector_Length(v)
  if l ~= 0 then
    return vec3d(v.x/l, v.y/l, v.z/l)
  else
    return v
  end
end

function Vector_CrossProduct(line1,line2)
    return vec3d(line1.y*line2.z - line1.z*line2.y,
                 line1.z*line2.x - line1.x*line2.z,
                 line1.x*line2.y - line1.y*line2.x)
end

function Matrix_MakeIdentity()
  matrix = mat4x4()
  for i=1,4 do
    matrix[i][i] = 1
  end
  return matrix
end

function Matrix_MakeProjection(fFovDegrees, fAspectRatio, fNear, fFar)
  local fFovRad = 1 / math.tan(math.rad(fFovDegrees) * 0.5)
  local matrix = mat4x4()
  matrix[1][1] = fAspectRatio * fFovRad
  matrix[2][2] = fFovRad
  matrix[3][3] = fFar / (fFar - fNear)
  matrix[4][3] = (-fFar * fNear) / (fFar - fNear)
  matrix[3][4] = 1
  matrix[4][4] = 0
  return matrix
end

local chars = {'x','y','z'}

function Matrix_MultiplyVector(i,m)
  v = vec3d(0,0,0,0)
  v.x = i.x * m[1][1] + i.y * m[2][1] + i.z * m[3][1] + i.w * m[4][1]
  v.y = i.x * m[1][2] + i.y * m[2][2] + i.z * m[3][2] + i.w * m[4][2]
  v.z = i.x * m[1][3] + i.y * m[2][3] + i.z * m[3][3] + i.w * m[4][3]
  v.w = i.x * m[1][4] + i.y * m[2][4] + i.z * m[3][4] + i.w * m[4][4]
  return v
end

function Matrix_MakeRotationZ(theta)
  matrix = mat4x4()
  matrix[1][1] = math.cos(theta)
  matrix[1][2] = math.sin(theta)
  matrix[2][1] = -math.sin(theta)
  matrix[2][2] = math.cos(theta)
  matrix[3][3] = 1
  matrix[4][4] = 1
  return matrix
end

function Matrix_MakeRotationY(theta)
  local matrix = mat4x4()
  matrix[1][1] = math.cos(theta)
  matrix[1][3] = math.sin(theta)
  matrix[2][2] = 1
  matrix[3][1] = -math.sin(theta)
  matrix[3][3] = math.cos(theta)
  matrix[4][4] = 1
  return matrix
end


function Matrix_MakeRotationX(theta)
  local matrix = mat4x4()
  matrix[1][1] = 1
  matrix[2][2] = math.cos(theta)
  matrix[2][3] = math.sin(theta)
  matrix[3][2] = -math.sin(theta)
  matrix[3][3] = math.cos(theta)
  matrix[4][4] = 1
  return matrix
end

function Matrix_Add(m1,m2)
  m = mat4x4()
  for c=1,4 do
    for r=1,4 do
      m[r][c] = m1[r][c] + m2[r][c]
    end
  end
end

function Matrix_MultiplyMatrix(m1,m2)
  matrix = mat4x4()
  for c=1,4 do
    for r=1,4 do
      matrix[r][c] = m1[r][1] * m2[1][c] + m1[r][2] * m2[2][c] + m1[r][3] * m2[3][c] + m1[r][4] * m2[4][c]
    end
  end
  return matrix
end

function Matrix_PointAt(pos,target,up)
  -- Calculate a new forward direction
  newForward = Vector_Sub(target,pos)
  newForward = Vector_Normalise(newForward)

  -- Calculate a new Up direction
  a = Vector_Mul(Vector_DotProduct(up,newForward), newForward)
  newUp = Vector_Sub(up,a)
  newUp = Vector_Normalise(newUp)

  -- New right direction is easy, it's just a crossproduct from up and forward directions
  newRight = Vector_CrossProduct(newUp,(newForward))
  local matrix = mat4x4()
  matrix[1][1] = newRight.x   matrix[1][2] = newRight.y   matrix[1][3] = newRight.z   matrix[1][4] = 0
  matrix[2][1] = newUp.x      matrix[2][2] = newUp.y      matrix[2][3] = newUp.z      matrix[2][4] = 0
  matrix[3][1] = newForward.x matrix[3][2] = newForward.y matrix[3][3] = newForward.z matrix[3][4] = 0
  matrix[4][1] = pos.x        matrix[4][2] = pos.y        matrix[4][3] = pos.z        matrix[4][4] = 1
  return matrix
end

function Matrix_QuickInverse(m)
  local matrix = mat4x4()
  matrix[1][1] = m[1][1] matrix[1][2] = m[2][1] matrix[1][3] = m[3][1] matrix[1][4] = 0
  matrix[2][1] = m[1][2] matrix[2][2] = m[2][2] matrix[2][3] = m[3][2] matrix[2][4] = 0
  matrix[3][1] = m[1][3] matrix[3][2] = m[2][3] matrix[3][3] = m[3][3] matrix[3][4] = 0
  matrix[4][1] = -(m[4][1] * matrix[1][1] + m[4][2] * matrix[2][1] + m[4][3] * matrix[3][1])
  matrix[4][2] = -(m[4][1] * matrix[1][2] + m[4][2] * matrix[2][2] + m[4][3] * matrix[3][2])
  matrix[4][3] = -(m[4][1] * matrix[1][3] + m[4][2] * matrix[2][3] + m[4][3] * matrix[3][3])
  matrix[4][4] = 1
  return matrix
end

function Matrix_MakeTranslation(x,y,z)
  local matrix = Matrix_MakeIdentity()
  matrix[4][1] = x
  matrix[4][2] = y
  matrix[4][3] = z
  return matrix
end

function projectCube(theta)
  matRotX = Matrix_MakeRotationX(theta)
  matRotY = Matrix_MakeRotationY(0)
  matRotZ = Matrix_MakeRotationZ(theta*0.5)

  matCenterTrans = Matrix_MakeTranslation(-0.5,-0.5,-0.5)
  matTrans = Matrix_MakeTranslation(0,0,16)

  matWorld = Matrix_MakeIdentity()
  matWorld = Matrix_MultiplyMatrix(matRotZ,matRotX)
  matWorld = Matrix_MultiplyMatrix(matWorld,matTrans)

  vUp = vec3d(0,1,0)
  vTarget = vec3d(0,0,1)
  matCameraRot = Matrix_MakeRotationY(math.rad(fYaw))
  vLookDir = Matrix_MultiplyVector(vTarget,matCameraRot)
  vTarget = Vector_Add(vCamera, vLookDir)

  matCamera = Matrix_PointAt(vCamera,vTarget,vUp)

  -- Make View matrix for camera
  matView = Matrix_QuickInverse(matCamera)

  matProj = Matrix_MakeProjection(120,1,1,1000)

  for i=1,#meshCube do
    triTransformed = triangle(vec3d(0,0,0),vec3d(0,0,0),vec3d(0,0,0))
    triProjected =   triangle(vec3d(0,0,0),vec3d(0,0,0),vec3d(0,0,0))
    triViewed =      triangle(vec3d(0,0,0),vec3d(0,0,0),vec3d(0,0,0))
    pack = {}

    -- Ajustement au centre de rotation, transformation et projection
    triTransformed[1] = Matrix_MultiplyVector(meshCube[i][1],matCenterTrans)
    triTransformed[2] = Matrix_MultiplyVector(meshCube[i][2],matCenterTrans)
    triTransformed[3] = Matrix_MultiplyVector(meshCube[i][3],matCenterTrans)
    triTransformed[1] = Matrix_MultiplyVector(triTransformed[1],matWorld)
    triTransformed[2] = Matrix_MultiplyVector(triTransformed[2],matWorld)
    triTransformed[3] = Matrix_MultiplyVector(triTransformed[3],matWorld)

    -- Determine le vecteur normale au triangle
    line1,line2,normal = vec3d(0,0,0),vec3d(0,0,0),vec3d(0,0,0)
    line1 = Vector_Sub(triTransformed[2], triTransformed[1])
    line2 = Vector_Sub(triTransformed[3], triTransformed[1])
    normal = Vector_CrossProduct(line1,line2)
    normal = Vector_Normalise(normal)

    -- Get Ray from Triangle to camera
    vCameraRay = Vector_Sub(triTransformed[1], vCamera)

    if (Vector_DotProduct(normal,vCameraRay) < 0) then
      -- Illumination
      light_direction = vec3d(0,0,-1)
      light_direction = Vector_Normalise(light_direction)

      -- How "aligned" are light direction and triangle surface normal ?
      dp = Vector_DotProduct(light_direction,normal)

      -- Convert World Space --> View Space
      triViewed[1] = Matrix_MultiplyVector(triTransformed[1],matView)
      triViewed[2] = Matrix_MultiplyVector(triTransformed[2],matView)
      triViewed[3] = Matrix_MultiplyVector(triTransformed[3],matView)

      -- Projection triangles from 3D --> 2D
      triProjected[1] = Matrix_MultiplyVector(triViewed[1],matProj)
      triProjected[2] = Matrix_MultiplyVector(triViewed[2],matProj)
      triProjected[3] = Matrix_MultiplyVector(triViewed[3],matProj)
      triProjected[1] = Vector_Div(triProjected[1], triProjected[1].w)
      triProjected[2] = Vector_Div(triProjected[2], triProjected[2].w)
      triProjected[3] = Vector_Div(triProjected[3], triProjected[3].w)

      -- Mise a l'echelle
      for i=1,3 do
        pack[i] = {}
        pack[i].x = math.ceil((triProjected[i].x + 1) * 0.5*width) - 10
        pack[i].y = math.ceil((triProjected[i].y + 1) * 0.5*height) - 315
      end

      -- Affichage
      p.addPolygon(0xFFFFFF,dp,pack[1],pack[2],pack[3])
    end
  end
  p.sync()
end
meshCube = {
  -- SOUTH
  triangle(vec3d(0,0,0),vec3d(0,1,0),vec3d(1,1,0)),
  triangle(vec3d(0,0,0),vec3d(1,1,0),vec3d(1,0,0)),
  -- EAST
  triangle(vec3d(1,0,0),vec3d(1,1,0),vec3d(1,1,1)),
  triangle(vec3d(1,0,0),vec3d(1,1,1),vec3d(1,0,1)),
  -- NORTH
  triangle(vec3d(1,0,1),vec3d(1,1,1),vec3d(0,1,1)),
  triangle(vec3d(1,0,1),vec3d(0,1,1),vec3d(0,0,1)),
  -- WEST
  triangle(vec3d(0,0,1),vec3d(0,1,1),vec3d(0,1,0)),
  triangle(vec3d(0,0,1),vec3d(0,1,0),vec3d(0,0,0)),
  -- TOP
  triangle(vec3d(0,1,0),vec3d(0,1,1),vec3d(1,1,1)),
  triangle(vec3d(0,1,0),vec3d(1,1,1),vec3d(1,1,0)),
  -- BOTTOM
  triangle(vec3d(1,0,1),vec3d(0,0,1),vec3d(0,0,0)),
  triangle(vec3d(1,0,1),vec3d(0,0,0),vec3d(1,0,0))
}

vCamera = vec3d(0,0,0)
width =   1300
height =  1300
fYaw = 0
theta = 0

while true do
  if cube ~= nil then
    cube.delete()
  end
  player = s.getPlayerByName("MarkFergus")
  data = player.all()
  fYaw = data.living.yaw
  p.clear()
  --theta = theta+0.05
  cube = projectCube(theta)
  sleep(0.02)
end