parry3d/query/contact_manifolds/
contact_manifolds_voxels_ball.rs

1use crate::bounding_volume::BoundingVolume;
2use crate::math::{Isometry, Point, Real, Vector};
3use crate::query::{ContactManifold, PointQuery, TrackedContact};
4use crate::shape::{
5    Ball, Cuboid, OctantPattern, PackedFeatureId, Shape, VoxelState, VoxelType, Voxels,
6};
7use alloc::vec::Vec;
8
9/// Computes the contact manifold between a convex shape and a ball, both represented as a `Shape` trait-object.
10pub fn contact_manifolds_voxels_ball_shapes<ManifoldData, ContactData>(
11    pos12: &Isometry<Real>,
12    shape1: &dyn Shape,
13    shape2: &dyn Shape,
14    prediction: Real,
15    manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
16) where
17    ManifoldData: Default,
18    ContactData: Default + Copy,
19{
20    if let (Some(voxels1), Some(ball2)) = (shape1.as_voxels(), shape2.as_ball()) {
21        contact_manifolds_voxels_ball(pos12, voxels1, ball2, prediction, manifolds, false);
22    } else if let (Some(ball1), Some(voxels2)) = (shape1.as_ball(), shape2.as_voxels()) {
23        contact_manifolds_voxels_ball(
24            &pos12.inverse(),
25            voxels2,
26            ball1,
27            prediction,
28            manifolds,
29            true,
30        );
31    }
32}
33
34/// Computes the contact manifold between a convex shape and a ball.
35pub fn contact_manifolds_voxels_ball<'a, ManifoldData, ContactData>(
36    pos12: &Isometry<Real>,
37    voxels1: &'a Voxels,
38    ball2: &'a Ball,
39    prediction: Real,
40    manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
41    flipped: bool,
42) where
43    ManifoldData: Default,
44    ContactData: Default + Copy,
45{
46    // TODO: don’t generate one manifold per voxel.
47    manifolds.clear();
48
49    let radius2 = ball2.radius;
50    let center2 = Point::origin(); // The ball’s center.
51    let radius1 = voxels1.voxel_size() / 2.0;
52
53    // FIXME: optimize this.
54    let aabb1 = voxels1.local_aabb().loosened(prediction / 2.0);
55    let aabb2 = ball2.aabb(pos12).loosened(prediction / 2.0);
56    if let Some(aabb_intersection) = aabb1.intersection(&aabb2) {
57        for vox1 in voxels1.voxels_intersecting_local_aabb(&aabb_intersection) {
58            match vox1.state.voxel_type() {
59                #[cfg(feature = "dim2")]
60                VoxelType::Vertex | VoxelType::Face => { /* Ok */ }
61                #[cfg(feature = "dim3")]
62                VoxelType::Vertex | VoxelType::Edge | VoxelType::Face => { /* Ok */ }
63                _ => continue,
64            }
65
66            detect_hit_voxel_ball(
67                *pos12,
68                vox1.center,
69                radius1,
70                vox1.state,
71                center2,
72                radius2,
73                prediction,
74                flipped,
75                manifolds,
76            );
77        }
78    }
79}
80
81pub(crate) fn detect_hit_voxel_ball<ManifoldData, ContactData>(
82    pos12: Isometry<Real>,
83    center1: Point<Real>,
84    radius1: Vector<Real>,
85    data1: VoxelState,
86    center2: Point<Real>,
87    radius2: Real,
88    prediction: Real,
89    flipped: bool,
90    manifolds: &mut Vec<ContactManifold<ManifoldData, ContactData>>,
91) where
92    ManifoldData: Default,
93    ContactData: Default + Copy,
94{
95    let center2_1 = pos12 * center2;
96    let projection = project_point_on_pseudo_cube(center1, radius1, data1.octant_mask(), center2_1);
97
98    if let Some((local_n1, dist_to_voxel)) = projection {
99        let dist = dist_to_voxel - radius2;
100
101        if dist <= prediction {
102            let local_n2 = pos12.inverse_transform_vector(&-local_n1);
103            let local_p1 = center2_1 - local_n1 * dist_to_voxel;
104            let local_p2 = center2 + local_n2 * radius2;
105            let contact_point = TrackedContact::<ContactData>::flipped(
106                local_p1,
107                local_p2,
108                PackedFeatureId::UNKNOWN,
109                PackedFeatureId::UNKNOWN,
110                dist,
111                flipped,
112            );
113
114            let mut manifold = ContactManifold::<ManifoldData, ContactData>::new();
115            manifold.points.push(contact_point);
116
117            if flipped {
118                manifold.local_n1 = local_n2;
119                manifold.local_n2 = local_n1;
120            } else {
121                manifold.local_n1 = local_n1;
122                manifold.local_n2 = local_n2;
123            }
124
125            manifolds.push(manifold);
126        }
127    }
128}
129
130pub fn project_point_on_pseudo_cube(
131    voxel_center: Point<Real>,
132    voxel_radius: Vector<Real>,
133    voxel_mask: u32,
134    point: Point<Real>,
135) -> Option<(Vector<Real>, Real)> {
136    let dpos = point - voxel_center;
137
138    // This maps our easy-to map octant_key, to the vertex key as
139    // used by the Aabb::FACES_VERTEX_IDS and Aabb::EDGES_VERTEX_IDS
140    // arrays. Could we find a way to avoid this map by computing the
141    // correct key right away?
142    //
143    // NOTE: in 2D the array is [0, 1, 3, 2] which matches the one in 3D.
144    const AABB_OCTANT_KEYS: [u32; 8] = [0, 1, 3, 2, 4, 5, 7, 6];
145    #[cfg(feature = "dim2")]
146    let octant_key = ((dpos.x >= 0.0) as usize) | (((dpos.y >= 0.0) as usize) << 1);
147    #[cfg(feature = "dim3")]
148    let octant_key = ((dpos.x >= 0.0) as usize)
149        | (((dpos.y >= 0.0) as usize) << 1)
150        | (((dpos.z >= 0.0) as usize) << 2);
151    let aabb_octant_key = AABB_OCTANT_KEYS[octant_key];
152    let dpos_signs = dpos.map(|x| x.signum());
153    let unit_dpos = dpos.abs().component_div(&voxel_radius); // Project the point in "local unit octant space".
154
155    // Extract the feature pattern specific to the selected octant.
156    let pattern = (voxel_mask >> (aabb_octant_key * 3)) & 0b0111;
157
158    let unit_result = match pattern {
159        OctantPattern::INTERIOR => None,
160        OctantPattern::VERTEX => {
161            // PERF: inline the projection on cuboid to improve performances further.
162            //       In particular we already know on what octant we are to compute the
163            //       collision.
164            let cuboid = Cuboid::new(Vector::repeat(1.0));
165            let unit_dpos_pt = Point::from(unit_dpos);
166            let proj = cuboid.project_local_point(&unit_dpos_pt, false);
167            let mut normal = unit_dpos_pt - proj.point;
168            let dist = normal.try_normalize_mut(1.0e-8)?;
169            Some((normal, dist))
170        }
171        #[cfg(feature = "dim3")]
172        OctantPattern::EDGE_X | OctantPattern::EDGE_Y | OctantPattern::EDGE_Z => {
173            let cuboid = Cuboid::new(Vector::repeat(1.0));
174            let unit_dpos_pt = Point::from(unit_dpos);
175            let proj = cuboid.project_local_point(&unit_dpos_pt, false);
176            let mut normal = unit_dpos_pt - proj.point;
177            let dist = normal.try_normalize_mut(1.0e-8)?;
178            Some((normal, dist))
179        }
180        #[cfg(feature = "dim2")]
181        OctantPattern::FACE_X | OctantPattern::FACE_Y => {
182            let i1 = pattern as usize - OctantPattern::FACE_X as usize;
183            let i2 = (i1 + 1) % 2;
184
185            if unit_dpos[i2] > 1.0 || unit_dpos[i2] < 0.0 {
186                None
187            } else {
188                let dist = unit_dpos[i1] - 1.0; // Subtract 1 to get the distance wrt. the boundary of the unit voxel.
189                Some((Vector::ith(i1, 1.0), dist))
190            }
191        }
192        #[cfg(feature = "dim3")]
193        OctantPattern::FACE_X | OctantPattern::FACE_Y | OctantPattern::FACE_Z => {
194            let i1 = pattern as usize - OctantPattern::FACE_X as usize;
195            let i2 = (i1 + 1) % 3;
196            let i3 = (i1 + 2) % 3;
197
198            if unit_dpos[i2] > 1.0
199                || unit_dpos[i2] < 0.0
200                || unit_dpos[i3] > 1.0
201                || unit_dpos[i3] < 0.0
202            {
203                None
204            } else {
205                let dist = unit_dpos[i1] - 1.0; // Subtract 1 to get the distance wrt. the boundary of the unit voxel.
206                Some((Vector::ith(i1, 1.0), dist))
207            }
208        }
209        _ => unreachable!(),
210    };
211
212    unit_result.map(|(n, d)| {
213        let mut scaled_n = n.component_mul(&dpos_signs).component_mul(&voxel_radius);
214        let scaled_d = scaled_n.normalize_mut();
215        (scaled_n, d * scaled_d)
216    })
217}
parry3d/query/contact_manifolds/contact_manifolds_voxels_ball.rs

parry3d/query/contact_manifolds/
contact_manifolds_voxels_ball.rs
