bevy_rapier3d/plugin/systems/
joint.rs

1use crate::dynamics::ImpulseJoint;
2use crate::dynamics::MultibodyJoint;
3use crate::dynamics::RapierImpulseJointHandle;
4use crate::dynamics::RapierMultibodyJointHandle;
5use crate::plugin::context::systemparams::RAPIER_CONTEXT_EXPECT_ERROR;
6use crate::plugin::context::DefaultRapierContext;
7use crate::plugin::context::RapierContextEntityLink;
8use crate::plugin::context::RapierContextJoints;
9use crate::plugin::context::RapierRigidBodySet;
10use bevy::prelude::*;
11
12/// System responsible for creating new Rapier joints from the related `bevy_rapier` components.
13pub fn init_joints(
14    mut commands: Commands,
15    mut context_access: Query<(&RapierRigidBodySet, &mut RapierContextJoints)>,
16    default_context_access: Query<Entity, With<DefaultRapierContext>>,
17    impulse_joints: Query<
18        (Entity, Option<&RapierContextEntityLink>, &ImpulseJoint),
19        Without<RapierImpulseJointHandle>,
20    >,
21    multibody_joints: Query<
22        (Entity, Option<&RapierContextEntityLink>, &MultibodyJoint),
23        Without<RapierMultibodyJointHandle>,
24    >,
25    child_of_query: Query<&ChildOf>,
26) {
27    for (entity, entity_context_link, joint) in impulse_joints.iter() {
28        // Get rapier context from RapierContextEntityLink or insert its default value.
29        let context_entity = entity_context_link.map_or_else(
30            || {
31                let context_entity = default_context_access.single().ok()?;
32                commands
33                    .entity(entity)
34                    .insert(RapierContextEntityLink(context_entity));
35                Some(context_entity)
36            },
37            |link| Some(link.0),
38        );
39        let Some(context_entity) = context_entity else {
40            continue;
41        };
42
43        let Ok(rigidbody_set_joints) = context_access.get_mut(context_entity) else {
44            log::error!("Could not find entity {context_entity} with rapier context while initializing {entity}");
45            continue;
46        };
47        let rigidbody_set = rigidbody_set_joints.0;
48        let mut target = None;
49        let mut body_entity = entity;
50        while target.is_none() {
51            target = rigidbody_set.entity2body.get(&body_entity).copied();
52            if let Ok(child_of) = child_of_query.get(body_entity) {
53                body_entity = child_of.parent();
54            } else {
55                break;
56            }
57        }
58        let joints = rigidbody_set_joints.1.into_inner();
59
60        if let (Some(target), Some(source)) = (target, rigidbody_set.entity2body.get(&joint.parent))
61        {
62            let handle = joints.impulse_joints.insert(
63                *source,
64                target,
65                joint.data.as_ref().into_rapier(),
66                true,
67            );
68            commands
69                .entity(entity)
70                .insert(RapierImpulseJointHandle(handle));
71            joints.entity2impulse_joint.insert(entity, handle);
72        }
73    }
74
75    for (entity, entity_context_link, joint) in multibody_joints.iter() {
76        // Get rapier context from RapierContextEntityLink or insert its default value.
77        let context_entity = entity_context_link.map_or_else(
78            || {
79                let context_entity = default_context_access.single().ok()?;
80                commands
81                    .entity(entity)
82                    .insert(RapierContextEntityLink(context_entity));
83                Some(context_entity)
84            },
85            |link| Some(link.0),
86        );
87        let Some(context_entity) = context_entity else {
88            continue;
89        };
90
91        let Ok(context_joints) = context_access.get_mut(context_entity) else {
92            log::error!("Could not find entity {context_entity} with rapier context while initializing {entity}");
93            continue;
94        };
95        let context = context_joints.0;
96        let target = context.entity2body.get(&entity);
97        let joints = context_joints.1.into_inner();
98
99        if let (Some(target), Some(source)) = (target, context.entity2body.get(&joint.parent)) {
100            if let Some(handle) = joints.multibody_joints.insert(
101                *source,
102                *target,
103                joint.data.as_ref().into_rapier(),
104                true,
105            ) {
106                commands
107                    .entity(entity)
108                    .insert(RapierMultibodyJointHandle(handle));
109                joints.entity2multibody_joint.insert(entity, handle);
110            } else {
111                log::error!("Failed to create multibody joint: loop detected.")
112            }
113        }
114    }
115}
116
117/// System responsible for applying changes the user made to a joint component.
118pub fn apply_joint_user_changes(
119    mut context: Query<&mut RapierContextJoints>,
120    changed_impulse_joints: Query<
121        (
122            &RapierContextEntityLink,
123            &RapierImpulseJointHandle,
124            &ImpulseJoint,
125        ),
126        Changed<ImpulseJoint>,
127    >,
128    changed_multibody_joints: Query<
129        (
130            &RapierContextEntityLink,
131            &RapierMultibodyJointHandle,
132            &MultibodyJoint,
133        ),
134        Changed<MultibodyJoint>,
135    >,
136) {
137    // TODO: right now, we only support propagating changes made to the joint data.
138    //       Re-parenting the joint isn’t supported yet.
139    for (link, handle, changed_joint) in changed_impulse_joints.iter() {
140        let mut context = context.get_mut(link.0).expect(RAPIER_CONTEXT_EXPECT_ERROR);
141        if let Some(joint) = context.impulse_joints.get_mut(handle.0, false) {
142            joint.data = changed_joint.data.as_ref().into_rapier();
143        }
144    }
145
146    for (link, handle, changed_joint) in changed_multibody_joints.iter() {
147        let mut context = context.get_mut(link.0).expect(RAPIER_CONTEXT_EXPECT_ERROR);
148        // TODO: not sure this will always work properly, e.g., if the number of Dofs is changed.
149        if let Some((mb, link_id)) = context.multibody_joints.get_mut(handle.0) {
150            if let Some(link) = mb.link_mut(link_id) {
151                link.joint.data = changed_joint.data.as_ref().into_rapier();
152            }
153        }
154    }
155}
bevy_rapier3d/plugin/systems/joint.rs

bevy_rapier3d/plugin/systems/
joint.rs
