Project

General

Profile

Download (27.1 KB)

User Story #36 » 0001-Aim-axis-switch-from-Y-top-edge-wand-to-Z-screen-nor.patch

knight8241, 08/07/2026 18:31

View differences:

androidApp/src/main/java/com/aether/mofe/platform/MofeEngineHost.kt
val aimXDeg: Double, val aimYDeg: Double, val aimZDeg: Double,
/** The single best-aligned body axis (e.g. "+Z", "-Y") and its error (deg). */
val aimBestAxis: String, val aimBestDeg: Double,
/** HEADING error (deg): +Y-facing azimuth vs the true azimuth to the root, in the
* horizontal plane only. Z-INDEPENDENT, so it isolates a yaw/heading fault from a
/** HEADING error (deg): aim-axis (−Z lens) azimuth vs the true azimuth to the root, in
* the horizontal plane only. Z-INDEPENDENT, so it isolates a yaw/heading fault from a
* depth fault — a large value here with the device aimed at Aether 1 means the
* attitude heading disagrees with the position solution (AoA yaw-anchor suspect). */
val aimAzErrDeg: Double,
......
val yaw = kotlin.math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))
val pitch = kotlin.math.asin((2.0 * (w * y - z * x)).coerceIn(-1.0, 1.0))
val roll = kotlin.math.atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))
// On-screen "facing" azimuth: exactly the vector the viewport orbits around
// (body +Y, R(q)·(0,1,0)) projected into the horizontal plane.
val fx = 2.0 * (x * y - w * z)
val fy = 1.0 - 2.0 * (x * x + z * z)
// On-screen AIM azimuth: the lens axis the viewport looks along and the aim ray
// follows (body −Z, R(q)·(0,0,−1)) projected into the horizontal plane.
val fx = -2.0 * (x * z + w * y)
val fy = 2.0 * (w * x - y * z)
val az = kotlin.math.atan2(fy, fx)
val dt = if (_lastDiagMs == 0L) 0.0 else (now - _lastDiagMs) / 1000.0
val yawRate = if (dt > 0.0 && !_lastDiagYawRad.isNaN()) wrapPi(yaw - _lastDiagYawRad) / dt else 0.0
androidApp/src/main/java/com/aether/mofe/ui/HudScreen.kt
onNodeTapped = { viewModel.selectNode(it) },
liveFacing = viewModel::latestFacing,
targets = targets,
modifier = Modifier.fillMaxSize()
modifier = Modifier.fillMaxSize(),
liveTopEdge = viewModel::latestTopEdge,
)
topBar(Modifier.align(Alignment.TopCenter).padding(top = 4.dp))
......
onNodeTapped = { viewModel.selectNode(it) },
liveFacing = viewModel::latestFacing,
targets = targets,
modifier = Modifier.fillMaxSize()
modifier = Modifier.fillMaxSize(),
liveTopEdge = viewModel::latestTopEdge,
)
topBar(Modifier.align(Alignment.TopCenter).padding(top = 4.dp))
......
}
/** AoA-convention calibration. Primary path is AUTO: aim the wand axis (top edge) steadily
* at Aether 1 and press AUTO — the engine collects ~60 AoA samples and solves the
* convention that maps them onto the wand, applies it, and reports OK or "NO FIT" (→ the
* fault is position/frame, run the depth diagnostic). Manual buttons remain as a fallback
* fine-trim; the general "aoaResid" row (kept low while moving) validates either way. */
/** AoA-convention calibration. Primary path is AUTO: aim the lens axis (the back of the
* device / screen-normal) steadily at Aether 1 and press AUTO — the engine collects ~60 AoA
* samples and solves the convention that maps them onto the aim axis, applies it, and reports
* OK or "NO FIT" (→ the fault is position/frame, run the depth diagnostic). Aiming along the
* lens puts the target near the AoA boresight, where bearings are well-conditioned. Manual
* buttons remain a fallback fine-trim; the "aoaResid" row (kept low while moving) validates. */
@Composable
private fun AoaCalibControls() {
var tick by remember { mutableStateOf(0) }
tick // read → recompose on retune
val res = AoaAutoCalibrator.lastResult
Spacer(Modifier.height(4.dp))
Text("AoA CAL · aim wand at Aether 1, AUTO", color = AetherColors.TextDim, fontSize = 7.sp,
Text("AoA CAL · aim lens (back) at Aether 1, AUTO", color = AetherColors.TextDim, fontSize = 7.sp,
fontFamily = FontFamily.Monospace, letterSpacing = 0.5.sp)
val status = when {
AoaAutoCalibrator.collecting -> "capturing… hold the wand on target"
AoaAutoCalibrator.collecting -> "capturing… hold the lens on target"
res != null -> res.message
else -> AoaCalibration.summary()
}
androidApp/src/main/java/com/aether/mofe/ui/components/Mesh3DCanvas.kt
import kotlin.math.cos
import kotlin.math.sin
// Device "facing" vector = body +Y (the long axis, out the top edge — the wand
// direction). It is supplied live via the [liveFacing] lambda (computed in
// HudViewModel.latestFacing as R(q)·(0,1,0)), matching BehaviorEngine.forwardOf so
// the aiming ray, first-person camera, and pointing predicate all agree.
// Device AIM vector = body −Z (screen-normal, out the back — the "lens" direction).
// Supplied live via the [liveFacing] lambda (computed in HudViewModel.latestFacing as
// R(q)·(0,0,−1)), matching BehaviorEngine.forwardOf so the aiming ray, first-person
// camera, and pointing predicate all agree. The self phone GLYPH is drawn along the
// separate [liveTopEdge] (body +Y) so it still reads as a phone; the aim arrow follows
// the lens axis.
private data class Cam3D(
val azimuth: Float = (-PI / 4).toFloat(),
......
targets: List<Vector3D>,
firstPerson: Boolean,
modifier: Modifier = Modifier,
liveTopEdge: () -> Vector3D? = { null },
) {
val textMeasurer = rememberTextMeasurer()
var cam by remember { mutableStateOf(Cam3D()) }
......
}
) {
frameTick.value // subscribe → redraw at 60 fps
val facing = liveFacing() ?: Vector3D(0.0, 1.0, 0.0)
val facing = liveFacing() ?: Vector3D(0.0, 0.0, -1.0) // AIM = lens axis (−Z)
val topEdge = liveTopEdge() ?: Vector3D(0.0, 1.0, 0.0) // phone long axis (+Y) for the glyph
val proj = projectorFor(firstPerson, cam, target, facing, size.width, size.height)
drawTableGrid(proj, textMeasurer)
......
val pr = proj.project(a.spatial.position) ?: return@forEach
val sel = selectedNode is SelectedNode.Anchor && selectedNode.id == a.id
val color = anchorColor(a.status)
// The self device is the only node whose orientation we know here (the
// live facing vector); peers render position-only (flat, no arrow).
val face = if (a.isLocalDevice) facing else null
// The self device is the only node whose orientation we know here: its
// top-edge orients the glyph and its lens axis draws the aim arrow. Peers
// render position-only (flat, no arrow).
val bodyLong = if (a.isLocalDevice) topEdge else null
val aimArrow = if (a.isLocalDevice) facing else null
drawables += Drawable(pr.second) {
drawDropLine(proj, a.spatial.position)
drawPhone3D(proj, a.spatial.position, face, color, a.label, sel, a.isLocalDevice, textMeasurer)
drawPhone3D(proj, a.spatial.position, bodyLong, aimArrow, color, a.label, sel, a.isLocalDevice, textMeasurer)
}
}
snapshot.meshPoints.forEach { mp ->
......
val color = if (mp.isRangingEnabled && !mp.isStale) AetherColors.MeshPointColor else AetherColors.Offline
drawables += Drawable(pr.second) {
drawDropLine(proj, mp.spatial.position)
drawPhone3D(proj, mp.spatial.position, null, color, mp.label, sel, false, textMeasurer)
drawPhone3D(proj, mp.spatial.position, null, null, color, mp.label, sel, false, textMeasurer)
}
}
drawables.sortedByDescending { it.depth }.forEach { it.draw(this) }
......
private fun projectorFor(firstPerson: Boolean, cam: Cam3D, target: Vector3D, facing: Vector3D?, w: Float, h: Float): Projector {
val focal = minOf(w, h) * 0.9f
return if (firstPerson) Projector.firstPerson(Vector3D.ZERO, facing ?: Vector3D(0.0, 1.0, 0.0), focal, w / 2f, h / 2f)
return if (firstPerson) Projector.firstPerson(Vector3D.ZERO, facing ?: Vector3D(0.0, 0.0, -1.0), focal, w / 2f, h / 2f)
else Projector.orbit(cam, target, focal, w / 2f, h / 2f)
}
......
}
/** Draw a device as a phone-shaped rectangle lying in the frame, its long (top) axis
* along [facing] (the solved +Y wand direction) when known — so the app's idea of the
* device's rotation is visible. Peers (facing == null) render flat with no arrow. The
* device's solved coordinates are printed beside it. */
* along [bodyLong] (the solved +Y top-edge direction) when known — so the app's idea of
* the device's rotation is visible — and an aim arrow along [aim] (the −Z lens axis).
* Peers ([bodyLong] == null) render flat with no arrow. The device's solved coordinates
* are printed beside it. */
private fun DrawScope.drawPhone3D(
proj: Projector, center: Vector3D, facing: Vector3D?, color: Color,
proj: Projector, center: Vector3D, bodyLong: Vector3D?, aim: Vector3D?, color: Color,
label: String, selected: Boolean, isSelf: Boolean, tm: TextMeasurer,
) {
// Phone body ~ 15 × 7.5 cm; long axis = facing (fwd), width = fwd × up.
val fwd = (facing ?: Vector3D(0.0, 1.0, 0.0)).let { if (it.magnitude < 1e-6) Vector3D(0.0, 1.0, 0.0) else it.normal() }
// Phone body ~ 15 × 7.5 cm; long axis = the device top-edge (+Y), width = long × up.
// The AIM arrow follows the separate lens axis (−Z), so the glyph reads as a phone
// while the aim points out the back.
val fwd = (bodyLong ?: Vector3D(0.0, 1.0, 0.0)).let { if (it.magnitude < 1e-6) Vector3D(0.0, 1.0, 0.0) else it.normal() }
var right = fwd.cross(WORLD_UP)
if (right.magnitude < 1e-3f) right = Vector3D(1.0, 0.0, 0.0)
right = right.normal()
......
val body = androidx.compose.ui.graphics.Path().apply { moveTo(tr.x, tr.y); lineTo(tl.x, tl.y); lineTo(bl.x, bl.y); lineTo(br.x, br.y); close() }
drawPath(body, color.copy(alpha = if (isSelf) 0.30f else 0.20f))
drawPath(body, color, style = Stroke(if (selected) 2.5f else 1.5f))
// Top edge accent (where the "wand tip" points) + facing arrow when known.
// Top edge accent + AIM (lens) arrow along −Z when known.
drawLine(color, tr, tl, strokeWidth = 3f)
if (facing != null) {
drawSegment(proj, center, center + fwd * 0.26, AetherColors.Accent, 2.5f)
proj.project(center + fwd * 0.26)?.first?.let { drawCircle(AetherColors.Accent, 3f, it) }
if (aim != null && aim.magnitude > 1e-6) {
val a = aim.normal()
drawSegment(proj, center, center + a * 0.26, AetherColors.Accent, 2.5f)
proj.project(center + a * 0.26)?.first?.let { drawCircle(AetherColors.Accent, 3f, it) }
}
if (isSelf) drawCircle(color, 3.5f, at)
androidApp/src/main/java/com/aether/mofe/ui/components/MeshViewport.kt
modifier: Modifier = Modifier,
liveFacing: () -> Vector3D? = { null },
targets: List<Vector3D> = emptyList(),
liveTopEdge: () -> Vector3D? = { null },
) {
var threeD by remember { mutableStateOf(true) }
var firstPerson by remember { mutableStateOf(false) }
......
targets = targets,
firstPerson = firstPerson,
modifier = Modifier.fillMaxSize(),
liveTopEdge = liveTopEdge,
)
} else {
MeshMapCanvas(snapshot, selectedNode, onNodeTapped, Modifier.fillMaxSize())
androidApp/src/main/java/com/aether/mofe/viewmodel/HudViewModel.kt
// ── 3D viewport: device facing + synthetic targets ────────────────────────
/**
* The device's live "facing" vector in the mesh frame — body +Y, the long axis
* out the top edge (the wand direction), the single source of truth for "forward"
* shared with BehaviorEngine's pointing predicate. Sampled at ~60 Hz off the engine
* host (see MofeEngineHost.latestSelfPose), so the aiming ray and first-person camera
* track the hand smoothly, decoupled from the ~10 Hz UWB solve. Returns null until a
* pose exists; the viewport falls back to +Y. Self is at the snapshot origin, so the
* aiming ray runs origin→facing.
* The device's live AIM vector in the mesh frame — body −Z, the screen-normal out the
* BACK of the device (the "lens" direction): hold the phone up, look at the screen, and
* aim through it at the target. Single source of truth for "forward" shared with
* BehaviorEngine's pointing predicate. Sampled at ~60 Hz off the engine host (see
* MofeEngineHost.latestSelfPose), so the aiming ray and first-person camera track the
* hand smoothly, decoupled from the ~10 Hz UWB solve. Returns null until a pose exists;
* the viewport falls back to −Z. Self is at the snapshot origin, so the aim ray runs
* origin→facing. (Was body +Y / top-edge "wand"; changed to the lens gesture.)
*/
fun latestFacing(): Vector3D? {
val q = app.mofeEngineHost.latestSelfPose()?.orientation ?: return null
val w = q.w.toFloat(); val x = q.x.toFloat(); val y = q.y.toFloat(); val z = q.z.toFloat()
val fx = -2.0 * (x * z + w * y)
val fy = 2.0 * (w * x - y * z)
val fz = 2.0 * (x * x + y * y) - 1.0
val m = kotlin.math.sqrt(fx * fx + fy * fy + fz * fz)
return if (m > 1e-6f) Vector3D(fx / m, fy / m, fz / m) else Vector3D(0.0, 0.0, -1.0)
}
/**
* The device's live TOP-EDGE vector (body +Y) in the mesh frame — the phone's long
* axis. Used ONLY to draw the self device as a correctly-oriented phone glyph in the
* orbit view; the AIM is [latestFacing] (−Z). Separating the two lets the glyph still
* read as a phone while the aim points out the lens. Falls back to +Y.
*/
fun latestTopEdge(): Vector3D? {
val q = app.mofeEngineHost.latestSelfPose()?.orientation ?: return null
val w = q.w.toFloat(); val x = q.x.toFloat(); val y = q.y.toFloat(); val z = q.z.toFloat()
val fx = 2.0 * (x * y - w * z)
val fy = 1.0 - 2f * (x * x + z * z)
val fy = 1.0 - 2.0 * (x * x + z * z)
val fz = 2.0 * (y * z + w * x)
val m = kotlin.math.sqrt(fx * fx + fy * fy + fz * fz)
return if (m > 1e-6f) Vector3D(fx / m, fy / m, fz / m) else Vector3D(0.0, 1.0, 0.0)
common/src/commonMain/kotlin/com/aether/mofe/engine/AoaAutoCalibrator.kt
import kotlin.math.round
/**
* Automated AoA-convention calibration. The user aims the render's wand axis (body +Y,
* the top edge) steadily at a known target and starts a capture; the engine feeds each
* Automated AoA-convention calibration. The user aims the render's aim axis (body −Z,
* the screen-normal / lens) steadily at a known target and starts a capture; the engine feeds each
* self-AoA sample here, and once enough are collected this brute-forces every convention
* (boresight × az-sign × el-sign × azimuth-offset) for the one that maps the measured AoA
* onto the wand axis, applies it to [AoaCalibration], and reports the result.
......
const val SUCCESS_DEG = 15.0 // best aim residual must beat this to "fit"
private const val OFFSET_STEP_DEG = 3.0
private val SIGNS = doubleArrayOf(1.0, -1.0)
/** Render wand axis in the body frame — body +Y (top edge), matching Mesh3DCanvas. */
val WAND_AXIS: Vector3D = Vector3D(0.0, 1.0, 0.0)
/** Aim axis in the body frame — body −Z (screen-normal / lens), matching the render aim
* ([HudViewModel.latestFacing]) and [BehaviorEngine.forwardOf]. The user aims THIS axis
* at the target; the solve maps the measured AoA onto it. Aiming along the screen-normal
* also puts the target near the UWB AoA boresight, where bearings are well-conditioned
* (aiming with the old +Y top edge pointed the target ~90° off boresight, into the noisy
* ±FoV region). Named WAND_AXIS for source compatibility; it is the lens axis now. */
val WAND_AXIS: Vector3D = Vector3D(0.0, 0.0, -1.0)
data class Result(
val ok: Boolean,
common/src/commonMain/kotlin/com/aether/mofe/engine/BehaviorEngine.kt
}
private fun forwardOf(p: PoseSample): Vector3D {
// DEVICE "FACING" AXIS = body +Y — the vector running through the LENGTH of
// the device and out its top edge. Holding the phone like a wand pointed
// away, this is the direction it points. This is the single source of truth
// for "forward" in all reasoning (the pointing predicate, the aiming ray,
// and the first-person camera). R(q)·(0,1,0) = the +Y body axis in the mesh
// world frame. (Was body +Z / screen-normal; changed per the wand gesture.) // body +Z pointing axis (corpus contract; A11 resolved)
// DEVICE AIM AXIS = body −Z — the screen-normal pointing OUT THE BACK of the
// device (the camera / "lens" direction). Held up like a viewer, the user looks
// at the screen and aims through it at the target. This is the single source of
// truth for "forward" in all reasoning (the pointing predicate, the aiming ray,
// the first-person camera, and the AoA-calibration gesture). R(q)·(0,0,−1) = the
// −Z body axis in the mesh world frame. It also aligns the aim with the phone's
// UWB AoA boresight (screen-normal), so self bearings are well-conditioned while
// aiming. (Was body +Y / top-edge "wand"; changed to the lens gesture.)
val w = p.qw; val x = p.qx; val y = p.qy; val z = p.qz
return normalize(
Vector3D(
2 * (x * y - w * z),
1 - 2 * (x * x + z * z),
2 * (y * z + w * x),
-2 * (x * z + w * y),
2 * (w * x - y * z),
2 * (x * x + y * y) - 1,
)
)
}
common/src/commonTest/kotlin/com/aether/mofe/engine/PointingPredicateTest.kt
* exercised directly against [BehaviorEngine] + [PointingPredicates] (the
* platform-agnostic realization core; no EKF/transport involved).
*
* Body +Y (the device's LONG axis / "facing" vector) is the pointing axis
* (see BehaviorEngine.forwardOf). With the identity orientation the facing axis
* is (0,1,0), so a target on +Y of the device is dead-centre in the aiming cone.
* Body −Z (the device's screen-normal / "lens" axis) is the pointing axis
* (see BehaviorEngine.forwardOf). With the identity orientation the aim axis
* is (0,0,-1), so a target on −Z of the device is dead-centre in the aiming cone.
*/
class PointingPredicateTest {
private val identity = doubleArrayOf(1.0, 0.0, 0.0, 0.0) // w,x,y,z → facing +Y
private val flipX = doubleArrayOf(0.0, 1.0, 0.0, 0.0) // 180° about X → facing -Y
private val identity = doubleArrayOf(1.0, 0.0, 0.0, 0.0) // w,x,y,z → aim -Z
private val flipX = doubleArrayOf(0.0, 1.0, 0.0, 0.0) // 180° about X → aim +Z
private val origin = Vector3D(0.0, 0.0, 0.0)
private fun pose(q: DoubleArray, tMicros: Long, at: Vector3D = origin) = PoseSample(
......
val engine = BehaviorEngine()
val manifest = PointingPredicates.pointAtLocation("pred-loc")
val ok = engine.register(manifest, mapOf(
PointingPredicates.INPUT_TARGET to TargetBinding.Fixed(Vector3D(0.0, 5.0, 0.0)),
PointingPredicates.INPUT_TARGET to TargetBinding.Fixed(Vector3D(0.0, 0.0, -5.0)),
))
assertTrue(ok, "manifest with a bound fixed target must register")
val fired = collector(engine)
val self = DeviceId("self")
// Aimed at the +Y target from the start; below dwell → not yet satisfied.
// Aimed at the -Z target from the start; below dwell → not yet satisfied.
engine.tick(pose(identity, tMicros = 0), self)
assertTrue(fired.isEmpty(), "must not fire before the dwell elapses")
......
fun pointAtLocation_never_fires_when_aimed_away() {
val engine = BehaviorEngine()
engine.register(PointingPredicates.pointAtLocation("pred-loc"), mapOf(
PointingPredicates.INPUT_TARGET to TargetBinding.Fixed(Vector3D(0.0, 5.0, 0.0)),
PointingPredicates.INPUT_TARGET to TargetBinding.Fixed(Vector3D(0.0, 0.0, -5.0)),
))
val fired = collector(engine)
val self = DeviceId("self")
// Facing axis is -Y (180° from the +Y target) → outside the cone.
// Aim axis is +Z (180° from the -Z target) → outside the cone.
engine.tick(pose(flipX, tMicros = 0), self)
engine.tick(pose(flipX, tMicros = 600_000), self)
engine.tick(pose(flipX, tMicros = 1_200_000), self)
......
engine.tick(pose(identity, tMicros = 600_000), self)
assertTrue(fired.isEmpty(), "cannot fire before the target device is localized")
// Peer appears on +Y of self and is aimed at; hold across the dwell window.
engine.updateDeviceTarget("peer-42", Vector3D(0.0, 5.0, 0.0))
// Peer appears on -Z of self and is aimed at; hold across the dwell window.
engine.updateDeviceTarget("peer-42", Vector3D(0.0, 0.0, -5.0))
engine.tick(pose(identity, tMicros = 1_000_000), self) // establishes hold baseline
engine.tick(pose(identity, tMicros = 1_600_000), self) // dwell elapsed → fires
(2-2/2)