@lumikmz/kmz 0.2.1 → 0.3.0

package/package.json

@@ -1,6 +1,6 @@
 {
   "name": "@lumikmz/kmz",
-  "version": "0.2.1",
+  "version": "0.3.0",
   "description": "DJI KMZ/WPML domain model + plan() algorithm + validate(), spec-aligned types mirroring docs/kmz/",
   "license": "MIT",
   "type": "module",

package/src/index.ts

@@ -1,3 +1,4 @@
 export * from "./types/index.js";
-export { plan } from "./plan/index.js";
+export { plan, type PlanOptions } from "./plan/index.js";
 export { validate } from "./validate/index.js";
+export { haversineMeters } from "./plan/geometry.js";

package/src/plan/geometry.test.ts

@@ -0,0 +1,96 @@
+import { describe, expect, it } from "vitest";
+import { offsetPolygon, rotateEnu, surveyGrid, type EnuPoint } from "./geometry.js";
+
+const SQUARE: EnuPoint[] = [
+  [0, 0],
+  [100, 0],
+  [100, 100],
+  [0, 100],
+];
+
+describe("offsetPolygon", () => {
+  it("dilates a square outward by ~distM on each side", () => {
+    const out = offsetPolygon(SQUARE, 10);
+    expect(out).toHaveLength(4);
+    const xs = out.map((p) => p[0]);
+    const ys = out.map((p) => p[1]);
+    // 90° corners → miter √2·10 ≈ 14.1 along the diagonal, i.e. -10 / 110 per axis.
+    expect(Math.min(...xs)).toBeCloseTo(-10, 1);
+    expect(Math.max(...xs)).toBeCloseTo(110, 1);
+    expect(Math.min(...ys)).toBeCloseTo(-10, 1);
+    expect(Math.max(...ys)).toBeCloseTo(110, 1);
+  });
+
+  it("does not spike on a near-collinear vertex (miter-stable)", () => {
+    // A quad whose right side is split by an almost-straight extra vertex — the
+    // edge-intersection method would shoot this corner far to the right.
+    const ring: EnuPoint[] = [
+      [0, 0],
+      [100, 0],
+      [100.5, 50], // near-collinear with its neighbours on the right edge
+      [100, 100],
+      [0, 100],
+    ];
+    const out = offsetPolygon(ring, 20);
+    // No vertex may land beyond distM·MITER_LIMIT (2.5) of the original area.
+    expect(Math.max(...out.map((p) => p[0]))).toBeLessThan(100.5 + 20 * 2.5 + 1);
+  });
+
+  it("tolerates a duplicate vertex without exploding", () => {
+    const ring: EnuPoint[] = [
+      [0, 0],
+      [100, 0],
+      [100, 0], // duplicate
+      [100, 100],
+      [0, 100],
+    ];
+    const out = offsetPolygon(ring, 10);
+    expect(out).toHaveLength(4);
+    expect(out.every((p) => Number.isFinite(p[0]) && Number.isFinite(p[1]))).toBe(true);
+  });
+});
+
+describe("rotateEnu", () => {
+  it("rotates +x by 90° CCW to +y", () => {
+    const [x, y] = rotateEnu([1, 0], Math.PI / 2);
+    expect(x).toBeCloseTo(0, 6);
+    expect(y).toBeCloseTo(1, 6);
+  });
+
+  it("rotates +y by 90° CCW to -x", () => {
+    const [x, y] = rotateEnu([0, 1], Math.PI / 2);
+    expect(x).toBeCloseTo(-1, 6);
+    expect(y).toBeCloseTo(0, 6);
+  });
+
+  it("identity at 0 radians", () => {
+    const [x, y] = rotateEnu([3, 4], 0);
+    expect(x).toBeCloseTo(3, 6);
+    expect(y).toBeCloseTo(4, 6);
+  });
+});
+
+describe("surveyGrid", () => {
+  it("scans a square and returns paired flight-line endpoints", () => {
+    const pts = surveyGrid(SQUARE, { directionDeg: 90, marginM: 0, lineSpacing: 25 });
+    expect(pts.length).toBeGreaterThanOrEqual(8); // ≥4 lines × 2 endpoints
+    expect(pts.length % 2).toBe(0);
+  });
+
+  it("direction=90 → flight lines run east-west (endpoints share y)", () => {
+    const pts = surveyGrid(SQUARE, { directionDeg: 90, marginM: 0, lineSpacing: 25 });
+    expect(pts[0]![1]).toBeCloseTo(pts[1]![1], 3); // first line: same y
+  });
+
+  it("direction=0 → flight lines run north-south (endpoints share x)", () => {
+    const pts = surveyGrid(SQUARE, { directionDeg: 0, marginM: 0, lineSpacing: 25 });
+    expect(pts[0]![0]).toBeCloseTo(pts[1]![0], 3); // first line: same x
+  });
+
+  it("margin extends each flight line lengthwise", () => {
+    const base = surveyGrid(SQUARE, { directionDeg: 90, marginM: 0, lineSpacing: 25 });
+    const widened = surveyGrid(SQUARE, { directionDeg: 90, marginM: 10, lineSpacing: 25 });
+    const span = (pts: EnuPoint[]) => Math.abs(pts[1]![0] - pts[0]![0]);
+    expect(span(widened)).toBeGreaterThan(span(base));
+  });
+});

package/src/plan/geometry.ts

@@ -41,6 +41,33 @@ export function bearingDeg(from: EnuPoint, to: EnuPoint): number {
   return Math.atan2(to[0] - from[0], to[1] - from[1]) * RAD2DEG;
 }
 
+/**
+ * Orient an ordered survey path so its first point is the end nearest `home`.
+ * A boustrophedon grid has two natural entry ends; the aircraft should start at
+ * whichever is closer to the take-off point. Returns the input unchanged when
+ * `home` is absent or the start is already the nearer end.
+ */
+export function orientPathToHome(
+  enuWps: readonly EnuPoint[],
+  origin: LngLat,
+  home: LngLat | undefined,
+): readonly EnuPoint[] {
+  if (!home || enuWps.length < 2) return enuWps;
+  const h = lngLatToEnu(home, origin);
+  const sqDist = (p: EnuPoint) => (p[0] - h[0]) ** 2 + (p[1] - h[1]) ** 2;
+  return sqDist(enuWps[enuWps.length - 1]!) < sqDist(enuWps[0]!) ? [...enuWps].reverse() : enuWps;
+}
+
+/** Great-circle distance between two WGS-84 coordinates, in meters. */
+export function haversineMeters(a: LngLat, b: LngLat): number {
+  const dLat = (b.lat - a.lat) * DEG2RAD;
+  const dLng = (b.lng - a.lng) * DEG2RAD;
+  const lat1 = a.lat * DEG2RAD;
+  const lat2 = b.lat * DEG2RAD;
+  const h = Math.sin(dLat / 2) ** 2 + Math.cos(lat1) * Math.cos(lat2) * Math.sin(dLng / 2) ** 2;
+  return EARTH_RADIUS * 2 * Math.atan2(Math.sqrt(h), Math.sqrt(1 - h));
+}
+
 /** Clip a horizontal scan line `y = const` against a polygon; returns `[x_enter, x_exit]` pairs. */
 function clipHorizontal(y: number, polygon: readonly EnuPoint[]): [number, number][] {
   const intersections: number[] = [];
@@ -61,31 +88,6 @@ function clipHorizontal(y: number, polygon: readonly EnuPoint[]): [number, numbe
   return out;
 }
 
-/** Serpentine boustrophedon scan over a polygon; returns ENU waypoints. */
-export function scanWaypoints(polygon: readonly EnuPoint[], lineSpacing: number): EnuPoint[] {
-  let minY = Infinity;
-  let maxY = -Infinity;
-  for (const [, y] of polygon) {
-    if (y < minY) minY = y;
-    if (y > maxY) maxY = y;
-  }
-
-  const waypoints: EnuPoint[] = [];
-  let leftToRight = true;
-  for (let y = minY; y <= maxY + lineSpacing * 0.5; y += lineSpacing) {
-    const segments = clipHorizontal(y, polygon);
-    for (const [x1, x2] of segments) {
-      if (leftToRight) {
-        waypoints.push([x1, y], [x2, y]);
-      } else {
-        waypoints.push([x2, y], [x1, y]);
-      }
-    }
-    leftToRight = !leftToRight;
-  }
-  return waypoints;
-}
-
 /**
  * Parse a KML coordinates string like `"lon,lat lon,lat lon,lat"` or `"lon,lat,alt ..."`
  * into LngLat array (altitude dropped).
@@ -107,3 +109,137 @@ export function encodePolygonCoordinates(
 ): string {
   return ring.map((p) => `${p.lng},${p.lat},${altitudeMeters}`).join(" ");
 }
+
+/** Rotate an ENU point counter-clockwise by `rad` radians about the origin. */
+export function rotateEnu([x, y]: EnuPoint, rad: number): EnuPoint {
+  const c = Math.cos(rad);
+  const s = Math.sin(rad);
+  return [x * c - y * s, x * s + y * c];
+}
+
+/** Drop a closing duplicate and any consecutive vertices closer than `epsM`. */
+function dedupeRing(ring: readonly EnuPoint[], epsM = 0.5): EnuPoint[] {
+  const pts: EnuPoint[] = [];
+  for (const p of ring) {
+    const last = pts[pts.length - 1];
+    if (last && Math.hypot(p[0] - last[0], p[1] - last[1]) < epsM) continue;
+    pts.push([p[0], p[1]]);
+  }
+  const f = pts[0];
+  const l = pts[pts.length - 1];
+  if (pts.length >= 2 && f && l && Math.hypot(f[0] - l[0], f[1] - l[1]) < epsM) pts.pop();
+  return pts;
+}
+
+/** Beyond this multiple of `distM`, a corner miter is clamped to avoid spikes. */
+const OFFSET_MITER_LIMIT = 2.5;
+
+/**
+ * Outward-offset (dilate) a simple polygon ring by `distM` meters in ENU.
+ * Each vertex moves along the bisector of its two adjacent outward edge normals
+ * by `distM / cos(halfAngle)`, clamped by a miter limit. The bisector form is
+ * stable when adjacent edges are near-collinear (where edge-intersection would
+ * shoot a spike far outward); the clamp bounds sharp corners; degenerate/duplicate
+ * vertices are removed first. Intended for convex-ish survey areas.
+ */
+export function offsetPolygon(ring: readonly EnuPoint[], distM: number): EnuPoint[] {
+  const pts = dedupeRing(ring);
+  const n = pts.length;
+  if (distM === 0 || n < 3) return pts;
+
+  // Signed area → winding. Outward normal of edge dir (dx,dy) is the right normal
+  // (dy,−dx) for a CCW ring, the left normal otherwise.
+  let area2 = 0;
+  for (let i = 0; i < n; i++) {
+    const a = pts[i]!;
+    const b = pts[(i + 1) % n]!;
+    area2 += a[0] * b[1] - b[0] * a[1];
+  }
+  const ccw = area2 > 0;
+  const outwardNormal = (dx: number, dy: number): EnuPoint => (ccw ? [dy, -dx] : [-dy, dx]);
+
+  const out: EnuPoint[] = [];
+  for (let i = 0; i < n; i++) {
+    const prev = pts[(i - 1 + n) % n]!;
+    const cur = pts[i]!;
+    const next = pts[(i + 1) % n]!;
+
+    const inLen = Math.hypot(cur[0] - prev[0], cur[1] - prev[1]) || 1;
+    const outLen = Math.hypot(next[0] - cur[0], next[1] - cur[1]) || 1;
+    const nIn = outwardNormal((cur[0] - prev[0]) / inLen, (cur[1] - prev[1]) / inLen);
+    const nOut = outwardNormal((next[0] - cur[0]) / outLen, (next[1] - cur[1]) / outLen);
+
+    let bx = nIn[0] + nOut[0];
+    let by = nIn[1] + nOut[1];
+    const bl = Math.hypot(bx, by);
+    if (bl < 1e-9) {
+      // ~180° reversal — offset straight out along the outgoing edge normal.
+      out.push([cur[0] + nOut[0] * distM, cur[1] + nOut[1] * distM]);
+      continue;
+    }
+    bx /= bl;
+    by /= bl;
+    const cosHalf = nOut[0] * bx + nOut[1] * by; // = cos(half exterior angle)
+    const scale = distM / Math.max(cosHalf, 1 / OFFSET_MITER_LIMIT);
+    out.push([cur[0] + bx * scale, cur[1] + by * scale]);
+  }
+  return out;
+}
+
+export interface SurveyGridOptions {
+  /** Main flight-line heading, degrees clockwise from North. */
+  directionDeg: number;
+  /** Outward survey-area margin (meters); extends each flight line lengthwise. */
+  marginM: number;
+  /** Perpendicular distance between adjacent flight lines (meters). */
+  lineSpacing: number;
+  /**
+   * Across-track phase shift (meters) applied to every flight line.
+   * Used to interleave two grids on the same axis by half a line spacing
+   * (e.g. mapping3d forward vs backward oblique passes). Default 0.
+   */
+  phaseOffsetM?: number;
+  /**
+   * Along-track translation (meters) of the whole grid, in the `directionDeg`
+   * direction. Used for the mapping3d oblique stand-off: a tilted camera images
+   * ahead of the aircraft, so the flight grid is shifted opposite the look so it
+   * overhangs the boundary on one side only. Default 0.
+   */
+  lengthwiseShiftM?: number;
+}
+
+/**
+ * Boustrophedon survey grid over an arbitrary simple polygon, aligned to `directionDeg`.
+ * Flight lines run parallel to the heading; consecutive lines step by `lineSpacing`.
+ * `marginM` extends each line lengthwise past the polygon edge.
+ * Returns ENU waypoints in flight order — no transit/entry point
+ * (entry climb is a missionConfig behaviour, not a grid concern).
+ */
+export function surveyGrid(ring: readonly EnuPoint[], opts: SurveyGridOptions): EnuPoint[] {
+  const { directionDeg, marginM, lineSpacing, phaseOffsetM = 0, lengthwiseShiftM = 0 } = opts;
+  // Map the heading clockwise-from-north → CCW angle from +x axis
+  const t = directionDeg * DEG2RAD;
+  const alpha = Math.atan2(Math.cos(t), Math.sin(t));
+  const scanRing = ring.map((p) => rotateEnu(p, -alpha));
+
+  let minY = Infinity;
+  let maxY = -Infinity;
+  for (const [, y] of scanRing) {
+    if (y < minY) minY = y;
+    if (y > maxY) maxY = y;
+  }
+
+  const scanPts: EnuPoint[] = [];
+  let leftToRight = true;
+  for (let y = minY + phaseOffsetM; y <= maxY + lineSpacing * 0.5; y += lineSpacing) {
+    for (const [x1, x2] of clipHorizontal(y, scanRing)) {
+      const a = x1 - marginM + lengthwiseShiftM;
+      const b = x2 + marginM + lengthwiseShiftM;
+      if (leftToRight) scanPts.push([a, y], [b, y]);
+      else scanPts.push([b, y], [a, y]);
+    }
+    leftToRight = !leftToRight;
+  }
+
+  return scanPts.map((p) => rotateEnu(p, alpha));
+}

package/src/plan/index.ts

@@ -1,4 +1,5 @@
 import { PlannerError } from "../types/errors.js";
+import type { Meters } from "../types/branded.js";
 import type {
   Mapping2dTemplate,
   Mapping3dTemplate,
@@ -12,6 +13,23 @@ import { planMapping3d } from "./plan-mapping3d.js";
 import { planMappingStrip } from "./plan-mapping-strip.js";
 import { planWaypoint } from "./plan-waypoint.js";
 
+/**
+ * Options for `plan()`.
+ *
+ * Mapping templates (`mapping2d`, `mapping3d`) require `lineSpacing` — the perpendicular
+ * distance between adjacent flight lines in meters. The formula is:
+ * `2 · height · tan(HFOV / 2) · (1 − sideOverlap)`.
+ *
+ * `shootInterval` — time-lapse interval in seconds for `startTimeLapse` actions.
+ * Formula: `forwardGroundSpacing / speed`,
+ * where `forwardGroundSpacing = 2 · height · tan(VFOV / 2) · (1 − forwardOverlap)`.
+ * Camera FOV and sensor data live in the demo layer, not in this package.
+ */
+export interface PlanOptions {
+  lineSpacing?: Meters;
+  shootInterval?: number;
+}
+
 const isWaypoint = (t: Template): t is WaypointTemplate => t.Folder.templateType === "waypoint";
 const isMapping2d = (t: Template): t is Mapping2dTemplate => t.Folder.templateType === "mapping2d";
 const isMapping3d = (t: Template): t is Mapping3dTemplate => t.Folder.templateType === "mapping3d";
@@ -21,11 +39,13 @@ const isMappingStrip = (t: Template): t is MappingStripTemplate =>
 /**
  * Compile a high-level `Template` into executable `Waylines`.
  * Dispatch is by `template.Folder.templateType` per the DJI WPML spec.
+ *
+ * Mapping templates require `options.lineSpacing` (meters between flight lines).
  */
-export function plan(template: Template): Waylines {
+export function plan(template: Template, options?: PlanOptions): Waylines {
   if (isWaypoint(template)) return planWaypoint(template);
-  if (isMapping2d(template)) return planMapping2d(template);
-  if (isMapping3d(template)) return planMapping3d(template);
+  if (isMapping2d(template)) return planMapping2d(template, options);
+  if (isMapping3d(template)) return planMapping3d(template, options);
   if (isMappingStrip(template)) return planMappingStrip(template);
   throw new PlannerError(`Unknown templateType: ${(template as Template).Folder.templateType}`);
 }

package/src/plan/plan-mapping-strip.ts

@@ -22,9 +22,9 @@ export function planMappingStrip(template: MappingStripTemplate): Waylines {
     const isLast = index === coords.length - 1;
     const headingAngle = isLast ? 0 : bearingDeg(enuPath[index]!, enuPath[index + 1]!);
     const executeHeight =
-      folder.stripUseTemplateAltitude && c.alt !== undefined
-        ? (c.alt as ReturnType<typeof folder.height extends infer T ? () => T : never>)
-        : folder.height;
+      folder.Placemark.stripUseTemplateAltitude && c.alt !== undefined
+        ? (c.alt as ReturnType<typeof folder.Placemark.height extends infer T ? () => T : never>)
+        : folder.Placemark.height;
 
     return {
       Point: { coordinates: { lng: c.lng, lat: c.lat } },