linear_sim/

system.rs

//! Simulation model

use std;

use log;
use rs_utils::for_sequence;
use vec_map::VecMap;
#[cfg(feature = "derive_serdes")]
use serde::{Deserialize, Serialize};

use crate::{
  constraint, event, force, geometry, integrator, math, object, Collision, Integrator
};
#[cfg(feature = "debug_dump")]
use crate::collision;
use crate::object::InternalId;

/// A system representing the computational model for the simulation.
///
/// There are three categories of entities that are found in the system (model):
///
/// - Objects -- `Static`, `Dynamic`, `Nodetect`
/// - Forces -- `Gravity`
/// - Constraints -- 'Planar', 'Penetration' (TODO)
#[cfg_attr(feature = "derive_serdes", derive(Deserialize, Serialize))]
#[derive(Clone, Debug, Default)]
pub struct System <INTG : Integrator> {
  /// Step counter
  step               : u64,
  /// No integration
  objects_static     : VecMap <object::Static>,
  /// Integration and collision
  objects_dynamic    : VecMap <object::Dynamic>,
  /// Integration only; no collision detection or response
  objects_nodetect   : VecMap <object::Nodetect>,
  /// Global gravity force
  gravity            : Option <force::Gravity>,
  /// Collision subsystem
  collision          : Collision,
  /// Constraint islands
  constraint_manager : constraint::Manager,
  #[cfg_attr(feature = "derive_serdes", serde(skip))]
  _phantom_data      : std::marker::PhantomData <INTG>
}

impl <INTG : Integrator> System <INTG> {
  #[inline]
  pub fn new() -> Self where INTG : Default {
    Default::default()
  }
  #[inline]
  pub const fn step (&self) -> u64 {
    self.step
  }
  #[inline]
  pub const fn objects_dynamic (&self) -> &VecMap <object::Dynamic> {
    &self.objects_dynamic
  }
  #[inline]
  pub const fn objects_nodetect (&self) -> &VecMap <object::Nodetect> {
    &self.objects_nodetect
  }
  #[inline]
  pub const fn objects_static (&self) -> &VecMap <object::Static> {
    &self.objects_static
  }

  #[expect(mismatched_lifetime_syntaxes)]
  pub fn get_object (&self, id : object::Id) -> object::VariantRef {
    match id.kind {
      object::Kind::Static   => (&self.objects_static[id.key.index()]).into(),
      object::Kind::Dynamic  => (&self.objects_dynamic[id.key.index()]).into(),
      object::Kind::Nodetect => (&self.objects_nodetect[id.key.index()]).into()
    }
  }

  pub fn segment_query (&self,
    segment     : geometry::Segment3 <f64>,
    ignore_list : &[object::Id]
  ) -> Vec <(math::Normalized <f64>, math::Point3 <f64>, object::Id)> {
    use object::Bounded;
    let mut out = Vec::<(math::Normalized <f64>, math::Point3 <f64>, object::Id)>::new();
    let aabb = segment.aabb3();
    let mut overlaps : Vec <object::Id> = self.collision.broad()
      .overlaps_discrete (aabb).into_iter().map (object::Id::from).collect();
    overlaps.retain (|id| !ignore_list.contains (id));
    for object_id in overlaps {
      if let Some ((frac, point)) = match object_id.kind {
        object::Kind::Static  =>
          self.objects_static[object_id.key.index()].hit_test (segment),
        object::Kind::Dynamic =>
          self.objects_dynamic[object_id.key.index()].hit_test (segment),
        object::Kind::Nodetect => unreachable!()
      } {
        out.insert (
          out.binary_search_by (|(f, _, _)| f.partial_cmp (&frac).unwrap())
            .unwrap_or_else (|i| i),
          (frac, point, object_id)
        );
      }
    }
    out
  }

  #[expect(clippy::doc_overindented_list_items)]
  /// Input event handling.
  ///
  /// - `CreateObject` -- if an object is found to be in *non-colliding* contact
  ///    with any other objects, those contacts will be registered with the
  ///    collision system as *persistent contacts* for the upcoming step
  /// - `DestroyObject`
  /// - `SetGravity`
  /// - `ClearGravity`
  /// - `Step` -- advance the simulation by a single *timestep*:
  ///     1. Derivative evaluation: forces are accumulated and acceleration computed
  ///     2. Velocities are integrated
  ///     3. Velocity constraints are solved
  ///     4. Positions are integrated using the new velocity (semi-implicit Euler)
  ///     5. Position constraints are solved
  ///     6. Collision detects and resolves collisions in the normalized sub-timestep
  ///        [0.0-1.0).
  ///
  ///        Note that colliding contacts detected at t==1.0 will *not* produce
  ///        a collision response event (the next step will detect and resolve this
  ///        collision), however resting or separating contacts detected
  ///        at t==1.0 will be registered as a *persistent contact* for the next
  ///        simulation step.
  pub fn handle_event (&mut self, input : event::Input) -> Vec <event::Output> {
    let input_step = self.step;
    log::debug!(step=input_step, input=input.to_string().as_str();
      "input event");
    let mut output = Vec::new();
    match input {

      //
      // event::Input::CreateObject
      //
      event::Input::CreateObject (object, key) => {
        let key = key.unwrap_or_else (
          || self.new_object_key (object.kind()).unwrap());
        match object {
          object::Variant::Static (object) => {
            self.create_object_static (object, key, &mut output);
          }
          object::Variant::Dynamic (object) => {
            self.create_object_dynamic (object, key, &mut output);
          }
          object::Variant::Nodetect (object) => {
            self.create_object_nodetect (object, key);
          }
        }
      }

      //
      // event::Input::ModifyObject
      //
      event::Input::ModifyObject (object_modify_event) => {
        match object_modify_event {
          event::ObjectModify::Dynamic (key, event) => {
            self.modify_object_dynamic (key, event);
          }
          event::ObjectModify::Static  (key, event) => {
            self.modify_object_static (key, event);
          }
        }
      }

      //
      // event::Input::DestroyObject
      //
      event::Input::DestroyObject (object::Id { kind, key }) => {
        match kind {
          object::Kind::Static => {
            self.collision.remove_object_static (key);
            let id = InternalId::new_static (key);
            assert!(self.constraint_manager.remove_object (id));
            assert!(self.objects_static.remove (key.index()).is_some());
          }
          object::Kind::Dynamic => {
            self.collision.remove_object_dynamic (key);
            assert!(self.objects_dynamic.remove (key.index()).is_some());
          }
          object::Kind::Nodetect =>
            assert!(self.objects_nodetect.remove (key.index()).is_some())
        }
      }

      //
      // event::Input::SetGravity
      //
      event::Input::SetGravity (gravity) => {
        assert!(self.gravity.is_none());
        self.gravity = Some (gravity);
      }

      //
      // event::Input::ClearGravity
      //
      event::Input::ClearGravity => {
        assert!(self.gravity.is_some());
        self.gravity = None;
      }

      //
      // event::Input::Step
      //
      event::Input::Step => {
        #[cfg(feature = "debug_dump")]
        let s = self.clone();

        // 1. derivative evaluation (i.e., accumulate forces and compute
        //    accelerations)
        self.derivative_evaluation();
        // 2. integrate velocity
        self.integrate_velocity();
        // 3. constrain velocities
        self.constrain_velocity();
        // 4. integrate position
        self.integrate_position();
        // 5. constrain position
        self.constrain_position();
        // 6. detect and solve collisions in a loop
        self.collision (&mut output);

        #[cfg(feature = "debug_dump")]
        if collision::DEBUG_DUMP.swap (false, std::sync::atomic::Ordering::SeqCst)
          && std::env::var ("LINEAR_SIM_DEBUG_DUMP").is_ok()
        {
          let p = format!("linear-sim-{}.dump", s.step);
          log::info!(filepath=p.as_str(); "dumping system");
          let mut f = std::fs::File::create (&p).unwrap();
          bincode::serde::encode_into_std_write (&s, &mut f, bincode::config::standard())
            .unwrap();
        }

        self.step += 1;
      }

    }
    log::debug!(
      step=input_step,
      output:?=output.iter().map (ToString::to_string).collect::<Vec<_>>();
      "output events");
    output
  }

  /// Returns `None` if object container is full
  pub fn new_object_key (&self, kind : object::Kind) -> Option <object::Key> {
    fn next_available_key <T> (map : &VecMap <T>) -> Option <object::Key> {
      for i in 0..object::KEY_MAX as usize {
        if !map.contains_key (i) {
          return Some (i.into())
        }
      }
      None
    }
    match kind {
      object::Kind::Static   => next_available_key (&self.objects_static),
      object::Kind::Dynamic  => next_available_key (&self.objects_dynamic),
      object::Kind::Nodetect => next_available_key (&self.objects_nodetect)
    }
  }

  /// Returns constraints partitioned by constraint islands
  pub fn constraints (&self)
    -> Vec <Vec <(object::Id, object::Id, constraint::Variant)>>
  {
    let mut islands = vec![];
    for (_, island) in self.constraint_manager.islands.as_ref().unwrap().iter() {
      let mut v = vec![];
      for (object_pair, constraint) in island.constraints.iter().cloned() {
        let (object_id_a, object_id_b) = object_pair.into();
        v.push ((object_id_a.into(), object_id_b.into(), constraint));
      }
      islands.push (v);
    }
    islands
  }

  pub fn add_constraint (&mut self,
    object_id_a : object::Id, object_id_b : object::Id, constraint : constraint::Variant
  ) {
    self.constraint_manager
      .add_constraint ((object_id_a, object_id_b).try_into().unwrap(), constraint)
  }

  //
  //  private
  //

  fn create_object_static (&mut self,
    object : object::Static,
    key    : object::Key,
    output : &mut Vec <event::Output>
  ) {
    let object_id = object::Id { kind: object::Kind::Static, key };
    // try to add to collision system: detects intersections in case of failure
    match self.collision.try_add_object_static (
      &self.objects_dynamic, &object, key
    ) {
      Ok  (()) => {
        // object successfully added
        assert!(self.objects_static.insert (key.index(), object).is_none());
        output.push (event::CreateObjectResult::Created (object_id).into());
      }
      Err (intersections) => {
        debug_assert!(!intersections.is_empty());
        output.push (
          event::CreateObjectResult::Intersection (intersections).into());
      }
    }
  }

  fn create_object_dynamic (&mut self,
    object : object::Dynamic,
    key    : object::Key,
    output : &mut Vec <event::Output>
  ) {
    let object_id = object::Id { kind: object::Kind::Dynamic, key };
    // try to add to collision system: detects intersections in case of failure
    match self.collision.try_add_object_dynamic (
      &mut self.constraint_manager.pseudo_velocities,
      &self.objects_static, &self.objects_dynamic, &object, key
    ) {
      Ok  (()) => {
        // object successfully added
        assert!(self.objects_dynamic.insert (key.index(), object).is_none());
        output.push (event::CreateObjectResult::Created (object_id).into());
      }
      Err (intersections) => {
        debug_assert!(!intersections.is_empty());
        output.push (
          event::CreateObjectResult::Intersection (intersections).into());
      }
    }
  }

  fn create_object_nodetect (&mut self,
    object : object::Nodetect,
    key    : object::Key,
  ) {
    assert!(self.objects_nodetect.insert (key.index(), object).is_none());
  }

  /// Panics if object with the given key is not present
  fn modify_object_dynamic (&mut self,
    key   : object::Key,
    event : event::ObjectModifyDynamic
  ) {
    let object = &mut self.objects_dynamic[key.index()];
    match event {
      event::ObjectModifyDynamic::ApplyImpulse (impulse) =>
        object.derivatives.velocity += impulse,
      event::ObjectModifyDynamic::SetForceFlags (force_flags) =>
        object.derivatives.force_flags = force_flags,
      event::ObjectModifyDynamic::SetDrag (drag) => object.drag.0 = drag,
      event::ObjectModifyDynamic::SetPosition (position) => {
        object.position = position;
        self.collision.update_object_dynamic (object, key);
        log::warn!("TODO: dynamic object set position results");
      }
    }
  }

  /// Panics if object with the given key is not present
  fn modify_object_static (&mut self,
    key   : object::Key,
    event : event::ObjectModifyStatic
  ) {
    let object = &mut self.objects_static[key.index()];
    match event {
      // TODO: has this been tested?
      event::ObjectModifyStatic::Move (move_vector) => {
        object.position.0 += move_vector;
        self.collision.update_object_static (object, key);
        unimplemented!("TODO: move static object results");
      }
    }
  }

  /// Computes accelerations from accumulated forces
  fn derivative_evaluation (&mut self) {
    // clear forces
    for_sequence!{
      (_, object) in (
        self.objects_dynamic.iter_mut(), self.objects_nodetect.iter_mut()
      ) {
        object.derivatives.force = [0.0, 0.0, 0.0].into();
      }
    }

    // accumulate forces
    if let Some (gravity) = &self.gravity {
      for_sequence!{
        (_, object) in (
          self.objects_dynamic.iter_mut(), self.objects_nodetect.iter_mut()
        ) {
          if object.derivatives.force_flags.contains (force::Flag::Gravity) {
            use crate::Force;
            let impulse = gravity.impulse (object, self.step, 1.0);
            object.derivatives.force += impulse;
          }
        }
      }
    }

    // compute accelerations from accumulated forces
    for_sequence!{
      (_, object) in (
        self.objects_dynamic.iter_mut(), self.objects_nodetect.iter_mut()
      ) {
        use object::Inertial;
        object.compute_acceleration_inplace();
      }
    }
  }

  /// Integrates velocity from acceleration
  fn integrate_velocity (&mut self) {
    for_sequence!{
      (_, object) in (
        self.objects_dynamic.iter_mut(), self.objects_nodetect.iter_mut()
      ) {
        INTG::integrate_velocity (object);
        // TODO: we are applying drag here; this is not a physically accurate
        // drag force, but simply scales the velocity by a fixed factor; is this
        // the correct place to apply drag?
        // apply drag
        object.derivatives.velocity *= 1.0 - object.drag.0;
      }
    }
  }

  /// Solve velocity constraints
  #[inline]
  fn constrain_velocity (&mut self) {
    self.constraint_manager
      .constrain_velocities (&self.objects_static, &mut self.objects_dynamic);
  }

  /// Integrates position from velocity
  fn integrate_position (&mut self) {
    for_sequence!{
      (_, object) in (
        self.objects_dynamic.iter_mut(), self.objects_nodetect.iter_mut()
      ) {
        INTG::integrate_position (object);
      }
    }
  }

  /// Solve position constraints
  #[inline]
  fn constrain_position (&mut self) {
    self.constraint_manager
      .constrain_positions (&self.objects_static, &mut self.objects_dynamic);
  }

  /// Collision detection and response
  fn collision (&mut self, output : &mut Vec <event::Output>) {
    self.collision.detect_resolve_loop (
      &mut self.constraint_manager, &self.objects_static, &mut self.objects_dynamic,
      self.step, output);
  }

}

/// Print system size information
pub fn report_sizes() {
  use std::mem::size_of;
  println!("system report sizes...");

  println!("  size of System <integrator::SemiImplicitEuler>: {}",
    size_of::<System <integrator::SemiImplicitEuler>>());

  println!("...system report sizes");
}

#[cfg(test)]
mod tests {
  use super::*;
  use crate::component;
  use crate::geometry::{self, shape};
  #[test]
  fn segment_query() {
    let segment = geometry::Segment3::noisy (
      [-1.0, 0.0, 0.5].into(),
      [ 2.0, 0.0, 0.5].into());
    let mut system = System::<integrator::SemiImplicitEuler>::new();
    let sphere1 = {
      let position    = component::Position ([0.5, 0.0, 0.5].into());
      let mass        = component::Mass::new (20.0);
      let derivatives = component::Derivatives::zero();
      let drag        = component::Drag::zero();
      let bound       = component::Bound::from (shape::Sphere::noisy (0.5));
      let material    = component::MATERIAL_STONE;
      let collidable  = true;
      object::Dynamic {
        position, mass, derivatives, drag, bound, material, collidable
      }.into()
    };
    let mut result = system.handle_event (event::Input::CreateObject (sphere1, None));
    let event::Output::CreateObjectResult (
      event::CreateObjectResult::Created (sphere1_id)
    ) = result.pop().unwrap() else { unreachable!() };
    assert!(result.pop().is_none());
    let block1 = {
      let position   = component::Position ([1.5, 0.0, 0.5].into());
      let bound      = component::Bound    (
        shape::Bounded::from (shape::Cuboid::noisy ([0.5, 0.5, 0.5])).into());
      let material   = component::MATERIAL_STONE;
      let collidable = true;
      object::Static { position, bound, material, collidable }.into()
    };
    let mut result = system.handle_event (event::Input::CreateObject (block1, None));
    let event::Output::CreateObjectResult (
      event::CreateObjectResult::Created (block1_id)
    ) = result.pop().unwrap() else { unreachable!() };
    assert!(result.pop().is_none());
    let result = system.segment_query (segment, &[]);
    let (frac, point, id) = result.first().unwrap();
    assert_eq!(**frac, 1.0/3.0);
    assert_eq!(*point, [0.0, 0.0, 0.5].into());
    assert_eq!(*id, sphere1_id);
    let (frac, point, id) = result.get (1).unwrap();
    assert_eq!(**frac, 2.0/3.0);
    assert_eq!(*point, [1.0, 0.0, 0.5].into());
    assert_eq!(*id, block1_id);
  }
}