JAVASCRIPT   17

math3d.js

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

  1. // math3d.js
  2. /*
  3. Copyright  Google Inc.
  4.  
  5. Licensed under the Apache License, Version 2.0 (the "License");
  6. you may not use this file except in compliance with the License.
  7. You may obtain a copy of the License at
  8.  
  9.      http://www.apache.org/licenses/LICENSE-2.0
  10.  
  11. Unless required by applicable law or agreed to in writing, software
  12. distributed under the License is distributed on an "AS IS" BASIS,
  13. WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14. See the License for the specific language governing permissions and
  15. limitations under the License.
  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 = {
  27.   EARTH_RADIUS: 6378100,
  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.  
  57.   add: function(a, b) {
  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.  
  93.   // Returns v rotated counterclockwise about axis by radians.
  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),
  100.                         V3.add(V3.scale(vPerpAxis, Math.cos(radians)),
  101.                                V3.scale(vPerpPerpAxis, Math.sin(radians))));
  102.     return result;
  103.   },
  104.  
  105.   // Takes a set of Euler angles and converts from degrees to radians.
  106.   toRadians: function(v) {
  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)
  124.   // (near Ecuador).
  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)
  283.   headingTiltRollToLocalOrientationMatrix: function(htr) {
  284.     return M33.eulToMat(V3.toRadians(htr));
  285.   },
  286.  
  287.   // [[right], [dir], [up]] (in local cartesian coords) to [heading, tilt, roll]
  288.   // (in degrees)
  289.   localOrientationMatrixToHeadingTiltRoll: function(mat) {
  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. };

Raw Paste

Login or Register to edit or fork this paste. It's free.