public static Vector3 Trilaterate(SignalSnapShot p1, SignalSnapShot p2, SignalSnapShot p3)
{
var ex = vector_divide(vector_subtract(p2.pos, p1.pos), norm(vector_subtract(p2.pos, p1.pos)));
var i = dot(ex, vector_subtract(p3.pos, p1.pos));
var a = vector_subtract(vector_subtract(p3.pos, p1.pos), vector_multiply(ex, i));
var ey = vector_divide(a, norm(a));
var ez = vector_cross(ex, ey);
var d = norm(vector_subtract(p2.pos, p1.pos));
var j = dot(ey, vector_subtract(p3.pos, p1.pos));
var x = (sqr(p1.radius) - sqr(p2.radius) + sqr(d)) / (2 * d);
var y = (sqr(p1.radius) - sqr(p3.radius) + sqr(i) + sqr(j)) / (2 * j) - (i / j) * x;
var b = sqr(p1.radius) - sqr(x) - sqr(y);
Debug.Log(b);
var z = Mathf.Sqrt(b);
// no solution found
if (float.IsNaN(z))
{
Debug.Log("NO SOLUTION FOUND");
return(Vector3.one);
}
Vector3 middlePoint = vector_add(p1.pos, vector_add(vector_multiply(ex, x), vector_multiply(ey, y)));
//unused for now...
//Vector3 point1 = vector_add (a, vector_multiply (ez, z));
//Vector3 point2 = vector_subtract (a, vector_multiply (ez, z));
if (z.Equals(0))
{
Debug.Log("1 Solution");
}
else
{
Debug.Log("2 Solutions");
}
return(middlePoint);
}
public void Trilaterate()
{
bool shouldFindSolution = true;
foreach (Transform sphere in sphereList)
{
//make sure spheres are colliding...
if (!sphere.GetComponent <SphereBehavior> ().isColliding)
{
sphere.GetComponent <SphereBehavior> ().MakeCollide();
return;
}
}
if (shouldFindSolution)
{
foreach (Transform sphere in sphereList)
{
SignalSnapShot snap = new SignalSnapShot {
pos = sphere.position,
radius = sphere.localScale.x / 2
};
signals.Add(snap);
}
Vector3 newPos = Trilateration.Trilaterate(
signals [0],
signals [1],
signals [2]
);
if (newPos != Vector3.one)
{
marker.position = newPos;
//debug positions
textList [2].text = "ROUTER: " + marker.position.ToString();
}
else
{
textList [2].text = "NO Solution";
}
signals.Clear();
}
}
