alice-physics 3.0.1

//! Adaptive SDF Resolution
//!
//! Evaluates SDF at variable resolution based on collision proximity.
//! Near-collision regions get high-res evaluation; distant regions use cached coarse values.
//!
//! # Strategy
//!
//! - BVH broad-phase determines which bodies are near SDF surfaces
//! - Bodies near SDF → full-resolution evaluation
//! - Bodies far from SDF → cached distance (skip re-evaluation)
//! - Bodies very far → skip entirely
//!
//! Author: Moroya Sakamoto

use crate::math::Vec3Fix;
use crate::sdf_collider::SdfCollider;

#[cfg(not(feature = "std"))]
use alloc::vec::Vec;

// ============================================================================
// Adaptive SDF Configuration
// ============================================================================

/// Adaptive evaluation configuration
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct AdaptiveConfig {
    /// Distance threshold for high-resolution evaluation
    pub high_res_threshold: f32,
    /// Distance threshold for cached evaluation (beyond = skip)
    pub cache_threshold: f32,
    /// Maximum age (frames) before cache entry expires
    pub cache_max_age: u32,
    /// Reserved: **not read**. The high-res normal refinement uses a fixed
    /// six-sample central difference (step 0.0005); this field is kept for
    /// compatibility and has no effect.
    pub refinement_samples: usize,
}

impl Default for AdaptiveConfig {
    fn default() -> Self {
        Self {
            high_res_threshold: 2.0,
            cache_threshold: 10.0,
            cache_max_age: 8,
            refinement_samples: 4,
        }
    }
}

// ============================================================================
// Cached SDF Entry
// ============================================================================

/// Cached SDF evaluation result
#[derive(Clone, Copy, Debug, PartialEq)]
struct CacheEntry {
    /// Cached distance value
    distance: f32,
    /// Cached normal (world space)
    normal: (f32, f32, f32),
    /// Position where this was evaluated
    position: Vec3Fix,
    /// Frame when this was last updated
    last_update: u32,
    /// Whether this entry is valid
    valid: bool,
}

impl Default for CacheEntry {
    fn default() -> Self {
        Self {
            distance: f32::MAX,
            normal: (0.0, 1.0, 0.0),
            position: Vec3Fix::ZERO,
            last_update: 0,
            valid: false,
        }
    }
}

// ============================================================================
// Evaluation Level
// ============================================================================

/// Resolution level for SDF evaluation
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum EvalLevel {
    /// Full evaluation with refinement (near collision)
    HighRes,
    /// Single evaluation (moderate distance)
    Standard,
    /// Use cached value (far from surface)
    Cached,
    /// Skip evaluation (very far)
    Skip,
}

// ============================================================================
// Adaptive SDF Evaluator
// ============================================================================

/// Adaptive SDF evaluation with distance-based LOD
///
/// The cache is kept per body index, not per SDF: evaluating one body against
/// several colliders in the same frame returns the first collider's cached
/// value for the others (use one evaluator per collider, or `invalidate`).
pub struct AdaptiveSdfEvaluator {
    /// Per-body cache entries
    cache: Vec<CacheEntry>,
    /// Current frame counter
    frame: u32,
    /// Configuration
    pub config: AdaptiveConfig,
    /// Statistics: evaluations saved this frame
    pub stats_saved: usize,
    /// Statistics: total evaluations this frame
    pub stats_total: usize,
}

impl AdaptiveSdfEvaluator {
    /// Create evaluator for N bodies
    #[must_use]
    pub fn new(num_bodies: usize, config: AdaptiveConfig) -> Self {
        Self {
            cache: vec![CacheEntry::default(); num_bodies],
            frame: 0,
            config,
            stats_saved: 0,
            stats_total: 0,
        }
    }

    /// Begin a new frame
    pub fn begin_frame(&mut self) {
        self.frame += 1;
        self.stats_saved = 0;
        self.stats_total = 0;
    }

    /// Resize cache for new body count
    pub fn resize(&mut self, num_bodies: usize) {
        self.cache.resize(num_bodies, CacheEntry::default());
    }

    /// Determine evaluation level for a body
    fn determine_level(&self, body_idx: usize, position: Vec3Fix) -> EvalLevel {
        if body_idx >= self.cache.len() {
            return EvalLevel::Standard;
        }

        let entry = &self.cache[body_idx];
        if !entry.valid {
            return EvalLevel::Standard;
        }

        let age = self.frame.saturating_sub(entry.last_update);
        if age > self.config.cache_max_age {
            return EvalLevel::Standard;
        }

        // Check how far the body has moved since last evaluation
        let movement = (position - entry.position).length().to_f32();

        if entry.distance > self.config.cache_threshold && movement < self.config.high_res_threshold
        {
            return EvalLevel::Skip;
        }

        if entry.distance > self.config.high_res_threshold && movement < entry.distance * 0.5 {
            return EvalLevel::Cached;
        }

        if entry.distance < self.config.high_res_threshold {
            return EvalLevel::HighRes;
        }

        EvalLevel::Standard
    }

    /// Evaluate SDF for a body with adaptive resolution
    ///
    /// Returns the distance in world metres and the unit normal in world space.
    #[cfg(feature = "std")]
    pub fn evaluate(
        &mut self,
        body_idx: usize,
        position: Vec3Fix,
        sdf: &SdfCollider,
    ) -> (f32, (f32, f32, f32)) {
        self.stats_total += 1;

        let level = self.determine_level(body_idx, position);

        match level {
            EvalLevel::Skip => {
                self.stats_saved += 1;
                if body_idx < self.cache.len() {
                    let entry = &self.cache[body_idx];
                    (entry.distance, entry.normal)
                } else {
                    (f32::MAX, (0.0, 1.0, 0.0))
                }
            }

            EvalLevel::Cached => {
                self.stats_saved += 1;
                if body_idx < self.cache.len() && self.cache[body_idx].valid {
                    let entry = &self.cache[body_idx];
                    (entry.distance, entry.normal)
                } else {
                    self.evaluate_and_cache(body_idx, position, sdf, false)
                }
            }

            EvalLevel::Standard => self.evaluate_and_cache(body_idx, position, sdf, false),

            EvalLevel::HighRes => self.evaluate_and_cache(body_idx, position, sdf, true),
        }
    }

    /// Evaluate SDF and update cache
    #[cfg(feature = "std")]
    fn evaluate_and_cache(
        &mut self,
        body_idx: usize,
        position: Vec3Fix,
        sdf: &SdfCollider,
        high_res: bool,
    ) -> (f32, (f32, f32, f32)) {
        let (lx, ly, lz) = sdf.world_to_local(position);
        let (dist, normal) = sdf.field.distance_and_normal(lx, ly, lz);
        let world_dist = dist * sdf.scale_f32;

        let final_normal = if high_res && world_dist.abs() < self.config.high_res_threshold {
            // Refined normal: use tighter finite differences
            let eps = 0.0005;
            let dx = sdf.field.distance(lx + eps, ly, lz) - sdf.field.distance(lx - eps, ly, lz);
            let dy = sdf.field.distance(lx, ly + eps, lz) - sdf.field.distance(lx, ly - eps, lz);
            let dz = sdf.field.distance(lx, ly, lz + eps) - sdf.field.distance(lx, ly, lz - eps);
            let len = (dx * dx + dy * dy + dz * dz).sqrt();
            if len > 1e-10 {
                (dx / len, dy / len, dz / len)
            } else {
                normal
            }
        } else {
            normal
        };

        // The field reports a local-space normal; callers work in world space.
        // An unrotated collider needs no transform (and keeps its value bit
        // for bit), a rotated one needs the same rotation `sphere_trace_sdf`
        // and the contact queries apply.
        let final_normal = if sdf.rotation == crate::math::QuatFix::IDENTITY {
            final_normal
        } else {
            sdf.local_normal_to_world(final_normal.0, final_normal.1, final_normal.2)
                .to_f32()
        };

        // Update cache
        if body_idx < self.cache.len() {
            self.cache[body_idx] = CacheEntry {
                distance: world_dist,
                normal: final_normal,
                position,
                last_update: self.frame,
                valid: true,
            };
        }

        (world_dist, final_normal)
    }

    /// Get evaluation statistics as (saved, total) counts
    #[must_use]
    pub const fn stats(&self) -> (usize, usize) {
        (self.stats_saved, self.stats_total)
    }

    /// Invalidate cache for a specific body (e.g., after teleport)
    pub fn invalidate(&mut self, body_idx: usize) {
        if body_idx < self.cache.len() {
            self.cache[body_idx].valid = false;
        }
    }

    /// Invalidate all cache entries
    pub fn invalidate_all(&mut self) {
        for entry in &mut self.cache {
            entry.valid = false;
        }
    }
}

// ============================================================================
// Tests
// ============================================================================

#[cfg(test)]
mod tests {
    use super::*;
    use crate::math::QuatFix;
    use crate::sdf_collider::{ClosureSdf, SdfCollider};

    fn unit_sphere() -> ClosureSdf {
        ClosureSdf::new(
            |x, y, z| z.mul_add(z, x.mul_add(x, y * y)).sqrt() - 1.0,
            |x, y, z| {
                let len = z.mul_add(z, x.mul_add(x, y * y)).sqrt();
                if len < 1e-10 {
                    (0.0, 1.0, 0.0)
                } else {
                    (x / len, y / len, z / len)
                }
            },
        )
    }

    #[test]
    fn test_adaptive_evaluator() {
        let sdf =
            SdfCollider::new_static(Box::new(unit_sphere()), Vec3Fix::ZERO, QuatFix::IDENTITY);

        let mut evaluator = AdaptiveSdfEvaluator::new(2, AdaptiveConfig::default());

        evaluator.begin_frame();
        let (dist, _) = evaluator.evaluate(0, Vec3Fix::from_f32(5.0, 0.0, 0.0), &sdf);
        assert!(dist > 0.0, "Should be outside sphere");

        // Second evaluation should potentially use cache
        evaluator.begin_frame();
        let (dist2, _) = evaluator.evaluate(0, Vec3Fix::from_f32(5.0, 0.0, 0.0), &sdf);
        assert!((dist2 - dist).abs() < 0.1, "Cached value should be close");
    }

    #[test]
    fn test_eval_levels() {
        let evaluator = AdaptiveSdfEvaluator::new(1, AdaptiveConfig::default());
        // Fresh cache should return Standard
        let level = evaluator.determine_level(0, Vec3Fix::ZERO);
        assert_eq!(level, EvalLevel::Standard);
    }

    #[test]
    fn test_invalidation() {
        let sdf =
            SdfCollider::new_static(Box::new(unit_sphere()), Vec3Fix::ZERO, QuatFix::IDENTITY);

        let mut evaluator = AdaptiveSdfEvaluator::new(1, AdaptiveConfig::default());
        evaluator.begin_frame();
        evaluator.evaluate(0, Vec3Fix::from_f32(5.0, 0.0, 0.0), &sdf);

        evaluator.invalidate(0);
        let level = evaluator.determine_level(0, Vec3Fix::from_f32(5.0, 0.0, 0.0));
        assert_eq!(
            level,
            EvalLevel::Standard,
            "After invalidation should use Standard"
        );
    }

    #[cfg(feature = "std")]
    #[test]
    fn invalidate_all_forces_fresh_evaluation_for_every_body() {
        let sdf =
            SdfCollider::new_static(Box::new(unit_sphere()), Vec3Fix::ZERO, QuatFix::IDENTITY);
        // 同じ球を +X に 1 ずらした SDF: 位置 (5,0,0) の距離は 4 → 3 に変わる
        let shifted = SdfCollider::new_static(
            Box::new(unit_sphere()),
            Vec3Fix::from_int(1, 0, 0),
            QuatFix::IDENTITY,
        );
        let positions = [
            Vec3Fix::from_f32(5.0, 0.0, 0.0),
            Vec3Fix::from_f32(0.0, 5.0, 0.0),
            Vec3Fix::from_f32(0.0, 0.0, 5.0),
        ];

        let mut evaluator = AdaptiveSdfEvaluator::new(3, AdaptiveConfig::default());
        evaluator.begin_frame();
        for (i, p) in positions.iter().enumerate() {
            let (d, _) = evaluator.evaluate(i, *p, &sdf);
            assert!((d - 4.0).abs() < 1e-5, "body {i} dist {d}");
        }
        // 全 body が cache 済 (距離 4 > high_res 2、移動 0) → 次 frame は Cached
        evaluator.begin_frame();
        for (i, p) in positions.iter().enumerate() {
            assert_eq!(
                evaluator.determine_level(i, *p),
                EvalLevel::Cached,
                "body {i}"
            );
        }
        // 対照: invalidate しないと shifted SDF に対しても stale な 4 が返る
        let (stale, _) = evaluator.evaluate(0, positions[0], &shifted);
        assert!((stale - 4.0).abs() < 1e-5, "stale cached dist {stale}");
        assert_eq!(evaluator.stats(), (1, 1));

        evaluator.invalidate_all();
        for (i, p) in positions.iter().enumerate() {
            assert_eq!(
                evaluator.determine_level(i, *p),
                EvalLevel::Standard,
                "body {i}"
            );
        }
        // 全 body が再評価され、shifted SDF の値が返る (saved は増えない)
        evaluator.begin_frame();
        let expected = [3.0_f32, (26.0_f32).sqrt() - 1.0, (26.0_f32).sqrt() - 1.0];
        for (i, p) in positions.iter().enumerate() {
            let (d, _) = evaluator.evaluate(i, *p, &shifted);
            assert!(
                (d - expected[i]).abs() < 1e-4,
                "body {i} dist {d}, want {}",
                expected[i]
            );
        }
        assert_eq!(evaluator.stats(), (0, 3));
        // 再評価後は再び valid (Cached に戻る)
        evaluator.begin_frame();
        for (i, p) in positions.iter().enumerate() {
            assert_eq!(
                evaluator.determine_level(i, *p),
                EvalLevel::Cached,
                "body {i}"
            );
        }
        // 空 evaluator でも panic しない
        let mut empty = AdaptiveSdfEvaluator::new(0, AdaptiveConfig::default());
        empty.invalidate_all();
        assert_eq!(empty.determine_level(0, Vec3Fix::ZERO), EvalLevel::Standard);
    }
}