parry3d/shape/convex_polyhedron.rs
1use crate::math::{Real, Vector, DIM};
2use crate::shape::{FeatureId, PackedFeatureId, PolygonalFeature, PolygonalFeatureMap, SupportMap};
3// use crate::transformation;
4#[cfg(not(feature = "std"))]
5use crate::math::ComplexField;
6use crate::utils::hashmap::{Entry, HashMap};
7use crate::utils::{self, SortedPair};
8#[cfg(feature = "alloc")]
9use alloc::vec::Vec;
10use core::f64; // for .abs(), .sqrt(), and .sin_cos()
11
12#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
13#[cfg_attr(
14 feature = "rkyv",
15 derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize)
16)]
17#[derive(PartialEq, Debug, Copy, Clone)]
18pub struct Vertex {
19 pub first_adj_face_or_edge: u32,
20 pub num_adj_faces_or_edge: u32,
21}
22
23#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
24#[cfg_attr(
25 feature = "rkyv",
26 derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize)
27)]
28#[derive(PartialEq, Debug, Copy, Clone)]
29pub struct Edge {
30 pub vertices: [u32; 2],
31 pub faces: [u32; 2],
32 pub dir: Vector,
33 deleted: bool,
34}
35
36impl Edge {
37 fn other_triangle(&self, id: u32) -> u32 {
38 if id == self.faces[0] {
39 self.faces[1]
40 } else {
41 self.faces[0]
42 }
43 }
44}
45
46#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
47#[cfg_attr(
48 feature = "rkyv",
49 derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize)
50)]
51#[derive(PartialEq, Debug, Copy, Clone)]
52pub struct Face {
53 pub first_vertex_or_edge: u32,
54 pub num_vertices_or_edges: u32,
55 pub normal: Vector,
56}
57
58#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
59#[cfg_attr(
60 feature = "rkyv",
61 derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize)
62)]
63#[derive(PartialEq, Debug, Copy, Clone)]
64struct Triangle {
65 vertices: [u32; 3],
66 edges: [u32; 3],
67 normal: Vector,
68 parent_face: Option<u32>,
69 is_degenerate: bool,
70}
71
72impl Triangle {
73 fn next_edge_id(&self, id: u32) -> u32 {
74 for i in 0..3 {
75 if self.edges[i] == id {
76 return (i as u32 + 1) % 3;
77 }
78 }
79
80 unreachable!()
81 }
82}
83
84#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
85#[cfg_attr(
86 feature = "rkyv",
87 derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize)
88)]
89#[derive(PartialEq, Debug, Clone)]
90/// A 3D convex polyhedron without degenerate faces.
91///
92/// A convex polyhedron is a 3D solid shape where all faces are flat polygons, all interior angles
93/// are less than 180 degrees, and any line segment drawn between two points inside the polyhedron
94/// stays entirely within it. Common examples include cubes, tetrahedra (pyramids), and prisms.
95///
96/// # What is a convex polyhedron?
97///
98/// In 3D space, a polyhedron is **convex** if:
99/// - All faces are flat, convex polygons
100/// - All dihedral angles (angles between adjacent faces) are less than or equal to 180 degrees
101/// - The line segment between any two points inside the polyhedron lies entirely inside
102/// - All vertices "bulge outward" - there are no indentations or concave regions
103///
104/// Examples of **convex** polyhedra: cube, tetrahedron, octahedron, rectangular prism, pyramid
105/// Examples of **non-convex** polyhedra: star shapes, torus, L-shapes, any shape with holes
106///
107/// # Use cases
108///
109/// Convex polyhedra are widely used in:
110/// - **Game development**: Collision hulls for characters, vehicles, and complex objects
111/// - **Physics simulations**: Rigid body dynamics (more efficient than arbitrary meshes)
112/// - **Robotics**: Simplified robot link shapes, workspace boundaries
113/// - **Computer graphics**: Level of detail (LOD) representations, occlusion culling
114/// - **Computational geometry**: Building blocks for complex mesh operations
115/// - **3D printing**: Simplified collision detection for print validation
116///
117/// # Representation
118///
119/// This structure stores the complete topological information:
120/// - **Vectors**: The 3D coordinates of all vertices
121/// - **Faces**: Polygonal faces with their outward-pointing normals
122/// - **Edges**: Connections between vertices, shared by exactly two faces
123/// - **Adjacency information**: Which faces/edges connect to each vertex, and vice versa
124///
125/// This rich topology enables efficient collision detection algorithms like GJK, EPA, and SAT.
126///
127/// # Important: No degenerate faces
128///
129/// This structure ensures that there are no degenerate (flat or zero-area) faces. Coplanar
130/// adjacent triangles are automatically merged into larger polygonal faces during construction.
131///
132/// # Example: Creating a simple tetrahedron (pyramid)
133///
134/// ```
135/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
136/// use parry3d::shape::ConvexPolyhedron;
137/// use parry3d::math::Vector;
138///
139/// // Define the 4 vertices of a tetrahedron
140/// let points = vec![
141/// Vector::ZERO, // base vertex 1
142/// Vector::new(1.0, 0.0, 0.0), // base vertex 2
143/// Vector::new(0.5, 1.0, 0.0), // base vertex 3
144/// Vector::new(0.5, 0.5, 1.0), // apex
145/// ];
146///
147/// // Define the 4 triangular faces (indices into the points array)
148/// let indices = vec![
149/// [0u32, 1, 2], // base triangle
150/// [0, 1, 3], // side 1
151/// [1, 2, 3], // side 2
152/// [2, 0, 3], // side 3
153/// ];
154///
155/// let tetrahedron = ConvexPolyhedron::from_convex_mesh(points, &indices)
156/// .expect("Failed to create tetrahedron");
157///
158/// // A tetrahedron has 4 vertices and 4 faces
159/// assert_eq!(tetrahedron.points().len(), 4);
160/// assert_eq!(tetrahedron.faces().len(), 4);
161/// # }
162/// ```
163pub struct ConvexPolyhedron {
164 points: Vec<Vector>,
165 vertices: Vec<Vertex>,
166 faces: Vec<Face>,
167 edges: Vec<Edge>,
168 // Faces adjacent to a vertex.
169 faces_adj_to_vertex: Vec<u32>,
170 // Edges adjacent to a vertex.
171 edges_adj_to_vertex: Vec<u32>,
172 // Edges adjacent to a face.
173 edges_adj_to_face: Vec<u32>,
174 // Vertices adjacent to a face.
175 vertices_adj_to_face: Vec<u32>,
176}
177
178impl ConvexPolyhedron {
179 /// Creates a new convex polyhedron from an arbitrary set of points by computing their convex hull.
180 ///
181 /// This is the most flexible constructor - it automatically computes the **convex hull** of the
182 /// given 3D points, which is the smallest convex polyhedron that contains all the input points.
183 /// Think of it as shrink-wrapping the points with an elastic surface.
184 ///
185 /// Use this when:
186 /// - You have an arbitrary collection of 3D points and want the convex boundary
187 /// - You're not sure if your points form a convex shape
188 /// - You want to simplify a point cloud to its convex outer surface
189 /// - You need to create a collision hull from a detailed mesh
190 ///
191 /// # Returns
192 ///
193 /// - `Some(ConvexPolyhedron)` if successful
194 /// - `None` if the convex hull computation failed (e.g., all points are coplanar, collinear,
195 /// or coincident, or if the mesh is not manifold)
196 ///
197 /// # Example: Creating a convex hull from point cloud
198 ///
199 /// ```
200 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
201 /// use parry3d::shape::ConvexPolyhedron;
202 /// use parry3d::math::Vector;
203 ///
204 /// // Vectors defining a cube, plus some interior points
205 /// let points = vec![
206 /// Vector::ZERO,
207 /// Vector::new(1.0, 0.0, 0.0),
208 /// Vector::new(1.0, 1.0, 0.0),
209 /// Vector::new(0.0, 1.0, 0.0),
210 /// Vector::new(0.0, 0.0, 1.0),
211 /// Vector::new(1.0, 0.0, 1.0),
212 /// Vector::new(1.0, 1.0, 1.0),
213 /// Vector::new(0.0, 1.0, 1.0),
214 /// Vector::new(0.5, 0.5, 0.5), // Interior point - will be excluded
215 /// ];
216 ///
217 /// let polyhedron = ConvexPolyhedron::from_convex_hull(&points)
218 /// .expect("Failed to create convex hull");
219 ///
220 /// // The cube has 8 vertices (interior point excluded)
221 /// assert_eq!(polyhedron.points().len(), 8);
222 /// // And 6 faces (one per side of the cube)
223 /// assert_eq!(polyhedron.faces().len(), 6);
224 /// # }
225 /// ```
226 ///
227 /// # Example: Simplifying a complex mesh
228 ///
229 /// ```
230 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
231 /// use parry3d::shape::ConvexPolyhedron;
232 /// use parry3d::math::Vector;
233 ///
234 /// // Imagine these are vertices from a detailed character mesh
235 /// let detailed_mesh_vertices = vec![
236 /// Vector::new(-1.0, -1.0, -1.0),
237 /// Vector::new(1.0, -1.0, -1.0),
238 /// Vector::new(1.0, 1.0, -1.0),
239 /// Vector::new(-1.0, 1.0, -1.0),
240 /// Vector::new(-1.0, -1.0, 1.0),
241 /// Vector::new(1.0, -1.0, 1.0),
242 /// Vector::new(1.0, 1.0, 1.0),
243 /// Vector::new(-1.0, 1.0, 1.0),
244 /// // ... many more vertices in the original mesh
245 /// ];
246 ///
247 /// // Create a simplified collision hull
248 /// let collision_hull = ConvexPolyhedron::from_convex_hull(&detailed_mesh_vertices)
249 /// .expect("Failed to create collision hull");
250 ///
251 /// // The hull is much simpler than the original mesh
252 /// println!("Simplified to {} vertices", collision_hull.points().len());
253 /// # }
254 /// ```
255 pub fn from_convex_hull(points: &[Vector]) -> Option<ConvexPolyhedron> {
256 crate::transformation::try_convex_hull(points)
257 .ok()
258 .and_then(|(vertices, indices)| Self::from_convex_mesh(vertices, &indices))
259 }
260
261 /// Creates a new convex polyhedron from vertices and face indices, assuming the mesh is already convex.
262 ///
263 /// This constructor is more efficient than [`from_convex_hull`] because it **assumes** the input
264 /// mesh is already convex and manifold. The convexity is **not verified** - if you pass a non-convex
265 /// mesh, the resulting shape may behave incorrectly in collision detection.
266 ///
267 /// The method automatically:
268 /// - Merges coplanar adjacent triangular faces into larger polygonal faces
269 /// - Removes degenerate (zero-area) faces
270 /// - Builds complete topological connectivity information (vertices, edges, faces)
271 ///
272 /// # Important requirements
273 ///
274 /// The input mesh must be:
275 /// - **Manifold**: Each edge is shared by exactly 2 faces, no T-junctions, no holes
276 /// - **Closed**: The mesh completely encloses a volume with no gaps
277 /// - **Convex** (not enforced, but assumed): All faces bulge outward
278 /// - **Valid**: Faces use counter-clockwise winding when viewed from outside
279 ///
280 /// # When to use this
281 ///
282 /// Use this constructor when:
283 /// - You already have a triangulated convex mesh
284 /// - The mesh comes from a trusted source (e.g., generated procedurally)
285 /// - You want better performance by skipping convex hull computation
286 /// - You're converting from another 3D format (OBJ, STL, etc.)
287 ///
288 /// # Arguments
289 ///
290 /// * `points` - The vertex positions (3D coordinates)
291 /// * `indices` - The face triangles, each containing 3 indices into the `points` array
292 ///
293 /// # Returns
294 ///
295 /// - `Some(ConvexPolyhedron)` if successful
296 /// - `None` if the mesh is not manifold, has T-junctions, is not closed, or has invalid topology
297 ///
298 /// # Example: Creating a cube from vertices and faces
299 ///
300 /// ```
301 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
302 /// use parry3d::shape::ConvexPolyhedron;
303 /// use parry3d::math::Vector;
304 ///
305 /// // Define the 8 vertices of a unit cube
306 /// let vertices = vec![
307 /// Vector::ZERO, // 0: bottom-left-front
308 /// Vector::new(1.0, 0.0, 0.0), // 1: bottom-right-front
309 /// Vector::new(1.0, 1.0, 0.0), // 2: bottom-right-back
310 /// Vector::new(0.0, 1.0, 0.0), // 3: bottom-left-back
311 /// Vector::new(0.0, 0.0, 1.0), // 4: top-left-front
312 /// Vector::new(1.0, 0.0, 1.0), // 5: top-right-front
313 /// Vector::new(1.0, 1.0, 1.0), // 6: top-right-back
314 /// Vector::new(0.0, 1.0, 1.0), // 7: top-left-back
315 /// ];
316 ///
317 /// // Define the faces as triangles (2 triangles per cube face)
318 /// let indices = vec![
319 /// // Bottom face (z = 0)
320 /// [0, 2, 1], [0, 3, 2],
321 /// // Top face (z = 1)
322 /// [4, 5, 6], [4, 6, 7],
323 /// // Front face (y = 0)
324 /// [0, 1, 5], [0, 5, 4],
325 /// // Back face (y = 1)
326 /// [2, 3, 7], [2, 7, 6],
327 /// // Left face (x = 0)
328 /// [0, 4, 7], [0, 7, 3],
329 /// // Right face (x = 1)
330 /// [1, 2, 6], [1, 6, 5],
331 /// ];
332 ///
333 /// let cube = ConvexPolyhedron::from_convex_mesh(vertices, &indices)
334 /// .expect("Failed to create cube");
335 ///
336 /// // The cube has 8 vertices
337 /// assert_eq!(cube.points().len(), 8);
338 /// // The 12 triangles are merged into 6 square faces
339 /// assert_eq!(cube.faces().len(), 6);
340 /// # }
341 /// ```
342 ///
343 /// # Example: Creating a triangular prism
344 ///
345 /// ```
346 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
347 /// use parry3d::shape::ConvexPolyhedron;
348 /// use parry3d::math::Vector;
349 ///
350 /// // 6 vertices: 3 on bottom, 3 on top
351 /// let vertices = vec![
352 /// Vector::ZERO,
353 /// Vector::new(1.0, 0.0, 0.0),
354 /// Vector::new(0.5, 1.0, 0.0),
355 /// Vector::new(0.0, 0.0, 2.0),
356 /// Vector::new(1.0, 0.0, 2.0),
357 /// Vector::new(0.5, 1.0, 2.0),
358 /// ];
359 ///
360 /// let indices = vec![
361 /// // Bottom triangle
362 /// [0, 2, 1],
363 /// // Top triangle
364 /// [3, 4, 5],
365 /// // Side faces (2 triangles each)
366 /// [0, 1, 4], [0, 4, 3],
367 /// [1, 2, 5], [1, 5, 4],
368 /// [2, 0, 3], [2, 3, 5],
369 /// ];
370 ///
371 /// let prism = ConvexPolyhedron::from_convex_mesh(vertices, &indices)
372 /// .expect("Failed to create prism");
373 ///
374 /// assert_eq!(prism.points().len(), 6);
375 /// // 2 triangular faces + 3 rectangular faces = 5 faces
376 /// assert_eq!(prism.faces().len(), 5);
377 /// # }
378 /// ```
379 ///
380 /// [`from_convex_hull`]: ConvexPolyhedron::from_convex_hull
381 pub fn from_convex_mesh(
382 points: Vec<Vector>,
383 indices: &[[u32; DIM]],
384 ) -> Option<ConvexPolyhedron> {
385 let eps = crate::math::DEFAULT_EPSILON.sqrt();
386
387 let mut vertices = Vec::new();
388 let mut edges = Vec::<Edge>::new();
389 let mut faces = Vec::<Face>::new();
390 let mut triangles = Vec::new();
391 let mut edge_map = HashMap::default();
392
393 let mut faces_adj_to_vertex = Vec::new();
394 let mut edges_adj_to_vertex = Vec::new();
395 let mut edges_adj_to_face = Vec::new();
396 let mut vertices_adj_to_face = Vec::new();
397
398 if points.len() + indices.len() <= 2 {
399 return None;
400 }
401
402 //// Euler characteristic.
403 let nedges = points.len() + indices.len() - 2;
404 edges.reserve(nedges);
405
406 /*
407 * Initialize triangles and edges adjacency information.
408 */
409 for idx in indices {
410 let mut edges_id = [u32::MAX; DIM];
411 let face_id = triangles.len();
412
413 if idx[0] == idx[1] || idx[0] == idx[2] || idx[1] == idx[2] {
414 return None;
415 }
416
417 for i1 in 0..3 {
418 // Deal with edges.
419 let i2 = (i1 + 1) % 3;
420 let key = SortedPair::new(idx[i1], idx[i2]);
421
422 match edge_map.entry(key) {
423 Entry::Occupied(e) => {
424 let edge = &mut edges[*e.get() as usize];
425 let out_face_id = &mut edge.faces[1];
426
427 if *out_face_id == u32::MAX {
428 edges_id[i1] = *e.get();
429 *out_face_id = face_id as u32
430 } else {
431 // We have a t-junction.
432 return None;
433 }
434 }
435 Entry::Vacant(e) => {
436 edges_id[i1] = *e.insert(edges.len() as u32);
437
438 let dir =
439 (points[idx[i2] as usize] - points[idx[i1] as usize]).try_normalize();
440
441 edges.push(Edge {
442 vertices: [idx[i1], idx[i2]],
443 faces: [face_id as u32, u32::MAX],
444 dir: dir.unwrap_or(Vector::X),
445 deleted: dir.is_none(),
446 });
447 }
448 }
449 }
450
451 let normal = utils::ccw_face_normal([
452 points[idx[0] as usize],
453 points[idx[1] as usize],
454 points[idx[2] as usize],
455 ]);
456 let triangle = Triangle {
457 vertices: *idx,
458 edges: edges_id,
459 normal: normal.unwrap_or(Vector::ZERO),
460 parent_face: None,
461 is_degenerate: normal.is_none(),
462 };
463
464 triangles.push(triangle);
465 }
466
467 // Find edges that must be deleted.
468
469 for e in &mut edges {
470 let tri1 = triangles.get(e.faces[0] as usize)?;
471 let tri2 = triangles.get(e.faces[1] as usize)?;
472 if tri1.normal.dot(tri2.normal) > 1.0 - eps {
473 e.deleted = true;
474 }
475 }
476
477 /*
478 * Extract faces by following contours.
479 */
480 for i in 0..triangles.len() {
481 if triangles[i].parent_face.is_none() {
482 for j1 in 0..3 {
483 if !edges[triangles[i].edges[j1] as usize].deleted {
484 // Create a new face, setup its first edge/vertex and construct it.
485 let new_face_id = faces.len();
486 let mut new_face = Face {
487 first_vertex_or_edge: edges_adj_to_face.len() as u32,
488 num_vertices_or_edges: 1,
489 normal: triangles[i].normal,
490 };
491
492 edges_adj_to_face.push(triangles[i].edges[j1]);
493 vertices_adj_to_face.push(triangles[i].vertices[j1]);
494
495 let j2 = (j1 + 1) % 3;
496 let start_vertex = triangles[i].vertices[j1];
497
498 // NOTE: variables ending with _id are identifier on the
499 // fields of a triangle. Other variables are identifier on
500 // the triangles/edges/vertices arrays.
501 let mut curr_triangle = i;
502 let mut curr_edge_id = j2;
503
504 while triangles[curr_triangle].vertices[curr_edge_id] != start_vertex {
505 let curr_edge = triangles[curr_triangle].edges[curr_edge_id];
506 let curr_vertex = triangles[curr_triangle].vertices[curr_edge_id];
507 // NOTE: we should use this assertion. However, it can currently
508 // happen if there are some isolated non-deleted edges due to
509 // rounding errors.
510 //
511 // assert!(triangles[curr_triangle].parent_face.is_none());
512 triangles[curr_triangle].parent_face = Some(new_face_id as u32);
513
514 if !edges[curr_edge as usize].deleted {
515 edges_adj_to_face.push(curr_edge);
516 vertices_adj_to_face.push(curr_vertex);
517 new_face.num_vertices_or_edges += 1;
518
519 curr_edge_id = (curr_edge_id + 1) % 3;
520 } else {
521 // Find adjacent edge on the next triangle.
522 curr_triangle = edges[curr_edge as usize]
523 .other_triangle(curr_triangle as u32)
524 as usize;
525 curr_edge_id =
526 triangles[curr_triangle].next_edge_id(curr_edge) as usize;
527 assert!(
528 triangles[curr_triangle].vertices[curr_edge_id] == curr_vertex
529 );
530 }
531 }
532
533 if new_face.num_vertices_or_edges > 2 {
534 // Sometimes degenerate faces may be generated
535 // due to numerical errors resulting in an isolated
536 // edge not being deleted.
537 //
538 // This kind of degenerate faces are not valid.
539 faces.push(new_face);
540 }
541 break;
542 }
543 }
544 }
545 }
546
547 // Update face ids inside edges so that they point to the faces instead of the triangles.
548 for e in &mut edges {
549 if let Some(fid) = triangles.get(e.faces[0] as usize)?.parent_face {
550 e.faces[0] = fid;
551 }
552
553 if let Some(fid) = triangles.get(e.faces[1] as usize)?.parent_face {
554 e.faces[1] = fid;
555 }
556 }
557
558 /*
559 * Initialize vertices
560 */
561 let empty_vertex = Vertex {
562 first_adj_face_or_edge: 0,
563 num_adj_faces_or_edge: 0,
564 };
565
566 vertices.resize(points.len(), empty_vertex);
567
568 // First, find their multiplicities.
569 for face in &faces {
570 let first_vid = face.first_vertex_or_edge;
571 let last_vid = face.first_vertex_or_edge + face.num_vertices_or_edges;
572
573 for i in &vertices_adj_to_face[first_vid as usize..last_vid as usize] {
574 vertices[*i as usize].num_adj_faces_or_edge += 1;
575 }
576 }
577
578 // Now, find their starting id.
579 let mut total_num_adj_faces = 0;
580 for v in &mut vertices {
581 v.first_adj_face_or_edge = total_num_adj_faces;
582 total_num_adj_faces += v.num_adj_faces_or_edge;
583 }
584 faces_adj_to_vertex.resize(total_num_adj_faces as usize, 0);
585 edges_adj_to_vertex.resize(total_num_adj_faces as usize, 0);
586
587 // Reset the number of adjacent faces.
588 // It will be set again to the right value as
589 // the adjacent face list is filled.
590 for v in &mut vertices {
591 v.num_adj_faces_or_edge = 0;
592 }
593
594 for (face_id, face) in faces.iter().enumerate() {
595 let first_vid = face.first_vertex_or_edge;
596 let last_vid = face.first_vertex_or_edge + face.num_vertices_or_edges;
597
598 for vid in first_vid..last_vid {
599 let v = &mut vertices[vertices_adj_to_face[vid as usize] as usize];
600 faces_adj_to_vertex
601 [(v.first_adj_face_or_edge + v.num_adj_faces_or_edge) as usize] =
602 face_id as u32;
603 edges_adj_to_vertex
604 [(v.first_adj_face_or_edge + v.num_adj_faces_or_edge) as usize] =
605 edges_adj_to_face[vid as usize];
606 v.num_adj_faces_or_edge += 1;
607 }
608 }
609
610 // Note numerical errors may throw off the Euler characteristic.
611 // So we don't check it right now.
612
613 let res = ConvexPolyhedron {
614 points,
615 vertices,
616 faces,
617 edges,
618 faces_adj_to_vertex,
619 edges_adj_to_vertex,
620 edges_adj_to_face,
621 vertices_adj_to_face,
622 };
623
624 // TODO: for debug.
625 // res.check_geometry();
626
627 Some(res)
628 }
629
630 /// Verify if this convex polyhedron is actually convex.
631 #[inline]
632 pub fn check_geometry(&self) {
633 for face in &self.faces {
634 let p0 =
635 self.points[self.vertices_adj_to_face[face.first_vertex_or_edge as usize] as usize];
636
637 for v in &self.points {
638 assert!((v - p0).dot(face.normal) <= crate::math::DEFAULT_EPSILON);
639 }
640 }
641 }
642
643 /// Returns the 3D coordinates of all vertices in this convex polyhedron.
644 ///
645 /// Each point represents a corner of the polyhedron where three or more edges meet.
646 ///
647 /// # Example
648 ///
649 /// ```
650 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
651 /// use parry3d::shape::ConvexPolyhedron;
652 /// use parry3d::math::Vector;
653 ///
654 /// let points = vec![
655 /// Vector::ZERO,
656 /// Vector::new(1.0, 0.0, 0.0),
657 /// Vector::new(0.5, 1.0, 0.0),
658 /// Vector::new(0.5, 0.5, 1.0),
659 /// ];
660 /// let indices = vec![[0u32, 1, 2], [0, 1, 3], [1, 2, 3], [2, 0, 3]];
661 ///
662 /// let tetrahedron = ConvexPolyhedron::from_convex_mesh(points, &indices).unwrap();
663 ///
664 /// assert_eq!(tetrahedron.points().len(), 4);
665 /// assert_eq!(tetrahedron.points()[0], Vector::ZERO);
666 /// # }
667 /// ```
668 #[inline]
669 pub fn points(&self) -> &[Vector] {
670 &self.points[..]
671 }
672
673 /// Returns the topology information for all vertices.
674 ///
675 /// Each `Vertex` contains indices into the adjacency arrays, telling you which
676 /// faces and edges are connected to that vertex. This is useful for advanced
677 /// topological queries and mesh processing algorithms.
678 ///
679 /// Most users will want [`points()`] instead to get the 3D coordinates.
680 ///
681 /// [`points()`]: ConvexPolyhedron::points
682 #[inline]
683 pub fn vertices(&self) -> &[Vertex] {
684 &self.vertices[..]
685 }
686
687 /// Returns the topology information for all edges.
688 ///
689 /// Each `Edge` contains:
690 /// - The two vertex indices it connects
691 /// - The two face indices it borders
692 /// - The edge direction as a unit vector
693 ///
694 /// This is useful for advanced geometric queries and rendering wireframes.
695 ///
696 /// # Example
697 ///
698 /// ```
699 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
700 /// use parry3d::shape::ConvexPolyhedron;
701 /// use parry3d::math::Vector;
702 ///
703 /// let points = vec![
704 /// Vector::ZERO,
705 /// Vector::new(1.0, 0.0, 0.0),
706 /// Vector::new(0.5, 1.0, 0.0),
707 /// Vector::new(0.5, 0.5, 1.0),
708 /// ];
709 /// let indices = vec![[0u32, 1, 2], [0, 1, 3], [1, 2, 3], [2, 0, 3]];
710 ///
711 /// let tetrahedron = ConvexPolyhedron::from_convex_mesh(points, &indices).unwrap();
712 ///
713 /// // A tetrahedron has 6 edges (4 vertices choose 2)
714 /// assert_eq!(tetrahedron.edges().len(), 6);
715 ///
716 /// // Each edge connects two vertices
717 /// let edge = &tetrahedron.edges()[0];
718 /// println!("Edge connects vertices {} and {}", edge.vertices[0], edge.vertices[1]);
719 /// # }
720 /// ```
721 #[inline]
722 pub fn edges(&self) -> &[Edge] {
723 &self.edges[..]
724 }
725
726 /// Returns the topology information for all faces.
727 ///
728 /// Each `Face` contains:
729 /// - Indices into the vertex and edge adjacency arrays
730 /// - The number of vertices/edges in the face (faces can be triangles, quads, or larger polygons)
731 /// - The outward-pointing unit normal vector
732 ///
733 /// Faces are polygonal and can have 3 or more vertices. Adjacent coplanar triangles
734 /// are automatically merged into larger polygonal faces.
735 ///
736 /// # Example
737 ///
738 /// ```
739 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
740 /// use parry3d::shape::ConvexPolyhedron;
741 /// use parry3d::math::Vector;
742 ///
743 /// // Create a cube (8 vertices, 12 triangular input faces)
744 /// let vertices = vec![
745 /// Vector::ZERO, Vector::new(1.0, 0.0, 0.0),
746 /// Vector::new(1.0, 1.0, 0.0), Vector::new(0.0, 1.0, 0.0),
747 /// Vector::new(0.0, 0.0, 1.0), Vector::new(1.0, 0.0, 1.0),
748 /// Vector::new(1.0, 1.0, 1.0), Vector::new(0.0, 1.0, 1.0),
749 /// ];
750 /// let indices = vec![
751 /// [0, 2, 1], [0, 3, 2], [4, 5, 6], [4, 6, 7],
752 /// [0, 1, 5], [0, 5, 4], [2, 3, 7], [2, 7, 6],
753 /// [0, 4, 7], [0, 7, 3], [1, 2, 6], [1, 6, 5],
754 /// ];
755 ///
756 /// let cube = ConvexPolyhedron::from_convex_mesh(vertices, &indices).unwrap();
757 ///
758 /// // The 12 triangles are merged into 6 square faces
759 /// assert_eq!(cube.faces().len(), 6);
760 ///
761 /// // Each face has 4 vertices (it's a square)
762 /// assert_eq!(cube.faces()[0].num_vertices_or_edges, 4);
763 /// # }
764 /// ```
765 #[inline]
766 pub fn faces(&self) -> &[Face] {
767 &self.faces[..]
768 }
769
770 /// The array containing the indices of the vertices adjacent to each face.
771 #[inline]
772 pub fn vertices_adj_to_face(&self) -> &[u32] {
773 &self.vertices_adj_to_face[..]
774 }
775
776 /// The array containing the indices of the edges adjacent to each face.
777 #[inline]
778 pub fn edges_adj_to_face(&self) -> &[u32] {
779 &self.edges_adj_to_face[..]
780 }
781
782 /// The array containing the indices of the faces adjacent to each vertex.
783 #[inline]
784 pub fn faces_adj_to_vertex(&self) -> &[u32] {
785 &self.faces_adj_to_vertex[..]
786 }
787
788 /// Computes a scaled version of this convex polyhedron.
789 ///
790 /// This method scales the polyhedron by multiplying each vertex coordinate by the corresponding
791 /// component of the `scale` vector. This allows for non-uniform scaling (different scale factors
792 /// for x, y, and z axes).
793 ///
794 /// The face normals and edge directions are also updated to reflect the scaling transformation.
795 ///
796 /// # Returns
797 ///
798 /// - `Some(ConvexPolyhedron)` with the scaled shape
799 /// - `None` if the scaling results in degenerate normals (e.g., if the scale factor along
800 /// one axis is zero or nearly zero)
801 ///
802 /// # Example: Uniform scaling
803 ///
804 /// ```
805 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
806 /// use parry3d::shape::ConvexPolyhedron;
807 /// use parry3d::math::Vector;
808 ///
809 /// let points = vec![
810 /// Vector::ZERO,
811 /// Vector::new(1.0, 0.0, 0.0),
812 /// Vector::new(0.5, 1.0, 0.0),
813 /// Vector::new(0.5, 0.5, 1.0),
814 /// ];
815 /// let indices = vec![[0u32, 1, 2], [0, 1, 3], [1, 2, 3], [2, 0, 3]];
816 ///
817 /// let tetrahedron = ConvexPolyhedron::from_convex_mesh(points, &indices).unwrap();
818 ///
819 /// // Scale uniformly by 2x
820 /// let scaled = tetrahedron.scaled(Vector::new(2.0, 2.0, 2.0))
821 /// .expect("Failed to scale");
822 ///
823 /// // All coordinates are doubled
824 /// assert_eq!(scaled.points()[1], Vector::new(2.0, 0.0, 0.0));
825 /// assert_eq!(scaled.points()[3], Vector::new(1.0, 1.0, 2.0));
826 /// # }
827 /// ```
828 ///
829 /// # Example: Non-uniform scaling to create a rectangular prism
830 ///
831 /// ```
832 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
833 /// use parry3d::shape::ConvexPolyhedron;
834 /// use parry3d::math::Vector;
835 ///
836 /// // Start with a unit cube
837 /// let vertices = vec![
838 /// Vector::ZERO, Vector::new(1.0, 0.0, 0.0),
839 /// Vector::new(1.0, 1.0, 0.0), Vector::new(0.0, 1.0, 0.0),
840 /// Vector::new(0.0, 0.0, 1.0), Vector::new(1.0, 0.0, 1.0),
841 /// Vector::new(1.0, 1.0, 1.0), Vector::new(0.0, 1.0, 1.0),
842 /// ];
843 /// let indices = vec![
844 /// [0, 2, 1], [0, 3, 2], [4, 5, 6], [4, 6, 7],
845 /// [0, 1, 5], [0, 5, 4], [2, 3, 7], [2, 7, 6],
846 /// [0, 4, 7], [0, 7, 3], [1, 2, 6], [1, 6, 5],
847 /// ];
848 ///
849 /// let cube = ConvexPolyhedron::from_convex_mesh(vertices, &indices).unwrap();
850 ///
851 /// // Scale to make it wider (3x), deeper (2x), and taller (4x)
852 /// let box_shape = cube.scaled(Vector::new(3.0, 2.0, 4.0))
853 /// .expect("Failed to scale");
854 ///
855 /// assert_eq!(box_shape.points()[6], Vector::new(3.0, 2.0, 4.0));
856 /// # }
857 /// ```
858 pub fn scaled(mut self, scale: Vector) -> Option<Self> {
859 self.points.iter_mut().for_each(|pt| *pt *= scale);
860
861 for f in &mut self.faces {
862 f.normal = (f.normal * scale).try_normalize().unwrap_or(f.normal);
863 }
864
865 for e in &mut self.edges {
866 e.dir = (e.dir * scale).try_normalize().unwrap_or(e.dir);
867 }
868
869 Some(self)
870 }
871
872 fn support_feature_id_toward_eps(&self, local_dir: Vector, eps: Real) -> FeatureId {
873 let (seps, ceps) = eps.sin_cos();
874 let support_pt_id = utils::point_cloud_support_point_id(local_dir, &self.points);
875 let vertex = &self.vertices[support_pt_id];
876
877 // Check faces.
878 for i in 0..vertex.num_adj_faces_or_edge {
879 let face_id = self.faces_adj_to_vertex[(vertex.first_adj_face_or_edge + i) as usize];
880 let face = &self.faces[face_id as usize];
881
882 if face.normal.dot(local_dir) >= ceps {
883 return FeatureId::Face(face_id);
884 }
885 }
886
887 // Check edges.
888 for i in 0..vertex.num_adj_faces_or_edge {
889 let edge_id = self.edges_adj_to_vertex[(vertex.first_adj_face_or_edge + i) as usize];
890 let edge = &self.edges[edge_id as usize];
891
892 if edge.dir.dot(local_dir).abs() <= seps {
893 return FeatureId::Edge(edge_id);
894 }
895 }
896
897 // The vertex is the support feature.
898 FeatureId::Vertex(support_pt_id as u32)
899 }
900
901 /// Computes the ID of the features with a normal that maximize the dot-product with `local_dir`.
902 pub fn support_feature_id_toward(&self, local_dir: Vector) -> FeatureId {
903 #[cfg_attr(feature = "f64", expect(clippy::unnecessary_cast))]
904 let eps: Real = (f64::consts::PI / 180.0) as Real;
905 self.support_feature_id_toward_eps(local_dir, eps)
906 }
907
908 /// The normal of the given feature.
909 pub fn feature_normal(&self, feature: FeatureId) -> Option<Vector> {
910 match feature {
911 FeatureId::Face(id) => Some(self.faces[id as usize].normal),
912 FeatureId::Edge(id) => {
913 let edge = &self.edges[id as usize];
914 Some(
915 (self.faces[edge.faces[0] as usize].normal
916 + self.faces[edge.faces[1] as usize].normal)
917 .normalize(),
918 )
919 }
920 FeatureId::Vertex(id) => {
921 let vertex = &self.vertices[id as usize];
922 let first = vertex.first_adj_face_or_edge;
923 let last = vertex.first_adj_face_or_edge + vertex.num_adj_faces_or_edge;
924 let mut normal = Vector::ZERO;
925
926 for face in &self.faces_adj_to_vertex[first as usize..last as usize] {
927 normal += self.faces[*face as usize].normal
928 }
929
930 Some((normal).normalize())
931 }
932 FeatureId::Unknown => None,
933 }
934 }
935}
936
937impl SupportMap for ConvexPolyhedron {
938 #[inline]
939 fn local_support_point(&self, dir: Vector) -> Vector {
940 utils::point_cloud_support_point(dir, self.points())
941 }
942}
943
944impl PolygonalFeatureMap for ConvexPolyhedron {
945 fn local_support_feature(&self, dir: Vector, out_feature: &mut PolygonalFeature) {
946 let mut best_fid = 0;
947 let mut best_dot = self.faces[0].normal.dot(dir);
948
949 for (fid, face) in self.faces[1..].iter().enumerate() {
950 let new_dot = face.normal.dot(dir);
951
952 if new_dot > best_dot {
953 best_fid = fid + 1;
954 best_dot = new_dot;
955 }
956 }
957
958 let face = &self.faces[best_fid];
959 let i1 = face.first_vertex_or_edge;
960 // TODO: if there are more than 4 vertices, we need to select four vertices that maximize the area.
961 let num_vertices = face.num_vertices_or_edges.min(4);
962 let i2 = i1 + num_vertices;
963
964 for (i, (vid, eid)) in self.vertices_adj_to_face[i1 as usize..i2 as usize]
965 .iter()
966 .zip(self.edges_adj_to_face[i1 as usize..i2 as usize].iter())
967 .enumerate()
968 {
969 out_feature.vertices[i] = self.points[*vid as usize];
970 out_feature.vids[i] = PackedFeatureId::vertex(*vid);
971 out_feature.eids[i] = PackedFeatureId::edge(*eid);
972 }
973
974 out_feature.fid = PackedFeatureId::face(best_fid as u32);
975 out_feature.num_vertices = num_vertices as usize;
976 }
977
978 fn is_convex_polyhedron(&self) -> bool {
979 true
980 }
981}
982
983/*
984impl ConvexPolyhedron for ConvexPolyhedron {
985 fn vertex(&self, id: FeatureId) -> Vector {
986 self.points[id.unwrap_vertex() as usize]
987 }
988
989 fn edge(&self, id: FeatureId) -> (Vector, Vector, FeatureId, FeatureId) {
990 let edge = &self.edges[id.unwrap_edge() as usize];
991 let v1 = edge.vertices[0];
992 let v2 = edge.vertices[1];
993
994 (
995 self.points[v1 as usize],
996 self.points[v2 as usize],
997 FeatureId::Vertex(v1),
998 FeatureId::Vertex(v2),
999 )
1000 }
1001
1002 fn face(&self, id: FeatureId, out: &mut ConvexPolygonalFeature) {
1003 out.clear();
1004
1005 let face = &self.faces[id.unwrap_face() as usize];
1006 let first_vertex = face.first_vertex_or_edge;
1007 let last_vertex = face.first_vertex_or_edge + face.num_vertices_or_edges;
1008
1009 for i in first_vertex..last_vertex {
1010 let vid = self.vertices_adj_to_face[i];
1011 let eid = self.edges_adj_to_face[i];
1012 out.push(self.points[vid], FeatureId::Vertex(vid));
1013 out.push_edge_feature_id(FeatureId::Edge(eid));
1014 }
1015
1016 out.set_normal(face.normal);
1017 out.set_feature_id(id);
1018 out.recompute_edge_normals();
1019 }
1020
1021 fn support_face_toward(
1022 &self,
1023 m: &Pose,
1024 dir: Vector,
1025 out: &mut ConvexPolygonalFeature,
1026 ) {
1027 let ls_dir = m.inverse_transform_vector(dir);
1028 let mut best_face = 0;
1029 let mut max_dot = self.faces[0].normal.dot(ls_dir);
1030
1031 for i in 1..self.faces.len() {
1032 let face = &self.faces[i];
1033 let dot = face.normal.dot(ls_dir);
1034
1035 if dot > max_dot {
1036 max_dot = dot;
1037 best_face = i;
1038 }
1039 }
1040
1041 self.face(FeatureId::Face(best_face), out);
1042 out.transform_by(m);
1043 }
1044
1045 fn support_feature_toward(
1046 &self,
1047 transform: &Pose,
1048 dir: Vector,
1049 angle: Real,
1050 out: &mut ConvexPolygonalFeature,
1051 ) {
1052 out.clear();
1053 let local_dir = transform.inverse_transform_unit_vector(dir);
1054 let fid = self.support_feature_id_toward_eps(&local_dir, angle);
1055
1056 match fid {
1057 FeatureId::Vertex(_) => {
1058 let v = self.vertex(fid);
1059 out.push(v, fid);
1060 out.set_feature_id(fid);
1061 }
1062 FeatureId::Edge(_) => {
1063 let edge = self.edge(fid);
1064 out.push(edge.0, edge.2);
1065 out.push(edge.1, edge.3);
1066 out.set_feature_id(fid);
1067 out.push_edge_feature_id(fid);
1068 }
1069 FeatureId::Face(_) => self.face(fid, out),
1070 FeatureId::Unknown => unreachable!(),
1071 }
1072
1073 out.transform_by(transform);
1074 }
1075}
1076*/