@aibee/owlly 1.0.25

package/src/owlly/utils/geometry-utils.ts

@@ -0,0 +1,749 @@
+import earcut from 'earcut';
+import polygonClipping from 'polygon-clipping';
+import { vec2, vec3 } from 'gl-matrix';
+import { Externals, NS_THREE } from '../stage/externals';
+
+interface Cell {
+    x: number;
+    y: number;
+    h: number;
+    d: number;
+    max: number;
+}
+
+type Pair = [number, number];
+type Ring = Pair[];
+type Polygon = Ring[];
+type MultiPolygon = Polygon[];
+type Geom = Polygon | MultiPolygon;
+
+/**
+ * 图形相关的工具函数
+ * @ignore
+ */
+class GeometryUtils {
+  static Epsilon = 1e-6;
+
+  /**
+   * 计算环多边形面积
+   * @param polygon 二维顶点数组
+   * 借鉴area-polygon的实现
+   */
+  static RingPolygonArea(points: [number, number][], signed: boolean): number {
+    const l = points.length;
+    let det = 0;
+    const isSigned = signed || false;
+
+    function normalize(point: [number, number]): { x: number; y: number } {
+      if (!Array.isArray(point)) return point;
+      return {
+        x: point[0],
+        y: point[1],
+      };
+    }
+
+    let opoints = points.map(normalize);
+    if (opoints[0] !== opoints[opoints.length - 1]) {
+      opoints = opoints.concat(opoints[0]);
+    }
+
+    for (let i = 0; i < l; i++) {
+      det += opoints[i].x * opoints[i + 1].y
+        - opoints[i].y * opoints[i + 1].x;
+    }
+    if (isSigned) return det / 2;
+    return Math.abs(det) / 2;
+  }
+
+  /**
+     * 求多边形质心
+     * https://jingsam.github.io/2016/10/05/centroid.html
+     * @param polygon 按逆时针排列的二维顶点数组
+     */
+  static Centroid(polygon: [number, number][]): [number, number] {
+    let totalArea = 0;
+    let totalX = 0;
+    let totalY = 0;
+    for (let i = 0; i < polygon.length; i += 1) {
+      // p1, p2以及原点构成一个三角形
+      const p1 = polygon[i];
+      const p2 = (i === polygon.length - 1) ? polygon[0] : polygon[i + 1];
+
+      const area = 0.5 * (p1[0] * p2[1] - p2[0] * p1[1]); // 计算面积
+      const x = (p1[0] + p2[0]) / 3; // 计算x方向质心
+      const y = (p1[1] + p2[1]) / 3; // 计算y方向质心
+
+      totalArea += area;
+      totalX += area * x;
+      totalY += area * y;
+    }
+
+    return [totalX / totalArea, totalY / totalArea];
+  }
+
+  /**
+     * 求点到线段的距离
+     * https://jingsam.github.io/2016/09/26/polydist.html
+     * @param p 求到线段距离的点
+     * @param a 线段端点1
+     * @param b 线段端点2
+     */
+  static PointToSegmentDistance(p: [number, number], a: [number, number], b: [number, number]): number {
+    const AB = [b[0] - a[0], b[1] - a[1]];
+    const AP = [p[0] - a[0], p[1] - a[1]];
+
+    const AB_AP = AB[0] * AP[0] + AB[1] * AP[1];
+    const distAB2 = AB[0] * AB[0] + AB[1] * AB[1];
+
+    let D = [a[0], a[1]];
+    if (distAB2 !== 0) {
+      const t = AB_AP / distAB2;
+
+      if (t > 1) {
+        D = [b[0], b[1]];
+      } else if (t > 0) {
+        D = [a[0] + AB[0] * t, a[1] + AB[1] * t];
+      } else {
+        D = [a[0], a[1]];
+      }
+    }
+
+    const DP = [p[0] - D[0], p[1] - D[1]];
+    return Math.sqrt(DP[0] * DP[0] + DP[1] * DP[1]);
+  }
+
+  /**
+     * 判断点是否在多边形内部
+     * @param p 二维点
+     * @param polygonLoops 二维多边形顶点环数组，至少有一个环，第二个及以后的环代表内部孔
+     */
+  static PointInPolygon(p: [number, number], polygonLoops: [number, number][][]): boolean {
+    // 统计p点右边交点的个数
+    let count = 0;
+    for (let k = 0; k < polygonLoops.length; k += 1) {
+      const loop = polygonLoops[k];
+      for (let i = 0; i < loop.length; i += 1) {
+        const v1 = loop[i];
+        const v2 = (i === loop.length - 1) ? loop[0] : loop[i + 1];
+        if ((v1[1] > p[1]) !== (v2[1] > p[1])
+                    && (p[0] < (v2[0] - v1[0]) * (p[1] - v1[1]) / (v2[1] - v1[1]) + v1[0])) {
+          count += 1;
+        }
+      }
+    }
+    return (count % 2) !== 0;
+  }
+
+  /**
+     * 不考虑点是否在多边形内部，求最小距离
+     * https://jingsam.github.io/2016/09/26/polydist.html
+     * @param p 二维点
+     * @param polygonLoops 二维多边形顶点环数组，至少有一个环，第二个及以后的环代表内部孔
+     */
+  static MinDistanceFromPointToPolygon(p: [number, number], polygonLoops: [number, number][][]): number {
+    let minDist = Infinity;
+    for (let k = 0; k < polygonLoops.length; k += 1) {
+      const loop = polygonLoops[k];
+      for (let i = 0; i < loop.length; i += 1) {
+        const v1 = loop[i];
+        const v2 = (i === loop.length - 1) ? loop[0] : loop[i + 1];
+        minDist = Math.min(minDist, this.PointToSegmentDistance(p, v1, v2));
+      }
+    }
+    return minDist;
+  }
+
+  /**
+     * 考虑点是否在多边形内部，求最小距离。若在多边形外部，距离为负值
+     * https://jingsam.github.io/2016/09/26/polydist.html
+     * @param p 二维点
+     * @param polygonLoops 二维多边形顶点环数组，至少有一个环，第二个及以后的环代表内部孔
+     */
+  static PointDistanceToPolygon(p: [number, number], polygonLoops: [number, number][][]): number {
+    const dist = this.MinDistanceFromPointToPolygon(p, polygonLoops);
+    if (!this.PointInPolygon(p, polygonLoops)) {
+      // console.info(`#Centroid# PointDistanceToPolygon()=${dist}: p out of polygon:`, p, polygonLoops);
+      return -dist;
+    }
+    return dist;
+  }
+
+  static CellOfPolygon(x: number, y: number, h: number, polygon: [number, number][][]): Cell {
+    const d = this.PointDistanceToPolygon([x, y], polygon);
+    const max = d + h * Math.SQRT2;
+    const ret = {
+      x,
+      y,
+      h,
+      d,
+      max,
+    };
+    return ret;
+  }
+
+  /**
+     * 得到多边形标注中心
+     * https://jingsam.github.io/2016/09/26/polylabel2.html
+     * @param polygonLoops 多边形
+     */
+  static CentroidForPOI(polygonLoops: [number, number][][]): [number, number] {
+    if (!polygonLoops || polygonLoops.length === 0 || polygonLoops[0].length === 0) {
+      return [0, 0];
+    }
+
+    const polygon = polygonLoops[0];
+    let minX = polygon[0][0];
+    let minY = polygon[0][1];
+    let maxX = polygon[0][0];
+    let maxY = polygon[0][1];
+    for (let i = 1; i < polygon.length; i += 1) {
+      const [x, y] = polygon[i];
+      if (x < minX) minX = x;
+      if (y < minY) minY = y;
+      if (x > maxX) maxX = x;
+      if (y > maxY) maxY = y;
+    }
+    const width = maxX - minX;
+    const height = maxY - minY;
+    const cellSize = Math.min(width, height);
+    // console.log(`#Centroid# Bound=(${minX}, ${minY})~(${maxX}, ${maxY}), cellSize=${cellSize}`);
+    let h = cellSize / 2;
+
+    let candidates: Cell[] = [];
+    // 将多边形切分
+    for (let x = minX; x < maxX; x += cellSize) {
+      for (let y = minY; y < maxY; y += cellSize) {
+        const cell = this.CellOfPolygon(x + h, y + h, h, polygonLoops);
+        candidates.push(cell);
+      }
+    }
+    // 最优格网
+    // let bestCell = candidates[0];
+    const [x, y] = this.Centroid(polygon);
+    let bestCell: Cell = this.CellOfPolygon(x, y, h, polygonLoops);
+    candidates.push(bestCell);
+    // console.info('#Centroid# Best cell:', bestCell);
+    const minH = cellSize / 1024;
+    while (candidates.length > 1 && candidates[0].h >= minH) {
+      candidates = candidates.sort((cell1: Cell, cell2: Cell) => cell2.max - cell1.max);
+      // console.info('#Centroid# After sorting:', candidates);
+      for (let i = 0; i < candidates.length; i += 1) {
+        const cell = candidates[i];
+        if (cell.d > bestCell.d) {
+          // console.info('#Centroid# New bestCell:', cell);
+          bestCell = cell;
+        }
+      }
+      for (let i = 0; i < candidates.length; i += 1) {
+        const cell = candidates[i];
+        if (cell.max - bestCell.d <= this.Epsilon) {
+          candidates = candidates.slice(0, i);
+          break;
+        }
+      }
+      // console.info('#Centroid# After slicing:', candidates);
+      const nextCandidates = [];
+      // 将格网裂为4个小格网
+      h = candidates[0].h / 2;
+      for (let i = 0; i < candidates.length; i += 1) {
+        const cell = candidates[i];
+        nextCandidates.push(this.CellOfPolygon(cell.x - h, cell.y - h, h, polygonLoops));
+        nextCandidates.push(this.CellOfPolygon(cell.x + h, cell.y - h, h, polygonLoops));
+        nextCandidates.push(this.CellOfPolygon(cell.x - h, cell.y + h, h, polygonLoops));
+        nextCandidates.push(this.CellOfPolygon(cell.x + h, cell.y + h, h, polygonLoops));
+      }
+      candidates = nextCandidates;
+      // console.info('#Centroid# After splitting:', candidates);
+    }
+
+    return [bestCell.x, bestCell.y];
+  }
+
+  // (x - px) * nx + (y - py) * ny + (z - pz) * nz = 0;
+  // (x - x1) / (x2 - x1) = (y - y1) / (y2 - y1) = (z - z1) / (z2 - z1)
+  static LinePlaneIntersection(
+    lineP0: NS_THREE.Vector3,
+    lineP1: NS_THREE.Vector3,
+    planeP: NS_THREE.Vector3,
+    planeNormal: NS_THREE.Vector3,
+  ): NS_THREE.Vector3 {
+    const { THREE } = Externals.getInstance();
+    const { x: x0, y: y0, z: z0 } = lineP0;
+    const { x: x1, y: y1, z: z1 } = lineP1;
+    const dx = x1 - x0;
+    const dy = y1 - y0;
+    const dz = z1 - z0;
+    const { x: nx, y: ny, z: nz } = planeNormal;
+    const { x: px, y: py, z: pz } = planeP;
+    const den = nx * dx + ny * dy + nz * dz;
+    const num = nx * (px - x0) + ny * (py - y0) + nz * (pz - z0);
+    const t = num / den;
+    const ret = new THREE.Vector3(x0 + t * dx, y0 + t * dy, z0 + t * dz);
+    // console.info('#CutPlane# LinePlaneIntersection of line(v0, v1) with plane(p, n) is:', lineP0, lineP1, planeP, planeNormal, ret);
+    return ret;
+  }
+
+  static SegmentPlaneIntersection(
+    lineP0: NS_THREE.Vector3,
+    lineP1: NS_THREE.Vector3,
+    planeP: NS_THREE.Vector3,
+    planeNormal: NS_THREE.Vector3,
+  ): NS_THREE.Vector3|undefined {
+    const interPoint = this.LinePlaneIntersection(lineP0, lineP1, planeP, planeNormal);
+    if (Number.isNaN(interPoint.x) || !Number.isFinite(interPoint.x)) return undefined;
+    if (Number.isNaN(interPoint.y) || !Number.isFinite(interPoint.y)) return undefined;
+    if (Number.isNaN(interPoint.z) || !Number.isFinite(interPoint.z)) return undefined;
+    const dot = interPoint.clone().sub(lineP0).dot(interPoint.clone().sub(lineP1));
+    if (dot <= 0) {
+      // console.info('#CutPlane# SegmentPlaneIntersection of line(v0, v1) with plane(p, n) is:', lineP0, lineP1, planeP, planeNormal, interPoint);
+      return interPoint;
+    }
+    // console.info('#CutPlane# SegmentPlaneIntersection of line(v0, v1) with plane(p, n) is undefined', lineP0, lineP1, planeP, planeNormal);
+    return undefined;
+  }
+
+  static MapPointOnPlane(
+    v: NS_THREE.Vector3,
+    planeP: NS_THREE.Vector3,
+    planeNormal: NS_THREE.Vector3,
+  ): NS_THREE.Vector3 {
+    const { THREE } = Externals.getInstance();
+    let up = new THREE.Vector3(0, 1, 0);
+    if (up.angleTo(planeNormal) < this.Epsilon) {
+      up = new THREE.Vector3(0, 0, 1);
+    }
+    const m = new THREE.Matrix4().lookAt(planeP, planeP.clone().add(planeNormal), up);
+    return v.clone().applyMatrix4(m);
+  }
+
+  static ParameterToPlane(a: number, b: number, c: number, d: number):
+  {
+    normal: NS_THREE.Vector3;
+    constant: number;
+  } {
+    const { THREE } = Externals.getInstance();
+    const distance = Math.abs(d) / Math.sqrt(a * a + b * b + c * c);
+    return { normal: new THREE.Vector3(a, b, c), constant: distance };
+  }
+
+  static PlaneCutMeshPolygon(
+    p: NS_THREE.Vector3,
+    n: NS_THREE.Vector3,
+    meshVertices: NS_THREE.Vector3[],
+    meshTriangles: number[],
+  ): {
+    vertices: NS_THREE.Vector3[];
+    holeIndices: number[];
+    vertices2D: [number, number][];
+  } {
+    // Collect intersections:
+    const vertices: NS_THREE.Vector3[] = [];
+    const segments: [number, number][] = [];
+    for (let i = 0; i < meshTriangles.length; i += 3) {
+      const v0 = meshVertices[meshTriangles[i]];
+      const v1 = meshVertices[meshTriangles[i + 1]];
+      const v2 = meshVertices[meshTriangles[i + 2]];
+      const trimmedIntersections: NS_THREE.Vector3[] = [];
+      const intersections: (NS_THREE.Vector3|undefined)[] = [];
+      intersections.push(this.SegmentPlaneIntersection(v0, v1, p, n));
+      intersections.push(this.SegmentPlaneIntersection(v0, v2, p, n));
+      intersections.push(this.SegmentPlaneIntersection(v2, v1, p, n));
+      // console.info(`## intersections of #${meshTriangles[i]}, #${meshTriangles[i + 1]}, #${meshTriangles[i + 2]}:`, v0, v1, v2);
+      // console.info('## with plane p,n:', p, n);
+      // console.info('## is:', intersections);
+      intersections.forEach((v: NS_THREE.Vector3|undefined) => {
+        if (!v) return;
+        let collide = false;
+        for (let j = 0; j < trimmedIntersections.length; j += 1) {
+          if (v.distanceTo(trimmedIntersections[j]) <= this.Epsilon) {
+            collide = true;
+            break;
+          }
+        }
+        if (!collide) {
+          trimmedIntersections.push(v);
+        }
+      });
+      if (trimmedIntersections.length === 1) {
+        trimmedIntersections.push(trimmedIntersections[0]);
+      }
+      if (trimmedIntersections.length > 0) {
+        segments.push([vertices.length, vertices.length + 1]);
+        vertices.push(trimmedIntersections[0], trimmedIntersections[1]);
+      }
+    }
+    // console.info(`## PlaneCutMeshPolygon : segments(${segments.length})=`, segments);
+    // console.info(`## PlaneCutMeshPolygon : vertices(${vertices.length})=`, vertices);
+    // Link segments to polygon:
+    const polygonWithIndex: {loop: NS_THREE.Vector3[]; index: number}[] = [];
+    const polygon2D: [number, number][][] = [];
+    while (segments.length > 0) {
+      let edge = segments.pop()!;
+      const loop: number[] = [edge[0], edge[1]];
+      let nextEdge: [number, number]|undefined;
+      do {
+        const e01 = vertices[edge[1]];
+        nextEdge = undefined;
+        // console.info(`## PlaneCutMeshPolygon #1 ： segments.length=${segments.length}`);
+        for (let i = 0; i < segments.length; i += 1) {
+          const segment = segments[i];
+          const e10 = vertices[segment[0]];
+          const e11 = vertices[segment[1]];
+          if (e10.distanceTo(e01) <= this.Epsilon) {
+            nextEdge = [segment[0], segment[1]];
+            segments.splice(i, 1);
+            // console.info(`## PlaneCutMeshPolygon #2A ： segments.length=${segments.length}`);
+            break;
+          } else if (e11.distanceTo(e01) <= this.Epsilon) {
+            nextEdge = [segment[1], segment[0]];
+            segments.splice(i, 1);
+            // console.info(`## PlaneCutMeshPolygon #2B ： segments.length=${segments.length}`);
+            break;
+          }
+        }
+        if (nextEdge) {
+          loop.push(nextEdge[1]);
+          edge = nextEdge;
+        }
+      } while (nextEdge);
+      // console.info('## PlaneCutMeshPolygon #3');
+      polygonWithIndex.push({
+        loop: loop.map((vid: number) => vertices[vid]),
+        index: polygon2D.length,
+      });
+      const loop2D = loop.map((vid: number) => this.MapPointOnPlane(vertices[vid], p, n));
+      polygon2D.push(loop2D.map((v: NS_THREE.Vector3) => [v.x, v.y]));
+    }
+    // Distinguish outline from holes:
+    polygonWithIndex.sort((a, b) => {
+      const loopA = polygon2D[a.index];
+      const loopB = polygon2D[b.index];
+      if (loopB.length === 0) return -1;
+      if (loopA.length === 0) return 1;
+      if (this.PointInPolygon(loopB[0], [loopA])) {
+        return -1;
+      }
+      if (this.PointInPolygon(loopA[0], [loopB])) {
+        return 1;
+      }
+      return 0;
+    });
+    const polygonVertices: NS_THREE.Vector3[] = [];
+    const vertices2D: [number, number][] = [];
+    const holeIndices: number[] = [];
+    if (polygonWithIndex.length > 1) {
+      for (let i = 0; i < polygonWithIndex.length; i += 1) {
+        polygonVertices.push(...polygonWithIndex[i].loop);
+        vertices2D.push(...polygon2D[polygonWithIndex[i].index]);
+        holeIndices.push(polygonVertices.length);
+      }
+    } else {
+      for (let i = 0; i < polygonWithIndex.length; i += 1) {
+        polygonVertices.push(...polygonWithIndex[i].loop);
+        vertices2D.push(...polygon2D[polygonWithIndex[i].index]);
+      }
+    }
+    const ret = {
+      vertices: polygonVertices,
+      holeIndices,
+      vertices2D,
+    };
+    return ret;
+  }
+
+  static TriangulatePolygon2D(vertices: [number, number][], holeIndices: number[]): number[] {
+    const plainVertices: number[] = [];
+    vertices.forEach((vertex) => {
+      plainVertices.push(...vertex);
+    });
+    const triangleIndices = earcut(plainVertices, holeIndices, 2);
+    return triangleIndices;
+  }
+
+  static TriangulatePolygonLoops2D(polygonLoops: [number, number][][]):
+  {
+    vertices: [number, number][];
+    triangles: number[];
+  } {
+    const vertices: [number, number][] = [];
+    const holes: number[] = [];
+    polygonLoops.forEach((loop: [number, number][]) => {
+      vertices.push(...loop);
+      holes.push(vertices.length);
+    });
+    if (holes.length > 0) {
+      holes.splice(holes.length - 1, 1);
+    }
+    const triangles = GeometryUtils.TriangulatePolygon2D(vertices, holes);
+    return {
+      vertices,
+      triangles,
+    };
+  }
+
+  static LookAtMatrix(
+    eye: NS_THREE.Vector3,
+    target: NS_THREE.Vector3,
+    up: NS_THREE.Vector3,
+  ): NS_THREE.Matrix4 {
+    const { THREE } = Externals.getInstance();
+    const Fc = target.clone().sub(eye);
+    const Rc = Fc.clone().cross(up);
+    const Uc = Rc.clone().cross(Fc);
+    Fc.normalize();
+    Rc.normalize();
+    Uc.normalize();
+    const tx = -Rc.dot(eye);
+    const ty = -Uc.dot(eye);
+    const tz = Fc.dot(eye);
+    const m = new THREE.Matrix4();
+    m.set(Rc.x, Rc.y, Rc.z, tx, Uc.x, Uc.y, Uc.z, ty, -Fc.x, -Fc.y, -Fc.z, tz, 0, 0, 0, 1);
+    return m;
+  }
+
+  static FrustumMesh(
+    eye: NS_THREE.Vector3, target: NS_THREE.Vector3, up: NS_THREE.Vector3,
+    fovYDegrees: number, aspectRatio: number,
+    near: number, far: number,
+  ): {
+    vertices: NS_THREE.Vector3[];
+    triangles: number[];
+  } {
+    const { THREE } = Externals.getInstance();
+    const triangles = [0, 1, 3, 3, 1, 2, 0, 4, 1, 1, 4, 5, 1, 5, 6, 1, 6, 2,
+      2, 6, 7, 2, 7, 3, 3, 7, 4, 3, 4, 0, 4, 5, 7, 7, 5, 6];
+    const t = Math.tan(Math.PI * fovYDegrees / 360);
+    const halfHNear = near * t;
+    const halfWNear = near * aspectRatio * t;
+    const halfHFar = far * t;
+    const halfWFar = far * aspectRatio * t;
+    const vertices: NS_THREE.Vector3[] = [];
+    vertices.push(new THREE.Vector3(-halfWNear, -halfHNear, -near));
+    vertices.push(new THREE.Vector3(-halfWNear, halfHNear, -near));
+    vertices.push(new THREE.Vector3(halfWNear, halfHNear, -near));
+    vertices.push(new THREE.Vector3(halfWNear, -halfHNear, -near));
+    vertices.push(new THREE.Vector3(-halfWFar, -halfHFar, -far));
+    vertices.push(new THREE.Vector3(-halfWFar, halfHFar, -far));
+    vertices.push(new THREE.Vector3(halfWFar, halfHFar, -far));
+    vertices.push(new THREE.Vector3(halfWFar, -halfHFar, -far));
+    const lookAt = this.LookAtMatrix(eye, target, up);
+    const m = lookAt.clone().invert();
+    // console.info('## lookAt=, m=', lookAt, m);
+    return { vertices: vertices.map((v) => v.applyMatrix4(m)), triangles };
+  }
+
+  static PolygonIntersection(geom: Geom, ...geoms: Geom[]): MultiPolygon {
+    return polygonClipping.intersection(geom, ...geoms);
+  }
+
+  static PolygonDiff(geom: Geom, ...geoms: Geom[]): MultiPolygon {
+    return polygonClipping.difference(geom, ...geoms);
+  }
+
+  static PolygonUnion(geom: Geom, ...geoms: Geom[]): MultiPolygon {
+    return polygonClipping.union(geom, ...geoms);
+  }
+
+  static PolygonXOR(geom: Geom, ...geoms: Geom[]): MultiPolygon {
+    return polygonClipping.xor(geom, ...geoms);
+  }
+
+  /**
+   * 计算路径每个路径点与初始点在路径上的距离
+   * @param pathPoints 路径点数组
+   * @returns 距离数组
+   */
+  static GetPathDistances(pathPoints: [number, number][] | [number, number, number][]): number[] {
+    if (!pathPoints.length) return [];
+
+    const result: number[] = [0];
+    if (pathPoints[0].length === 2) {
+      for (
+        let
+          i = 1,
+          l = pathPoints.length,
+          prevVec = vec2.set(vec2.create(), pathPoints[0][0], pathPoints[0][1]),
+          currentVec = vec2.create();
+        i < l;
+        i += 1
+      ) {
+        vec2.set(currentVec, pathPoints[i][0], pathPoints[i][1]);
+        result.push(result[i - 1] + vec2.dist(prevVec, currentVec));
+        vec2.copy(prevVec, currentVec);
+      }
+    } else {
+      for (
+        let
+          i = 1,
+          l = pathPoints.length,
+          prevVec = vec3.set(vec3.create(), pathPoints[0][0], pathPoints[0][1], pathPoints[0][2]),
+          currentVec = vec3.create();
+        i < l;
+        i += 1
+      ) {
+        vec3.set(currentVec, pathPoints[i][0], pathPoints[i][1], pathPoints[i][2]!);
+        result.push(result[i - 1] + vec3.dist(prevVec, currentVec));
+        vec3.copy(prevVec, currentVec);
+      }
+    }
+    return result;
+  }
+
+  /**
+   * 获取路径上某个距离上的点
+   * @param path 路径
+   * @param distances 距离数组
+   * @param targetDistance 目标距离
+   */
+  static GetPointOnDistance(
+    path: [number, number][] | [number, number, number][],
+    distances: number[],
+    targetDistance: number,
+  ): [number, number] | [number, number, number] {
+    let left = 0;
+    let right = path.length - 1;
+    let p = -1;
+    while (left < right - 1) {
+      p = Math.floor((left + right) / 2);
+      if (distances[p] > targetDistance) right = p;
+      else if (distances[p] < targetDistance) left = p;
+      else {
+        left = p;
+        break;
+      }
+    }
+    p = left;
+    if (p > -1) {
+      if (path[0].length === 2) {
+        const from = vec2.set(vec2.create(), path[p][0], path[p][1]);
+        if (p === path.length - 1 || targetDistance <= 0) return [from[0], from[1]];
+
+        const fromDist = distances[p];
+        const to = vec2.set(vec2.create(), path[p + 1][0], path[p + 1][1]);
+        const toDist = distances[p + 1];
+        const distDiff = toDist - fromDist;
+        const vecDiff = vec2.sub(vec2.create(), to, from);
+        const result = vec2.add(
+          vec2.create(),
+          from,
+          vec2.scale(vec2.create(), vecDiff, (targetDistance - fromDist) / distDiff),
+        );
+        return [result[0], result[1]];
+      }
+      if (path[0].length === 3) {
+        const from = vec3.set(vec3.create(), path[p][0], path[p][1], path[p][2]!);
+        if (p === path.length - 1 || targetDistance <= 0) return [from[0], from[1], from[2]];
+
+        const fromDist = distances[p];
+        const to = vec3.set(vec3.create(), path[p + 1][0], path[p + 1][1], path[p + 1][2]!);
+        const toDist = distances[p + 1];
+        const distDiff = toDist - fromDist;
+        const vecDiff = vec3.sub(vec3.create(), to, from);
+        const result = vec3.add(
+          vec3.create(),
+          from,
+          vec3.scale(vec3.create(), vecDiff, (targetDistance - fromDist) / distDiff),
+        );
+        return [result[0], result[1], result[2]];
+      }
+    }
+    throw new Error(`Cannot find point on distance ${targetDistance}`);
+  }
+
+  /**
+   * 获取路径上距离某位置最近的点
+   * @param position 目标点
+   * @param path 路径
+   * @param distances 距离数组
+   * @param targetPrecision 计算精度, 默认为0.1
+   * @param stepLength 搜索步长距离, 默认为8
+   */
+  static GetNearestPoint(
+    position: [number, number] | [number, number, number],
+    path: [number, number][] | [number, number, number][],
+    distances: number[],
+    targetPrecision = 0.1,
+    stepLength = 8,
+  ): {
+    point: [number, number] | [number, number, number];
+    distance: number;
+  } {
+    const pathLength = distances[distances.length - 1];
+    let precision = stepLength;
+    let best: [number, number] | [number, number, number] = position.length === 2 ? [0, 0] : [0, 0, 0];
+    let bestLength = 0;
+    let bestDistance = Infinity;
+
+    for (let scan, scanLength = 0, scanDistance; scanLength <= pathLength; scanLength += precision) {
+      scan = this.GetPointOnDistance(path, distances, scanLength);
+      if (position.length === 2) {
+        scanDistance = vec2.dist(
+          vec2.set(vec2.create(), position[0], position[1]),
+          vec2.set(vec2.create(), scan[0], scan[1]),
+        );
+      } else {
+        scanDistance = vec3.dist(
+          vec3.set(vec3.create(), position[0], position[1], position[2]),
+          vec3.set(vec3.create(), scan[0], scan[1], scan[2]!),
+        );
+      }
+      if (scanDistance < bestDistance) {
+        best = scan;
+        bestLength = scanLength;
+        bestDistance = scanDistance;
+      }
+    }
+
+    let before;
+    let after;
+    let beforeLength;
+    let afterLength;
+    let beforeDistance;
+    let afterDistance;
+    precision /= 2;
+    while (precision > targetPrecision) {
+      beforeLength = bestLength - precision;
+      before = this.GetPointOnDistance(path, distances, beforeLength);
+      if (position.length === 2) {
+        beforeDistance = vec2.dist(
+          vec2.set(vec2.create(), position[0], position[1]),
+          vec2.set(vec2.create(), before[0], before[1]),
+        );
+      } else {
+        beforeDistance = vec3.dist(
+          vec3.set(vec3.create(), position[0], position[1], position[2]),
+          vec3.set(vec3.create(), before[0], before[1], before[2]!),
+        );
+      }
+      afterLength = bestLength + precision;
+      after = this.GetPointOnDistance(path, distances, afterLength);
+      if (position.length === 2) {
+        afterDistance = vec2.dist(
+          vec2.set(vec2.create(), position[0], position[1]),
+          vec2.set(vec2.create(), after[0], after[1]),
+        );
+      } else {
+        afterDistance = vec3.dist(
+          vec3.set(vec3.create(), position[0], position[1], position[2]),
+          vec3.set(vec3.create(), after[0], after[1], after[2]!),
+        );
+      }
+      if (beforeLength >= 0 && beforeDistance < bestDistance) {
+        best = before; bestLength = beforeLength; bestDistance = beforeDistance;
+      } else if (afterLength <= pathLength && afterDistance < bestDistance) {
+        best = after; bestLength = afterLength; bestDistance = afterDistance;
+      } else {
+        precision /= 2;
+      }
+    }
+
+    return {
+      point: best,
+      distance: bestLength,
+    };
+  }
+}
+
+export {
+  GeometryUtils,
+};