use crate::math::{Point, Real, Vector, DIM};
use crate::shape::{FeatureId, PolygonalFeature, PolygonalFeatureMap, SupportMap};
use crate::utils::hashmap::{Entry, HashMap};
use crate::utils::{self, SortedPair};
use na::{self, ComplexField, Point2, Unit};
use std::f64;
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
#[derive(PartialEq, Debug, Copy, Clone)]
pub struct Vertex {
pub first_adj_face_or_edge: u32,
pub num_adj_faces_or_edge: u32,
}
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
#[derive(PartialEq, Debug, Copy, Clone)]
pub struct Edge {
pub vertices: Point2<u32>,
pub faces: Point2<u32>,
pub dir: Unit<Vector<Real>>,
deleted: bool,
}
impl Edge {
fn other_triangle(&self, id: u32) -> u32 {
if id == self.faces[0] {
self.faces[1]
} else {
self.faces[0]
}
}
}
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
#[derive(PartialEq, Debug, Copy, Clone)]
pub struct Face {
pub first_vertex_or_edge: u32,
pub num_vertices_or_edges: u32,
pub normal: Unit<Vector<Real>>,
}
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
#[derive(PartialEq, Debug, Copy, Clone)]
struct Triangle {
vertices: [u32; 3],
edges: [u32; 3],
normal: Vector<Real>,
parent_face: Option<u32>,
is_degenerate: bool,
}
impl Triangle {
fn next_edge_id(&self, id: u32) -> u32 {
for i in 0..3 {
if self.edges[i] == id {
return (i as u32 + 1) % 3;
}
}
unreachable!()
}
}
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
#[derive(PartialEq, Debug, Clone)]
pub struct ConvexPolyhedron {
points: Vec<Point<Real>>,
vertices: Vec<Vertex>,
faces: Vec<Face>,
edges: Vec<Edge>,
faces_adj_to_vertex: Vec<u32>,
edges_adj_to_vertex: Vec<u32>,
edges_adj_to_face: Vec<u32>,
vertices_adj_to_face: Vec<u32>,
}
impl ConvexPolyhedron {
pub fn from_convex_hull(points: &[Point<Real>]) -> Option<ConvexPolyhedron> {
let (vertices, indices) = crate::transformation::convex_hull(points);
Self::from_convex_mesh(vertices, &indices)
}
pub fn from_convex_mesh(
points: Vec<Point<Real>>,
indices: &[[u32; DIM]],
) -> Option<ConvexPolyhedron> {
let eps = ComplexField::sqrt(crate::math::DEFAULT_EPSILON);
let mut vertices = Vec::new();
let mut edges = Vec::<Edge>::new();
let mut faces = Vec::<Face>::new();
let mut triangles = Vec::new();
let mut edge_map = HashMap::default();
let mut faces_adj_to_vertex = Vec::new();
let mut edges_adj_to_vertex = Vec::new();
let mut edges_adj_to_face = Vec::new();
let mut vertices_adj_to_face = Vec::new();
let nedges = points.len() + indices.len() - 2;
edges.reserve(nedges);
for vtx in indices {
let mut edges_id = [u32::MAX; DIM];
let face_id = triangles.len();
assert!(vtx[0] != vtx[1]);
assert!(vtx[0] != vtx[2]);
assert!(vtx[2] != vtx[1]);
for i1 in 0..3 {
let i2 = (i1 + 1) % 3;
let key = SortedPair::new(vtx[i1], vtx[i2]);
match edge_map.entry(key) {
Entry::Occupied(e) => {
let edge = &mut edges[*e.get() as usize];
let out_face_id = &mut edge.faces[1];
if *out_face_id == u32::MAX {
edges_id[i1] = *e.get();
*out_face_id = face_id as u32
} else {
return None;
}
}
Entry::Vacant(e) => {
edges_id[i1] = *e.insert(edges.len() as u32);
let dir = Unit::try_new(
points[vtx[i2] as usize] - points[vtx[i1] as usize],
crate::math::DEFAULT_EPSILON,
);
edges.push(Edge {
vertices: Point2::new(vtx[i1], vtx[i2]),
faces: Point2::new(face_id as u32, u32::MAX),
dir: dir.unwrap_or(Vector::x_axis()),
deleted: dir.is_none(),
});
}
}
}
let normal = utils::ccw_face_normal([
&points[vtx[0] as usize],
&points[vtx[1] as usize],
&points[vtx[2] as usize],
]);
let triangle = Triangle {
vertices: *vtx,
edges: edges_id,
normal: normal.map(|n| *n).unwrap_or(Vector::zeros()),
parent_face: None,
is_degenerate: normal.is_none(),
};
triangles.push(triangle);
}
for e in &mut edges {
let tri1 = triangles.get(e.faces[0] as usize)?;
let tri2 = triangles.get(e.faces[1] as usize)?;
if tri1.normal.dot(&tri2.normal) > 1.0 - eps {
e.deleted = true;
}
}
for i in 0..triangles.len() {
if triangles[i].parent_face.is_none() {
for j1 in 0..3 {
if !edges[triangles[i].edges[j1] as usize].deleted {
let new_face_id = faces.len();
let mut new_face = Face {
first_vertex_or_edge: edges_adj_to_face.len() as u32,
num_vertices_or_edges: 1,
normal: Unit::new_unchecked(triangles[i].normal),
};
edges_adj_to_face.push(triangles[i].edges[j1]);
vertices_adj_to_face.push(triangles[i].vertices[j1]);
let j2 = (j1 + 1) % 3;
let start_vertex = triangles[i].vertices[j1];
let mut curr_triangle = i;
let mut curr_edge_id = j2;
while triangles[curr_triangle].vertices[curr_edge_id] != start_vertex {
let curr_edge = triangles[curr_triangle].edges[curr_edge_id];
let curr_vertex = triangles[curr_triangle].vertices[curr_edge_id];
triangles[curr_triangle].parent_face = Some(new_face_id as u32);
if !edges[curr_edge as usize].deleted {
edges_adj_to_face.push(curr_edge);
vertices_adj_to_face.push(curr_vertex);
new_face.num_vertices_or_edges += 1;
curr_edge_id = (curr_edge_id + 1) % 3;
} else {
curr_triangle = edges[curr_edge as usize]
.other_triangle(curr_triangle as u32)
as usize;
curr_edge_id =
triangles[curr_triangle].next_edge_id(curr_edge) as usize;
assert!(
triangles[curr_triangle].vertices[curr_edge_id] == curr_vertex
);
}
}
if new_face.num_vertices_or_edges > 2 {
faces.push(new_face);
}
break;
}
}
}
}
for e in &mut edges {
if let Some(fid) = triangles.get(e.faces[0] as usize)?.parent_face {
e.faces[0] = fid;
}
if let Some(fid) = triangles.get(e.faces[1] as usize)?.parent_face {
e.faces[1] = fid;
}
}
let empty_vertex = Vertex {
first_adj_face_or_edge: 0,
num_adj_faces_or_edge: 0,
};
vertices.resize(points.len(), empty_vertex);
for face in &faces {
let first_vid = face.first_vertex_or_edge;
let last_vid = face.first_vertex_or_edge + face.num_vertices_or_edges;
for i in &vertices_adj_to_face[first_vid as usize..last_vid as usize] {
vertices[*i as usize].num_adj_faces_or_edge += 1;
}
}
let mut total_num_adj_faces = 0;
for v in &mut vertices {
v.first_adj_face_or_edge = total_num_adj_faces;
total_num_adj_faces += v.num_adj_faces_or_edge;
}
faces_adj_to_vertex.resize(total_num_adj_faces as usize, 0);
edges_adj_to_vertex.resize(total_num_adj_faces as usize, 0);
for v in &mut vertices {
v.num_adj_faces_or_edge = 0;
}
for face_id in 0..faces.len() {
let face = &faces[face_id];
let first_vid = face.first_vertex_or_edge;
let last_vid = face.first_vertex_or_edge + face.num_vertices_or_edges;
for vid in first_vid..last_vid {
let v = &mut vertices[vertices_adj_to_face[vid as usize] as usize];
faces_adj_to_vertex
[(v.first_adj_face_or_edge + v.num_adj_faces_or_edge) as usize] =
face_id as u32;
edges_adj_to_vertex
[(v.first_adj_face_or_edge + v.num_adj_faces_or_edge) as usize] =
edges_adj_to_face[vid as usize];
v.num_adj_faces_or_edge += 1;
}
}
let res = ConvexPolyhedron {
points,
vertices,
faces,
edges,
faces_adj_to_vertex,
edges_adj_to_vertex,
edges_adj_to_face,
vertices_adj_to_face,
};
Some(res)
}
#[inline]
pub fn check_geometry(&self) {
for face in &self.faces {
let p0 =
self.points[self.vertices_adj_to_face[face.first_vertex_or_edge as usize] as usize];
for v in &self.points {
assert!((v - p0).dot(face.normal.as_ref()) <= crate::math::DEFAULT_EPSILON);
}
}
}
#[inline]
pub fn points(&self) -> &[Point<Real>] {
&self.points[..]
}
#[inline]
pub fn vertices(&self) -> &[Vertex] {
&self.vertices[..]
}
#[inline]
pub fn edges(&self) -> &[Edge] {
&self.edges[..]
}
#[inline]
pub fn faces(&self) -> &[Face] {
&self.faces[..]
}
#[inline]
pub fn vertices_adj_to_face(&self) -> &[u32] {
&self.vertices_adj_to_face[..]
}
#[inline]
pub fn edges_adj_to_face(&self) -> &[u32] {
&self.edges_adj_to_face[..]
}
#[inline]
pub fn faces_adj_to_vertex(&self) -> &[u32] {
&self.faces_adj_to_vertex[..]
}
fn support_feature_id_toward_eps(
&self,
local_dir: &Unit<Vector<Real>>,
eps: Real,
) -> FeatureId {
let (seps, ceps) = ComplexField::sin_cos(eps);
let support_pt_id = utils::point_cloud_support_point_id(local_dir.as_ref(), &self.points);
let vertex = &self.vertices[support_pt_id];
for i in 0..vertex.num_adj_faces_or_edge {
let face_id = self.faces_adj_to_vertex[(vertex.first_adj_face_or_edge + i) as usize];
let face = &self.faces[face_id as usize];
if face.normal.dot(local_dir.as_ref()) >= ceps {
return FeatureId::Face(face_id);
}
}
for i in 0..vertex.num_adj_faces_or_edge {
let edge_id = self.edges_adj_to_vertex[(vertex.first_adj_face_or_edge + i) as usize];
let edge = &self.edges[edge_id as usize];
if edge.dir.dot(local_dir.as_ref()).abs() <= seps {
return FeatureId::Edge(edge_id);
}
}
FeatureId::Vertex(support_pt_id as u32)
}
pub fn support_feature_id_toward(&self, local_dir: &Unit<Vector<Real>>) -> FeatureId {
let eps: Real = na::convert::<f64, Real>(f64::consts::PI / 180.0);
self.support_feature_id_toward_eps(local_dir, eps)
}
}
impl SupportMap for ConvexPolyhedron {
#[inline]
fn local_support_point(&self, dir: &Vector<Real>) -> Point<Real> {
utils::point_cloud_support_point(dir, self.points())
}
}
impl PolygonalFeatureMap for ConvexPolyhedron {
fn local_support_feature(&self, dir: &Unit<Vector<Real>>, out_feature: &mut PolygonalFeature) {
let mut best_fid = 0;
let mut best_dot = self.faces[0].normal.dot(dir);
for (fid, face) in self.faces[1..].iter().enumerate() {
let new_dot = face.normal.dot(dir);
if new_dot > best_dot {
best_fid = fid + 1;
best_dot = new_dot;
}
}
let face = &self.faces[best_fid];
let i1 = face.first_vertex_or_edge;
let num_vertices = face.num_vertices_or_edges.min(4);
let i2 = i1 + num_vertices;
for (i, (vid, eid)) in self.vertices_adj_to_face[i1 as usize..i2 as usize]
.iter()
.zip(self.edges_adj_to_face[i1 as usize..i2 as usize].iter())
.enumerate()
{
out_feature.vertices[i] = self.points[*vid as usize];
out_feature.vids[i] = *vid;
out_feature.eids[i] = *eid;
}
out_feature.fid = best_fid as u32;
out_feature.num_vertices = num_vertices as usize;
}
}