feat: developer mode, SWEEP placement, gradient guide, spot flicker fix
- Developer mode: 캐릭터 3탭 토글, dev-only settings/graphs/logs - SWEEP placement (CKLaw): BladderSphereSeek circle fit + RTracker + argmax + coronal lateral + R-peak gate GREEN lock - Gradient placement: weighted center gradient 기반 상하 판정, 방향 로직 수정 (ch0=위, ch3=아래), 초기 멘트 "치골 위에 배치" - Spot 측정: isSpotInProgress 플래그로 중간값 화면 깜빡임 제거 - 좌우 애매한 ↔ 멘트 제거 → 구체적 방향 또는 상태 안내 Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
@@ -0,0 +1,72 @@
|
||||
package com.example.medilightv2android.managers
|
||||
|
||||
import kotlin.math.abs
|
||||
|
||||
/**
|
||||
* R-fit trend tracker — watches per-frame sphere-fit radius and classifies:
|
||||
* RISING — sliding toward bladder centre (R increasing)
|
||||
* PEAK — at / near session maximum R
|
||||
* FALLING — sliding past centre (R decreasing)
|
||||
* NOISE — fluctuating without clear trend
|
||||
* IDLE — no valid fit (too few channels)
|
||||
*
|
||||
* Ported from CKLaw BladderSeekRTracker.kt.
|
||||
*/
|
||||
class BladderSeekRTracker(
|
||||
private val historySize: Int = 8,
|
||||
private val peakToleranceMm: Double = 1.5,
|
||||
private val noiseDeltaMm: Double = 0.4,
|
||||
) {
|
||||
enum class Trend { RISING, PEAK, FALLING, NOISE, IDLE }
|
||||
|
||||
data class Status(
|
||||
val currentR: Double?,
|
||||
val bestR: Double,
|
||||
val trend: Trend,
|
||||
val improvementMm: Double,
|
||||
val hintEn: String,
|
||||
)
|
||||
|
||||
private val history = ArrayDeque<Double>()
|
||||
var bestR: Double = 0.0
|
||||
private set
|
||||
|
||||
fun reset() {
|
||||
history.clear()
|
||||
bestR = 0.0
|
||||
}
|
||||
|
||||
fun submit(r: Double?): Status {
|
||||
if (r == null || r.isNaN() || r <= 0.0) {
|
||||
return Status(null, bestR, Trend.IDLE, 0.0,
|
||||
"Not enough channels — check signal")
|
||||
}
|
||||
history.addLast(r)
|
||||
while (history.size > historySize) history.removeFirst()
|
||||
if (r > bestR) bestR = r
|
||||
|
||||
val trend = computeTrend(r)
|
||||
val gap = r - bestR
|
||||
val en = when (trend) {
|
||||
Trend.RISING -> "Getting closer (R=${"%.0f".format(r)}mm)"
|
||||
Trend.PEAK -> "At peak (R=${"%.0f".format(r)}mm)"
|
||||
Trend.FALLING -> "Past peak — reverse (best ${"%.0f".format(bestR)}mm)"
|
||||
Trend.NOISE -> "Searching (R=${"%.0f".format(r)}mm)"
|
||||
Trend.IDLE -> ""
|
||||
}
|
||||
return Status(r, bestR, trend, gap, en)
|
||||
}
|
||||
|
||||
private fun computeTrend(currentR: Double): Trend {
|
||||
if (history.size < 3) return Trend.NOISE
|
||||
if (bestR - currentR < peakToleranceMm) return Trend.PEAK
|
||||
val prev = history.toList().dropLast(1).takeLast(3)
|
||||
val prevMean = prev.average()
|
||||
val delta = currentR - prevMean
|
||||
return when {
|
||||
abs(delta) < noiseDeltaMm -> Trend.NOISE
|
||||
delta > 0 -> Trend.RISING
|
||||
else -> Trend.FALLING
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,190 @@
|
||||
package com.example.medilightv2android.managers
|
||||
|
||||
import kotlin.math.cos
|
||||
import kotlin.math.sin
|
||||
import kotlin.math.sqrt
|
||||
|
||||
/**
|
||||
* Sagittal-plane circle fit + coronal lateral offset + argmax.
|
||||
*
|
||||
* Ported from CKLaw BladderSphereSeek.kt, simplified to use PiezoHW
|
||||
* geometry directly (no SphereFit2Step / Geometry dependencies).
|
||||
*
|
||||
* Ch0..Ch3 wall points are projected to the YZ plane (sagittal slice)
|
||||
* and fitted with a 2D algebraic circle (Kasa method). The fit radius
|
||||
* serves as real-time alignment quality — maximised when probe is
|
||||
* centred over the bladder.
|
||||
*/
|
||||
object BladderSphereSeek {
|
||||
|
||||
data class FitResult(
|
||||
val radiusMm: Double,
|
||||
val centerY: Double, // along body axis (cranial+, caudal-)
|
||||
val centerZ: Double, // depth from probe surface
|
||||
val residualStdMm: Double,
|
||||
val nPoints: Int, // 3..8 wall points
|
||||
val confidence: Float, // 0..1
|
||||
)
|
||||
|
||||
data class CoronalResult(
|
||||
val lateralOffsetMm: Double,
|
||||
val nChannels: Int,
|
||||
val ch4MidXMm: Double?,
|
||||
val ch5MidXMm: Double?,
|
||||
val confidence: Float,
|
||||
)
|
||||
|
||||
/**
|
||||
* Convert wall sample index to YZ coordinates (mm) for a given channel.
|
||||
* Y = sensor_z + depth * sin(theta) (along body, cranial+)
|
||||
* Z = depth * cos(theta) (perpendicular depth into body)
|
||||
*/
|
||||
private fun wallIdxToYZ(ch: Int, sampleIdx: Int): DoubleArray {
|
||||
val hw = PiezoHW
|
||||
val depth = sampleIdx * hw.distancePerSample + hw.delayOffsetMm
|
||||
val theta = hw.degreeAll[ch] * Math.PI / 180.0
|
||||
val y = hw.sensorZMmAll[ch] + depth * sin(theta)
|
||||
val z = depth * cos(theta)
|
||||
return doubleArrayOf(y, z)
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert wall sample index to X coordinate (mm) for lateral channels.
|
||||
* X = sensor_x + depth * sin(theta_lr)
|
||||
*/
|
||||
private fun wallIdxToX(ch: Int, sampleIdx: Int): Double {
|
||||
val hw = PiezoHW
|
||||
val depth = sampleIdx * hw.distancePerSample + hw.delayOffsetMm
|
||||
val thetaLR = hw.degreeLRAll[ch] * Math.PI / 180.0
|
||||
return hw.sensorXMmAll[ch] + depth * sin(thetaLR)
|
||||
}
|
||||
|
||||
/**
|
||||
* 2D circle fit (Kasa algebraic method) on YZ wall points from Ch0..Ch3.
|
||||
*
|
||||
* @param walls list of 6 Pair(antIdx, postIdx)?, null for undetected channels
|
||||
* @param detected list of 6 booleans
|
||||
*/
|
||||
fun fitSagittalCircle(
|
||||
walls: List<Pair<Int, Int>?>,
|
||||
detected: List<Boolean>,
|
||||
): FitResult? {
|
||||
val pts = mutableListOf<DoubleArray>()
|
||||
for (ch in 0..3) {
|
||||
if (!detected.getOrElse(ch) { false }) continue
|
||||
val w = walls.getOrNull(ch) ?: continue
|
||||
pts.add(wallIdxToYZ(ch, w.first)) // ant
|
||||
pts.add(wallIdxToYZ(ch, w.second)) // post
|
||||
}
|
||||
if (pts.size < 3) return null
|
||||
|
||||
// Kasa circle fit: minimize algebraic distance
|
||||
// Solve [y^2+z^2, y, z, 1] * [1, -2cy, -2cz, cy^2+cz^2-r^2]^T = 0
|
||||
val n = pts.size
|
||||
var syy = 0.0; var sy = 0.0; var sz = 0.0
|
||||
var syz = 0.0; var szz = 0.0; var s1 = n.toDouble()
|
||||
var syyy = 0.0; var syzz = 0.0; var syyz = 0.0; var szzz = 0.0
|
||||
var sd = 0.0; var sdy = 0.0; var sdz = 0.0
|
||||
|
||||
for (p in pts) {
|
||||
val y = p[0]; val z = p[1]
|
||||
val d = y * y + z * z
|
||||
sy += y; sz += z; syy += y * y; szz += z * z; syz += y * z
|
||||
sd += d; sdy += d * y; sdz += d * z
|
||||
}
|
||||
|
||||
// Normal equations for Kasa fit
|
||||
val a11 = syy; val a12 = syz; val a13 = sy
|
||||
val a21 = syz; val a22 = szz; val a23 = sz
|
||||
val a31 = sy; val a32 = sz; val a33 = s1
|
||||
val b1 = sdy; val b2 = sdz; val b3 = sd
|
||||
|
||||
// Solve 3x3 system via Cramer's rule
|
||||
val det = a11 * (a22 * a33 - a23 * a32) -
|
||||
a12 * (a21 * a33 - a23 * a31) +
|
||||
a13 * (a21 * a32 - a22 * a31)
|
||||
if (kotlin.math.abs(det) < 1e-12) return null
|
||||
|
||||
val cy = (b1 * (a22 * a33 - a23 * a32) -
|
||||
a12 * (b2 * a33 - a23 * b3) +
|
||||
a13 * (b2 * a32 - a22 * b3)) / det / 2.0
|
||||
val cz = (a11 * (b2 * a33 - a23 * b3) -
|
||||
b1 * (a21 * a33 - a23 * a31) +
|
||||
a13 * (a21 * b3 - b2 * a31)) / det / 2.0
|
||||
val c3 = (a11 * (a22 * b3 - b2 * a32) -
|
||||
a12 * (a21 * b3 - b2 * a31) +
|
||||
b1 * (a21 * a32 - a22 * a31)) / det
|
||||
|
||||
val rSq = cy * cy + cz * cz + c3
|
||||
if (rSq <= 0) return null
|
||||
val r = sqrt(rSq)
|
||||
|
||||
// Residual: std of (|p - center| - r)
|
||||
var sumResidSq = 0.0
|
||||
for (p in pts) {
|
||||
val dist = sqrt((p[0] - cy) * (p[0] - cy) + (p[1] - cz) * (p[1] - cz))
|
||||
val resid = dist - r
|
||||
sumResidSq += resid * resid
|
||||
}
|
||||
val residStd = sqrt(sumResidSq / n)
|
||||
|
||||
// Sanity: anatomically possible range
|
||||
if (r < 15.0 || r > 120.0) return null
|
||||
if (residStd > 5.0) return null
|
||||
|
||||
val nFactor = ((n - 3).coerceAtLeast(0).toFloat() / 5f).coerceIn(0f, 1f)
|
||||
val residFactor = (1.0 - (residStd / 5.0).coerceIn(0.0, 1.0)).toFloat()
|
||||
val conf = (0.4f * nFactor + 0.6f * residFactor).coerceIn(0f, 1f)
|
||||
|
||||
return FitResult(r, cy, cz, residStd, n, conf)
|
||||
}
|
||||
|
||||
/**
|
||||
* Among Ch0..Ch3, returns the index of the channel with the longest chord.
|
||||
* 0 = centred over bladder, 1-3 = increasingly off-centre.
|
||||
* Returns null if any of Ch0..Ch3 is not detected.
|
||||
*/
|
||||
fun sagittalArgMaxCh(
|
||||
walls: List<Pair<Int, Int>?>,
|
||||
detected: List<Boolean>,
|
||||
): Int? {
|
||||
var bestCh = -1
|
||||
var bestLen = -1
|
||||
for (ch in 0..3) {
|
||||
if (!detected.getOrElse(ch) { false }) return null
|
||||
val w = walls.getOrNull(ch) ?: return null
|
||||
val len = w.second - w.first
|
||||
if (len > bestLen) { bestLen = len; bestCh = ch }
|
||||
}
|
||||
return if (bestCh >= 0) bestCh else null
|
||||
}
|
||||
|
||||
/**
|
||||
* Coronal lateral offset from Ch4/Ch5 chord midpoints.
|
||||
* Returns null if neither Ch4 nor Ch5 detected.
|
||||
*/
|
||||
fun computeCoronalLateral(
|
||||
walls: List<Pair<Int, Int>?>,
|
||||
detected: List<Boolean>,
|
||||
): CoronalResult? {
|
||||
var ch4Mid: Double? = null
|
||||
var ch5Mid: Double? = null
|
||||
for (ch in 4..5) {
|
||||
if (!detected.getOrElse(ch) { false }) continue
|
||||
val w = walls.getOrNull(ch) ?: continue
|
||||
val xAnt = wallIdxToX(ch, w.first)
|
||||
val xPost = wallIdxToX(ch, w.second)
|
||||
val midX = (xAnt + xPost) / 2.0
|
||||
if (ch == 4) ch4Mid = midX else ch5Mid = midX
|
||||
}
|
||||
val xs = listOfNotNull(ch4Mid, ch5Mid)
|
||||
if (xs.isEmpty()) return null
|
||||
return CoronalResult(
|
||||
lateralOffsetMm = xs.average(),
|
||||
nChannels = xs.size,
|
||||
ch4MidXMm = ch4Mid,
|
||||
ch5MidXMm = ch5Mid,
|
||||
confidence = if (xs.size == 2) 1.0f else 0.5f,
|
||||
)
|
||||
}
|
||||
}
|
||||
@@ -13,7 +13,7 @@ package com.example.medilightv2android.managers
|
||||
* 각 상수의 의미와 영향 범위를 아래 주석 참고.
|
||||
*/
|
||||
enum class DetectionMethod { METHOD_A, METHOD_B, METHOD_C }
|
||||
enum class PlacementGuideMode { SIMPLE, BOUNDARY }
|
||||
enum class PlacementGuideMode { SIMPLE, BOUNDARY, SWEEP }
|
||||
|
||||
object GreenZoneConstants {
|
||||
|
||||
|
||||
Reference in New Issue
Block a user