Created
March 11, 2016 17:54
-
-
Save AngeloGiurano/7a6ee79535b835aa1791 to your computer and use it in GitHub Desktop.
Trilateration 2D - Swift
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
//: Playground - noun: a place where people can play | |
import UIKit | |
func trilateration(point1: Point, point2: Point, point3: Point) { | |
let x1 = point1.position.xCoord | |
let y1 = point1.position.yCoord | |
let x2 = point2.position.xCoord | |
let y2 = point2.position.yCoord | |
let x3 = point3.position.xCoord | |
let y3 = point3.position.yCoord | |
var P1 = [x1, y1] | |
var P2 = [x2, y2] | |
var P3 = [x3, y3] | |
if let z1 = point1.position.zCoord { | |
P1.append(z1) | |
} | |
if let z2 = point2.position.zCoord { | |
P2.append(z2) | |
} | |
if let z3 = point3.position.zCoord { | |
P3.append(z3) | |
} | |
let DistA = point1.distance | |
let DistB = point2.distance | |
let DistC = point3.distance | |
var ex: [Double] = [] | |
var tmp: Double = 0 | |
var P3P1: [Double] = [] | |
var ival: Double = 0 | |
var ey: [Double] = [] | |
var P3P1i: Double = 0 | |
var ez: [Double] = [] | |
var ezx: Double = 0 | |
var ezy: Double = 0 | |
var ezz: Double = 0 | |
// ex = (P2 - P1)/||P2-P1|| | |
for var i = 0; i < P1.count; i++ { | |
let t1 = P2[i] | |
let t2 = P1[i] | |
let t:Double = t1-t2 | |
tmp += (t*t) | |
} | |
for var i = 0; i < P1.count; i++ { | |
let t1 = P2[i] | |
let t2 = P1[i] | |
let exx: Double = (t1-t2)/sqrt(tmp) | |
ex.append(exx) | |
} | |
// i = ex(P3 - P1) | |
for var i = 0; i < P3.count; i++ { | |
let t1 = P3[i] | |
let t2 = P1[i] | |
let t3 = t1-t2 | |
P3P1.append(t3) | |
} | |
for var i = 0; i < ex.count; i++ { | |
let t1 = ex[i] | |
let t2 = P3P1[i] | |
ival += (t1*t2) | |
} | |
//ey = (P3 - P1 - i · ex) / ‖P3 - P1 - i · ex‖ | |
for var i = 0; i < P3.count; i++ { | |
let t1 = P3[i] | |
let t2 = P1[i] | |
let t3 = ex[i] * ival | |
let t = t1 - t2 - t3 | |
P3P1i += (t*t) | |
} | |
for var i = 0; i < P3.count; i++ { | |
let t1 = P3[i] | |
let t2 = P1[i] | |
let t3 = ex[i] * ival | |
let eyy = (t1 - t2 - t3)/sqrt(P3P1i) | |
ey.append(eyy) | |
} | |
if P1.count == 3 { | |
ezx = ex[1]*ey[2] - ex[2]*ey[1] | |
ezy = ex[2]*ey[0] - ex[0]*ey[2] | |
ezz = ex[0]*ey[1] - ex[1]*ey[0] | |
} | |
ez.append(ezx) | |
ez.append(ezy) | |
ez.append(ezz) | |
//d = ‖P2 - P1‖ | |
let d:Double = sqrt(tmp) | |
var j:Double = 0 | |
//j = ey(P3 - P1) | |
for var i = 0; i < ey.count; i++ { | |
let t1 = ey[i] | |
let t2 = P3P1[i] | |
j += (t1*t2) | |
} | |
//x = (r12 - r22 + d2) / 2d | |
let x = (pow(DistA,2) - pow(DistB,2) + pow(d,2))/(2*d) | |
//y = (r12 - r32 + i2 + j2) / 2j - ix / j | |
let y = ((pow(DistA,2) - pow(DistC,2) + pow(ival,2) + pow(j,2))/(2*j)) - ((ival/j)*x) | |
var z: Double = 0 | |
print(x) | |
print(y) | |
if P1.count == 3 { | |
z = sqrt(pow(DistA,2) - pow(x,2) - pow(y,2)) | |
} | |
var unknowPoint:[Double] = [] | |
for var i = 0; i < P1.count; i++ { | |
let t1 = P1[i] | |
let t2 = ex[i] * x | |
let t3 = ey[i] * y | |
let t4 = ez[i] * z | |
let unknownPointCoord = t1 + t2 + t3 + t4 | |
unknowPoint.append(unknownPointCoord) | |
} | |
print(unknowPoint) | |
} | |
struct Point { | |
let position: (xCoord: Double, yCoord: Double, zCoord: Double?) | |
let distance: Double | |
init(position: (xCoord: Double, yCoord: Double, zCoord: Double?), distance: Double) { | |
self.position = position | |
self.distance = distance | |
} | |
} | |
let point1 = Point(position: (xCoord: 152, yCoord: 50, zCoord: nil), distance: 50.342) | |
let point2 = Point(position: (xCoord: 258, yCoord: 90, zCoord: nil), distance: 40.121) | |
let point3 = Point(position: (xCoord: 258, yCoord: 100, zCoord: nil), distance: 50.984) | |
trilateration(point1, point2: point2, point3: point3) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Can you provide the mathematical principle behind this code?