Advertisement
osmarks

Secure GTech Navigation System

Dec 20th, 2019
374
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. local config = dofile "config.lua"
  2. local modems = {}
  3. for name, location in pairs(config.modems) do
  4.     modems[name] = peripheral.wrap(name)
  5.     modems[name].location = location
  6.     modems[name].open(21592)
  7. end
  8.  
  9. local key = config.spudnet_key
  10. if not key then error "SPUDNET key not found." end
  11. local ws
  12.  
  13. local function connect()
  14.     while true do
  15.         ws, err = http.websocket("wss://osmarks.tk/wsthing/SGNS/admin", { authorization = "Key " .. key })
  16.         if err then
  17.             printError(err)
  18.             sleep(1)
  19.         else
  20.             break
  21.         end
  22.     end
  23. end
  24.  
  25. connect()
  26.  
  27. local function send(msg)
  28.     local ok, err = pcall(ws.send, json.encode(msg))
  29.     if not ok then
  30.         connect()
  31.     end
  32. end
  33.  
  34. -- Trilateration code from GPS and modified slightly
  35.  
  36. local function trilaterate( A, B, C )
  37.     local a2b = B.position - A.position
  38.     local a2c = C.position - A.position
  39.        
  40.     if math.abs( a2b:normalize():dot( a2c:normalize() ) ) > 0.999 then
  41.         return nil
  42.     end
  43.    
  44.     local d = a2b:length()
  45.     local ex = a2b:normalize( )
  46.     local i = ex:dot( a2c )
  47.     local ey = (a2c - (ex * i)):normalize()
  48.     local j = ey:dot( a2c )
  49.     local ez = ex:cross( ey )
  50.  
  51.     local r1 = A.distance
  52.     local r2 = B.distance
  53.     local r3 = C.distance
  54.        
  55.     local x = (r1*r1 - r2*r2 + d*d) / (2*d)
  56.     local y = (r1*r1 - r3*r3 - x*x + (x-i)*(x-i) + j*j) / (2*j)
  57.        
  58.     local result = A.position + (ex * x) + (ey * y)
  59.  
  60.     local zSquared = r1*r1 - x*x - y*y
  61.     if zSquared > 0 then
  62.         local z = math.sqrt( zSquared )
  63.         local result1 = result + (ez * z)
  64.         local result2 = result - (ez * z)
  65.        
  66.         local rounded1, rounded2 = result1:round( 0.01 ), result2:round( 0.01 )
  67.         if rounded1.x ~= rounded2.x or rounded1.y ~= rounded2.y or rounded1.z ~= rounded2.z then
  68.             return rounded1, rounded2
  69.         else
  70.             return rounded1
  71.         end
  72.     end
  73.     return result:round( 0.01 )
  74. end
  75.  
  76. local function narrow( p1, p2, fix )
  77.     local dist1 = math.abs( (p1 - fix.position):length() - fix.distance )
  78.     local dist2 = math.abs( (p2 - fix.position):length() - fix.distance )
  79.    
  80.     if math.abs(dist1 - dist2) < 0.01 then
  81.         return p1, p2
  82.     elseif dist1 < dist2 then
  83.         return p1:round( 0.01 )
  84.     else
  85.         return p2:round( 0.01 )
  86.     end
  87. end
  88.  
  89. local fixes = {}
  90.  
  91. while true do
  92.     local _, modem, channel, reply_channel, message, distance = os.pullEvent "modem_message"
  93.     if distance and (type(message) == "string" or type(message) == "number") then
  94.         local reply_modem = modems[modem]
  95.         reply_modem.transmit(reply_channel, gps.CHANNEL_GPS, reply_modem.location)
  96.         table.insert(fixes, { position = vector.new(unpack(reply_modem.location)), distance = distance })
  97.         if #fixes == 4 then
  98.             local p1, p2 = trilaterate(fixes[1], fixes[2], fixes[3])
  99.             if p1 and p2 then
  100.                 local pos = narrow(p1, p2, fixes[4])
  101.                 send { type = "position_fix", dimension = config.dimension, x = pos.x, y = pos.y, z = pos.z, ID = message }
  102.             else
  103.                 send { type = "error", error = "position fix failed", ID = message }
  104.             end
  105.             fixes = {}
  106.         end
  107.     end
  108. end
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement