Skip to content

Commit

Permalink
Move math to AR utils
Browse files Browse the repository at this point in the history
  • Loading branch information
@kylecorry31
kylecorry31 committed Nov 5, 2023
1 parent 122cb6b commit 8dc9dde
Show file tree
Hide file tree
Showing 2 changed files with 136 additions and 116 deletions.
187 changes: 116 additions & 71 deletions app/src/main/java/com/kylecorry/trail_sense/shared/camera/AugmentedRealityUtils.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,73 +4,27 @@ 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
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
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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<Float, Float> {
// 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<Float, Float> {
// 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
}

}

65 changes: 20 additions & 45 deletions app/src/main/java/com/kylecorry/trail_sense/tools/augmented_reality/AugmentedRealityView.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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 {
Expand Down Expand Up @@ -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
Expand Down

0 comments on commit 8dc9dde

Please sign in to comment.