diff --git a/packages/blocks/package.json b/packages/blocks/package.json index ee10656a..a19d880b 100644 --- a/packages/blocks/package.json +++ b/packages/blocks/package.json @@ -1,6 +1,6 @@ { "name": "@viamrobotics/prime-blocks", - "version": "0.0.30", + "version": "0.1.0", "publishConfig": { "access": "public" }, @@ -33,17 +33,15 @@ "prime.css" ], "peerDependencies": { - "@threlte/core": ">=7", - "@threlte/extras": ">=8", + "@threlte/core": ">=7 <8", + "@threlte/extras": ">=8 <9", "@viamrobotics/prime-core": ">=0.0.48", "@viamrobotics/three": ">=0.0.3", + "maplibre-gl": ">=4", "svelte": ">=4 <5", "tailwindcss": ">=3", "three": ">=0.159" }, - "dependencies": { - "maplibre-gl": "^4.1.3" - }, "devDependencies": { "@sveltejs/adapter-auto": "^3.0.0", "@sveltejs/kit": "^2.0.3", @@ -52,9 +50,9 @@ "@testing-library/dom": "^9.3.3", "@testing-library/jest-dom": "^6.1.5", "@testing-library/svelte": "^4.1.0", - "@threlte/core": "^7.0.12", - "@threlte/extras": "^8.0.10", - "@types/three": "^0.159.0", + "@threlte/core": "^7.3.1", + "@threlte/extras": "^8.11.4", + "@types/three": "^0.166.0", "@typescript-eslint/eslint-plugin": "^6.15.0", "@typescript-eslint/parser": "^6.15.0", "@viamrobotics/eslint-config": "^0.3.0", @@ -72,6 +70,7 @@ "eslint-plugin-tailwindcss": "^3.13.0", "eslint-plugin-unicorn": "^49.0.0", "jsdom": "^23.0.1", + "maplibre-gl": "^4.5.0", "postcss": "^8.4.32", "prettier": "^3.1.1", "prettier-plugin-svelte": "^3.1.2", @@ -80,7 +79,7 @@ "svelte": "^4.2.8", "svelte-check": "^3.6.2", "tailwindcss": "^3.3.7", - "three": "^0.159.0", + "three": "^0.166.1", "tslib": "^2.6.2", "typescript": "^5.3.3", "vite": "^5.0.10", diff --git a/packages/blocks/src/lib/slam-map-2d/axes-helper.svelte b/packages/blocks/src/lib/axes-helper/axes-helper.svelte similarity index 73% rename from packages/blocks/src/lib/slam-map-2d/axes-helper.svelte rename to packages/blocks/src/lib/axes-helper/axes-helper.svelte index c9dc9205..1ca0cc43 100644 --- a/packages/blocks/src/lib/slam-map-2d/axes-helper.svelte +++ b/packages/blocks/src/lib/axes-helper/axes-helper.svelte @@ -1,13 +1,21 @@ - + null} +> diff --git a/packages/blocks/src/lib/index.ts b/packages/blocks/src/lib/index.ts index c744fe44..9287fbec 100644 --- a/packages/blocks/src/lib/index.ts +++ b/packages/blocks/src/lib/index.ts @@ -1,10 +1,27 @@ +// Three.js components +export { default as AxesHelper } from './axes-helper/axes-helper.svelte'; + // MapLibre components +export { LngLat, MercatorCoordinate } from 'maplibre-gl'; export { default as MapLibre } from './maplibre/index.svelte'; export { default as MapLibreMarker } from './maplibre/marker.svelte'; -export { default as MapLibreDirectionalMarker } from './maplibre/directional-marker.svelte'; -export { default as MapLibreControls } from './maplibre/controls.svelte'; +export { default as DirectionalMarker } from './maplibre/directional-marker.svelte'; +export { default as NavigationControls } from './maplibre/controls/navigation.svelte'; +export { default as CenterControls } from './maplibre/controls/center.svelte'; +export { default as FollowControls } from './maplibre/controls/follow.svelte'; +export { default as SatelliteControls } from './maplibre/controls/satellite.svelte'; +export { default as LngLatInput } from './maplibre/lnglat-input.svelte'; export { useMapLibre, useMapLibreEvent } from './maplibre/hooks'; -export type { LngLat, GeoPose, Waypoint } from './maplibre/types'; +export { useMapLibreThreeRenderer } from './maplibre/plugins/three'; +export { useMapLibreThreeRaycast } from './maplibre/plugins/raycast'; +export { + lngLatToMercator, + mercatorToCartesian, + lngLatToCartesian, + cartesianToLngLat, + cartesianToMercator, +} from './maplibre/math'; +export { GeoPose, Waypoint } from './maplibre/types'; // Slam components export { default as SlamMap2D } from './slam-map-2d/index.svelte'; diff --git a/packages/blocks/src/lib/navigation-map/components/center-inputs.svelte b/packages/blocks/src/lib/maplibre/controls/center.svelte similarity index 62% rename from packages/blocks/src/lib/navigation-map/components/center-inputs.svelte rename to packages/blocks/src/lib/maplibre/controls/center.svelte index e250f0f2..1d822e13 100644 --- a/packages/blocks/src/lib/navigation-map/components/center-inputs.svelte +++ b/packages/blocks/src/lib/maplibre/controls/center.svelte @@ -1,7 +1,12 @@ + -
- -
+ diff --git a/packages/blocks/src/lib/maplibre/controls/follow.svelte b/packages/blocks/src/lib/maplibre/controls/follow.svelte new file mode 100644 index 00000000..4e4a13ea --- /dev/null +++ b/packages/blocks/src/lib/maplibre/controls/follow.svelte @@ -0,0 +1,60 @@ + + + + diff --git a/packages/blocks/src/lib/maplibre/controls.svelte b/packages/blocks/src/lib/maplibre/controls/navigation.svelte similarity index 87% rename from packages/blocks/src/lib/maplibre/controls.svelte rename to packages/blocks/src/lib/maplibre/controls/navigation.svelte index e7379d26..9499e35d 100644 --- a/packages/blocks/src/lib/maplibre/controls.svelte +++ b/packages/blocks/src/lib/maplibre/controls/navigation.svelte @@ -1,13 +1,13 @@ + + + diff --git a/packages/blocks/src/lib/maplibre/index.svelte b/packages/blocks/src/lib/maplibre/index.svelte index 4ceb8cf8..1f771c89 100644 --- a/packages/blocks/src/lib/maplibre/index.svelte +++ b/packages/blocks/src/lib/maplibre/index.svelte @@ -19,7 +19,7 @@ import { onMount, tick } from 'svelte'; import { provideMapContext } from './hooks'; import { Map, type MapOptions } from 'maplibre-gl'; import { style } from './style'; -import type { LngLat } from '$lib'; +import { LngLat } from '$lib'; /** The minimum camera pitch. */ export let minPitch = 0; @@ -42,7 +42,7 @@ export let maxZoom = 22; * @default { lng: -73.984421, lat: 40.7718116 } * The Viam Robotics office. */ -export let center: LngLat = { lng: -73.984_421, lat: 40.771_811_6 }; +export let center: LngLat = new LngLat(-73.984_421, 40.771_811_6); /** A binding to the MapLibre Map instance */ export let map: Map | undefined = undefined; diff --git a/packages/blocks/src/lib/navigation-map/components/input/lnglat.svelte b/packages/blocks/src/lib/maplibre/lnglat-input.svelte similarity index 100% rename from packages/blocks/src/lib/navigation-map/components/input/lnglat.svelte rename to packages/blocks/src/lib/maplibre/lnglat-input.svelte diff --git a/packages/blocks/src/lib/navigation-map/lib/math.ts b/packages/blocks/src/lib/maplibre/math.ts similarity index 75% rename from packages/blocks/src/lib/navigation-map/lib/math.ts rename to packages/blocks/src/lib/maplibre/math.ts index 71c3205a..bc82dd55 100644 --- a/packages/blocks/src/lib/navigation-map/lib/math.ts +++ b/packages/blocks/src/lib/maplibre/math.ts @@ -1,10 +1,4 @@ -import { MercatorCoordinate } from 'maplibre-gl'; -import type { LngLat } from '$lib'; - -export const toPrecisionLevel = (number: number, decimals: number): number => { - const multiplier = 10 ** decimals; - return Math.floor(number * multiplier) / multiplier; -}; +import { MercatorCoordinate, LngLat } from 'maplibre-gl'; export const lngLatToMercator = (lngLat: LngLat): MercatorCoordinate => { return MercatorCoordinate.fromLngLat(lngLat, 0); diff --git a/packages/blocks/src/lib/maplibre/plugins/raycast.ts b/packages/blocks/src/lib/maplibre/plugins/raycast.ts new file mode 100644 index 00000000..b31240c4 --- /dev/null +++ b/packages/blocks/src/lib/maplibre/plugins/raycast.ts @@ -0,0 +1,42 @@ +import { onMount } from 'svelte'; +import { type Camera, type Raycaster, Vector2, Vector3, Matrix4 } from 'three'; +import type { MapMouseEvent } from 'maplibre-gl'; +import { useMapLibre } from '../hooks'; + +/** + * Provides raycasting against THREE objects projected on to a maplibre map. + */ +export const useMapLibreThreeRaycast = (cameraSignal: { current: Camera }) => { + const { map } = useMapLibre(); + const pointer = new Vector2(); + + const handleMouseMove = (event: MapMouseEvent) => { + pointer.set( + (event.point.x / map.transform.width) * 2 - 1, + -(event.point.y / map.transform.height) * 2 + 1 + ); + }; + + onMount(() => { + map.on('mousemove', handleMouseMove); + return () => map.off('mousemove', handleMouseMove); + }); + + const cameraPosition = new Vector3(); + const mousePosition = new Vector3(); + const viewDirection = new Vector3(); + const camInverseProjection = new Matrix4(); + + const compute = (raycaster: Raycaster) => { + camInverseProjection.copy(cameraSignal.current.projectionMatrix).invert(); + cameraPosition.set(0, 0, 0).applyMatrix4(camInverseProjection); + mousePosition + .set(pointer.x, pointer.y, 1) + .applyMatrix4(camInverseProjection); + viewDirection.copy(mousePosition).sub(cameraPosition).normalize(); + + raycaster.set(cameraPosition, viewDirection); + }; + + return { compute, pointer }; +}; diff --git a/packages/blocks/src/lib/maplibre/plugins/three.ts b/packages/blocks/src/lib/maplibre/plugins/three.ts new file mode 100644 index 00000000..9d5a0b68 --- /dev/null +++ b/packages/blocks/src/lib/maplibre/plugins/three.ts @@ -0,0 +1,95 @@ +import { Camera, Matrix4, Line, BufferGeometry, Vector3, Scene } from 'three'; +import { MercatorCoordinate, type LngLat } from 'maplibre-gl'; +import { useMapLibre } from '../hooks'; +import { lngLatToCartesian, mercatorToCartesian } from '../math'; +import { onMount } from 'svelte'; + +/** + * + * A plugin to integrate Three.js scenes into a Maplibre map. + * + * It syncs the projection matrix of a mapbibre-gl camera to a THREE.Camera in order to render objects + * from the perspective of the map viewer. The camera is set to look at the current center of the visible map. + * + * It also manages the position of any THREE.Object3D that has a `userData.lngLat` property. The position + * is determined by calculating the difference of the map's center lng,lat and the object's lng,lat and then + * making an approximate transform to an x,z plane offset in meters. + * + * The userData.lngLat property must be a maplibre LngLat object, or in the case of a THREE.Line, a LngLat[]. + * + * @param scene A THREE.Scene instance. + * @param cameraSignal This should only be a THREE.Camera instance. Perspective and Orthographic cameras make projection defaults that will not work with Maplibre. + * @param renderFn A callback that runs on each map draw. Use it to render your scene. + */ +export const useMapLibreThreeRenderer = ( + scene: Scene, + cameraSignal: { current: Camera }, + renderFn: (scene: Scene, camera: Camera) => void +) => { + const { map } = useMapLibre(); + const cameraTransform = new Matrix4(); + const cameraMatrix = new Matrix4(); + const scale = new Matrix4(); + const rotation = new Matrix4().multiplyMatrices( + new Matrix4().makeRotationX(-0.5 * Math.PI), + new Matrix4().makeRotationY(Math.PI) + ); + + onMount(() => { + map.addLayer({ + id: 'scene-layer', + type: 'custom', + renderingMode: '3d', + render(_, viewProjectionMatrix) { + const center = map.getCenter(); + const mercator = MercatorCoordinate.fromLngLat(center, 0); + const mercatorScale = mercator.meterInMercatorCoordinateUnits(); + const { x: cx, y: cy } = mercatorToCartesian(mercator, mercatorScale); + + scale.makeScale(mercatorScale, mercatorScale, -mercatorScale); + cameraTransform + .multiplyMatrices(scale, rotation) + .setPosition(mercator.x, mercator.y, mercator.z); + + cameraSignal.current.projectionMatrix = cameraMatrix + .fromArray(viewProjectionMatrix) + .multiply(cameraTransform); + + scene.traverse((object) => { + const { lngLat } = object.userData as { + lngLat?: LngLat | LngLat[] | undefined; + }; + + if (lngLat === undefined) { + return; + } + + if (Array.isArray(lngLat)) { + if (object instanceof Line) { + (object.geometry as BufferGeometry).setFromPoints( + lngLat.map((value) => { + const { x: ox, y: oy } = lngLatToCartesian( + value, + mercatorScale + ); + return new Vector3(cx - ox, 0, cy - oy); + }) + ); + object.computeLineDistances(); + } + } else { + const { x: ox, y: oy } = lngLatToCartesian(lngLat, mercatorScale); + object.position.set(cx - ox, 0, cy - oy); + } + }); + + renderFn(scene, cameraSignal.current); + map.triggerRepaint(); + }, + }); + + return () => { + map.removeLayer('scene-layer'); + }; + }); +}; diff --git a/packages/blocks/src/lib/maplibre/types.ts b/packages/blocks/src/lib/maplibre/types.ts index 8cd2fb10..1d73f860 100644 --- a/packages/blocks/src/lib/maplibre/types.ts +++ b/packages/blocks/src/lib/maplibre/types.ts @@ -1,13 +1,18 @@ -export interface LngLat { - lng: number; - lat: number; -} +// eslint-disable-next-line max-classes-per-file +import { LngLat } from 'maplibre-gl'; -export interface GeoPose { - lng: number; - lat: number; - /** The rotation, where 0 is north */ +export class GeoPose extends LngLat { rotation: number; + constructor(lng: number, lat: number, rotation: number) { + super(lng, lat); + this.rotation = rotation; + } } -export type Waypoint = LngLat & { id: string }; +export class Waypoint extends LngLat { + id: string; + constructor(lng: number, lat: number, id: string) { + super(lng, lat); + this.id = id; + } +} diff --git a/packages/blocks/src/lib/navigation-map/components/axes-helper.svelte b/packages/blocks/src/lib/navigation-map/components/axes-helper.svelte deleted file mode 100644 index f66fdd7c..00000000 --- a/packages/blocks/src/lib/navigation-map/components/axes-helper.svelte +++ /dev/null @@ -1,59 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/packages/blocks/src/lib/navigation-map/components/draw-tool.svelte b/packages/blocks/src/lib/navigation-map/components/draw-tool.svelte index 241e5069..d1af2a07 100644 --- a/packages/blocks/src/lib/navigation-map/components/draw-tool.svelte +++ b/packages/blocks/src/lib/navigation-map/components/draw-tool.svelte @@ -5,7 +5,12 @@ - -
+
{ minZoom={6} bind:map > - +
- +
diff --git a/packages/blocks/src/lib/navigation-map/components/nav/obstacles.svelte b/packages/blocks/src/lib/navigation-map/components/nav/obstacles.svelte index 0acdaa57..9c874ee5 100644 --- a/packages/blocks/src/lib/navigation-map/components/nav/obstacles.svelte +++ b/packages/blocks/src/lib/navigation-map/components/nav/obstacles.svelte @@ -11,8 +11,8 @@ import { type Geometry, useMapLibre, type Obstacle, + LngLatInput, } from '$lib'; -import LnglatInput from '../input/lnglat.svelte'; import GeometryInputs from '../input/geometry.svelte'; import OrientationInput from '../input/orientation.svelte'; import ObstaclesLegend from './obstacles-legend.svelte'; @@ -200,7 +200,7 @@ $: debugMode = $environment === 'debug'; /> - { }; useMapLibreEvent('click', (event) => { - const lngLat = { - lng: event.lngLat.lng, - lat: event.lngLat.lat, - }; + const waypoint = new Waypoint( + event.lngLat.lng, + event.lngLat.lat, + crypto.randomUUID() + ); - $waypoints = [ - ...$waypoints, - { - id: crypto.randomUUID(), - ...lngLat, - }, - ]; + $waypoints = [...$waypoints, waypoint]; - dispatch('add-waypoint', lngLat); + dispatch('add-waypoint', waypoint); }); diff --git a/packages/blocks/src/lib/navigation-map/components/obstacle.svelte b/packages/blocks/src/lib/navigation-map/components/obstacle.svelte index a13a663c..3780355a 100644 --- a/packages/blocks/src/lib/navigation-map/components/obstacle.svelte +++ b/packages/blocks/src/lib/navigation-map/components/obstacle.svelte @@ -7,9 +7,8 @@ import type { MapLayerMouseEvent, MapLayerTouchEvent, } from 'maplibre-gl'; -import { useMapLibre, type Obstacle, useMapLibreEvent } from '$lib'; +import { useMapLibre, type Obstacle, useMapLibreEvent, AxesHelper } from '$lib'; import { view, hovered, selected, environment, obstacles } from '../stores'; -import AxesHelper from './axes-helper.svelte'; /** The obstacle name. */ export let name: string; @@ -170,13 +169,9 @@ useMapLibreEvent('mousedown', handleMapPointerDown); {#if geometry.type === 'box'} {#if active} {/if} @@ -200,8 +195,8 @@ useMapLibreEvent('mousedown', handleMapPointerDown); --> {#if active} {/if} @@ -221,8 +216,8 @@ useMapLibreEvent('mousedown', handleMapPointerDown); {:else if geometry.type === 'capsule'} {#if active} {/if} diff --git a/packages/blocks/src/lib/navigation-map/components/robot-marker.svelte b/packages/blocks/src/lib/navigation-map/components/robot-marker.svelte index ee86ac49..0d6fe4b9 100644 --- a/packages/blocks/src/lib/navigation-map/components/robot-marker.svelte +++ b/packages/blocks/src/lib/navigation-map/components/robot-marker.svelte @@ -1,12 +1,12 @@ {#if pose} -
- +
diff --git a/packages/blocks/src/lib/navigation-map/components/scene.svelte b/packages/blocks/src/lib/navigation-map/components/scene.svelte index eef7d183..1ed093fa 100644 --- a/packages/blocks/src/lib/navigation-map/components/scene.svelte +++ b/packages/blocks/src/lib/navigation-map/components/scene.svelte @@ -1,15 +1,15 @@ + + - - - - diff --git a/packages/blocks/src/lib/slam-map-2d/hooks/use-raycast-click/normalize-device-coordinates.ts b/packages/blocks/src/lib/slam-map-2d/hooks/use-raycast-click/normalize-device-coordinates.ts index e08305d2..21fba9b5 100644 --- a/packages/blocks/src/lib/slam-map-2d/hooks/use-raycast-click/normalize-device-coordinates.ts +++ b/packages/blocks/src/lib/slam-map-2d/hooks/use-raycast-click/normalize-device-coordinates.ts @@ -1,8 +1,10 @@ +import { Vector2 } from 'three'; + export const normalizeDeviceCoordinates = ( element: HTMLElement, x: number, y: number, - target: THREE.Vector2 + target: Vector2 ) => { if (element.clientWidth === 0 || element.clientHeight === 0) { throw new Error( diff --git a/packages/blocks/src/routes/+page.svelte b/packages/blocks/src/routes/+page.svelte index 5aa21e87..9e141587 100644 --- a/packages/blocks/src/routes/+page.svelte +++ b/packages/blocks/src/routes/+page.svelte @@ -1,7 +1,7 @@