#[allow(unused_imports)]
use super::functions::*;
#[allow(unused_imports)]
use super::functions_2::*;
pub use super::specialized::*;
#[allow(dead_code)]
#[derive(Debug, Clone)]
pub struct RayCastResult {
pub hit: bool,
pub toi: f64,
pub hit_point: [f64; 3],
pub normal: [f64; 3],
}
#[allow(dead_code)]
#[derive(Debug, Clone)]
pub struct NarrowPhaseContact {
pub normal: [f64; 3],
pub depth: f64,
pub point_a: [f64; 3],
pub point_b: [f64; 3],
}
impl NarrowPhaseContact {
#[allow(dead_code)]
pub fn flipped(&self) -> Self {
Self {
normal: scale3(self.normal, -1.0),
depth: self.depth,
point_a: self.point_b,
point_b: self.point_a,
}
}
#[allow(dead_code)]
pub fn midpoint(&self) -> [f64; 3] {
scale3(add3(self.point_a, self.point_b), 0.5)
}
}
#[allow(dead_code)]
#[derive(Debug, Clone)]
pub struct CompoundShape {
pub children: Vec<ShapeKind>,
}
impl CompoundShape {
#[allow(dead_code)]
pub fn new(children: Vec<ShapeKind>) -> Self {
CompoundShape { children }
}
#[allow(dead_code)]
pub fn aabb(&self) -> ([f64; 3], [f64; 3]) {
let mut mn = [f64::INFINITY; 3];
let mut mx = [f64::NEG_INFINITY; 3];
for child in &self.children {
let (cmin, cmax) = child.aabb();
for i in 0..3 {
mn[i] = mn[i].min(cmin[i]);
mx[i] = mx[i].max(cmax[i]);
}
}
(mn, mx)
}
}
#[allow(dead_code)]
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum ContactFeature {
FaceFace {
face_a: u32,
face_b: u32,
},
FaceEdge {
face: u32,
edge: u32,
},
EdgeEdge {
edge_a: u32,
edge_b: u32,
},
VertexFace {
vertex: u32,
face: u32,
},
Unknown,
}
#[allow(dead_code)]
#[derive(Debug, Clone)]
pub struct ContactFilter {
pub min_depth: f64,
pub max_depth: f64,
pub flip_normal: bool,
}
impl ContactFilter {
#[allow(dead_code)]
pub fn apply(&self, mut c: NarrowPhaseContact) -> Option<NarrowPhaseContact> {
if c.depth < self.min_depth {
return None;
}
if c.depth > self.max_depth {
c.depth = self.max_depth;
}
if self.flip_normal {
c.normal = scale3(c.normal, -1.0);
}
Some(c)
}
}
#[allow(dead_code)]
#[derive(Debug, Clone)]
pub struct FeatureContact {
pub contact: NarrowPhaseContact,
pub feature: ContactFeature,
}
impl FeatureContact {
#[allow(dead_code)]
pub fn from_plain(c: NarrowPhaseContact) -> Self {
FeatureContact {
contact: c,
feature: ContactFeature::Unknown,
}
}
}
#[allow(dead_code)]
#[derive(Default)]
pub struct BatchNarrowPhase {
pub filter: ContactFilter,
}
impl BatchNarrowPhase {
#[allow(dead_code)]
pub fn new() -> Self {
Self::default()
}
#[allow(dead_code)]
pub fn run(
&self,
shapes: &[ShapeKind],
pairs: &[(usize, usize)],
) -> Vec<Option<NarrowPhaseContact>> {
pairs
.iter()
.map(|(i, j)| {
if *i >= shapes.len() || *j >= shapes.len() {
return None;
}
let contact = shape_shape_contact(&shapes[*i], &shapes[*j])?;
self.filter.apply(contact)
})
.collect()
}
#[allow(dead_code)]
pub fn run_compact(
&self,
shapes: &[ShapeKind],
pairs: &[(usize, usize)],
) -> Vec<((usize, usize), NarrowPhaseContact)> {
pairs
.iter()
.filter_map(|(i, j)| {
if *i >= shapes.len() || *j >= shapes.len() {
return None;
}
let contact = shape_shape_contact(&shapes[*i], &shapes[*j])?;
let filtered = self.filter.apply(contact)?;
Some(((*i, *j), filtered))
})
.collect()
}
}
#[allow(dead_code)]
#[derive(Debug, Clone)]
pub struct PointQueryResult {
pub is_inside: bool,
pub closest_surface_point: [f64; 3],
pub signed_distance: f64,
pub normal: [f64; 3],
}
#[allow(dead_code)]
#[derive(Debug, Clone)]
pub struct TriangleMesh {
pub triangles: Vec<[f64; 3]>,
}
impl TriangleMesh {
#[allow(dead_code)]
pub fn new(triangles: Vec<[f64; 3]>) -> Self {
debug_assert!(
triangles.len().is_multiple_of(3),
"triangle count must be a multiple of 3"
);
TriangleMesh { triangles }
}
#[allow(dead_code)]
pub fn tri_count(&self) -> usize {
self.triangles.len() / 3
}
#[allow(dead_code)]
pub fn triangle(&self, i: usize) -> [[f64; 3]; 3] {
let base = i * 3;
[
self.triangles[base],
self.triangles[base + 1],
self.triangles[base + 2],
]
}
#[allow(dead_code)]
pub fn face_normal(&self, i: usize) -> [f64; 3] {
let [v0, v1, v2] = self.triangle(i);
let e0 = sub3(v1, v0);
let e1 = sub3(v2, v0);
cross3(e0, e1)
}
}
#[allow(dead_code)]
#[derive(Debug, Clone)]
pub enum ShapeKind {
Sphere {
center: [f64; 3],
radius: f64,
},
Box {
center: [f64; 3],
half_extents: [f64; 3],
},
Capsule {
p0: [f64; 3],
p1: [f64; 3],
radius: f64,
},
Plane {
normal: [f64; 3],
offset: f64,
},
Convex {
vertices: Vec<[f64; 3]>,
},
}
impl ShapeKind {
#[allow(dead_code)]
pub fn aabb(&self) -> ([f64; 3], [f64; 3]) {
match self {
ShapeKind::Sphere { center, radius } => (
[center[0] - radius, center[1] - radius, center[2] - radius],
[center[0] + radius, center[1] + radius, center[2] + radius],
),
ShapeKind::Box {
center,
half_extents,
} => (sub3(*center, *half_extents), add3(*center, *half_extents)),
ShapeKind::Capsule { p0, p1, radius } => {
let mn = [
p0[0].min(p1[0]) - radius,
p0[1].min(p1[1]) - radius,
p0[2].min(p1[2]) - radius,
];
let mx = [
p0[0].max(p1[0]) + radius,
p0[1].max(p1[1]) + radius,
p0[2].max(p1[2]) + radius,
];
(mn, mx)
}
ShapeKind::Plane { normal, offset } => {
let n = *normal;
let d = *offset;
let _ = (n, d);
([-1e15; 3], [1e15; 3])
}
ShapeKind::Convex { vertices } => {
let mut mn = [f64::INFINITY; 3];
let mut mx = [f64::NEG_INFINITY; 3];
for v in vertices {
for i in 0..3 {
mn[i] = mn[i].min(v[i]);
mx[i] = mx[i].max(v[i]);
}
}
(mn, mx)
}
}
}
#[allow(dead_code)]
pub fn bounding_radius(&self) -> f64 {
match self {
ShapeKind::Sphere { radius, .. } => *radius,
ShapeKind::Box { half_extents, .. } => len3(*half_extents),
ShapeKind::Capsule { p0, p1, radius } => len3(sub3(*p1, *p0)) * 0.5 + radius,
ShapeKind::Plane { .. } => f64::INFINITY,
ShapeKind::Convex { vertices } => {
vertices.iter().map(|v| len3(*v)).fold(0.0_f64, f64::max)
}
}
}
#[allow(dead_code)]
pub fn support(&self, dir: [f64; 3]) -> [f64; 3] {
match self {
ShapeKind::Sphere { center, radius } => {
let d = normalize3(dir);
add3(*center, scale3(d, *radius))
}
ShapeKind::Box {
center,
half_extents,
} => [
center[0] + half_extents[0] * dir[0].signum(),
center[1] + half_extents[1] * dir[1].signum(),
center[2] + half_extents[2] * dir[2].signum(),
],
ShapeKind::Capsule { p0, p1, radius } => {
let d0 = dot3(*p0, dir);
let d1 = dot3(*p1, dir);
let base = if d0 > d1 { *p0 } else { *p1 };
add3(base, scale3(normalize3(dir), *radius))
}
ShapeKind::Plane { normal, offset } => scale3(*normal, *offset + 1e9),
ShapeKind::Convex { vertices } => vertices
.iter()
.max_by(|a, b| {
dot3(**a, dir)
.partial_cmp(&dot3(**b, dir))
.unwrap_or(std::cmp::Ordering::Equal)
})
.copied()
.unwrap_or([0.0; 3]),
}
}
}
#[allow(dead_code)]
#[derive(Debug, Clone)]
pub struct SegmentCastResult {
pub hit: bool,
pub t: f64,
pub hit_point: [f64; 3],
pub normal: [f64; 3],
}