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
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134

use crate::dynamics::RigidBodySet;
use crate::geometry::{
    Collider, ColliderHandle, ColliderSet, InteractionGroups, Ray, RayIntersection, WQuadtree,
};

/// A pipeline for performing queries on all the colliders of a scene.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct QueryPipeline {
    quadtree: WQuadtree<ColliderHandle>,
    tree_built: bool,
    dilation_factor: f32,
}

impl Default for QueryPipeline {
    fn default() -> Self {
        Self::new()
    }
}

impl QueryPipeline {
    /// Initializes an empty query pipeline.
    pub fn new() -> Self {
        Self {
            quadtree: WQuadtree::new(),
            tree_built: false,
            dilation_factor: 0.01,
        }
    }

    /// Update the acceleration structure on the query pipeline.
    pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) {
        if !self.tree_built {
            let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
            self.quadtree.clear_and_rebuild(data, self.dilation_factor);
            // FIXME: uncomment this once we handle insertion/removals properly.
            // self.tree_built = true;
            return;
        }

        for (_, body) in bodies
            .iter_active_dynamic()
            .chain(bodies.iter_active_kinematic())
        {
            for handle in &body.colliders {
                self.quadtree.pre_update(*handle)
            }
        }

        self.quadtree.update(colliders, self.dilation_factor);
    }

    /// Find the closest intersection between a ray and a set of collider.
    ///
    /// # Parameters
    /// - `position`: the position of this shape.
    /// - `ray`: the ray to cast.
    /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively
    ///   limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray.
    pub fn cast_ray<'a>(
        &self,
        colliders: &'a ColliderSet,
        ray: &Ray,
        max_toi: f32,
        groups: InteractionGroups,
    ) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> {
        // TODO: avoid allocation?
        let mut inter = Vec::new();
        self.quadtree.cast_ray(ray, max_toi, &mut inter);

        let mut best = f32::MAX;
        let mut result = None;

        for handle in inter {
            if let Some(collider) = colliders.get(handle) {
                if collider.collision_groups.test(groups) {
                    if let Some(inter) = collider.shape().toi_and_normal_with_ray(
                        collider.position(),
                        ray,
                        max_toi,
                        true,
                    ) {
                        if inter.toi < best {
                            best = inter.toi;
                            result = Some((handle, collider, inter));
                        }
                    }
                }
            }
        }

        result
    }

    /// Find the all intersections between a ray and a set of collider and passes them to a callback.
    ///
    /// # Parameters
    /// - `position`: the position of this shape.
    /// - `ray`: the ray to cast.
    /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively
    ///   limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray.
    /// - `callback`: function executed on each collider for which a ray intersection has been found.
    ///   There is no guarantees on the order the results will be yielded. If this callback returns `false`,
    ///   this method will exit early, ignory any further raycast.
    pub fn interferences_with_ray<'a>(
        &self,
        colliders: &'a ColliderSet,
        ray: &Ray,
        max_toi: f32,
        groups: InteractionGroups,
        mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool,
    ) {
        // TODO: avoid allocation?
        let mut inter = Vec::new();
        self.quadtree.cast_ray(ray, max_toi, &mut inter);

        for handle in inter {
            let collider = &colliders[handle];

            if collider.collision_groups.test(groups) {
                if let Some(inter) = collider.shape().toi_and_normal_with_ray(
                    collider.position(),
                    ray,
                    max_toi,
                    true,
                ) {
                    if !callback(handle, collider, inter) {
                        return;
                    }
                }
            }
        }
    }
}