diff --git a/.vscode/extensions.json b/.vscode/extensions.json
new file mode 100644
index 0000000000000000000000000000000000000000..ddb6ff85a307649b2e4b11175a6b4fa3ac3b20f9
--- /dev/null
+++ b/.vscode/extensions.json
@@ -0,0 +1,5 @@
+{
+    "recommendations": [
+      "visualstudiotoolsforunity.vstuc"
+    ]
+}
diff --git a/.vscode/launch.json b/.vscode/launch.json
new file mode 100644
index 0000000000000000000000000000000000000000..da60e25ae2b5a8148bbe8f2c810b85bd395e8f59
--- /dev/null
+++ b/.vscode/launch.json
@@ -0,0 +1,10 @@
+{
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "name": "Attach to Unity",
+            "type": "vstuc",
+            "request": "attach"
+        }
+     ]
+}
\ No newline at end of file
diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000000000000000000000000000000000000..36a022c67512531ca0e35a0bf99ef5f75a00355c
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,55 @@
+{
+    "files.exclude": {
+        "**/.DS_Store": true,
+        "**/.git": true,
+        "**/.gitmodules": true,
+        "**/*.booproj": true,
+        "**/*.pidb": true,
+        "**/*.suo": true,
+        "**/*.user": true,
+        "**/*.userprefs": true,
+        "**/*.unityproj": true,
+        "**/*.dll": true,
+        "**/*.exe": true,
+        "**/*.pdf": true,
+        "**/*.mid": true,
+        "**/*.midi": true,
+        "**/*.wav": true,
+        "**/*.gif": true,
+        "**/*.ico": true,
+        "**/*.jpg": true,
+        "**/*.jpeg": true,
+        "**/*.png": true,
+        "**/*.psd": true,
+        "**/*.tga": true,
+        "**/*.tif": true,
+        "**/*.tiff": true,
+        "**/*.3ds": true,
+        "**/*.3DS": true,
+        "**/*.fbx": true,
+        "**/*.FBX": true,
+        "**/*.lxo": true,
+        "**/*.LXO": true,
+        "**/*.ma": true,
+        "**/*.MA": true,
+        "**/*.obj": true,
+        "**/*.OBJ": true,
+        "**/*.asset": true,
+        "**/*.cubemap": true,
+        "**/*.flare": true,
+        "**/*.mat": true,
+        "**/*.meta": true,
+        "**/*.prefab": true,
+        "**/*.unity": true,
+        "build/": true,
+        "Build/": true,
+        "Library/": true,
+        "library/": true,
+        "obj/": true,
+        "Obj/": true,
+        "ProjectSettings/": true,
+        "temp/": true,
+        "Temp/": true
+    },
+    "dotnet.defaultSolution": "onlab.sln"
+}
\ No newline at end of file
diff --git a/Assets/Pcx/Editor/Default Point.mat b/Assets/Pcx/Editor/Default Point.mat
index d893032392a2abd732af06502fe828b16a6e3c2f..b19263bb1e014056b58f319749aa99be0f0eb96d 100644
--- a/Assets/Pcx/Editor/Default Point.mat	
+++ b/Assets/Pcx/Editor/Default Point.mat	
@@ -27,7 +27,7 @@ Material:
     m_Ints: []
     m_Floats:
     - _Distance: 1
-    - _PointSize: 0.05
+    - _PointSize: 2
     m_Colors:
     - _Tint: {r: 0.5, g: 0.5, b: 0.5, a: 1}
   m_BuildTextureStacks: []
diff --git a/Assets/XR/Settings/Open XR Package Settings.asset b/Assets/XR/Settings/Open XR Package Settings.asset
index ece9ce49b00e145a73e621958b4bcbe86f60ea12..1a8ae10804f39a07d837a0e12f03a8ee9b2bff82 100644
--- a/Assets/XR/Settings/Open XR Package Settings.asset	
+++ b/Assets/XR/Settings/Open XR Package Settings.asset	
@@ -630,6 +630,22 @@ MonoBehaviour:
   company: 
   priority: 0
   required: 0
+--- !u!114 &-2418165061576539680
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 0}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: b5a1f07dc5afe854f9f12a4194aca3fb, type: 3}
+  m_Name: WebGL
+  m_EditorClassIdentifier: 
+  features: []
+  m_renderMode: 1
+  m_depthSubmissionMode: 0
+  m_symmetricProjection: 0
 --- !u!114 &-2150382829162842205
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -845,12 +861,13 @@ MonoBehaviour:
   m_Script: {fileID: 11500000, guid: 9f0ebc320a151d3408ea1e9fce54d40e, type: 3}
   m_Name: Open XR Package Settings
   m_EditorClassIdentifier: 
-  Keys: 01000000070000000e00000004000000
+  Keys: 01000000070000000e000000040000000d000000
   Values:
   - {fileID: -3160651408743164439}
   - {fileID: -64324148185763206}
   - {fileID: 4257799398954384198}
   - {fileID: -9211917895290312687}
+  - {fileID: -2418165061576539680}
 --- !u!114 &32361891232910083
 MonoBehaviour:
   m_ObjectHideFlags: 0
diff --git a/ProjectSettings/ProjectSettings.asset b/ProjectSettings/ProjectSettings.asset
index cab930a96ae1a06a93db3e7ac1e069449102a9a8..7931930babddcbd996df9c4bd9f08592e048283e 100644
--- a/ProjectSettings/ProjectSettings.asset
+++ b/ProjectSettings/ProjectSettings.asset
@@ -536,6 +536,9 @@ PlayerSettings:
   - m_BuildTarget: WebGLSupport
     m_APIs: 0b000000
     m_Automatic: 1
+  - m_BuildTarget: WindowsStandaloneSupport
+    m_APIs: 1100000002000000
+    m_Automatic: 0
   m_BuildTargetVRSettings:
   - m_BuildTarget: Standalone
     m_Enabled: 0
diff --git a/TETRA.txt b/TETRA.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3c60911a7c9438175f2717d71479b1d418675077
--- /dev/null
+++ b/TETRA.txt
@@ -0,0 +1,720 @@
+-- Left thumbstick: move and strafe
+-- Right thumbstick: rotate the view horizontally
+
+-- Right grip: switch to snap locomotion mode
+-- Left grip: enable flying mode
+-- Left trigger: switch orientation to left hand controller (can look around while moving)
+
+local socket = require 'socket'
+
+local TEST = false
+local SAVE_TO_FILE = false
+
+function customTablePrint(t)
+	for k,v in pairs(t) do
+		print(k,v)
+		if type(v)=="table" then
+			customTablePrint(v)
+		end
+	end
+end
+
+
+function getTime()
+	return socket.gettime() * 1000 -- to ms
+end
+
+local last_data = nil
+
+function udp_init()
+	--local s_address, s_port = "10.3.18.178", 55555
+	local s_address, s_port = "0.0.0.0", 5555
+	udp = socket.udp()
+	assert(udp:settimeout(0))
+	--assert(udp:settimeout(1e-10))
+	assert(udp:setsockname(s_address, s_port))
+end
+
+function udp_update()
+	act_data = udp:receive(12000) -- string data
+
+	if TEST then
+		act_data = ""
+		local pts = math.random(300,800)
+		local range = 2
+		local step = range*2/(pts-0.5)
+		local tmp = 0
+		for deg = -range, range, step do
+			local z = math.random(1000,5000) -- mm
+			act_data = act_data .. deg .. ' ' .. z .. '\n'
+			tmp = tmp + 1
+		end
+		print('udp new pts', tmp)
+	end
+-- 	print(act_data)
+
+	if act_data then
+		last_data = act_data
+
+		if SAVE_TO_FILE then
+			local filename = "./udp.txt"
+			file = io.open (filename, "a")
+			io.output(file)
+			io.write("TS_" .. getTime() .. "\n")
+			io.write(act_data)
+			io.close(file)
+		end
+	end
+
+--[[
+  if act_data then
+    print(string.len(act_data))
+  else
+    --print(act_data)
+  end
+--]]
+  
+  return last_data
+end
+
+
+Cloud = {}
+CloudMeta = {__index = Cloud}
+
+function Cloud:new(mode, staticness, scanMode)
+ 	local obj = {
+		mesh = nil,
+		vertices = {},
+		mode = mode, -- "points", "lines", "triangles",
+		staticness = staticness, -- "static", "dynamic",
+		trisize = 0.01,
+		scanCounter = 0,
+		scanMode = scanMode,
+		shader = nil,
+		maxPointsPerScan = 1000, -- Upper bound
+		maxScansToStore = -1,
+		verticesPerPoint = 1,
+		current_buffer_index = 1,
+		file_write = 0,
+		color_rule = 0,
+		pcfile = io.open("pointcloud.xyz","w"),
+	}
+	
+	if mode == "lines" then
+		obj.verticesPerPoint = 12
+	end
+	if mode == "triangles" then
+		obj.verticesPerPoint = 12
+	end
+   
+-- 	if obj.mode == "triangles" then
+	--	table.insert(obj.vertices, {0,0,0}) --very strange stuff, why do I need this?
+ --	end
+	return setmetatable(obj, CloudMeta)
+end
+
+function Cloud.meshShaderForScan()
+   meshFormat = { { 'vPosition',  'float', 3 }, { 'scanCounter', 'int',   1 },{'vNorm','float',3} }
+   meshShader = lovr.graphics.newShader([[
+	const float LIFETIME_MS = 10000;
+	in vec3 vPosition;
+	in vec3 vNorm;
+	in int scanCounter;
+	uniform int currentScanCounter;
+	flat out int vScanCounter;
+	flat out vec3 vPos2;
+	vec4 position(mat4 projection, mat4 transform, vec4 vertex)
+	{
+		float diff = currentScanCounter - scanCounter;
+		if (diff > LIFETIME_MS)
+			return projection * transform * vec4(0.0,0.0,0.0,0.0);
+		vScanCounter = scanCounter;
+		vPos2 = vNorm;
+		return projection * transform * vec4(vPosition, 1.0);
+	}
+  ]],
+  [[
+	const float COLORTIME_MS = 10000;
+	flat in int vScanCounter;
+	flat in vec3 vPos2;
+	in vec3 vNorm;
+    uniform int currentScanCounter;
+    vec4 color(vec4 graphicsColor, sampler2D image, vec2 uv)
+	{
+		float diff = currentScanCounter - vScanCounter;
+		float intensity = diff > COLORTIME_MS ? 0 : 1. - sqrt(diff / COLORTIME_MS);
+		return vec4(intensity * vec3(fract(vPos2.x),fract(vPos2.y),fract(vPos2.z)), 1.0);
+    }
+  ]])
+
+	return meshFormat, meshShader
+end
+
+function Cloud.meshShaderForShip()
+   meshFormat = { { 'vPosition',  'float', 3 }, { 'shipColor', 'float', 3 } }
+   meshShader = lovr.graphics.newShader([[
+   in vec3 vPosition;
+   in vec3 shipColor;
+   out vec3 shipColorOut;
+    vec4 position(mat4 projection, mat4 transform, vec4 vertex)
+	{
+		shipColorOut = shipColor;
+		return projection * transform * vec4(vPosition,1.0);
+	}
+  ]],
+  [[
+	in vec3 shipColorOut;
+	vec4 color(vec4 graphicsColor, sampler2D image, vec2 uv)
+	{
+		return vec4(shipColorOut, 1.0);
+      //return vec4(1.0,0.0,1.0,1.0);
+    }
+  ]])
+
+	return meshFormat, meshShader
+end
+
+function Cloud:addLineInner(pointFrom, pointTo, additional)
+	self:addPointInner(pointFrom, additional)
+	self:addPointInner(pointTo, additional)
+end
+
+function Cloud:addTriangleInner(point1, point2, point3,additional)
+	self:addPointInner(point1, additional)
+	self:addPointInner(point2, additional)
+	self:addPointInner(point3, additional)
+end
+
+function Cloud:addPointInner(point, additional)
+	if additional ~= nil then
+		for _,v in ipairs(additional) do
+			table.insert(point,v)
+		end
+	end
+	
+--	x=point[1]
+--	y=point[2]
+--	z=point[3]
+--	if x==nil or y == nil or z == nil then
+--		print("NIL FOUND")
+--	end
+	
+	table.insert(self.vertices, point)
+end
+
+function Cloud:calculateTetrahedron(x,y,z)
+	A = { x, y, z+2*self.trisize/3 }           
+	B = {x + self.trisize* math.cos( 0 / 3 * 2 * math.pi), y + self.trisize* math.sin( 0 / 3 * 2 * math.pi), z-self.trisize/3}
+	C = {x + self.trisize* math.cos( 1 / 3 * 2 * math.pi), y + self.trisize* math.sin( 1 / 3 * 2 * math.pi), z-self.trisize/3}
+	D = {x + self.trisize* math.cos( 2 / 3 * 2 * math.pi), y + self.trisize* math.sin( 2 / 3 * 2 * math.pi), z-self.trisize/3}
+	--[[
+	print("--------------------------------------------------")
+	print(x,y,z)
+	customTablePrint(A)
+	customTablePrint(B)
+	customTablePrint(C)
+	customTablePrint(D)
+	--]]
+	return A,B,C,D
+end
+--[[
+function Cloud:calculateLinesABCD(x,y,z)
+	A = { x, y, z+2*self.trisize/3 }           
+	B = {x + self.trisize* math.cos( 0 / 3 * 2 * math.pi), y + self.trisize* math.sin( 0 / 3 * 2 * math.pi), z-self.trisize/3}
+	C = {x + self.trisize* math.cos( 1 / 3 * 2 * math.pi), y + self.trisize* math.sin( 1 / 3 * 2 * math.pi), z-self.trisize/3}
+	D = {x + self.trisize* math.cos( 2 / 3 * 2 * math.pi), y + self.trisize* math.sin( 2 / 3 * 2 * math.pi), z-self.trisize/3}
+	return A,B,C,D
+end
+--]]
+
+function Cloud:addPointXYZ(x,y,z, additional)
+	if self.mode == "points" then
+		self:addPointInner({x,y,z}, additional)
+	end
+	if self.mode == "triangles" then
+		A,B,C,D = self:calculateTetrahedron(x,y,z)
+		
+		self:addTriangleInner(A,B,C, additional)
+		self:addTriangleInner(A,B,D, additional)
+		self:addTriangleInner(A,D,B, additional)
+		self:addTriangleInner(B,C,D, additional)
+	end
+  	if self.mode == "lines" then
+		A,B,C,D = self:calculateTetrahedron(x,y,z)
+
+		self:addLineInner(A,B, additional)
+		self:addLineInner(A,C, additional)
+		self:addLineInner(A,D, additional)
+		
+		self:addLineInner(C,B, additional)
+		self:addLineInner(D,C, additional)
+		self:addLineInner(B,D, additional)
+	end
+end
+
+function Cloud:draw()
+	if self.shader ~= nil then
+		if self.scanMode then
+-- 			self.shader:send("currentScanCounter", self.scanCounter)
+			self.shader:send("currentScanCounter", getTime())
+		end
+
+		lovr.graphics.setShader(self.shader)
+	end
+	
+	self.mesh:draw()
+	lovr.graphics.setShader()
+end
+
+function Cloud:createMesh(meshFormat, meshShader)
+	if meshFormat == nil then
+		self.mesh = lovr.graphics.newMesh(self.vertices, self.mode,self.staticness, false)
+	else
+		self.mesh = lovr.graphics.newMesh(meshFormat, self.vertices, self.mode,self.staticness, true)
+	end
+
+	self.shader = meshShader
+	self.vertices = {}
+end
+
+function Cloud:getMaxVerticiesBufferSize()
+	return self.maxScansToStore * self.maxPointsPerScan * self.verticesPerPoint
+end
+function Cloud:setAllZeroAllVerticesInSelf()
+	size = self:getMaxVerticiesBufferSize()
+	self.vertices = {}
+	for index = 1, size do
+		table.insert(self.vertices, {0, 0, 0, 0})
+	end
+end
+function Cloud:prepare(maxScansToStore, meshFormat, meshShader)
+	self.maxScansToStore = maxScansToStore
+	self:setAllZeroAllVerticesInSelf()
+	self:createMesh(meshFormat, meshShader)
+end
+
+function Cloud:update(newMeasuredPoints)
+-- 	meshVerticesIndex = self.scanCounter % self.maxScansToStore
+-- 	self.scanCounter = self.scanCounter + 1
+
+	for _,v in ipairs(newMeasuredPoints) do
+		self:addPointXYZ(v[1],v[2],v[3], {getTime(), v[4],v[5],v[6]});
+		if (self.file_write==1) then
+			self.pcfile:write(string.format("%.4f %.4f %.4f",v[1],v[2],v[3])," ", math.floor(v[4]*255)," ",math.floor(v[5]*255)," ",math.floor(v[6]*255),"\n")
+		end
+	end
+	
+	local max_verticies_buffer_size = self:getMaxVerticiesBufferSize()
+
+	if #self.vertices > max_verticies_buffer_size then
+		print("ERROR - TOO SMALL VERTEX BUFFER") -- ERROR
+		ERROR() -- exception? XD -- fixme
+	end
+
+	local spaceInBuffer = math.min(#self.vertices,  max_verticies_buffer_size-self.current_buffer_index+1)
+	if spaceInBuffer > 0 then
+		scan.mesh:setVertices(self.vertices, self.current_buffer_index, spaceInBuffer)
+	end
+	if spaceInBuffer ~= #self.vertices then
+		self.current_buffer_index = 1 -- reset (circular)
+		spaceInBuffer = #self.vertices - spaceInBuffer -- the leftover
+		-- build rest of the vertices
+
+		-- Speedup?
+		local leftover = {}
+		for idx = spaceInBuffer+1, #self.vertices do
+			table.insert(leftover, self.vertices[idx])
+		end
+		spaceInBuffer = #leftover
+		scan.mesh:setVertices( leftover, self.current_buffer_index, spaceInBuffer)
+
+-- 		scan.mesh:setVertices( { unpack(self.vertices, spaceInBuffer+1) }, self.current_buffer_index, spaceInBuffer) -- sometimes crashes with too large unpack size... ???
+	end
+	self.current_buffer_index = self.current_buffer_index + spaceInBuffer
+	
+	self.vertices = {}
+end
+
+function Cloud:reset()
+	self:setAllZeroAllVerticesInSelf()
+	scan.mesh:setVertices(self.vertices, idx)
+	self.vertices = {}
+	self.scanCounter = 0
+	print('Scans are reseted')
+end
+
+local motion = {
+  pose = lovr.math.newMat4(), -- Transformation in VR initialized to origin (0,0,0) looking down -Z
+  thumbstickDeadzone = 0.4,   -- Smaller thumbstick displacements are ignored (too much noise)
+  directionFrom = 'head',     -- Movement can be relative to orientation of head or left controller
+  flying = false,
+  -- Snap motion parameters
+  snapTurnAngle = 2 * math.pi / 12,
+  dashDistance = 1.5,
+  thumbstickCooldownTime = 0.3,
+  thumbstickCooldown = 0,
+  -- Smooth motion parameters
+  turningSpeed = 2 * math.pi * 1 / 6,
+  walkingSpeed = 4,
+  last_direction_x = 0,
+  last_direction_y = 0,
+  last_direction_z = 0,
+  maxWalkingSpeed = 2,
+  drawing = false,
+ -- drawsize = 2000000,
+--   currentdrawindex = 0,
+  environment_behavior = 0,
+}
+
+
+function motion.smooth(dt)
+  if lovr.headset.isTracked('right') then
+    local x, y = lovr.headset.getAxis('right', 'touchpad')
+    -- Smooth horizontal turning
+    if math.abs(x) > motion.thumbstickDeadzone then
+      motion.pose:rotate(-x * motion.turningSpeed * dt, 0, 1, 0)
+    end
+  end
+  if lovr.headset.isTracked('left') then
+    local x, y = lovr.headset.getAxis('left', 'touchpad')
+    local direction = quat(lovr.headset.getOrientation(motion.directionFrom)):direction()
+   
+    if motion.directionFrom == 'left' then
+      direction = quat(-math.pi / 4, 1,0,0):mul(vec3(direction))
+    end
+    if not motion.flying then
+      direction.y = 0
+    end
+    last_direction = lovr.math.newVec3(0, 0, 0)
+    -- Smooth strafe movement
+    if math.abs(x) > motion.thumbstickDeadzone then
+      local strafeVector = quat(-math.pi / 2, 0,1,0):mul(vec3(direction))
+      last_direction = last_direction + (strafeVector * x * motion.maxWalkingSpeed * dt)
+--      motion.pose:translate(strafeVector * x * motion.walkingSpeed * dt)
+    end
+    -- Smooth Forward/backward movement
+    if math.abs(y) > motion.thumbstickDeadzone then
+
+--      motion.pose:translate(direction * y * motion.walkingSpeed * dt)
+      last_direction  = last_direction + (direction * y * motion.maxWalkingSpeed * dt)
+    end
+    motion.last_direction_x = motion.last_direction_x + last_direction.x
+    motion.last_direction_y = motion.last_direction_y + last_direction.y
+    motion.last_direction_z = motion.last_direction_z + last_direction.z
+    motion.pose:translate(motion.last_direction_x,motion.last_direction_y,motion.last_direction_z)
+    motion.last_direction_x = motion.last_direction_x * .9
+    motion.last_direction_y = motion.last_direction_y * .9
+    motion.last_direction_z = motion.last_direction_z * .9
+  end
+end
+
+function motion.snap(dt)
+  -- Snap horizontal turning
+  if lovr.headset.isTracked('right') then
+    local x, y = lovr.headset.getAxis('right', 'touchpad')
+    if math.abs(x) > motion.thumbstickDeadzone and motion.thumbstickCooldown < 0 then
+      local angle = -x / math.abs(x) * motion.snapTurnAngle
+      motion.pose:rotate(angle, 0, 1, 0)
+      motion.thumbstickCooldown = motion.thumbstickCooldownTime
+    end
+  end
+  -- Dashing forward/backward
+  if lovr.headset.isTracked('left') then
+    local x, y = lovr.headset.getAxis('left', 'touchpad')
+    if math.abs(y) > motion.thumbstickDeadzone and motion.thumbstickCooldown < 0 then
+      local moveVector = quat(lovr.headset.getOrientation('head')):direction()
+      if not motion.flying then
+        moveVector.y = 0
+      end
+      moveVector:mul(y / math.abs(y) * motion.dashDistance)
+      motion.pose:translate(moveVector)
+      motion.thumbstickCooldown = motion.thumbstickCooldownTime
+    end
+  end
+  motion.thumbstickCooldown = motion.thumbstickCooldown - dt
+end
+
+
+function motion_with_rotation()
+	local isTracked = lovr.headset.isTracked()
+	if lastHP and isTracked then
+		local actPos = vec3(actHP)
+		local lastPos = vec3(lastHP)
+		motion.pose:translate(lastPos - actPos)
+	end
+	if isTracked then
+		lastHP = lovr.math.newMat4(actHP)
+	end
+end
+function motion_as_static_env()
+	local isTracked = lovr.headset.isTracked()
+	if lastHP and isTracked then
+		motion.pose:mul(mat4(lastHP):mul(mat4(actHP):invert()))
+-- 		print(motion.pose:unpack())
+	end
+	if isTracked then
+		lastHP = lovr.math.newMat4(actHP)
+	end
+end
+
+local HUD = {
+	DISPLAY_FRAME_NUM = 200,
+	display_string = nill,
+	live = -1,
+}
+function lovr.update(dt)
+	data = udp_update()
+	--print("UPDATE")
+	--[[
+	if data then
+		print(string.len(data))
+	else
+		print(data)
+	end
+	--]]
+
+	------------------- ENV BEHAVIOR -------------------
+	if lovr.headset.wasReleased('right', 'grip') then
+		motion.environment_behavior = motion.environment_behavior + 1
+		if motion.environment_behavior > 2 then
+			motion.environment_behavior = 0
+			lastHP = nil -- reset
+		end
+		-- HUD display
+		HUD.live = HUD.DISPLAY_FRAME_NUM
+		if motion.environment_behavior == 0 then
+			HUD.display_string = 'Nomal tracked'
+		elseif motion.environment_behavior == 1 then
+			HUD.display_string = 'Just rotation'
+		elseif motion.environment_behavior == 2 then
+			HUD.display_string = 'No track'
+		else
+			HUD.display_string = 'FIX ME'
+		end
+	end
+
+	if lovr.headset.isTracked() then
+		actHP = lovr.math.newMat4(lovr.headset.getPose())
+	end
+	if motion.environment_behavior == 1 then
+		motion_with_rotation()
+	elseif motion.environment_behavior == 2 then
+		motion_as_static_env()
+	end
+	-----------------------------------------------------
+
+	---[[
+	motion.directionFrom = lovr.headset.isDown('left', 'trigger') and 'left' or 'head'
+	if lovr.headset.isDown('left', 'grip') then
+		motion.flying = true
+	elseif lovr.headset.wasReleased('left', 'grip') then
+		motion.flying = false
+		local height = vec3(motion.pose).y
+		motion.pose:translate(0, -height, 0)
+	end
+	
+	if lovr.headset.isDown('right', 'trigger') then
+		motion.drawing = true
+	elseif lovr.headset.wasReleased('right', 'trigger') then
+		motion.drawing = false
+	end
+	
+	if motion.drawing or TEST then
+		local handString = 'hand/right'
+
+		if TEST then handString = 'hand/left' end -- hack for test
+
+		local posehand = lovr.math.newMat4(lovr.headset.getPose(handString))
+		local globhand = lovr.math.newMat4(motion.pose)
+		globhand:mul(posehand);
+		local newpointcount = 0
+
+		lidardeg={}
+		lidardist={}
+
+		if data then
+-- 			print(string.len(data))
+
+			for deg, dist in string.gmatch(data, "([%w.e-]+) ([%w.e-]+)\n") do
+--					table.insert(lidardeg,-deg)
+--					table.insert(lidardist, -dist/1000)
+
+---[[
+				if tonumber(dist) > 20 then
+					table.insert(lidardeg,-deg)
+					table.insert(lidardist, -dist/1000)
+
+				else
+					table.insert(lidardeg, 0)
+					table.insert(lidardist, 10000)
+				end
+				--if deg == nil or dist == nil then
+				--	print(deg .. "  " .. dist)
+			--	end
+			--]]
+				--print(data)
+			end
+
+			newpointcount = #lidardeg
+-- 			print(newpointcount)
+			last_data = nil
+		end
+
+		--print(motion.currentdrawindex)
+
+		local rotation_offset = -math.pi/12
+		local translation_offset = vec3(-0.06,-0.1,0)
+
+		newMeasuredPoints = {}
+		for i=1,newpointcount do
+			if lidardeg[i] then
+				local locdir = lovr.math.newMat4(globhand)
+				locdir:rotate(quat(lidardeg[i]+rotation_offset, 1,0,0))
+				locdir:translate(vec3(0,0,lidardist[i]) + translation_offset);
+				local handx, handy, handz, handsx, handsy, handsz, handr1, handr2, handr3, handr4 = locdir:unpack();
+				local normx,normy, normz;
+				if scan.color_rule > 2 then
+					scan.color_rule = 0;
+				end
+				if scan.color_rule == 0 then
+						normx=0.99;
+						normy=0.99;
+						normz=0.99	;
+				end
+				if (scan.color_rule == 1) then
+					if i>1 and lidardeg[i-1] then
+						normx=10*(lidardist[i-1]-lidardist[i])
+						normy=0.5+2*(lidardist[i-1]-lidardist[i])
+						normz=1.0-10*(lidardist[i-1]-lidardist[i])
+					else
+						normx=0.5;
+						normy=0.5;
+						normz=0.5;
+					end
+				end
+				if (scan.color_rule == 2) then
+						normx=math.fmod(handx,1)
+						normy=math.fmod(handy,1)
+						normz=math.fmod(handz,1)
+				end
+				table.insert(newMeasuredPoints, {handx,handy,handz, normx, normy, normz})
+				--if handx == n
+-- 				motion.currentdrawindex = motion.currentdrawindex + newpointcount
+--				if (motion.currentdrawindex >= motion.drawsize- newpointcount) then
+--					motion.currentdrawindex = 0
+--				end
+			end
+		end
+		scan:update(newMeasuredPoints)
+		--scan:setVertices(newvertices, origmeshsize + motion.currentdrawindex)
+	end
+  
+	motion.smooth(dt)
+  --[[
+	if lovr.headset.isDown('right', 'grip') then
+		motion.snap(dt)
+	else
+		motion.smooth(dt)
+	end
+	--]]
+
+	if SAVE_TO_FILE then
+		local headPose = lovr.math.newMat4(lovr.headset.getPose('head'))
+		local handPose = lovr.math.newMat4(lovr.headset.getPose('hand/right'))
+		local filename = "./track.txt"
+		file = io.open (filename, "a")
+		io.output(file)
+		io.write("TS_" .. getTime() .. "\n")
+		io.write("HEAD_")
+		local tmp = { headPose:unpack(true) }
+		for idx = 1, 16 do
+			io.write(tmp[idx] .. "|")
+		end
+		io.write("\nHAND_")
+		local tmp = { handPose:unpack(true) }
+		for idx = 1, 16 do
+			io.write(tmp[idx] .. "|")
+		end
+		io.write("\n")
+		io.close(file)
+	end
+end
+
+----------------------------- EVENTS -----------------------------
+function lovr.keyreleased(key, scancode)
+	if key == 'r' then -- scan reset
+		scan:reset()
+	end
+	if key == 'f' then 
+		scan.file_write = 1-scan.file_write 
+	end
+	if key == 'c' then 
+		scan.color_rule = scan.color_rule+1;
+	end
+end
+------------------------------------------------------------------
+
+function lovr.draw()
+-- 	lovr.graphics.setBackgroundColor(0.1, 0.1, 0.1)
+	lovr.graphics.setBackgroundColor(0,0,0)
+	lovr.graphics.transform(mat4(motion.pose):invert())
+	
+	-- Render hands
+	lovr.graphics.setColor(1,1,1)
+	local radius = 0.05
+	for _, hand in ipairs(lovr.headset.getHands()) do
+		-- Whenever pose of hand or head is used, need to account for VR movement
+		local poseRW = mat4(lovr.headset.getPose(hand))
+		local poseVR = mat4(motion.pose):mul(poseRW)
+		poseVR:scale(radius)
+		lovr.graphics.box('line',poseVR)
+	end
+
+	if ship ~= nil and ship.mesh ~= nil then
+		--ship:draw()
+	end
+	if scan ~= nil and scan.mesh ~= nil then
+		scan:draw()
+	end
+
+	-- Display HUD
+	if HUD.live > 0 then
+		local pose = mat4(motion.pose):mul(mat4(lovr.headset.getPose()))
+		local textPos = pose:mul(vec3(0,0,-1,1))
+		local x,y,z, sx,sy,sz, angle,ax,ay,az = pose:unpack()
+		lovr.graphics.print(HUD.display_string, textPos[1],textPos[2],textPos[3], .1, angle,ax,ay,az)
+		HUD.live = HUD.live - 1
+	end
+end
+
+function lovr.load()
+	udp_init()
+
+	--local file = io.open("data/centered.ply") -- sunken "Hableány" ship measured by Norbit.
+	if file then
+	ship = Cloud:new("points","static", false)
+	  while true do
+		local x,z,y,r,g,b=file:read("*n","*n","*n","*n","*n","*n")
+		if x == nil then break end
+
+		ship:addPointXYZ(x,y,z, {r/255,g/255,b/255})
+	  end
+	  ship:createMesh(Cloud.meshShaderForShip())
+	else
+		print("open error")
+	end
+
+-- 	scan = Cloud:new("points", "dynamic", true)
+	scan = Cloud:new("lines", "dynamic", true)
+-- 	scan = Cloud:new("triangles", "dynamic", true)
+	scan:prepare(50, Cloud.meshShaderForScan())
+
+	print(scan.verticesPerPoint)
+
+	--customTablePrint(ship:getVertexFormat())
+end
+
+
+
+
+