From cfc58b8fc87dea6d6a8609f84d30b7788f876e3f Mon Sep 17 00:00:00 2001 From: Kyle Corry Date: Fri, 2 Feb 2024 18:44:59 -0500 Subject: [PATCH] Show clipped path points in AR --- .../trail_sense/shared/canvas/LineClipper.kt | 61 +++++++- .../shared/extensions/AndromedaExtensions.kt | 5 + .../ui/AugmentedRealityFragment.kt | 2 +- .../ui/layers/ARPathLayer.kt | 140 ++++++++++++------ 4 files changed, 154 insertions(+), 54 deletions(-) diff --git a/app/src/main/java/com/kylecorry/trail_sense/shared/canvas/LineClipper.kt b/app/src/main/java/com/kylecorry/trail_sense/shared/canvas/LineClipper.kt index bacbbb6a1..39fe0ba47 100644 --- a/app/src/main/java/com/kylecorry/trail_sense/shared/canvas/LineClipper.kt +++ b/app/src/main/java/com/kylecorry/trail_sense/shared/canvas/LineClipper.kt @@ -1,12 +1,15 @@ package com.kylecorry.trail_sense.shared.canvas import com.kylecorry.andromeda.core.units.PixelCoordinate +import com.kylecorry.sol.math.SolMath +import com.kylecorry.sol.math.SolMath.real import com.kylecorry.sol.math.Vector2 import com.kylecorry.sol.math.filters.RDPFilter import com.kylecorry.sol.math.geometry.Geometry import com.kylecorry.sol.math.geometry.Line import com.kylecorry.sol.math.geometry.Rectangle import com.kylecorry.trail_sense.shared.extensions.isSamePixel +import com.kylecorry.trail_sense.shared.extensions.squaredDistanceTo import com.kylecorry.trail_sense.shared.toPixelCoordinate import com.kylecorry.trail_sense.shared.toVector2 import kotlin.math.absoluteValue @@ -19,7 +22,9 @@ class LineClipper { output: MutableList, origin: PixelCoordinate = PixelCoordinate(0f, 0f), preventLineWrapping: Boolean = false, - rdpFilterEpsilon: Float? = null + rdpFilterEpsilon: Float? = null, + zValues: List? = null, + zOutput: MutableList? = null ) { // TODO: Is this allocation needed? What if the bounds were flipped? val vectors = pixels.map { it.toVector2(bounds.top) } @@ -47,9 +52,11 @@ class LineClipper { var previous: PixelCoordinate? = null var previousVector: Vector2? = null + var previousZ: Float? = null for (idx in filteredIndices) { val pixel = pixels[idx] + val z = zValues?.getOrNull(idx) val vector = vectors[idx] // Remove points that are NaN if (pixel.x.isNaN() || pixel.y.isNaN()) continue @@ -65,10 +72,22 @@ class LineClipper { if (previous.isSamePixel(pixel)) { continue } - addLine(bounds, previous, previousVector, pixel, vector, origin, output) + addLine( + bounds, + previous, + previousVector, + pixel, + vector, + origin, + output, + previousZ, + z, + zOutput + ) } previous = pixel previousVector = vector + previousZ = z } } @@ -96,7 +115,10 @@ class LineClipper { end: PixelCoordinate, endVector: Vector2, origin: PixelCoordinate, - lines: MutableList + lines: MutableList, + startZ: Float?, + endZ: Float?, + zOutput: MutableList? ) { // Both are in if (bounds.contains(startVector) && bounds.contains(endVector)) { @@ -104,11 +126,16 @@ class LineClipper { lines.add(start.y - origin.y) lines.add(end.x - origin.x) lines.add(end.y - origin.y) + if (zOutput != null) { + zOutput.add(interpolateZ(startZ, endZ, 0f)) + zOutput.add(interpolateZ(startZ, endZ, 1f)) + } return } val intersection = - Geometry.getIntersection(startVector, endVector, bounds).map { it.toPixelCoordinate(bounds.top) } + Geometry.getIntersection(startVector, endVector, bounds) + .map { it.toPixelCoordinate(bounds.top) } // A is in, B is not if (bounds.contains(startVector)) { @@ -117,6 +144,12 @@ class LineClipper { lines.add(start.y - origin.y) lines.add(intersection[0].x - origin.x) lines.add(intersection[0].y - origin.y) + if (zOutput != null) { + val originalDistance = start.squaredDistanceTo(end) + val t = start.squaredDistanceTo(intersection[0]) / originalDistance.real(1f) + zOutput.add(interpolateZ(startZ, endZ, 0f)) + zOutput.add(interpolateZ(startZ, endZ, t)) + } } return } @@ -128,6 +161,12 @@ class LineClipper { lines.add(intersection[0].y - origin.y) lines.add(end.x - origin.x) lines.add(end.y - origin.y) + if (zOutput != null) { + val originalDistance = start.squaredDistanceTo(end) + val t = start.squaredDistanceTo(intersection[0]) / originalDistance.real(1f) + zOutput.add(interpolateZ(startZ, endZ, t)) + zOutput.add(interpolateZ(startZ, endZ, 1f)) + } } return } @@ -138,7 +177,21 @@ class LineClipper { lines.add(intersection[0].y - origin.y) lines.add(intersection[1].x - origin.x) lines.add(intersection[1].y - origin.y) + if (zOutput != null) { + val originalDistance = start.squaredDistanceTo(end) + val t1 = start.squaredDistanceTo(intersection[0]) / originalDistance.real(1f) + val t2 = start.squaredDistanceTo(intersection[1]) / originalDistance.real(1f) + zOutput.add(interpolateZ(startZ, endZ, t1)) + zOutput.add(interpolateZ(startZ, endZ, t2)) + } + } + } + + private fun interpolateZ(start: Float?, end: Float?, t: Float): Float { + if (start == null || end == null) { + return start ?: end ?: 0f } + return start + (end - start) * t } } \ No newline at end of file diff --git a/app/src/main/java/com/kylecorry/trail_sense/shared/extensions/AndromedaExtensions.kt b/app/src/main/java/com/kylecorry/trail_sense/shared/extensions/AndromedaExtensions.kt index a81272493..67a0c162c 100644 --- a/app/src/main/java/com/kylecorry/trail_sense/shared/extensions/AndromedaExtensions.kt +++ b/app/src/main/java/com/kylecorry/trail_sense/shared/extensions/AndromedaExtensions.kt @@ -9,6 +9,7 @@ import com.google.android.material.progressindicator.CircularProgressIndicator import com.kylecorry.andromeda.alerts.Alerts import com.kylecorry.andromeda.core.system.Resources import com.kylecorry.andromeda.core.units.PixelCoordinate +import com.kylecorry.sol.math.SolMath.square import kotlin.math.roundToInt inline fun Alerts.withCancelableLoading( @@ -76,6 +77,10 @@ fun PixelCoordinate.isSamePixel(other: PixelCoordinate): Boolean { return x.roundToInt() == other.x.roundToInt() && y.roundToInt() == other.y.roundToInt() } +fun PixelCoordinate.squaredDistanceTo(other: PixelCoordinate): Float { + return square(x - other.x) + square(y - other.y) +} + fun Path.drawLines(lines: FloatArray) { // Lines are in the form [x1, y1, x2, y2, x3, y3, ...] // Where x1, y1 is the first point and x2, y2 is the second point of the line diff --git a/app/src/main/java/com/kylecorry/trail_sense/tools/augmented_reality/ui/AugmentedRealityFragment.kt b/app/src/main/java/com/kylecorry/trail_sense/tools/augmented_reality/ui/AugmentedRealityFragment.kt index 8c9129fe9..65e963894 100644 --- a/app/src/main/java/com/kylecorry/trail_sense/tools/augmented_reality/ui/AugmentedRealityFragment.kt +++ b/app/src/main/java/com/kylecorry/trail_sense/tools/augmented_reality/ui/AugmentedRealityFragment.kt @@ -261,7 +261,7 @@ class AugmentedRealityFragment : BoundFragment( this.mode = mode when (mode) { ARMode.Normal -> { - binding.arView.setLayers(listOf(gridLayer, astronomyLayer, beaconLayer)) + binding.arView.setLayers(listOf(gridLayer, astronomyLayer, pathsLayer, beaconLayer)) changeGuide(NavigationARGuide(navigator)) } diff --git a/app/src/main/java/com/kylecorry/trail_sense/tools/augmented_reality/ui/layers/ARPathLayer.kt b/app/src/main/java/com/kylecorry/trail_sense/tools/augmented_reality/ui/layers/ARPathLayer.kt index 1fdacdf55..95de7e8db 100644 --- a/app/src/main/java/com/kylecorry/trail_sense/tools/augmented_reality/ui/layers/ARPathLayer.kt +++ b/app/src/main/java/com/kylecorry/trail_sense/tools/augmented_reality/ui/layers/ARPathLayer.kt @@ -2,33 +2,38 @@ package com.kylecorry.trail_sense.tools.augmented_reality.ui.layers import com.kylecorry.andromeda.canvas.ICanvasDrawer import com.kylecorry.andromeda.core.units.PixelCoordinate +import com.kylecorry.sol.math.SolMath +import com.kylecorry.sol.math.SolMath.toDegrees +import com.kylecorry.sol.math.analysis.Trigonometry +import com.kylecorry.sol.math.geometry.Rectangle +import com.kylecorry.sol.units.Bearing import com.kylecorry.sol.units.Coordinate import com.kylecorry.sol.units.Distance +import com.kylecorry.trail_sense.shared.canvas.LineClipper +import com.kylecorry.trail_sense.tools.augmented_reality.domain.position.ARPoint import com.kylecorry.trail_sense.tools.augmented_reality.domain.position.GeographicARPoint -import com.kylecorry.trail_sense.tools.augmented_reality.ui.ARLine import com.kylecorry.trail_sense.tools.augmented_reality.ui.ARMarker import com.kylecorry.trail_sense.tools.augmented_reality.ui.AugmentedRealityView import com.kylecorry.trail_sense.tools.augmented_reality.ui.CanvasCircle +import com.kylecorry.trail_sense.tools.navigation.domain.NavigationService import com.kylecorry.trail_sense.tools.navigation.ui.IMappableLocation import com.kylecorry.trail_sense.tools.navigation.ui.IMappablePath -import com.kylecorry.trail_sense.tools.navigation.ui.MappablePath import com.kylecorry.trail_sense.tools.paths.ui.IPathLayer +import kotlin.math.atan2 +import kotlin.math.sqrt -class ARPathLayer(private val viewDistance: Distance) : ARLayer, IPathLayer { +class ARPathLayer(viewDistance: Distance) : ARLayer, IPathLayer { - private val lineLayer = ARLineLayer() private val pointLayer = ARMarkerLayer(0.1f, 6f) private var lastLocation = Coordinate.zero private val viewDistanceMeters = viewDistance.meters().distance override fun draw(drawer: ICanvasDrawer, view: AugmentedRealityView) { lastLocation = view.location - lineLayer.draw(drawer, view) pointLayer.draw(drawer, view) } override fun invalidate() { - lineLayer.invalidate() pointLayer.invalidate() } @@ -48,57 +53,94 @@ class ARPathLayer(private val viewDistance: Distance) : ARLayer, IPathLayer { val location = lastLocation - val splitPaths = paths.flatMap { path -> - clipPath(path, location, viewDistanceMeters) - } - - lineLayer.setLines(splitPaths.map { path -> - ARLine( - path.points.map { - GeographicARPoint(it.coordinate, it.elevation) - }, - path.color, - 1f, - ARLine.ThicknessUnits.Dp - ) - }) - - // Only render the closest 20 points - val nearby = splitPaths.flatMap { path -> - path.points.map { - it to path.color + val markers = paths.flatMap { path -> + val nearby = getNearbyARPoints(path, location, viewDistanceMeters) + nearby.map { + ARMarker( + it, + CanvasCircle(path.color) + ) } - }.sortedBy { - it.first.coordinate.distanceTo(location) - }.take(20).map { - ARMarker( - GeographicARPoint(it.first.coordinate, it.first.elevation), - CanvasCircle(it.second) - ) } - pointLayer.setMarkers(nearby) + + pointLayer.setMarkers(markers) } - private fun clipPath(path: IMappablePath, location: Coordinate, distance: Float): List { - val clipped = mutableListOf() - val currentPoints = mutableListOf() - - for (point in path.points) { - if (point.coordinate.distanceTo(location) < distance) { - currentPoints.add(point) - } else { - // TODO: Clip instead of remove - if (currentPoints.isNotEmpty()) { - clipped.add(MappablePath(path.id, currentPoints.toList(), path.color, path.style)) - currentPoints.clear() - } + private fun getNearbyARPoints( + path: IMappablePath, + location: Coordinate, + viewDistance: Float + ): List { + // Step 1: Project the path points + val projected = project(path.points, location) + val hasElevation = path.points.firstOrNull()?.elevation != null + val z = if (hasElevation) path.points.map { it.elevation ?: 0f } else null + + // Step 2: Clip the projected points + val clipper = LineClipper() + val bounds = Rectangle(-viewDistance, viewDistance, viewDistance, -viewDistance) + val output = mutableListOf() + val zOutput = if (hasElevation) mutableListOf() else null + clipper.clip( + projected, + bounds, + output, + zValues = z, + zOutput = zOutput, + rdpFilterEpsilon = 10f + ) + + // Step 3: Interpolate between the points for a higher resolution + // TODO: Not yet implemented + + // Step 4: Convert the clipped points back to geographic points + var lastX: Float? = null + var lastY: Float? = null + + val finalPoints = mutableListOf() + + for (i in output.indices step 2) { + val x = output[i] + val y = output[i + 1] + if (x == lastX && y == lastY) { + continue } + lastX = x + lastY = y + val elevation = zOutput?.getOrNull(i / 2) + val projectedCoordinate = inverseProject(PixelCoordinate(x, y), location) + finalPoints.add(GeographicARPoint(projectedCoordinate, elevation)) } - if (currentPoints.isNotEmpty()) { - clipped.add(MappablePath(path.id, currentPoints.toList(), path.color, path.style)) + return finalPoints + + } + + private fun project( + points: List, + location: Coordinate + ): List { + return points.map { + project(it.coordinate, location) } + } + + private val navigation = NavigationService() + private fun project(location: Coordinate, myLocation: Coordinate): PixelCoordinate { + val vector = navigation.navigate(myLocation, location, 0f, true) + val angle = Trigonometry.toUnitAngle(vector.direction.value, 90f, false) + val pixelDistance = vector.distance // Assumes 1 meter = 1 pixel + val xDiff = SolMath.cosDegrees(angle) * pixelDistance + val yDiff = SolMath.sinDegrees(angle) * pixelDistance + return PixelCoordinate(xDiff, -yDiff) + } - return clipped + private fun inverseProject(pixel: PixelCoordinate, myLocation: Coordinate): Coordinate { + val xDiff = pixel.x + val yDiff = -pixel.y + val pixelDistance = sqrt(xDiff * xDiff + yDiff * yDiff) + val angle = atan2(yDiff, xDiff).toDegrees() + val direction = Trigonometry.remapUnitAngle(angle, 90f, false) + return myLocation.plus(pixelDistance.toDouble(), Bearing(direction)) } } \ No newline at end of file