Aether

package com.aether.mofe.engine

import com.aether.mofe.model.DeviceId

import com.aether.mofe.model.Vector3D

/** How good the anchor PLACEMENT is for solving a stable, correctly-oriented frame — the signal that

* distinguishes a ranging problem (all links green, see [InterAnchorLinkReport]) from a GEOMETRY

* problem (links fine, but the layout can't be solved/oriented). Surfaced at calibration + placement

* so a bad layout is caught before the operator leaves, instead of read later as a mysterious

* perpetual-CONVERGING / twisted frame. */

data class ConstellationGeometryReport(

val anchorCount: Int,

/** Distinct inter-anchor ranging edges present. */

val edgeCount: Int,

/** Edges a rigid 3-D constellation of [anchorCount] nodes needs (3n − 6). */

val edgesForRigidity: Int,

/** The distance graph has enough edges to determine the shape (necessary condition). */

val rigid: Boolean,

/** A clearly-raised anchor exists → the frame can level exactly + fast. Without one the frame

* levels off the anchor plane (slower, ~a few-degree tilt) — the room-scale-slab case. */

val hasRaisedAnchor: Boolean,

/** λ1/λ0 of the anchor cloud — how planar it is (large ⇒ a near-2-D slab). 0 if undefined. */

val planarityRatio: Double,

/** Closest two anchors (metres); a tiny value is a near-collapsed/degenerate layout. */

val minSeparationMeters: Double,

val quality: GeometryQuality,

/** Actionable, operator-facing guidance for whatever is wrong (empty when [quality] is GOOD). */

val issues: List<String>,

) {

companion object {

/**

* Score a layout from its solved anchor [positions] (root at the gauge origin), the number of

* inter-anchor [edgeCount], and the root's AoA [elevations]. Pure + hardware-free. Reuses the

* same primitives the orientation solve uses ([ConstellationOrientation.pickRaisedAnchor] /

* [ConstellationOrientation.bestFitPlaneNormal]) so the diagnostic matches the solver's reality.

*/

fun evaluate(

positions: Map<DeviceId, Vector3D>,

edgeCount: Int,

elevations: Map<DeviceId, Double>,

rootId: DeviceId,

minSeparationMeters: Double = 0.10,

): ConstellationGeometryReport {

val n = positions.size

val edgesForRigidity = if (n >= 3) 3 * n - 6 else 0

val rigid = n >= 4 && edgeCount >= edgesForRigidity

val hasRaised = ConstellationOrientation.pickRaisedAnchor(positions, elevations, rootId) != null

val planarity = ConstellationOrientation.bestFitPlaneNormal(positions)?.planarityRatio ?: 0.0

val minSep = minPairwiseSeparation(positions)

val issues = mutableListOf<String>()

if (n < 4) {

issues += "Only $n anchor(s) placed — at least 4 are needed to solve the frame."

} else if (!rigid) {

issues += "Under-constrained: $edgeCount of $edgesForRigidity inter-anchor links — more " +

"anchor pairs must range each other (an obstruction may be blocking a link), or add an anchor."

}

if (n >= 4 && minSep < minSeparationMeters) {

issues += "Two anchors are almost on top of each other (${fmt(minSep)} m apart) — space them out."

}

if (n >= 4 && !hasRaised) {

issues += "No clearly-raised anchor — the frame levels off the anchor plane, which resolves " +

"slowly and can sit a few degrees off level. Raise one anchor ≥ ~15 cm above the others " +

"for a fast, exact orientation lock."

}

val degenerate = n < 4 || !rigid || (n >= 4 && minSep < minSeparationMeters)

val quality = when {

degenerate -> GeometryQuality.DEGENERATE

!hasRaised -> GeometryQuality.MARGINAL

else -> GeometryQuality.GOOD

}

return ConstellationGeometryReport(

anchorCount = n, edgeCount = edgeCount, edgesForRigidity = edgesForRigidity,

rigid = rigid, hasRaisedAnchor = hasRaised, planarityRatio = planarity,

minSeparationMeters = minSep, quality = quality, issues = issues,

)

}

private fun minPairwiseSeparation(positions: Map<DeviceId, Vector3D>): Double {

val pts = positions.values.toList()

if (pts.size < 2) return Double.POSITIVE_INFINITY

var min = Double.POSITIVE_INFINITY

for (i in pts.indices) for (j in i + 1 until pts.size) {

val d = pts[i].distanceTo(pts[j]); if (d < min) min = d

}

return min

}

private fun fmt(v: Double): String {

val scaled = kotlin.math.round(v * 100.0) / 100.0

return scaled.toString()

}

}

}

/** Placement quality, worst-first: DEGENERATE (won't solve/latch) → MARGINAL (solves, but slow/tilted

* — e.g. a room-scale slab with no raised anchor) → GOOD. */

enum class GeometryQuality { DEGENERATE, MARGINAL, GOOD }

/**

* Placement / GEOMETRY quality of the current anchor constellation — whether the layout can be

* solved and correctly oriented, distinct from ranging health ([interAnchorLinkReport]). This is

* the diagnostic that separates "all links green but the frame won't settle" (a bad LAYOUT: too

* few edges to be rigid, or no raised anchor so orientation is slow/tilted) from a ranging fault.

* Null before a topology exists. Pure read; safe to poll for the calibration/placement UI.

*/

fun constellationGeometryReport(): ConstellationGeometryReport? {

val topo = frameManager.topology ?: return null

val rootId = topo.frame.originDeviceId

val positions = buildMap {

put(rootId, Vector3D.ZERO) // the root IS the gauge origin (not in getAllReferencePoints)

frameManager.getAllReferencePoints().forEach { if (it.id != rootId) put(it.id, it.position) }

}

return ConstellationGeometryReport.evaluate(

positions = positions,

edgeCount = interAnchorDistances.size,

elevations = selfElevations,

rootId = rootId,

minSeparationMeters = config.anchorMinSeparationMeters,

)

}

package com.aether.mofe.engine

import com.aether.mofe.model.DeviceId

import com.aether.mofe.model.Vector3D

import kotlin.math.atan2

import kotlin.math.hypot

import kotlin.test.Test

import kotlin.test.assertEquals

import kotlin.test.assertFalse

import kotlin.test.assertTrue

/**

* Placement-geometry detection ([ConstellationGeometryReport]) — the diagnostic that catches a bad

* LAYOUT (fine ranging, but the frame can't solve/orient) at setup time. Cases mirror the field

* reports: the near-coplanar survey (all links green, no raised anchor → orientation slow/tilted) and

* the 5-anchor / 6-edge under-constrained graph (never latches).

*/

class ConstellationGeometryTest {

private fun id(s: String) = DeviceId(s)

/** Root-relative positions + true AoA elevations for a set of world anchors (first = root). */

private fun scene(world: List<Pair<String, Vector3D>>): Triple<Map<DeviceId, Vector3D>, Map<DeviceId, Double>, DeviceId> {

val root = id(world.first().first)

val rootPos = world.first().second

val pos = world.associate { (k, p) -> id(k) to (p - rootPos) }

val elev = world.drop(1).associate { (k, _) ->

val r = pos.getValue(id(k)); id(k) to atan2(r.z, hypot(r.x, r.y))

}

return Triple(pos, elev, root)

}

@Test

fun the_real_near_coplanar_survey_is_marginal_no_raised_anchor() {

val (pos, elev, root) = scene(listOf(

"A1" to Vector3D(0.0, 0.0, 1.2318),

"A2" to Vector3D(2.286, 0.0, 1.3843),

"A3" to Vector3D(0.0, 3.6957, 1.4478),

"A4" to Vector3D(2.286, 2.921, 0.889),

))

val r = ConstellationGeometryReport.evaluate(pos, edgeCount = 6, elevations = elev, rootId = root)

assertTrue(r.rigid, "4 anchors + all 6 edges is rigid")

assertFalse(r.hasRaisedAnchor, "similar heights ⇒ no clearly-raised anchor")

assertEquals(GeometryQuality.MARGINAL, r.quality, "solvable but orientation is slow/tilted")

assertTrue(r.issues.any { it.contains("Raise one anchor") }, "guidance tells the operator to raise an anchor")

}

@Test

fun five_anchors_with_too_few_edges_is_degenerate_under_constrained() {

// C(5,2)=10 possible; a rigid 3-D 5-node graph needs 3*5-6 = 9. Only 6 present → under-constrained.

val (pos, elev, root) = scene(listOf(

"A1" to Vector3D(0.0, 0.0, 1.2),

"A2" to Vector3D(2.3, 0.0, 1.3),

"A3" to Vector3D(0.0, 3.7, 1.4),

"A4" to Vector3D(2.3, 2.9, 0.9),

"A5" to Vector3D(1.1, 1.5, 1.1),

))

val r = ConstellationGeometryReport.evaluate(pos, edgeCount = 6, elevations = elev, rootId = root)

assertEquals(9, r.edgesForRigidity)

assertFalse(r.rigid, "6 of 9 required edges ⇒ not rigid")

assertEquals(GeometryQuality.DEGENERATE, r.quality)

assertTrue(r.issues.any { it.contains("Under-constrained") && it.contains("6 of 9") },

"guidance names the missing-link deficit")

}

@Test

fun a_table_with_a_clearly_raised_anchor_is_good() {

// 3 floor anchors + 1 raised 0.5 m; root is a floor anchor. The raised one reads far highest.

val (pos, elev, root) = scene(listOf(

"A1" to Vector3D(0.0, 0.0, 0.0),

"A2" to Vector3D(1.5, 0.0, 0.0),

"A3" to Vector3D(0.0, 1.5, 0.0),

"A4" to Vector3D(0.7, 0.7, 0.5), // raised

))

val r = ConstellationGeometryReport.evaluate(pos, edgeCount = 6, elevations = elev, rootId = root)

assertTrue(r.rigid)

assertTrue(r.hasRaisedAnchor, "the 0.5 m anchor reads clearly highest ⇒ raised")

assertEquals(GeometryQuality.GOOD, r.quality)

assertTrue(r.issues.isEmpty(), "a good layout has no complaints; got ${r.issues}")

}

@Test

fun too_few_anchors_is_degenerate() {

val (pos, elev, root) = scene(listOf(

"A1" to Vector3D(0.0, 0.0, 0.0),

"A2" to Vector3D(1.0, 0.0, 0.2),

"A3" to Vector3D(0.0, 1.0, 0.1),

))

val r = ConstellationGeometryReport.evaluate(pos, edgeCount = 3, elevations = elev, rootId = root)

assertEquals(3, r.anchorCount)

assertFalse(r.rigid)

assertEquals(GeometryQuality.DEGENERATE, r.quality)

assertTrue(r.issues.any { it.contains("at least 4") })

}

@Test

fun two_anchors_nearly_collapsed_is_degenerate() {

val (pos, elev, root) = scene(listOf(

"A1" to Vector3D(0.0, 0.0, 0.0),

"A2" to Vector3D(1.5, 0.0, 0.0),

"A3" to Vector3D(0.0, 1.5, 0.0),

"A4" to Vector3D(0.02, 0.02, 0.03), // ~4 cm from A1 in all axes → collapsed

))

val r = ConstellationGeometryReport.evaluate(pos, edgeCount = 6, elevations = elev, rootId = root)

assertEquals(GeometryQuality.DEGENERATE, r.quality)

assertTrue(r.issues.any { it.contains("on top of each other") })

}

}