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