trilateration.js/trilateration.js at master · gheja/trilateration.js · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
/**
  * JavaScript implementation of Trilateration to find the position of a
  * point (P4) from three known points in 3D space (P1, P2, P3) and their
  * distance from the point in question.
  *
  * The solution used here is based on the derivation found on the Wikipedia
  * page of Trilateration: https://en.wikipedia.org/wiki/Trilateration
  *
  * This library does not need any 3rd party tools as all the non-basic
  * geometric functions needed are declared inside the trilaterate() function.
  *
  * See the GitHub page: https://github.com/gheja/trilateration.js
  */

/**
  * Calculates the coordinates of a point in 3D space from three known points
  * and the distances between those points and the point in question.
  *
  * If no solution found then null will be returned.
  *
  * If two solutions found then both will be returned, unless the fourth
  * parameter (return_middle) is set to true when the middle of the two solution
  * will be returned.
  *
  * @param {Object} p1 Point and distance: { x, y, z, r }
  * @param {Object} p2 Point and distance: { x, y, z, r }
  * @param {Object} p3 Point and distance: { x, y, z, r }
  * @param {bool} return_middle If two solution found then return the center of them
  * @return {Object|Array|null} { x, y, z } or [ { x, y, z }, { x, y, z } ] or null
  */
function trilaterate(p1, p2, p3, return_middle)
{
	// based on: https://en.wikipedia.org/wiki/Trilateration

	// some additional local functions declared here for
	// scalar and vector operations

	function sqr(a)
	{
		return a * a;
	}

	function norm(a)
	{
		return Math.sqrt(sqr(a.x) + sqr(a.y) + sqr(a.z));
	}

	function dot(a, b)
	{
		return a.x * b.x + a.y * b.y + a.z * b.z;
	}

	function vector_subtract(a, b)
	{
		return {
			x: a.x - b.x,
			y: a.y - b.y,
			z: a.z - b.z
		};
	}

	function vector_add(a, b)
	{
		return {
			x: a.x + b.x,
			y: a.y + b.y,
			z: a.z + b.z
		};
	}

	function vector_divide(a, b)
	{
		return {
			x: a.x / b,
			y: a.y / b,
			z: a.z / b
		};
	}

	function vector_multiply(a, b)
	{
		return {
			x: a.x * b,
			y: a.y * b,
			z: a.z * b
		};
	}

	function vector_cross(a, b)
	{
		return {
			x: a.y * b.z - a.z * b.y,
			y: a.z * b.x - a.x * b.z,
			z: a.x * b.y - a.y * b.x
		};
	}

	var ex, ey, ez, i, j, d, a, x, y, z, b, p4;

	ex = vector_divide(vector_subtract(p2, p1), norm(vector_subtract(p2, p1)));

	i = dot(ex, vector_subtract(p3, p1));
	a = vector_subtract(vector_subtract(p3, p1), vector_multiply(ex, i));
	ey = vector_divide(a, norm(a));
	ez =  vector_cross(ex, ey);
	d = norm(vector_subtract(p2, p1));
	j = dot(ey, vector_subtract(p3, p1));

	x = (sqr(p1.r) - sqr(p2.r) + sqr(d)) / (2 * d);
	y = (sqr(p1.r) - sqr(p3.r) + sqr(i) + sqr(j)) / (2 * j) - (i / j) * x;

	b = sqr(p1.r) - sqr(x) - sqr(y);

	// floating point math flaw in IEEE 754 standard
	// see https://github.com/gheja/trilateration.js/issues/2
	if (Math.abs(b) < 0.0000000001)
	{
		b = 0;
	}

	z = Math.sqrt(b);

	// no solution found
	if (isNaN(z))
	{
		return null;
	}

	a = vector_add(p1, vector_add(vector_multiply(ex, x), vector_multiply(ey, y)))
	p4a = vector_add(a, vector_multiply(ez, z));
	p4b = vector_subtract(a, vector_multiply(ez, z));

	if (z == 0 || return_middle)
	{
		return a;
	}
	else
	{
		return [ p4a, p4b ];
	}
}

module.exports = trilaterate;