ncollide2d/pipeline/narrow_phase/contact_generator/
convex_polyhedron_convex_polyhedron_manifold_generator.rs

1use crate::math::{Isometry, Vector};
2use crate::pipeline::narrow_phase::{ContactDispatcher, ContactManifoldGenerator};
3use crate::query::algorithms::gjk::GJKResult;
4use crate::query::algorithms::VoronoiSimplex;
5use crate::query::{self, Contact, ContactManifold, ContactPrediction, ContactPreprocessor};
6#[cfg(feature = "dim3")]
7use crate::shape::ClippingCache;
8use crate::shape::ConvexPolygonalFeature;
9use crate::shape::{FeatureId, Shape};
10use na::{self, RealField, Unit};
11
12#[cfg(feature = "dim2")]
13pub struct ConvexPolyhedronConvexPolyhedronManifoldGenerator<N: RealField + Copy> {
14    simplex: VoronoiSimplex<N>,
15    last_gjk_dir: Option<Unit<Vector<N>>>,
16    #[allow(dead_code)]
17    last_optimal_dir: Option<Unit<Vector<N>>>,
18    new_contacts: Vec<(Contact<N>, FeatureId, FeatureId)>,
19    manifold1: ConvexPolygonalFeature<N>,
20    manifold2: ConvexPolygonalFeature<N>,
21}
22
23#[cfg(feature = "dim3")]
24pub struct ConvexPolyhedronConvexPolyhedronManifoldGenerator<N: RealField + Copy> {
25    simplex: VoronoiSimplex<N>,
26    last_gjk_dir: Option<Unit<Vector<N>>>,
27    #[allow(dead_code)]
28    last_optimal_dir: Option<Unit<Vector<N>>>,
29    clip_cache: ClippingCache<N>,
30    new_contacts: Vec<(Contact<N>, FeatureId, FeatureId)>,
31    manifold1: ConvexPolygonalFeature<N>,
32    manifold2: ConvexPolygonalFeature<N>,
33}
34
35impl<N: RealField + Copy> ConvexPolyhedronConvexPolyhedronManifoldGenerator<N> {
36    #[cfg(feature = "dim3")]
37    pub fn new() -> Self {
38        ConvexPolyhedronConvexPolyhedronManifoldGenerator {
39            simplex: VoronoiSimplex::new(),
40            last_gjk_dir: None,
41            last_optimal_dir: None,
42            clip_cache: ClippingCache::new(),
43            new_contacts: Vec::new(),
44            manifold1: ConvexPolygonalFeature::new(),
45            manifold2: ConvexPolygonalFeature::new(),
46        }
47    }
48
49    #[cfg(feature = "dim2")]
50    pub fn new() -> Self {
51        ConvexPolyhedronConvexPolyhedronManifoldGenerator {
52            simplex: VoronoiSimplex::new(),
53            last_gjk_dir: None,
54            last_optimal_dir: None,
55            new_contacts: Vec::new(),
56            manifold1: ConvexPolygonalFeature::new(),
57            manifold2: ConvexPolygonalFeature::new(),
58        }
59    }
60
61    fn clip_polyfaces(&mut self, prediction: &ContactPrediction<N>, normal: &Unit<Vector<N>>) {
62        #[cfg(feature = "dim2")]
63        {
64            self.manifold1
65                .clip(&self.manifold2, normal, prediction, &mut self.new_contacts)
66        }
67        #[cfg(feature = "dim3")]
68        {
69            self.manifold1.clip(
70                &self.manifold2,
71                normal,
72                prediction,
73                &mut self.clip_cache,
74                &mut self.new_contacts,
75            )
76        }
77    }
78}
79
80impl<N: RealField + Copy> ContactManifoldGenerator<N>
81    for ConvexPolyhedronConvexPolyhedronManifoldGenerator<N>
82{
83    fn generate_contacts(
84        &mut self,
85        _: &dyn ContactDispatcher<N>,
86        ma: &Isometry<N>,
87        a: &dyn Shape<N>,
88        proc1: Option<&dyn ContactPreprocessor<N>>,
89        mb: &Isometry<N>,
90        b: &dyn Shape<N>,
91        proc2: Option<&dyn ContactPreprocessor<N>>,
92        prediction: &ContactPrediction<N>,
93        manifold: &mut ContactManifold<N>,
94    ) -> bool {
95        if let (Some(cpa), Some(cpb)) = (a.as_convex_polyhedron(), b.as_convex_polyhedron()) {
96            let contact = query::contact_support_map_support_map_with_params(
97                ma,
98                cpa,
99                mb,
100                cpb,
101                prediction.linear(),
102                &mut self.simplex,
103                self.last_gjk_dir,
104            );
105
106            // Generate a contact manifold.
107            self.new_contacts.clear();
108            self.manifold1.clear();
109            self.manifold2.clear();
110
111            match contact {
112                GJKResult::ClosestPoints(world1, world2, dir) => {
113                    self.last_gjk_dir = Some(dir);
114                    let contact = Contact::new_wo_depth(world1, world2, dir);
115
116                    if contact.depth > na::zero() {
117                        cpa.support_face_toward(ma, &contact.normal, &mut self.manifold1);
118                        cpb.support_face_toward(mb, &-contact.normal, &mut self.manifold2);
119                        self.clip_polyfaces(prediction, &contact.normal);
120                    } else {
121                        cpa.support_feature_toward(
122                            ma,
123                            &contact.normal,
124                            prediction.angular1(),
125                            &mut self.manifold1,
126                        );
127                        cpb.support_feature_toward(
128                            mb,
129                            &-contact.normal,
130                            prediction.angular2(),
131                            &mut self.manifold2,
132                        );
133
134                        self.clip_polyfaces(prediction, &contact.normal);
135                    }
136
137                    if self.new_contacts.len() == 0 {
138                        self.new_contacts.push((
139                            contact.clone(),
140                            self.manifold1.feature_id,
141                            self.manifold2.feature_id,
142                        ));
143                    }
144                }
145                GJKResult::NoIntersection(dir) => self.last_gjk_dir = Some(dir),
146                _ => {}
147            }
148
149            for (c, f1, f2) in self.new_contacts.drain(..) {
150                self.manifold1.add_contact_to_manifold(
151                    &self.manifold2,
152                    c,
153                    ma,
154                    f1,
155                    proc1,
156                    mb,
157                    f2,
158                    proc2,
159                    manifold,
160                )
161            }
162
163            true
164        } else {
165            false
166        }
167    }
168}
ncollide2d/pipeline/narrow_phase/contact_generator/convex_polyhedron_convex_polyhedron_manifold_generator.rs

ncollide2d/pipeline/narrow_phase/contact_generator/
convex_polyhedron_convex_polyhedron_manifold_generator.rs
