rapier2d/dynamics/solver/
categorization.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
use crate::dynamics::{JointGraphEdge, JointIndex, MultibodyJointSet, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};

pub(crate) fn categorize_contacts(
    _bodies: &RigidBodySet, // Unused but useful to simplify the parallel code.
    multibody_joints: &MultibodyJointSet,
    manifolds: &[&mut ContactManifold],
    manifold_indices: &[ContactManifoldIndex],
    out_one_body: &mut Vec<ContactManifoldIndex>,
    out_two_body: &mut Vec<ContactManifoldIndex>,
    out_generic_one_body: &mut Vec<ContactManifoldIndex>,
    out_generic_two_body: &mut Vec<ContactManifoldIndex>,
) {
    for manifold_i in manifold_indices {
        let manifold = &manifolds[*manifold_i];

        if manifold
            .data
            .rigid_body1
            .and_then(|h| multibody_joints.rigid_body_link(h))
            .is_some()
            || manifold
                .data
                .rigid_body2
                .and_then(|h| multibody_joints.rigid_body_link(h))
                .is_some()
        {
            if manifold.data.relative_dominance != 0 {
                out_generic_one_body.push(*manifold_i);
            } else {
                out_generic_two_body.push(*manifold_i);
            }
        } else if manifold.data.relative_dominance != 0 {
            out_one_body.push(*manifold_i)
        } else {
            out_two_body.push(*manifold_i)
        }
    }
}

pub(crate) fn categorize_joints(
    bodies: &RigidBodySet,
    multibody_joints: &MultibodyJointSet,
    impulse_joints: &[JointGraphEdge],
    joint_indices: &[JointIndex],
    one_body_joints: &mut Vec<JointIndex>,
    two_body_joints: &mut Vec<JointIndex>,
    generic_one_body_joints: &mut Vec<JointIndex>,
    generic_two_body_joints: &mut Vec<JointIndex>,
) {
    for joint_i in joint_indices {
        let joint = &impulse_joints[*joint_i].weight;
        let rb1 = &bodies[joint.body1.0];
        let rb2 = &bodies[joint.body2.0];

        if multibody_joints.rigid_body_link(joint.body1).is_some()
            || multibody_joints.rigid_body_link(joint.body2).is_some()
        {
            if !rb1.is_dynamic() || !rb2.is_dynamic() {
                generic_one_body_joints.push(*joint_i);
            } else {
                generic_two_body_joints.push(*joint_i);
            }
        } else if !rb1.is_dynamic() || !rb2.is_dynamic() {
            one_body_joints.push(*joint_i);
        } else {
            two_body_joints.push(*joint_i);
        }
    }
}