1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105

//! Simple force integration and impulse solver

use std::ops::Mul;

use cgmath::num_traits::Float;
use cgmath::{BaseFloat, EuclideanSpace, InnerSpace, Rotation, VectorSpace, Zero};

use super::{
    ApplyAngular, ForceAccumulator, Inertia, Mass, PhysicalEntity, Velocity, WorldParameters,
};
use {NextFrame, Pose};

/// Do force integration for next frame.
///
/// ### Parameters:
///
/// - `data`: Iterator over tuple with:
///     - Velocity for the next frame, will be updated
///     - Pose for the next frame, used to compute the inertia tensor for the body in the next frame
///     - Force accumulator, will be consumed and added to the velocity
///     - Mass, used by integration
///     - PhysicalEntity, used for gravity and damping calculation
/// - `world_params`: World physics parameters like gravity and global damping
/// - `dt`: Time step
///
/// ### Type parameters:
///
/// - `D`: Iterator type
/// - `P`: Point, usually `Point2` or `Point3`
/// - `A`: Angular velocity, usually `Scalar` or `Vector3`
/// - `I`: Inertia, usually `Scalar` or `Matrix3`
/// - `R`: Rotational quantity, usually `Basis2` or `Quaternion`
pub fn next_frame_integration<'a, T, D, P, A, I, R>(
    data: D,
    world_params: &WorldParameters<P::Diff, P::Scalar>,
    dt: P::Scalar,
) where
    D: Iterator<
        Item = (
            &'a mut NextFrame<Velocity<P::Diff, A>>,
            &'a NextFrame<T>,
            &'a mut ForceAccumulator<P::Diff, A>,
            &'a Mass<P::Scalar, I>,
            &'a PhysicalEntity<P::Scalar>,
        ),
    >,
    T: Pose<P, R> + 'a,
    P: EuclideanSpace + 'a,
    P::Scalar: BaseFloat,
    P::Diff: VectorSpace + InnerSpace + 'a,
    I: Inertia<Orientation = R> + Mul<A, Output = A> + 'a,
    A: Mul<P::Scalar, Output = A> + Clone + Copy + Zero + 'a,
    R: Rotation<P> + ApplyAngular<P::Scalar, A> + 'a,
{
    // Do force integration
    for (next_velocity, next_pose, force, mass, entity) in data.filter(|(_, _, _, _, e)| e.active())
    {
        let a = force.consume_force() * mass.inverse_mass()
            + world_params.gravity() * entity.gravity_scale();
        let new_velocity = *next_velocity.value.linear() + a * dt;
        let damp = world_params.entity_damping(entity.damping()).powf(dt);
        next_velocity.value.set_linear(new_velocity * damp);
        let a = mass.world_inverse_inertia(&next_pose.value.rotation()) * force.consume_torque();
        let new_velocity = *next_velocity.value.angular() + a * dt;
        next_velocity.value.set_angular(new_velocity);
    }
}

/// Compute next frame pose
///
/// ### Parameters:
///
/// - `data`: Iterator over tuple with:
///     - Velocity for the next frame, used to compute next frame pose
///     - Pose for the current frame, will be updated
///     - Pose for the next frame, will be updated
/// - `dt`: Time step
///
/// ### Type parameters:
///
/// - `D`: Iterator type
/// - `P`: Point, usually `Point2` or `Point3`
/// - `A`: Angular velocity, usually `Scalar` or `Vector3`
/// - `R`: Rotational quantity, usually `Basis2` or `Quaternion`
pub fn next_frame_pose<'a, B, D, P, A, R>(data: D, dt: P::Scalar)
where
    D: Iterator<
        Item = (
            &'a NextFrame<Velocity<P::Diff, A>>,
            &'a B,
            &'a mut NextFrame<B>,
            &'a PhysicalEntity<P::Scalar>,
        ),
    >,
    B: Pose<P, R> + 'a,
    P: EuclideanSpace + 'a,
    P::Scalar: BaseFloat,
    P::Diff: VectorSpace + InnerSpace + 'a,
    A: Mul<P::Scalar, Output = A> + Clone + Copy + Zero + 'a,
    R: Rotation<P> + ApplyAngular<P::Scalar, A> + 'a,
{
    for (next_velocity, pose, next_pose, _) in data.filter(|(_, _, _, e)| e.active()) {
        next_pose.value = next_velocity.value.apply(pose, dt)
    }
}