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, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex, KinematicsCategory};
pub(crate) fn categorize_position_contacts(
bodies: &RigidBodySet,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
out_point_point_ground: &mut Vec<ContactManifoldIndex>,
out_plane_point_ground: &mut Vec<ContactManifoldIndex>,
out_point_point: &mut Vec<ContactManifoldIndex>,
out_plane_point: &mut Vec<ContactManifoldIndex>,
) {
for manifold_i in manifold_indices {
let manifold = &manifolds[*manifold_i];
let rb1 = &bodies[manifold.body_pair.body1];
let rb2 = &bodies[manifold.body_pair.body2];
if !rb1.is_dynamic() || !rb2.is_dynamic() {
match manifold.kinematics.category {
KinematicsCategory::PointPoint => out_point_point_ground.push(*manifold_i),
KinematicsCategory::PlanePoint => out_plane_point_ground.push(*manifold_i),
}
} else {
match manifold.kinematics.category {
KinematicsCategory::PointPoint => out_point_point.push(*manifold_i),
KinematicsCategory::PlanePoint => out_plane_point.push(*manifold_i),
}
}
}
}
pub(crate) fn categorize_velocity_contacts(
bodies: &RigidBodySet,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
out_ground: &mut Vec<ContactManifoldIndex>,
out_not_ground: &mut Vec<ContactManifoldIndex>,
) {
for manifold_i in manifold_indices {
let manifold = &manifolds[*manifold_i];
let rb1 = &bodies[manifold.body_pair.body1];
let rb2 = &bodies[manifold.body_pair.body2];
if !rb1.is_dynamic() || !rb2.is_dynamic() {
out_ground.push(*manifold_i)
} else {
out_not_ground.push(*manifold_i)
}
}
}
pub(crate) fn categorize_joints(
bodies: &RigidBodySet,
joints: &[JointGraphEdge],
joint_indices: &[JointIndex],
ground_joints: &mut Vec<JointIndex>,
nonground_joints: &mut Vec<JointIndex>,
) {
for joint_i in joint_indices {
let joint = &joints[*joint_i].weight;
let rb1 = &bodies[joint.body1];
let rb2 = &bodies[joint.body2];
if !rb1.is_dynamic() || !rb2.is_dynamic() {
ground_joints.push(*joint_i);
} else {
nonground_joints.push(*joint_i);
}
}
}
|
