veloren/voxygen/src/ecs/sys/interpolation.rs

use crate::ecs::comp::Interpolated;
use common::{
    comp::{Ori, Pos, Vel},
    state::DeltaTime,
};
use log::warn;
use specs::{Entities, Join, Read, ReadStorage, System, WriteStorage};
use vek::*;

/// This system will allow NPCs to modify their controller
pub struct Sys;
impl<'a> System<'a> for Sys {
    type SystemData = (
        Entities<'a>,
        Read<'a, DeltaTime>,
        ReadStorage<'a, Pos>,
        ReadStorage<'a, Ori>,
        ReadStorage<'a, Vel>,
        WriteStorage<'a, Interpolated>,
    );

    fn run(
        &mut self,
        (entities, dt, positions, orientations, velocities, mut interpolated): Self::SystemData,
    ) {
        // Update interpolated positions and orientations
        for (pos, ori, i, vel) in (&positions, &orientations, &mut interpolated, &velocities).join()
        {
            // Update interpolation values
            if i.pos.distance_squared(pos.0) < 64.0 * 64.0 {
                i.pos = Lerp::lerp(i.pos, pos.0 + vel.0 * 0.03, 10.0 * dt.0);
                let ori_interp = Slerp::slerp(i.ori, ori.0, 5.0 * dt.0);
                // Check for NaNs
                // TODO: why are we getting NaNs here! Zero-length ori vectors?
                i.ori = if !ori_interp.map(|e| e.is_nan()).reduce_or() {
                    ori_interp
                } else {
                    ori.0
                };
            } else {
                i.pos = pos.0;
                i.ori = ori.0;
            }
        }
        // Insert interpolation components for entities which don't have them
        for (entity, pos, ori) in (&entities, &positions, &orientations, !&interpolated)
            .join()
            .map(|(e, p, o, _)| (e, p.0, o.0))
            .collect::<Vec<_>>()
        {
            interpolated
                .insert(entity, Interpolated { pos, ori })
                .err()
                .map(|err| warn!("Error inserting Interpolated component: {}", err));
        }
        // Remove Interpolated component from entities which don't have a position or an
        // orientation or a velocity
        for entity in (&entities, !&positions, &interpolated)
            .join()
            .map(|(e, _, _)| e)
            .collect::<Vec<_>>()
        {
            interpolated.remove(entity);
        }
        for entity in (&entities, !&orientations, &interpolated)
            .join()
            .map(|(e, _, _)| e)
            .collect::<Vec<_>>()
        {
            interpolated.remove(entity);
        }
        for entity in (&entities, !&velocities, &interpolated)
            .join()
            .map(|(e, _, _)| e)
            .collect::<Vec<_>>()
        {
            interpolated.remove(entity);
        }
    }
}