sparkl2d_kernels/
gpu_collider.rs

1use crate::DevicePointer;
2use core::marker::PhantomData;
3use na::ComplexField;
4use parry::query::{PointProjection, PointQueryWithLocation};
5use parry::shape::{Cuboid, CudaHeightFieldPtr, CudaTriMeshPtr, Segment, SegmentPointLocation};
6use parry::utils::CudaArrayPointer1;
7use sparkl_core::dynamics::solver::BoundaryHandling;
8use sparkl_core::math::{Isometry, Point, Real};
9
10#[cfg_attr(not(target_os = "cuda"), derive(cust::DeviceCopy))]
11#[derive(Copy, Clone)]
12#[repr(C)]
13pub enum GpuColliderShape {
14    Cuboid(Cuboid),
15    HeightField {
16        heightfield: CudaHeightFieldPtr,
17        bellow_heightfield_is_solid: bool,
18        flip_interior: bool,
19    },
20    TriMesh {
21        trimesh: CudaTriMeshPtr,
22        flip_interior: bool,
23    },
24    Polyline {
25        vertices: parry::utils::CudaArrayPointer1<Point<Real>>,
26        flip_interior: bool,
27    },
28}
29
30impl GpuColliderShape {
31    #[cfg(not(target_os = "cuda"))]
32    pub fn project_point_with_max_dist(
33        &self,
34        _position: &Isometry<Real>,
35        _point: &Point<Real>,
36        _solid: bool,
37        _max_dist: Real,
38    ) -> Option<PointProjection> {
39        unreachable!()
40    }
41
42    #[cfg(target_os = "cuda")]
43    pub fn project_point_with_max_dist(
44        &self,
45        position: &Isometry<Real>,
46        point: &Point<Real>,
47        solid: bool,
48        max_dist: Real,
49    ) -> Option<PointProjection> {
50        use parry::query::PointQuery;
51
52        match self {
53            Self::Cuboid(s) => Some(s.project_point(position, point, solid)),
54            Self::Polyline {
55                vertices,
56                flip_interior,
57            } => polyline_project_point(vertices, position, point, solid, *flip_interior),
58            Self::TriMesh {
59                trimesh,
60                flip_interior,
61            } => trimesh
62                .project_point_with_max_dist(position, point, solid, Real::MAX) // max_dist)
63                .map(|mut proj| {
64                    if *flip_interior {
65                        proj.is_inside = !proj.is_inside;
66                    }
67                    proj
68                }),
69            Self::HeightField {
70                heightfield,
71                bellow_heightfield_is_solid,
72                flip_interior,
73            } => {
74                let local_point = position.inverse_transform_point(point);
75                let mut local_aabb = heightfield.local_aabb();
76                local_aabb.mins.y = -Real::MAX;
77                local_aabb.maxs.y = Real::MAX;
78                let mut local_proj =
79                    heightfield.project_local_point_with_max_dist(&local_point, solid, max_dist)?;
80
81                if *bellow_heightfield_is_solid {
82                    if !flip_interior {
83                        local_proj.is_inside = local_aabb.contains_local_point(&local_point)
84                            && local_point.y <= local_proj.point.y;
85                    } else {
86                        local_proj.is_inside = local_aabb.contains_local_point(&local_point)
87                            && local_point.y >= local_proj.point.y;
88                    }
89                }
90
91                Some(local_proj.transform_by(position))
92            }
93            _ => None,
94        }
95    }
96}
97
98#[cfg(target_os = "cuda")]
99pub fn polyline_project_point(
100    vertices: &CudaArrayPointer1<Point<Real>>,
101    position: &Isometry<Real>,
102    point: &Point<Real>,
103    solid: bool,
104    flip_interior: bool,
105) -> Option<PointProjection> {
106    #[derive(Copy, Clone)]
107    struct BestProjection {
108        proj: PointProjection,
109        location: SegmentPointLocation,
110        dist: Real,
111        id: usize,
112        segment: Segment,
113    }
114
115    let local_point = position.inverse_transform_point(point);
116
117    // 1) First, identify the closest segment.
118    let mut best_proj: Option<BestProjection> = None;
119
120    let ith_segment = |i| Segment::new(vertices.get(i), vertices.get((i + 1) % vertices.len()));
121
122    for i in 0..vertices.len() {
123        let segment = ith_segment(i);
124        let (proj, location) = segment.project_local_point_and_get_location(&local_point, false);
125        let candidate_dist = na::distance(&local_point, &proj.point);
126
127        if best_proj.map(|p| candidate_dist < p.dist).unwrap_or(true) {
128            best_proj = Some(BestProjection {
129                proj,
130                location,
131                dist: candidate_dist,
132                id: i,
133                segment,
134            });
135        }
136    }
137
138    if let Some(best_proj) = &mut best_proj {
139        // 2) Inside/outside test (copied from Polyline::project_local_point_assuming_solid_interior_ccw)
140        #[cfg(feature = "dim2")]
141        let normal1 = best_proj.segment.normal();
142        #[cfg(feature = "dim3")]
143        let normal1 = best_proj.segment.planar_normal(2);
144
145        if let Some(normal1) = normal1 {
146            best_proj.proj.is_inside = match best_proj.location {
147                SegmentPointLocation::OnVertex(i) => {
148                    let dir2 = if i == 0 {
149                        let adj_seg = if best_proj.id == 0 {
150                            vertices.len() - 1
151                        } else {
152                            best_proj.id - 1
153                        };
154
155                        assert_eq!(best_proj.segment.a, ith_segment(adj_seg).b);
156                        -ith_segment(adj_seg).scaled_direction()
157                    } else {
158                        assert_eq!(i, 1);
159                        let adj_seg = (best_proj.id + 1) % vertices.len();
160                        assert_eq!(best_proj.segment.b, ith_segment(adj_seg).a);
161
162                        ith_segment(adj_seg).scaled_direction()
163                    };
164
165                    let dot = normal1.dot(&dir2);
166                    // TODO: is this threshold too big? This corresponds to an angle equal to
167                    //       abs(acos(1.0e-3)) = (90 - 0.057) degrees.
168                    //       We did encounter some cases where this was needed, but perhaps the
169                    //       actual problem was an issue with the SegmentPointLocation (which should
170                    //       perhaps have been Edge instead of Vertex)?
171                    let threshold = 1.0e-3 * dir2.norm();
172                    if dot.abs() > threshold {
173                        // If the vertex is a reentrant vertex, then the point is
174                        // inside. Otherwise, it is outside.
175                        dot >= 0.0
176                    } else {
177                        // If the two edges are collinear, we can’t classify the vertex.
178                        // So check against the edge’s normal instead.
179                        (point - best_proj.proj.point).dot(&normal1) <= 0.0
180                    }
181                }
182                SegmentPointLocation::OnEdge(_) => {
183                    (point - best_proj.proj.point).dot(&normal1) <= 0.0
184                }
185            };
186        }
187    }
188
189    best_proj.map(|mut p| {
190        #[cfg(feature = "dim3")]
191        {
192            p.proj.point.z = local_point.z;
193        }
194
195        if flip_interior {
196            p.proj.is_inside = !p.proj.is_inside;
197        }
198
199        p.proj.transform_by(position)
200    })
201}
202
203#[cfg_attr(not(target_os = "cuda"), derive(cust::DeviceCopy))]
204#[derive(Copy, Clone)]
205#[repr(C)]
206pub struct GpuCollider {
207    pub shape: GpuColliderShape,
208    pub position: Isometry<Real>,
209    pub friction: Real,
210    pub penalty_stiffness: Real,
211    pub grid_boundary_handling: BoundaryHandling,
212}
213
214pub struct GpuColliderSet {
215    pub ptr: *const GpuCollider,
216    pub len: usize,
217}
218
219impl GpuColliderSet {
220    pub fn get(&self, i: usize) -> Option<&GpuCollider> {
221        if i >= self.len {
222            None
223        } else {
224            unsafe { Some(&*self.ptr.add(i)) }
225        }
226    }
227
228    pub fn iter(&self) -> GpuColliderIter {
229        GpuColliderIter {
230            ptr: self.ptr,
231            len: self.len,
232            _marker: PhantomData,
233        }
234    }
235}
236
237pub struct GpuColliderIter<'a> {
238    ptr: *const GpuCollider,
239    len: usize,
240    _marker: PhantomData<&'a ()>,
241}
242
243impl<'a> Iterator for GpuColliderIter<'a> {
244    type Item = &'a GpuCollider;
245
246    fn next(&mut self) -> Option<Self::Item> {
247        if self.len == 0 {
248            None
249        } else {
250            let curr = self.ptr;
251
252            unsafe {
253                self.ptr = self.ptr.offset(1);
254                self.len -= 1;
255                Some(&*curr)
256            }
257        }
258    }
259}
sparkl2d_kernels/gpu_collider.rs

sparkl2d_kernels/
gpu_collider.rs
