bevy_tnua_rapier3d/
spatial_ext.rs

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
use bevy::{ecs::system::SystemParam, prelude::*};
use bevy_rapier3d::prelude::*;
use bevy_tnua_physics_integration_layer::{
    math::{Float, Vector3},
    spatial_ext::{TnuaPointProjectionResult, TnuaSpatialExt},
};

use crate::get_collider;

#[derive(SystemParam)]
pub struct TnuaSpatialExtRapier3d<'w, 's> {
    rapier_context_query: Query<'w, 's, RapierContext<'static>>,
    context_links_query: Query<'w, 's, &'static RapierContextEntityLink>,
    colliders_query: Query<'w, 's, (&'static Collider, &'static GlobalTransform)>,
}

impl TnuaSpatialExt for TnuaSpatialExtRapier3d<'_, '_> {
    type ColliderData<'a>
        = (&'a Collider, Vec3, Quat)
    where
        Self: 'a;

    fn fetch_collider_data(&self, entity: Entity) -> Option<Self::ColliderData<'_>> {
        let (collider, transform) = self.colliders_query.get(entity).ok()?;
        let (_scale, rotation, translation) = transform.to_scale_rotation_translation();
        Some((collider, translation, rotation))
    }

    fn project_point<'a>(
        &'a self,
        point: Vector3,
        solid: bool,
        collider_data: &Self::ColliderData<'a>,
    ) -> TnuaPointProjectionResult {
        let (collider, position, rotation) = collider_data;
        let result = collider.project_point(*position, *rotation, point, solid);
        if result.is_inside {
            TnuaPointProjectionResult::Inside(result.point)
        } else {
            TnuaPointProjectionResult::Outside(result.point)
        }
    }

    fn cast_ray<'a>(
        &'a self,
        origin: Vector3,
        direction: Vector3,
        max_time_of_impact: Float,
        collider_data: &Self::ColliderData<'a>,
    ) -> Option<(Float, Vector3)> {
        let (collider, position, rotation) = collider_data;
        collider
            .cast_ray_and_get_normal(
                *position,
                *rotation,
                origin,
                direction,
                max_time_of_impact,
                true,
            )
            .map(|res| (res.time_of_impact, res.normal))
    }

    fn can_interact(&self, entity1: Entity, entity2: Entity) -> bool {
        let rapier_context =
            if let Ok([link1, link2]) = self.context_links_query.get_many([entity1, entity2]) {
                if link1 != link2 {
                    return false;
                }
                let Ok(rapier_context) = self.rapier_context_query.get(link1.0) else {
                    return false;
                };
                rapier_context
            } else {
                // One of the entities is not in any physical world
                return false;
            };
        let Some(collider1) = get_collider(rapier_context.colliders, entity1) else {
            return true;
        };
        if collider1.is_sensor() {
            return false;
        }
        let Some(collider2) = get_collider(rapier_context.colliders, entity2) else {
            return true;
        };
        if collider2.is_sensor() {
            return false;
        }
        collider1
            .collision_groups()
            .test(collider2.collision_groups())
            && collider1.solver_groups().test(collider2.solver_groups())
    }
}