Advertisement
Alakazard12

asd

Aug 9th, 2014
321
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Lua 1.61 KB | None | 0 0
  1.  
  2.  
  3. local function trilaterate( A, B, C )
  4.     local a2b = B.vPosition - A.vPosition
  5.     local a2c = C.vPosition - A.vPosition
  6.        
  7.     if math.abs( a2b:normalize():dot( a2c:normalize() ) ) > 0.999 then
  8.         return nil
  9.     end
  10.    
  11.     local d = a2b:length()
  12.     local ex = a2b:normalize( )
  13.     local i = ex:dot( a2c )
  14.     local ey = (a2c - (ex * i)):normalize()
  15.     local j = ey:dot( a2c )
  16.     local ez = ex:cross( ey )
  17.  
  18.     local r1 = A.nDistance
  19.     local r2 = B.nDistance
  20.     local r3 = C.nDistance
  21.        
  22.     local x = (r1*r1 - r2*r2 + d*d) / (2*d)
  23.     local y = (r1*r1 - r3*r3 - x*x + (x-i)*(x-i) + j*j) / (2*j)
  24.        
  25.     local result = A.vPosition + (ex * x) + (ey * y)
  26.  
  27.     local zSquared = r1*r1 - x*x - y*y
  28.     if zSquared > 0 then
  29.         local z = math.sqrt( zSquared )
  30.         local result1 = result + (ez * z)
  31.         local result2 = result - (ez * z)
  32.        
  33.         local rounded1, rounded2 = result1:round( nRoundSize ), result2:round( nRoundSize )
  34.         if rounded1.x ~= rounded2.x or rounded1.y ~= rounded2.y or rounded1.z ~= rounded2.z then
  35.             return rounded1, rounded2
  36.         else
  37.             return rounded1
  38.         end
  39.     end
  40.     return result:round( nRoundSize )
  41.    
  42. end
  43.  
  44. local fixes = {}
  45.  
  46. for i = 1, 3 do
  47.     local x, y, z
  48.     write("X: ")
  49.     x = tonumber(read())
  50.     write("Y: ")
  51.     y = tonumber(read())
  52.     write("Z: ")
  53.     z = tonumber(read())
  54.     rednet.open("back")
  55.  
  56.     local e, side, chan, rep, msg, dist
  57.     repeat
  58.         e, side, chan, rep, msg, dist = os.pullEvent("modem_message")
  59.     until rep == 3602
  60.  
  61.     local fix = {vPosition = vector.new(x, y, z), nDistance = dist}
  62.     table.insert(fixes, fix)
  63.     rednet.close("back")
  64. end
  65.  
  66. local pos = trilaterate(unpack(fixes))
  67.  
  68. print(pos.x .. ", " .. pos.y .. ", " .. pos.z)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement