JAVASCRIPT   17

math3d.js

Guest on 16th June 2022 01:30:22 AM

1. // math3d.js
2. /*
4.
6. you may not use this file except in compliance with the License.
7. You may obtain a copy of the License at
8.
10.
11. Unless required by applicable law or agreed to in writing, software
13. WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14. See the License for the specific language governing permissions and
16. */
17.
18. // Some Javascript math utilities.
19. //
20. // Exports V3 (3-vector utilities), M33 (3x3 matrix utilities)
21.
22. // NOTE: This will be refactored in a more Object
23. // Oriented style, so don't get attached to this syntax!
24.
25. // 3D vector functions.
26. V3 = {
28.
29.   dup: function(a) {
30.     return [a[0], a[1], a[2]];
31.   },
32.
33.   toString: function(a) {
34.     return "[" + a[0] + ", " + a[1] + ", " + a[2] + "]";
35.   },
36.
37.   nearlyEqual: function(a, b, tolerance) {
38.     if (!tolerance) {
39.       tolerance = 1e-6;
40.     }
41.     return Math.abs(a[0] - b[0]) <= tolerance
42.       && Math.abs(a[1] - b[1]) <= tolerance
43.       && Math.abs(a[2] - b[2]) <= tolerance;
44.   },
45.
46.   cross: function(a, b) {
47.     return [
48.         a[1] * b[2] - a[2] * b[1],
49.         a[2] * b[0] - a[0] * b[2],
50.         a[0] * b[1] - a[1] * b[0] ];
51.   },
52.
53.   dot: function(a, b) {
54.     return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
55.   },
56.
58.     return [
59.         a[0] + b[0],
60.         a[1] + b[1],
61.         a[2] + b[2]];
62.   },
63.
64.   sub: function(a, b) {
65.     return [
66.         a[0] - b[0],
67.         a[1] - b[1],
68.         a[2] - b[2]];
69.   },
70.
71.   scale: function(a, scale) {
72.     return [a[0] * scale, a[1] * scale, a[2] * scale];
73.   },
74.
75.   length: function(a) {
76.     return Math.sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
77.   },
78.
79.   normalize: function(a) {
80.     var len = V3.length(a);
81.     if (len <= 0) {
82.       return [NaN, NaN, NaN];
83.     }
84.     return V3.scale(a, 1.0 / len);
85.   },
86.
87.   bisect: function(a, b) {
88.     return [(a[0] + b[0]) / 2,
89.             (a[1] + b[1]) / 2,
90.             (a[2] + b[2]) / 2];
91.   },
92.
94.   // axis should be a unit vector; otherwise you'll get weird results.
95.   rotate: function(v, axis, radians) {
96.     var vDotAxis = V3.dot(v, axis);
97.     var vPerpAxis = V3.sub(v, V3.scale(axis, vDotAxis));
98.     var vPerpPerpAxis = V3.cross(axis, vPerpAxis);
99.     var result = V3.add(V3.scale(axis, vDotAxis),
102.     return result;
103.   },
104.
105.   // Takes a set of Euler angles and converts from degrees to radians.
107.     return [v[0] * Math.PI / 180,
108.             v[1] * Math.PI / 180,
109.             v[2] * Math.PI / 180];
110.   },
111.
112.   // Takes a set of Euler angles and converts from radians to degrees.
113.   toDegrees: function(v) {
114.     return [v[0] * 180 / Math.PI,
115.             v[1] * 180 / Math.PI,
116.             v[2] * 180 / Math.PI];
117.   },
118.
119.   // Input is [lat, lon, alt].  Lat & lon are in degrees, positive up
120.   // and east.  Alt in meters, relative to Earth's radius.
121.   //
122.   // Output is meters x,y,z.  x points out of (0,0) (just off West
123.   // Africa), y points out the North Pole, and z points out of (0,-90)
125.   latLonAltToCartesian: function(vert) {
126.     var sinTheta = Math.sin(vert[1] * Math.PI / 180);
127.     var cosTheta = Math.cos(vert[1] * Math.PI / 180);
128.     var sinPhi = Math.sin(vert[0] * Math.PI / 180);
129.     var cosPhi = Math.cos(vert[0] * Math.PI / 180);
130.
131.     var r = V3.EARTH_RADIUS + vert[2];
132.     var result = [
133.         r * cosTheta * cosPhi,
134.         r * sinPhi,
135.         r * -sinTheta * cosPhi ];
136.     return result;
137.   },
138.
139.   // Input is meters [x, y, z].  Output is [lat, lon, alt].  Lat & lon
140.   // in degrees, alt in meters.
141.   //
142.   // V3.cartesianToLatLonAlt([R, 0, 0]) ~= [0, 0, 0]
143.   // V3.cartesianToLatLonAlt([R/sqrt(2), R/sqrt(2), 0]) ~= [45, 0, 0]
144.   // V3.cartesianToLatLonAlt([R/sqrt(2), 0, R/sqrt(2)]) ~= [0, -45, 0]
145.   cartesianToLatLonAlt: function(a) {
146.     var r = V3.length(a);
147.     if (r <= 0) {
148.       return [NaN, NaN, NaN];
149.     }
150.     var alt = r - V3.EARTH_RADIUS;
151.     // Compute projection onto unit sphere.
152.     var n = V3.scale(a, 1 / r);
153.     var lat = Math.asin(n[1]) * 180 / Math.PI;
154.     if (lat > 90) {
155.       lat -= 180;
156.     }
157.     var lon = 0;
158.     if (Math.abs(lat) < 90) {
159.       lon = Math.atan2(n[2], n[0]) * -180 / Math.PI;
160.     }
161.     return [lat, lon, alt];
162.   },
163.
164.   // Return the signed perpendicular distance from the point c to the line
165.   // defined by [a, b].
166.   //
167.   // We get the sign by determining if point is to the left of the line,
168.   // from the point of view of looking towards the origin through vert0.
169.   // I.e. is it to the left, looking at the surface of the Earth from
170.   // above.
171.   leftDistance: function(a, b, c) {
172.     var ab = V3.sub(b, a);
173.     var ac = V3.sub(c, a);
174.     var cross = V3.cross(ab, ac);
175.
176.     var dot = V3.dot(a, cross);
177.     var lineLength = V3.length(ab);
178.     if (lineLength < 1e-6) {
179.       return NaN;
180.     }
181.     var perpendicularDistance = V3.length(cross) / lineLength;
182.
183.     if (dot > 0) {
184.       return perpendicularDistance;
185.     } else {
186.       return -perpendicularDistance;
187.     }
188.   },
189.
190.   // Return the distance between two cartesian 3d points, along the
191.   // surface of the Earth, assuming they are on the surface of the
192.   // Earth.  (If the inputs are not on the surface of the earth, they
193.   // are projected to the surface first.)
194.   earthDistance: function(a, b) {
195.     var dot = V3.dot(V3.normalize(a), V3.normalize(b));
196.     var angle = Math.acos(dot);
197.     var dist = V3.EARTH_RADIUS * angle;
198.     return dist;
199.   }
200. };
201.
202. M33 = {
203.   // Conventions:
204.   //
205.   // * V3 is a 3-element array representing a column vector
206.   //
207.   // * M33 is an array of 3 column vectors, representing a 3x3 matrix
208.   //
209.   //   [ [00] [10] [20] ]
210.   //   [ [01] [11] [21] ]
211.   //   [ [02] [12] [22] ]
212.
213.   toString: function(a) {
214.     return "[" + V3.toString(a[0]) + ", " +
215.       V3.toString(a[1]) + ", " + V3.toString(a[2]) + "]";
216.   },
217.
218.   nearlyEqual: function(a, b) {
219.     return V3.nearlyEqual(a[0], b[0])
220.       && V3.nearlyEqual(a[1], b[1])
221.       && V3.nearlyEqual(a[2], b[2]);
222.   },
223.
224.   transpose: function(a) {
225.     return [
226.         [a[0][0], a[1][0], a[2][0]],
227.         [a[0][1], a[1][1], a[2][1]],
228.         [a[0][2], a[1][2], a[2][2]]];
229.   },
230.
231.   multiply: function(a, b) {
232.     var result = [[0, 0, 0], [0, 0, 0], [0, 0, 0]];
233.     for (var i = 0; i < 3; i++) {
234.       for (var j = 0; j < 3; j++) {
235.         result[i][j] = a[0][j] * b[i][0]
236.                        + a[1][j] * b[i][1]
237.                        + a[2][j] * b[i][2];
238.       }
239.     }
240.     return result;
241.   },
242.
243.   // Applies matrix a to column vector b.  (I.e. returns a * b)
244.   transform: function(a, b) {
245.     return [
246.         a[0][0] * b[0] + a[1][0] * b[1] + a[2][0] * b[2],
247.         a[0][1] * b[0] + a[1][1] * b[1] + a[2][1] * b[2],
248.         a[0][2] * b[0] + a[1][2] * b[1] + a[2][2] * b[2]];
249.   },
250.
251.   // Applies the transpose of matrix a to column vector b.
252.   // (I.e. returns a.transpose() * b)
253.   transformByTranspose: function(a, b) {
254.     return [
255.         a[0][0] * b[0] + a[0][1] * b[1] + a[0][2] * b[2],
256.         a[1][0] * b[0] + a[1][1] * b[1] + a[1][2] * b[2],
257.         a[2][0] * b[0] + a[2][1] * b[1] + a[2][2] * b[2]];
258.   },
259.
260.   identity: function() {
261.     return [[1, 0, 0], [0, 1, 0], [0, 0, 1]];
262.   },
263.
264.   makeOrthonormalFrame: function(dir, up) {
265.     var newright = V3.normalize(V3.cross(dir, up));
266.     var newdir = V3.normalize(V3.cross(up, newright));
267.     var newup = V3.cross(newright, newdir);
268.     return [newright, newdir, newup];
269.   },
270.
271.   // [heading, tilt, roll] (degrees) to [[right],[dir],[up]] (local
272.   // coords).  The return value transforms global direction vectors
273.   // into local direction vectors.  The transpose of the return value
274.   // transforms local direction vectors into global direction vectors.
275.   //
276.   // heading, tilt, roll are in degrees, clockwise about z,x,y axes (!?).
277.   // heading of 0 means pointing North
278.   // heading of 90 means pointing West
279.   //
280.   // [1, 0, 0] in local coords points right (East, for 0, 0, 0 htr)
281.   // [0, 1, 0] in local coords points ahead (North, for 0, 0, 0 htr)
282.   // [0, 0, 1] in local coords points up (away from center of earth)
285.   },
286.
287.   // [[right], [dir], [up]] (in local cartesian coords) to [heading, tilt, roll]
288.   // (in degrees)
290.     var htr = M33.matToEul(mat);
291.     return V3.toDegrees(htr);
292.   },
293.
294.   // Builds a local orientation matrix, to transform from local coords
295.   // into global coords.  "Local" means that the local x basis vector
296.   // points East, the y basis vector points North and the z basis
297.   // vector points Up (towards the sky).
298.   makeLocalToGlobalFrame: function(latLonAlt) {
299.     var vertical = V3.normalize(V3.latLonAltToCartesian(latLonAlt));
300.     var east = V3.normalize(V3.cross([0, 1, 0], vertical));
301.     var north = V3.normalize(V3.cross(vertical, east));
302.
303.     return [east, north, vertical];
304.   },
305.
306.   // See Graphics Gems IV, Chapter III.5, "Euler Angle Conversion" by
307.   // Ken Shoemake.
308.   //
309.   // http://vered.rose.utoronto.ca/people/spike/GEMS/GEMS.html
310.   eulerConfig: {
311.     i: 2, j: 0, k: 1,     // NOTE: KML convention is Z, X, Y!
312.     counterClockwise: true,
313.     sameAxis: false,      // third axis same as first (i == k)
314.     frameRelative: false  // frame-relative (vs. static)
315.   },
316.
317.   // Construct matrix from Euler angles (in radians).
318.   //
319.   // Thanks to Ken Shoemake / Graphics Gems
320.   eulToMat: function(eulerAnglesIn) {
321.     var ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
322.     var config = M33.eulerConfig;
323.     var i = config.i;
324.     var j = config.j;
325.     var k = config.k;
326.
327.     var ea = V3.dup(eulerAnglesIn);
328.     var m = [[0, 0, 0], [0, 0, 0], [0, 0, 0]];
329.     if (config.frameRelative) { var t = ea[0]; ea[0] = ea[2]; ea[2] = t; }
330.     if (!config.counterClockwise) {
331.       ea[0] = -ea[0]; ea[1] = -ea[1]; ea[2] = -ea[2];
332.     }
333.     ti = ea[0];   tj = ea[1];   th = ea[2];
334.     ci = Math.cos(ti); cj = Math.cos(tj); ch = Math.cos(th);
335.     si = Math.sin(ti); sj = Math.sin(tj); sh = Math.sin(th);
336.     cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
337.     if (config.sameAxis) {
338.       m[i][i] = cj;     m[i][j] =  sj*si;    m[i][k] =  sj*ci;
339.       m[j][i] = sj*sh;  m[j][j] = -cj*ss+cc; m[j][k] = -cj*cs-sc;
340.       m[k][i] = -sj*ch; m[k][j] =  cj*sc+cs; m[k][k] =  cj*cc-ss;
341.     } else {
342.       m[i][i] = cj*ch; m[i][j] = sj*sc-cs; m[i][k] = sj*cc+ss;
343.       m[j][i] = cj*sh; m[j][j] = sj*ss+cc; m[j][k] = sj*cs-sc;
344.       m[k][i] = -sj;   m[k][j] = cj*si;    m[k][k] = cj*ci;
345.     }
346.     return m;
347.   },
348.
349.   // Convert matrix to Euler angles (in radians).
350.   //
351.   // Thanks to Ken Shoemake / Graphics Gems
352.   matToEul: function(m, config) {
353.     var config = M33.eulerConfig;
354.     var i = config.i;
355.     var j = config.j;
356.     var k = config.k;
357.
358.     var FLT_EPSILON = 1e-6;
359.     var ea = [0, 0, 0];
360.     if (config.sameAxis) {
361.       var sy = Math.sqrt(m[i][j] * m[i][j] + m[i][k] * m[i][k]);
362.       if (sy > 16 * FLT_EPSILON) {
363.         ea[0]= Math.atan2(m[i][j], m[i][k]);
364.         ea[1]= Math.atan2(sy, m[i][i]);
365.         ea[2]= Math.atan2(m[j][i], -m[k][i]);
366.       } else {
367.         ea[0]= Math.atan2(-m[j][k], m[j][j]);
368.         ea[1]= Math.atan2(sy, m[i][i]);
369.         ea[2]= 0;
370.       }
371.     } else {
372.       var cy = Math.sqrt(m[i][i] * m[i][i] + m[j][i] * m[j][i]);
373.       if (cy > 16 * FLT_EPSILON) {
374.         ea[0]= Math.atan2(m[k][j], m[k][k]);
375.         ea[1]= Math.atan2(-m[k][i], cy);
376.         ea[2]= Math.atan2(m[j][i], m[i][i]);
377.       } else {
378.         ea[0]= Math.atan2(-m[j][k], m[j][j]);
379.         ea[1]= Math.atan2(-m[k][i], cy);
380.         ea[2]= 0;
381.       }
382.     }
383.     if (!config.counterClockwise) {
384.       ea[0] = -ea[0]; ea[1] = -ea[1]; ea[2] = -ea[2];
385.     }
386.     if (config.frameRelative) { var t = ea[0]; ea[0] = ea[2]; ea[2] = t; }
387.     return ea;
388.   }
389. };