G2A Many GEOs
SHARE
TWEET

TrilateratorGPS

osmarks Nov 10th, 2019 (edited) 122 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(gps.CHANNEL_GPS)
  7. end
  8.  
  9. local function timestamp()
  10.     return os.date "!%X"
  11. end
  12.  
  13. -- Trilateration code from GPS and modified slightly
  14.  
  15. local function trilaterate( A, B, C )
  16.     local a2b = B.position - A.position
  17.     local a2c = C.position - A.position
  18.        
  19.     if math.abs( a2b:normalize():dot( a2c:normalize() ) ) > 0.999 then
  20.         return nil
  21.     end
  22.    
  23.     local d = a2b:length()
  24.     local ex = a2b:normalize( )
  25.     local i = ex:dot( a2c )
  26.     local ey = (a2c - (ex * i)):normalize()
  27.     local j = ey:dot( a2c )
  28.     local ez = ex:cross( ey )
  29.  
  30.     local r1 = A.distance
  31.     local r2 = B.distance
  32.     local r3 = C.distance
  33.        
  34.     local x = (r1*r1 - r2*r2 + d*d) / (2*d)
  35.     local y = (r1*r1 - r3*r3 - x*x + (x-i)*(x-i) + j*j) / (2*j)
  36.        
  37.     local result = A.position + (ex * x) + (ey * y)
  38.  
  39.     local zSquared = r1*r1 - x*x - y*y
  40.     if zSquared > 0 then
  41.         local z = math.sqrt( zSquared )
  42.         local result1 = result + (ez * z)
  43.         local result2 = result - (ez * z)
  44.        
  45.         local rounded1, rounded2 = result1:round( 0.01 ), result2:round( 0.01 )
  46.         if rounded1.x ~= rounded2.x or rounded1.y ~= rounded2.y or rounded1.z ~= rounded2.z then
  47.             return rounded1, rounded2
  48.         else
  49.             return rounded1
  50.         end
  51.     end
  52.     return result:round( 0.01 )
  53. end
  54.  
  55. local function narrow( p1, p2, fix )
  56.     local dist1 = math.abs( (p1 - fix.position):length() - fix.distance )
  57.     local dist2 = math.abs( (p2 - fix.position):length() - fix.distance )
  58.    
  59.     if math.abs(dist1 - dist2) < 0.01 then
  60.         return p1, p2
  61.     elseif dist1 < dist2 then
  62.         return p1:round( 0.01 )
  63.     else
  64.         return p2:round( 0.01 )
  65.     end
  66. end
  67.  
  68. local function short_float(x)
  69.     return ("%.0f"):format(x)
  70. end
  71.  
  72. local monitor = peripheral.find "monitor"
  73. if monitor then
  74.     monitor.setTextScale(0.5)
  75.     term.redirect(monitor)
  76. end
  77.  
  78. print(timestamp(), "Initialized")
  79.  
  80. local fixes = {}
  81.  
  82. while true do
  83.     local _, modem, channel, reply_channel, message, distance = os.pullEvent "modem_message"
  84.     if distance and message == "PING" then
  85.         local reply_modem = modems[modem]
  86.         reply_modem.transmit(reply_channel, gps.CHANNEL_GPS, {
  87.             reply_modem.location[1], reply_modem.location[2], reply_modem.location[3], dimension = config.dimension, server = config.server
  88.         })
  89.         table.insert(fixes, { position = vector.new(unpack(reply_modem.location)), distance = distance })
  90.         if #fixes == 4 then
  91.             local p1, p2 = trilaterate(fixes[1], fixes[2], fixes[3])
  92.             if p1 and p2 then
  93.                 local pos = narrow(p1, p2, fixes[4])
  94.                 print(timestamp(), ("Ping from %.0f %.0f %.0f"):format(pos.x, pos.y, pos.z))
  95.             end
  96.             fixes = {}
  97.         end
  98.     end
  99. end
RAW Paste Data
Ledger Nano X - The secure hardware wallet
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Top