Skip to content

Commit

Permalink
Show clipped path points in AR
Browse files Browse the repository at this point in the history
  • Loading branch information
@kylecorry31
kylecorry31 committed Feb 2, 2024
1 parent 5642cae commit cfc58b8
Show file tree
Hide file tree
Showing 4 changed files with 154 additions and 54 deletions.
61 changes: 57 additions & 4 deletions app/src/main/java/com/kylecorry/trail_sense/shared/canvas/LineClipper.kt
View file
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -19,7 +22,9 @@ class LineClipper {
output: MutableList<Float>,
origin: PixelCoordinate = PixelCoordinate(0f, 0f),
preventLineWrapping: Boolean = false,
rdpFilterEpsilon: Float? = null
rdpFilterEpsilon: Float? = null,
zValues: List<Float>? = null,
zOutput: MutableList<Float>? = null
) {
// TODO: Is this allocation needed? What if the bounds were flipped?
val vectors = pixels.map { it.toVector2(bounds.top) }
Expand Down Expand Up @@ -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
Expand All @@ -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
}
}

Expand Down Expand Up @@ -96,19 +115,27 @@ class LineClipper {
end: PixelCoordinate,
endVector: Vector2,
origin: PixelCoordinate,
lines: MutableList<Float>
lines: MutableList<Float>,
startZ: Float?,
endZ: Float?,
zOutput: MutableList<Float>?
) {
// Both are in
if (bounds.contains(startVector) && bounds.contains(endVector)) {
lines.add(start.x - origin.x)
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)) {
Expand All @@ -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
}
Expand All @@ -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
}
Expand All @@ -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
}

}
5 changes: 5 additions & 0 deletions app/src/main/java/com/kylecorry/trail_sense/shared/extensions/AndromedaExtensions.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion ...ain/java/com/kylecorry/trail_sense/tools/augmented_reality/ui/AugmentedRealityFragment.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -261,7 +261,7 @@ class AugmentedRealityFragment : BoundFragment<FragmentAugmentedRealityBinding>(
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))
}

Expand Down
140 changes: 91 additions & 49 deletions app/src/main/java/com/kylecorry/trail_sense/tools/augmented_reality/ui/layers/ARPathLayer.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
}

Expand All @@ -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<IMappablePath> {
val clipped = mutableListOf<IMappablePath>()
val currentPoints = mutableListOf<IMappableLocation>()

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<ARPoint> {
// 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<Float>()
val zOutput = if (hasElevation) mutableListOf<Float>() 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<GeographicARPoint>()

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<IMappableLocation>,
location: Coordinate
): List<PixelCoordinate> {
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))
}
}

0 comments on commit cfc58b8

Please sign in to comment.