|
| 1 | +package com.soar.tracker.ar |
| 2 | + |
| 3 | +import android.app.Activity |
| 4 | +import android.opengl.GLES20 |
| 5 | +import android.opengl.GLSurfaceView |
| 6 | +import com.google.ar.core.ArCoreApk |
| 7 | +import com.google.ar.core.Config |
| 8 | +import com.google.ar.core.Pose |
| 9 | +import com.google.ar.core.Session |
| 10 | +import com.google.ar.core.TrackingState |
| 11 | +import com.google.ar.core.exceptions.CameraNotAvailableException |
| 12 | +import com.google.ar.core.exceptions.UnavailableException |
| 13 | +import kotlinx.coroutines.flow.MutableStateFlow |
| 14 | +import kotlinx.coroutines.flow.StateFlow |
| 15 | +import javax.microedition.khronos.egl.EGLConfig |
| 16 | +import javax.microedition.khronos.opengles.GL10 |
| 17 | +import kotlin.math.asin |
| 18 | +import kotlin.math.atan |
| 19 | +import kotlin.math.atan2 |
| 20 | + |
| 21 | +/** |
| 22 | + * Manages an ARCore [Session] and extracts heading/pitch from the camera pose each frame. |
| 23 | + * |
| 24 | + * ARCore's visual-inertial odometry gives frame-accurate pose tracking, so the |
| 25 | + * overlay markers stay perfectly synchronized with the camera image. The heading is |
| 26 | + * calibrated to magnetic north using a one-time offset from the sensor-based compass. |
| 27 | + */ |
| 28 | +class ARCoreSessionManager(private val activity: Activity) : GLSurfaceView.Renderer { |
| 29 | + |
| 30 | + private var session: Session? = null |
| 31 | + private val backgroundRenderer = BackgroundRenderer() |
| 32 | + |
| 33 | + // North calibration: offset = magneticNorth - arcoreYaw (averaged over N samples) |
| 34 | + // Accessed from both main thread (calibrateNorth) and GL thread (onDrawFrame) |
| 35 | + private val calibrationLock = Any() |
| 36 | + private var northOffsetDegrees: Float? = null |
| 37 | + private val calibrationSamples = mutableListOf<Float>() |
| 38 | + @Volatile |
| 39 | + private var pendingMagneticHeading: Float? = null |
| 40 | + |
| 41 | + private val _headingDegrees = MutableStateFlow<Float?>(null) |
| 42 | + val headingDegrees: StateFlow<Float?> = _headingDegrees |
| 43 | + |
| 44 | + private val _pitchDegrees = MutableStateFlow<Float?>(null) |
| 45 | + val pitchDegrees: StateFlow<Float?> = _pitchDegrees |
| 46 | + |
| 47 | + private val _fovHDegrees = MutableStateFlow<Float?>(null) |
| 48 | + val fovHDegrees: StateFlow<Float?> = _fovHDegrees |
| 49 | + |
| 50 | + private val _fovVDegrees = MutableStateFlow<Float?>(null) |
| 51 | + val fovVDegrees: StateFlow<Float?> = _fovVDegrees |
| 52 | + |
| 53 | + private val _isAvailable = MutableStateFlow(false) |
| 54 | + val isAvailable: StateFlow<Boolean> = _isAvailable |
| 55 | + |
| 56 | + /** |
| 57 | + * Provide a magnetic heading sample for north calibration. |
| 58 | + * Should be called whenever the sensor collector has a new heading value. |
| 59 | + * Only the first [CALIBRATION_SAMPLE_COUNT] samples are used. |
| 60 | + */ |
| 61 | + fun calibrateNorth(magneticHeadingDegrees: Float) { |
| 62 | + synchronized(calibrationLock) { |
| 63 | + if (northOffsetDegrees != null) return // Already calibrated |
| 64 | + } |
| 65 | + pendingMagneticHeading = magneticHeadingDegrees |
| 66 | + } |
| 67 | + |
| 68 | + fun createSession(): Boolean { |
| 69 | + return try { |
| 70 | + val availability = ArCoreApk.getInstance().checkAvailability(activity) |
| 71 | + if (!availability.isSupported) { |
| 72 | + _isAvailable.value = false |
| 73 | + return false |
| 74 | + } |
| 75 | + |
| 76 | + val session = Session(activity) |
| 77 | + val config = Config(session).apply { |
| 78 | + updateMode = Config.UpdateMode.LATEST_CAMERA_IMAGE |
| 79 | + planeFindingMode = Config.PlaneFindingMode.DISABLED |
| 80 | + lightEstimationMode = Config.LightEstimationMode.DISABLED |
| 81 | + depthMode = Config.DepthMode.DISABLED |
| 82 | + focusMode = Config.FocusMode.AUTO |
| 83 | + } |
| 84 | + session.configure(config) |
| 85 | + this.session = session |
| 86 | + _isAvailable.value = true |
| 87 | + true |
| 88 | + } catch (_: UnavailableException) { |
| 89 | + _isAvailable.value = false |
| 90 | + false |
| 91 | + } |
| 92 | + } |
| 93 | + |
| 94 | + fun resume() { |
| 95 | + try { |
| 96 | + session?.resume() |
| 97 | + } catch (_: CameraNotAvailableException) { |
| 98 | + // Camera unavailable — will retry on next resume |
| 99 | + } |
| 100 | + } |
| 101 | + |
| 102 | + fun pause() { |
| 103 | + session?.pause() |
| 104 | + } |
| 105 | + |
| 106 | + fun destroy() { |
| 107 | + session?.close() |
| 108 | + session = null |
| 109 | + _isAvailable.value = false |
| 110 | + _headingDegrees.value = null |
| 111 | + _pitchDegrees.value = null |
| 112 | + _fovHDegrees.value = null |
| 113 | + _fovVDegrees.value = null |
| 114 | + northOffsetDegrees = null |
| 115 | + calibrationSamples.clear() |
| 116 | + } |
| 117 | + |
| 118 | + // --- GLSurfaceView.Renderer --- |
| 119 | + |
| 120 | + override fun onSurfaceCreated(gl: GL10?, config: EGLConfig?) { |
| 121 | + GLES20.glClearColor(0f, 0f, 0f, 1f) |
| 122 | + backgroundRenderer.createOnGlThread() |
| 123 | + session?.setCameraTextureName(backgroundRenderer.textureId) |
| 124 | + } |
| 125 | + |
| 126 | + @Suppress("DEPRECATION") // defaultDisplay is fine for our use |
| 127 | + override fun onSurfaceChanged(gl: GL10?, width: Int, height: Int) { |
| 128 | + GLES20.glViewport(0, 0, width, height) |
| 129 | + session?.setDisplayGeometry(activity.windowManager.defaultDisplay.rotation, width, height) |
| 130 | + } |
| 131 | + |
| 132 | + override fun onDrawFrame(gl: GL10?) { |
| 133 | + GLES20.glClear(GLES20.GL_COLOR_BUFFER_BIT or GLES20.GL_DEPTH_BUFFER_BIT) |
| 134 | + |
| 135 | + val session = this.session ?: return |
| 136 | + val frame = try { |
| 137 | + session.update() |
| 138 | + } catch (_: CameraNotAvailableException) { |
| 139 | + return |
| 140 | + } |
| 141 | + |
| 142 | + // Draw camera background |
| 143 | + backgroundRenderer.draw(frame) |
| 144 | + |
| 145 | + val camera = frame.camera |
| 146 | + if (camera.trackingState != TrackingState.TRACKING) return |
| 147 | + |
| 148 | + // Extract heading and pitch from the display-oriented pose |
| 149 | + val pose = camera.displayOrientedPose |
| 150 | + val (arcoreYawDeg, pitchDeg) = extractYawPitch(pose) |
| 151 | + |
| 152 | + // Collect north calibration samples (synchronized — written here on GL thread, |
| 153 | + // northOffsetDegrees read on main thread via calibrateNorth's early-return check) |
| 154 | + val offset = synchronized(calibrationLock) { |
| 155 | + if (northOffsetDegrees == null) { |
| 156 | + val magHeading = pendingMagneticHeading |
| 157 | + if (magHeading != null) { |
| 158 | + var diff = magHeading - arcoreYawDeg |
| 159 | + if (diff > 180f) diff -= 360f |
| 160 | + if (diff < -180f) diff += 360f |
| 161 | + calibrationSamples.add(diff) |
| 162 | + |
| 163 | + if (calibrationSamples.size >= CALIBRATION_SAMPLE_COUNT) { |
| 164 | + northOffsetDegrees = calibrationSamples.average().toFloat() |
| 165 | + } |
| 166 | + } |
| 167 | + } |
| 168 | + northOffsetDegrees |
| 169 | + } |
| 170 | + if (offset != null) { |
| 171 | + var heading = (arcoreYawDeg + offset) % 360f |
| 172 | + if (heading < 0) heading += 360f |
| 173 | + _headingDegrees.value = heading |
| 174 | + } |
| 175 | + |
| 176 | + _pitchDegrees.value = pitchDeg |
| 177 | + |
| 178 | + // Extract actual camera FOV from projection matrix |
| 179 | + val projMatrix = FloatArray(16) |
| 180 | + camera.getProjectionMatrix(projMatrix, 0, 0.1f, 100f) |
| 181 | + // projMatrix[0] = 1/tan(fovX/2), projMatrix[5] = 1/tan(fovY/2) |
| 182 | + if (projMatrix[0] != 0f) { |
| 183 | + _fovHDegrees.value = Math.toDegrees(2.0 * atan(1.0 / projMatrix[0])).toFloat() |
| 184 | + } |
| 185 | + if (projMatrix[5] != 0f) { |
| 186 | + _fovVDegrees.value = Math.toDegrees(2.0 * atan(1.0 / projMatrix[5])).toFloat() |
| 187 | + } |
| 188 | + } |
| 189 | + |
| 190 | + /** |
| 191 | + * Extract yaw and pitch from an ARCore [Pose] quaternion. |
| 192 | + * |
| 193 | + * ARCore coordinate system: Y up, -Z is the camera forward direction. |
| 194 | + * The displayOrientedPose accounts for screen rotation. |
| 195 | + */ |
| 196 | + private fun extractYawPitch(pose: Pose): Pair<Float, Float> { |
| 197 | + val qx = pose.qx() |
| 198 | + val qy = pose.qy() |
| 199 | + val qz = pose.qz() |
| 200 | + val qw = pose.qw() |
| 201 | + |
| 202 | + // Yaw: rotation around Y axis (gravity direction) |
| 203 | + val sinYaw = 2f * (qw * qy + qx * qz) |
| 204 | + val cosYaw = 1f - 2f * (qy * qy + qx * qx) |
| 205 | + val yawRad = atan2(sinYaw, cosYaw) |
| 206 | + // Negate because ARCore yaw increases counter-clockwise |
| 207 | + var yawDeg = -Math.toDegrees(yawRad.toDouble()).toFloat() |
| 208 | + if (yawDeg < 0) yawDeg += 360f |
| 209 | + |
| 210 | + // Pitch: rotation around X axis |
| 211 | + val sinPitch = 2f * (qw * qx - qy * qz) |
| 212 | + val pitchDeg = Math.toDegrees(asin(sinPitch.coerceIn(-1f, 1f)).toDouble()).toFloat() |
| 213 | + |
| 214 | + return Pair(yawDeg, pitchDeg) |
| 215 | + } |
| 216 | + |
| 217 | + companion object { |
| 218 | + private const val CALIBRATION_SAMPLE_COUNT = 5 |
| 219 | + } |
| 220 | +} |
0 commit comments