bevy_tnua_rapier2d/
lib.rs

//! # bevy_rapier2d Integration for bevy-tnua
//!
//! In addition to the instruction in bevy-tnua's documentation:
//!
//! * Add [`TnuaRapier2dPlugin`] to the Bevy app.
//! * Add [`TnuaRapier2dIOBundle`] to each character entity controlled by Tnua.
//! * Optionally: Add [`TnuaRapier2dSensorShape`] to the sensor entities. This means the entity of
//!   the characters controlled by Tnua, but also other things like the entity generated by
//!   `TnuaCrouchEnforcer`, that can be affected with a closure.
use bevy::ecs::schedule::{InternedScheduleLabel, ScheduleLabel};
use bevy::prelude::*;
use bevy::utils::HashSet;
use bevy_rapier2d::prelude::*;
use bevy_rapier2d::rapier;
use bevy_rapier2d::rapier::prelude::InteractionGroups;

use bevy_tnua_physics_integration_layer::data_for_backends::TnuaGhostPlatform;
use bevy_tnua_physics_integration_layer::data_for_backends::TnuaGhostSensor;
use bevy_tnua_physics_integration_layer::data_for_backends::TnuaGravity;
use bevy_tnua_physics_integration_layer::data_for_backends::TnuaToggle;
use bevy_tnua_physics_integration_layer::data_for_backends::{
    TnuaMotor, TnuaProximitySensor, TnuaProximitySensorOutput, TnuaRigidBodyTracker,
};
use bevy_tnua_physics_integration_layer::subservient_sensors::TnuaSubservientSensor;
use bevy_tnua_physics_integration_layer::TnuaPipelineStages;
use bevy_tnua_physics_integration_layer::TnuaSystemSet;

/// Add this plugin to use bevy_rapier2d as a physics backend.
///
/// This plugin should be used in addition to `TnuaControllerPlugin`.
pub struct TnuaRapier2dPlugin {
    schedule: InternedScheduleLabel,
}

impl TnuaRapier2dPlugin {
    pub fn new(schedule: impl ScheduleLabel) -> Self {
        Self {
            schedule: schedule.intern(),
        }
    }
}

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

impl Plugin for TnuaRapier2dPlugin {
    fn build(&self, app: &mut App) {
        app.register_required_components::<TnuaProximitySensor, Velocity>()
            .register_required_components::<TnuaProximitySensor, ExternalForce>()
            .register_required_components::<TnuaProximitySensor, ReadMassProperties>()
            .register_required_components_with::<TnuaGravity, GravityScale>(|| GravityScale(0.0));
        app.configure_sets(
            self.schedule,
            TnuaSystemSet.before(PhysicsSet::SyncBackend).run_if(
                |rapier_config: Single<&RapierConfiguration>| rapier_config.physics_pipeline_active,
            ),
        );
        app.add_systems(
            self.schedule,
            (
                update_rigid_body_trackers_system,
                update_proximity_sensors_system,
            )
                .in_set(TnuaPipelineStages::Sensors),
        );
        app.add_systems(
            self.schedule,
            apply_motors_system.in_set(TnuaPipelineStages::Motors),
        );
        app.add_systems(
            Update,
            ensure_subservient_sensors_are_linked_to_rapier_context,
        );
    }
}

fn ensure_subservient_sensors_are_linked_to_rapier_context(
    query: Query<(Entity, &TnuaSubservientSensor), Without<RapierContextEntityLink>>,
    links_query: Query<&RapierContextEntityLink>,
    mut commands: Commands,
) {
    for (entity, subservient) in query.iter() {
        let Ok(owner_link) = links_query.get(subservient.owner_entity) else {
            continue;
        };
        commands.entity(entity).insert_if_new(*owner_link);
    }
}

/// `bevy_rapier2d`-specific components required for Tnua to work.
#[derive(Bundle, Default)]
#[deprecated(
    note = "All uses can be safely removed, components are added via bevy's required components"
)]
pub struct TnuaRapier2dIOBundle {
    pub velocity: Velocity,
    pub external_force: ExternalForce,
    pub read_mass_properties: ReadMassProperties,
}

/// Add this component to make [`TnuaProximitySensor`] cast a shape instead of a ray.
#[derive(Component)]
pub struct TnuaRapier2dSensorShape(pub Collider);

fn update_rigid_body_trackers_system(
    rapier_config: Single<&RapierConfiguration>,
    mut query: Query<(
        &GlobalTransform,
        &Velocity,
        &mut TnuaRigidBodyTracker,
        Option<&TnuaToggle>,
        Option<&TnuaGravity>,
    )>,
) {
    for (transform, velocity, mut tracker, tnua_toggle, tnua_gravity) in query.iter_mut() {
        match tnua_toggle.copied().unwrap_or_default() {
            TnuaToggle::Disabled => continue,
            TnuaToggle::SenseOnly => {}
            TnuaToggle::Enabled => {}
        }
        let (_, rotation, translation) = transform.to_scale_rotation_translation();
        *tracker = TnuaRigidBodyTracker {
            translation,
            rotation,
            velocity: velocity.linvel.extend(0.0),
            angvel: Vec3::new(0.0, 0.0, velocity.angvel),
            gravity: tnua_gravity
                .map(|g| g.0)
                .unwrap_or(rapier_config.gravity.extend(0.0)),
        };
    }
}

fn get_collider(
    rapier_colliders: &RapierContextColliders,
    entity: Entity,
) -> Option<&rapier::geometry::Collider> {
    let collider_handle = rapier_colliders.entity2collider().get(&entity)?;
    rapier_colliders.colliders.get(*collider_handle)
}

#[allow(clippy::type_complexity)]
fn update_proximity_sensors_system(
    rapier_context_query: Query<RapierContext>,
    mut query: Query<(
        Entity,
        &RapierContextEntityLink,
        &GlobalTransform,
        &mut TnuaProximitySensor,
        Option<&TnuaRapier2dSensorShape>,
        Option<&mut TnuaGhostSensor>,
        Option<&TnuaSubservientSensor>,
        Option<&TnuaToggle>,
    )>,
    ghost_platforms_query: Query<(), With<TnuaGhostPlatform>>,
    other_object_query_query: Query<(&GlobalTransform, &Velocity)>,
) {
    query.par_iter_mut().for_each(
        |(
            owner_entity,
            rapier_context_entity_link,
            transform,
            mut sensor,
            shape,
            mut ghost_sensor,
            subservient,
            tnua_toggle,
        )| {
            match tnua_toggle.copied().unwrap_or_default() {
                TnuaToggle::Disabled => return,
                TnuaToggle::SenseOnly => {}
                TnuaToggle::Enabled => {}
            }

            let Ok(rapier_context) = rapier_context_query.get(rapier_context_entity_link.0) else {
                return;
            };

            let cast_origin = transform.transform_point(sensor.cast_origin);
            let cast_direction = sensor.cast_direction;

            struct CastResult {
                entity: Entity,
                proximity: f32,
                intersection_point: Vec2,
                // Use 3D and not 2D because converting a direction from 2D to 3D is more painful
                // than it should be.
                normal: Dir3,
            }

            let owner_entity = if let Some(subservient) = subservient {
                subservient.owner_entity
            } else {
                owner_entity
            };

            let mut query_filter = QueryFilter::new().exclude_rigid_body(owner_entity);
            let owner_solver_groups: InteractionGroups;

            let owner_collider = get_collider(&rapier_context.colliders, owner_entity);
            if let Some(owner_collider) = owner_collider {
                let collision_groups = owner_collider.collision_groups();
                query_filter.groups = Some(CollisionGroups {
                    memberships: Group::from_bits_truncate(collision_groups.memberships.bits()),
                    filters: Group::from_bits_truncate(collision_groups.filter.bits()),
                });
                owner_solver_groups = owner_collider.solver_groups();
            } else {
                owner_solver_groups = InteractionGroups::all();
            }

            let mut already_visited_ghost_entities = HashSet::<Entity>::default();

            let has_ghost_sensor = ghost_sensor.is_some();

            let do_cast = |cast_range_skip: f32,
                           already_visited_ghost_entities: &HashSet<Entity>|
             -> Option<CastResult> {
                let predicate = |other_entity: Entity| {
                    if let Some(other_collider) =
                        get_collider(&rapier_context.colliders, other_entity)
                    {
                        if !other_collider.solver_groups().test(owner_solver_groups) {
                            if has_ghost_sensor && ghost_platforms_query.contains(other_entity) {
                                if already_visited_ghost_entities.contains(&other_entity) {
                                    return false;
                                }
                            } else {
                                return false;
                            }
                        }
                        if other_collider.is_sensor() {
                            return false;
                        }
                    }
                    true
                };
                let query_filter = query_filter.predicate(&predicate);
                let cast_origin = cast_origin + cast_range_skip * *cast_direction;
                let cast_range = sensor.cast_range - cast_range_skip;
                if let Some(TnuaRapier2dSensorShape(shape)) = shape {
                    rapier_context
                        .query_pipeline
                        .cast_shape(
                            rapier_context.colliders,
                            rapier_context.rigidbody_set,
                            cast_origin.truncate(),
                            0.0,
                            cast_direction.truncate(),
                            shape,
                            ShapeCastOptions {
                                max_time_of_impact: cast_range,
                                target_distance: 0.0,
                                stop_at_penetration: false,
                                compute_impact_geometry_on_penetration: false,
                            },
                            query_filter,
                        )
                        .and_then(|(entity, hit)| {
                            let details = hit.details?;
                            Some(CastResult {
                                entity,
                                proximity: hit.time_of_impact + cast_range_skip,
                                intersection_point: details.witness1,
                                normal: Dir3::new(details.normal1.extend(0.0))
                                    .unwrap_or_else(|_| -cast_direction),
                            })
                        })
                } else {
                    rapier_context
                        .query_pipeline
                        .cast_ray_and_get_normal(
                            rapier_context.colliders,
                            rapier_context.rigidbody_set,
                            cast_origin.truncate(),
                            cast_direction.truncate(),
                            cast_range,
                            false,
                            query_filter,
                        )
                        .map(|(entity, hit)| CastResult {
                            entity,
                            proximity: hit.time_of_impact + cast_range_skip,
                            intersection_point: hit.point,
                            normal: Dir3::new(hit.normal.extend(0.0))
                                .unwrap_or_else(|_| -cast_direction),
                        })
                }
            };

            let mut cast_range_skip = 0.0;
            if let Some(ghost_sensor) = ghost_sensor.as_mut() {
                ghost_sensor.0.clear();
            }
            let isometry: rapier::na::Isometry2<f32> = {
                let (_, rotation, translation) = transform.to_scale_rotation_translation();
                (translation.truncate(), rotation.z).into()
            };
            sensor.output = 'sensor_output: loop {
                if let Some(CastResult {
                    entity,
                    proximity,
                    intersection_point,
                    normal,
                }) = do_cast(cast_range_skip, &already_visited_ghost_entities)
                {
                    // Alternative fix for https://github.com/idanarye/bevy-tnua/issues/14 - one
                    // that does not cause https://github.com/idanarye/bevy-tnua/issues/85
                    // Note that this does not solve https://github.com/idanarye/bevy-tnua/issues/87
                    if let Some(owner_collider) = owner_collider {
                        if owner_collider
                            .shape()
                            .contains_point(&isometry, &intersection_point.into())
                        {
                            // I hate having to do this so much, but without it it sometimes enters
                            // an infinte loop...
                            cast_range_skip = proximity
                                + if sensor.cast_range.is_finite() && 0.0 < sensor.cast_range {
                                    0.1 * sensor.cast_range
                                } else {
                                    0.1
                                };
                            continue;
                        };
                    }

                    let entity_linvel;
                    let entity_angvel;
                    if let Ok((entity_transform, entity_velocity)) =
                        other_object_query_query.get(entity)
                    {
                        entity_angvel = Vec3::new(0.0, 0.0, entity_velocity.angvel);
                        entity_linvel = entity_velocity.linvel.extend(0.0)
                            + if 0.0 < entity_velocity.angvel.abs() {
                                let relative_point =
                                    intersection_point - entity_transform.translation().truncate();
                                // NOTE: no need to project relative_point on the rotation plane, it will not
                                // affect the cross product.
                                entity_angvel.cross(relative_point.extend(0.0))
                            } else {
                                Vec3::ZERO
                            };
                    } else {
                        entity_angvel = Vec3::ZERO;
                        entity_linvel = Vec3::ZERO;
                    }
                    let sensor_output = TnuaProximitySensorOutput {
                        entity,
                        proximity,
                        normal,
                        entity_linvel,
                        entity_angvel,
                    };
                    if ghost_platforms_query.contains(entity) {
                        cast_range_skip = proximity;
                        already_visited_ghost_entities.insert(entity);
                        if let Some(ghost_sensor) = ghost_sensor.as_mut() {
                            ghost_sensor.0.push(sensor_output);
                        }
                    } else {
                        break 'sensor_output Some(sensor_output);
                    }
                } else {
                    break 'sensor_output None;
                }
            };
        },
    );
}

fn apply_motors_system(
    mut query: Query<(
        &TnuaMotor,
        &mut Velocity,
        &ReadMassProperties,
        &mut ExternalForce,
        Option<&TnuaToggle>,
        Option<&TnuaGravity>,
    )>,
) {
    for (motor, mut velocity, mass_properties, mut external_force, tnua_toggle, tnua_gravity) in
        query.iter_mut()
    {
        match tnua_toggle.copied().unwrap_or_default() {
            TnuaToggle::Disabled | TnuaToggle::SenseOnly => {
                *external_force = Default::default();
                return;
            }
            TnuaToggle::Enabled => {}
        }
        if motor.lin.boost.is_finite() {
            velocity.linvel += motor.lin.boost.truncate();
        }
        if motor.lin.acceleration.is_finite() {
            external_force.force = motor.lin.acceleration.truncate() * mass_properties.get().mass;
        }
        if motor.ang.boost.is_finite() {
            velocity.angvel += motor.ang.boost.z;
        }
        if motor.ang.acceleration.is_finite() {
            external_force.torque =
                motor.ang.acceleration.z * mass_properties.get().principal_inertia;
        }
        if let Some(gravity) = tnua_gravity {
            external_force.force += gravity.0.truncate() * mass_properties.get().mass;
        }
    }
}
bevy_tnua_rapier2d/lib.rs

bevy_tnua_rapier2d/
lib.rs
