diff --git a/app/src/main/java/com/kylecorry/trail_sense/shared/camera/AugmentedRealityUtils.kt b/app/src/main/java/com/kylecorry/trail_sense/shared/camera/AugmentedRealityUtils.kt index 6a546ef14..44b5bfe41 100644 --- a/app/src/main/java/com/kylecorry/trail_sense/shared/camera/AugmentedRealityUtils.kt +++ b/app/src/main/java/com/kylecorry/trail_sense/shared/camera/AugmentedRealityUtils.kt @@ -4,8 +4,10 @@ import android.hardware.SensorManager import android.opengl.Matrix import com.kylecorry.andromeda.core.units.PixelCoordinate import com.kylecorry.andromeda.sense.orientation.IOrientationSensor +import com.kylecorry.sol.math.Quaternion import com.kylecorry.sol.math.QuaternionMath import com.kylecorry.sol.math.SolMath +import com.kylecorry.sol.math.SolMath.real import com.kylecorry.sol.math.SolMath.toDegrees import com.kylecorry.sol.math.SolMath.toRadians import com.kylecorry.sol.math.Vector3 @@ -13,64 +15,16 @@ import com.kylecorry.sol.math.geometry.Size import com.kylecorry.sol.units.Coordinate import com.kylecorry.sol.units.Distance import com.kylecorry.trail_sense.tools.augmented_reality.AugmentedRealityView +import kotlin.math.asin import kotlin.math.atan2 import kotlin.math.cos -import kotlin.math.hypot import kotlin.math.sin object AugmentedRealityUtils { - // TODO: Take in full device orientation / quaternion - /** - * Gets the pixel coordinate of a point on the screen given the bearing and azimuth. The point is considered to be on a sphere. - * @param bearing The compass bearing in degrees of the point - * @param azimuth The compass bearing in degrees that the user is facing (center of the screen) - * @param altitude The altitude of the point in degrees - * @param inclination The inclination of the device in degrees - * @param size The size of the view in pixels - * @param fov The field of view of the camera in degrees - */ - fun getPixelSpherical( - bearing: Float, - azimuth: Float, - altitude: Float, - inclination: Float, - size: Size, - fov: Size - ): PixelCoordinate { - val diagonalFov = hypot(fov.width, fov.height) - val diagonalSize = hypot(size.width, size.height) - val radius = diagonalSize / (sin((diagonalFov / 2f).toRadians()) * 2f) - - val newBearing = SolMath.deltaAngle(azimuth, bearing) - val newAltitude = altitude - inclination - - val cartesian = sphericalToCartesian( - newBearing, - newAltitude, - radius - ) + private val worldVectorLock = Any() + private val tempWorldVector = FloatArray(4) - var x = size.width / 2f + cartesian.x - // If the coordinate is off the screen, ensure it is not drawn - if (newBearing > fov.width / 2f) { - x += size.width - } else if (newBearing < -fov.width / 2f) { - x -= size.width - } - - var y = size.height / 2f - cartesian.y - // If the coordinate is off the screen, ensure it is not drawn - if (newAltitude > fov.height / 2f) { - y += size.height - } else if (newAltitude < -fov.height / 2f) { - y -= size.height - } - - return PixelCoordinate(x, y) - } - - // TODO: Take in full device orientation / quaternion /** * Gets the pixel coordinate of a point on the screen given the bearing and azimuth. The point is considered to be on a plane. * @param bearing The compass bearing in degrees of the point @@ -125,31 +79,45 @@ object AugmentedRealityUtils { return AugmentedRealityView.HorizonCoordinate(bearing, elevationAngle, true) } - /** - * Converts a spherical coordinate to a cartesian coordinate. - * @param azimuth The azimuth in degrees (rotation around the z axis) - * @param elevation The elevation in degrees (rotation around the x axis) - * @param radius The radius + * Gets the pixel coordinate of a point on the screen given the bearing and azimuth. + * @param bearing The compass bearing in degrees of the point + * @param elevation The elevation in degrees of the point + * @param rotationMatrix The rotation matrix of the device in the AR coordinate system + * @param size The size of the view in pixels + * @param fov The field of view of the camera in degrees */ - private fun sphericalToCartesian( - azimuth: Float, + fun getPixel( + bearing: Float, elevation: Float, - radius: Float - ): Vector3 { - // https://stackoverflow.com/questions/5278417/rotating-body-from-spherical-coordinates - this may be useful when factoring in roll - val azimuthRad = azimuth.toRadians() - val pitchRad = elevation.toRadians() - - val sinAzimuth = sin(azimuthRad) - - val x = sinAzimuth * cos(pitchRad) * radius - val y = sinAzimuth * sin(pitchRad) * radius - val z = cos(azimuthRad) * radius - - return Vector3(x, y, z) + rotationMatrix: FloatArray, + size: Size, + fov: Size + ): PixelCoordinate { + val spherical = toRelative(bearing, elevation, 1f, rotationMatrix) + // The rotation of the device has been negated, so azimuth = 0 and inclination = 0 is used + return getPixelLinear(spherical.first, 0f, spherical.second, 0f, size, fov) } + /** + * Gets the pixel coordinate of a point on the screen given the bearing and azimuth. + * @param bearing The compass bearing in degrees of the point + * @param elevation The elevation in degrees of the point + * @param quaternion The quaternion of the device in the AR coordinate system + * @param size The size of the view in pixels + * @param fov The field of view of the camera in degrees + */ + fun getPixel( + bearing: Float, + elevation: Float, + quaternion: Quaternion, + size: Size, + fov: Size + ): PixelCoordinate { + val spherical = toRelative(bearing, elevation, 1f, quaternion) + // The rotation of the device has been negated, so azimuth = 0 and inclination = 0 is used + return getPixelLinear(spherical.first, 0f, spherical.second, 0f, size, fov) + } /** * Computes the orientation of the device in the AR coordinate system. @@ -190,5 +158,82 @@ object AugmentedRealityUtils { orientation[2] = -orientation[2].toDegrees() } + /** + * Converts a spherical coordinate to a cartesian coordinate in the AR coordinate system. + * @param bearing The azimuth in degrees (rotation around the z axis) + * @param elevation The elevation in degrees (rotation around the x axis) + * @param distance The distance in meters + */ + private fun toWorldCoordinate( + bearing: Float, + elevation: Float, + distance: Float + ): Vector3 { + val thetaRad = elevation.toRadians() + val phiRad = bearing.toRadians() + + val cosTheta = cos(thetaRad) + val x = distance * cosTheta * sin(phiRad) + val y = distance * cosTheta * cos(phiRad) + val z = distance * sin(thetaRad) + return Vector3(x, y, z) + } + + private fun toSpherical(vector: Vector3): Vector3 { + val r = vector.magnitude() + val theta = asin(vector.z / r).toDegrees().real(0f) + val phi = atan2(vector.x, vector.y).toDegrees().real(0f) + return Vector3(r, theta, phi) + } + + /** + * Converts a geographic spherical coordinate to a relative spherical coordinate in the AR coordinate system. + * @return The relative spherical coordinate (bearing, inclination) + */ + private fun toRelative( + bearing: Float, + elevation: Float, + distance: Float, + quaternion: Quaternion + ): Pair { + // Convert to world space + val worldVector = toWorldCoordinate(bearing, elevation, distance) + + // Rotate + val rotated = quaternion.rotate(worldVector) + + // Convert back to spherical + val spherical = toSpherical(rotated) + return spherical.z to spherical.y + } + + /** + * Converts a geographic spherical coordinate to a relative spherical coordinate in the AR coordinate system. + * @return The relative spherical coordinate (bearing, inclination) + */ + private fun toRelative( + bearing: Float, + elevation: Float, + distance: Float, + rotationMatrix: FloatArray + ): Pair { + // Convert to world space + val worldVector = toWorldCoordinate(bearing, elevation, distance) + + // Rotate + val rotated = synchronized(worldVectorLock) { + tempWorldVector[0] = worldVector.x + tempWorldVector[1] = worldVector.y + tempWorldVector[2] = worldVector.z + tempWorldVector[3] = 1f + Matrix.multiplyMV(tempWorldVector, 0, rotationMatrix, 0, tempWorldVector, 0) + Vector3(tempWorldVector[0], tempWorldVector[1], tempWorldVector[2]) + } + + // Convert back to spherical + val spherical = toSpherical(rotated) + return spherical.z to spherical.y + } + } diff --git a/app/src/main/java/com/kylecorry/trail_sense/tools/augmented_reality/AugmentedRealityView.kt b/app/src/main/java/com/kylecorry/trail_sense/tools/augmented_reality/AugmentedRealityView.kt index c0c00ba0f..69e2b008b 100644 --- a/app/src/main/java/com/kylecorry/trail_sense/tools/augmented_reality/AugmentedRealityView.kt +++ b/app/src/main/java/com/kylecorry/trail_sense/tools/augmented_reality/AugmentedRealityView.kt @@ -65,6 +65,7 @@ class AugmentedRealityView : CanvasView { private val quaternion = FloatArray(4) private val orientation = FloatArray(3) private val tempWorldVector = FloatArray(4) + private var size = Size(width.toFloat(), height.toFloat()) // Sensors / preferences @@ -361,36 +362,6 @@ class AugmentedRealityView : CanvasView { circle(width / 2f, height / 2f, reticleDiameter) } - private fun toWorldSpace(bearing: Float, elevation: Float, distance: Float): Vector3 { - val thetaRad = elevation.toRadians() - val phiRad = bearing.toRadians() - - val cosTheta = cos(thetaRad) - val x = distance * cosTheta * sin(phiRad) - val y = distance * cosTheta * cos(phiRad) - val z = distance * sin(thetaRad) - return Vector3(x, y, z) - } - - private fun applyRotation(vector: Vector3): Vector3 { - if (orientationSensor == null) { - return legacyOrientation.rotate(vector) - } - tempWorldVector[0] = vector.x - tempWorldVector[1] = vector.y - tempWorldVector[2] = vector.z - tempWorldVector[3] = 1f - Matrix.multiplyMV(tempWorldVector, 0, rotationMatrix, 0, tempWorldVector, 0) - return Vector3(tempWorldVector[0], tempWorldVector[1], tempWorldVector[2]) - } - - private fun toSpherical(vector: Vector3): Vector3 { - val r = vector.magnitude() - val theta = asin(vector.z / r).toDegrees().real(0f) - val phi = atan2(vector.x, vector.y).toDegrees().real(0f) - return Vector3(r, theta, phi) - } - /** * Converts an angular size to a pixel size * @param angularSize The angular size in degrees @@ -415,21 +386,23 @@ class AugmentedRealityView : CanvasView { fun toPixel(coordinate: HorizonCoordinate): PixelCoordinate { val bearing = getActualBearing(coordinate) - val world = toWorldSpace(bearing, coordinate.elevation, 1f) - val rotated = applyRotation(world) - val spherical = toSpherical(rotated) - - // TODO: Try out Matrix.perspectiveM - - // The rotation of the device has been negated, so azimuth = 0 and inclination = 0 is used - return AugmentedRealityUtils.getPixelLinear( - spherical.z, - 0f, - spherical.y, - 0f, - Size(width.toFloat(), height.toFloat()), - fov - ) + return if (orientationSensor == null) { + AugmentedRealityUtils.getPixel( + bearing, + coordinate.elevation, + legacyOrientation, + size, + fov + ) + } else { + AugmentedRealityUtils.getPixel( + bearing, + coordinate.elevation, + rotationMatrix, + size, + fov + ) + } } fun toPixel(coordinate: Coordinate, elevation: Float? = null): PixelCoordinate { @@ -462,6 +435,8 @@ class AugmentedRealityView : CanvasView { } private fun updateOrientation() { + size = Size(width.toFloat(), height.toFloat()) + val orientationSensor = orientationSensor if (orientationSensor == null) { // TODO: This fails when the device is pointed almost straight up or down