Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- local function trilaterate( A, B, C )
- local a2b = B.vPosition - A.vPosition
- local a2c = C.vPosition - A.vPosition
- if math.abs( a2b:normalize():dot( a2c:normalize() ) ) > 0.999 then
- return nil
- end
- local d = a2b:length()
- local ex = a2b:normalize( )
- local i = ex:dot( a2c )
- local ey = (a2c - (ex * i)):normalize()
- local j = ey:dot( a2c )
- local ez = ex:cross( ey )
- local r1 = A.nDistance
- local r2 = B.nDistance
- local r3 = C.nDistance
- local x = (r1*r1 - r2*r2 + d*d) / (2*d)
- local y = (r1*r1 - r3*r3 - x*x + (x-i)*(x-i) + j*j) / (2*j)
- local result = A.vPosition + (ex * x) + (ey * y)
- local zSquared = r1*r1 - x*x - y*y
- if zSquared > 0 then
- local z = math.sqrt( zSquared )
- local result1 = result + (ez * z)
- local result2 = result - (ez * z)
- local rounded1, rounded2 = result1:round( nRoundSize ), result2:round( nRoundSize )
- if rounded1.x ~= rounded2.x or rounded1.y ~= rounded2.y or rounded1.z ~= rounded2.z then
- return rounded1, rounded2
- else
- return rounded1
- end
- end
- return result:round( nRoundSize )
- end
- local fixes = {}
- for i = 1, 3 do
- local x, y, z
- write("X: ")
- x = tonumber(read())
- write("Y: ")
- y = tonumber(read())
- write("Z: ")
- z = tonumber(read())
- rednet.open("back")
- local e, side, chan, rep, msg, dist
- repeat
- e, side, chan, rep, msg, dist = os.pullEvent("modem_message")
- until rep == 3602
- local fix = {vPosition = vector.new(x, y, z), nDistance = dist}
- table.insert(fixes, fix)
- rednet.close("back")
- end
- local pos = trilaterate(unpack(fixes))
- print(pos.x .. ", " .. pos.y .. ", " .. pos.z)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement