use i_bound::IBound;
use i_shape::{IShape, ShapeType};
use i_vicinity::IVicinity;
use bound::AxisAlignedBBox;
use mat::Mat3x1;
use plane::Plane;
use ray::Ray3;
#[derive(Debug, Clone, Default)]
pub struct TriPrism {
pub _tri_base: [Mat3x1<f64>; 3],
pub _tri_base2: [Mat3x1<f64>; 3],
pub _normal_height: Mat3x1<f64>,
pub _bound: AxisAlignedBBox,
pub _vicinity: f64,
}
impl TriPrism {
pub fn init(tri_base: &[f64], height: f64) -> TriPrism {
assert!(tri_base.len() == 9);
let v0 = Mat3x1 {
_val: [tri_base[0], tri_base[1], tri_base[2]],
};
let v1 = Mat3x1 {
_val: [tri_base[3], tri_base[4], tri_base[5]],
};
let v2 = Mat3x1 {
_val: [tri_base[6], tri_base[7], tri_base[8]],
};
let d1 = v1.minus(&v0).unwrap();
let d2 = v2.minus(&v0).unwrap();
let normal = d1.cross(&d2).unwrap().normalize().unwrap();
let h_offset = normal.scale(height).unwrap();
let v00 = v0.plus(&h_offset).unwrap();
let v11 = v1.plus(&h_offset).unwrap();
let v22 = v2.plus(&h_offset).unwrap();
let base = [v0, v1, v2];
let base2 = [v00, v11, v22];
use std::cmp::Ordering::*;
let xs = [
base[0][0],
base[1][0],
base[2][0],
base2[0][0],
base2[1][0],
base2[2][0],
];
let ys = [
base[0][1],
base[1][1],
base[2][1],
base2[0][1],
base2[1][1],
base2[2][1],
];
let zs = [
base[0][2],
base[1][2],
base[2][2],
base2[0][2],
base2[1][2],
base2[2][2],
];
let x_min = *xs
.iter()
.min_by(|a, b| a.partial_cmp(b).unwrap_or(Equal))
.unwrap();
let x_max = *xs
.iter()
.max_by(|a, b| a.partial_cmp(b).unwrap_or(Equal))
.unwrap();
let y_min = *ys
.iter()
.min_by(|a, b| a.partial_cmp(b).unwrap_or(Equal))
.unwrap();
let y_max = *ys
.iter()
.max_by(|a, b| a.partial_cmp(b).unwrap_or(Equal))
.unwrap();
let z_min = *zs
.iter()
.min_by(|a, b| a.partial_cmp(b).unwrap_or(Equal))
.unwrap();
let z_max = *zs
.iter()
.max_by(|a, b| a.partial_cmp(b).unwrap_or(Equal))
.unwrap();
TriPrism {
_tri_base: base,
_tri_base2: base2,
_normal_height: h_offset,
_bound: AxisAlignedBBox::init(
ShapeType::Rect,
&[x_min, y_min, z_min, x_max, y_max, z_max],
),
_vicinity: 0.000001f64,
}
}
}
impl IShape for TriPrism {
fn get_shape_data(&self) -> Vec<f64> {
vec![
self._tri_base[0][0],
self._tri_base[0][1],
self._tri_base[0][2],
self._tri_base[1][0],
self._tri_base[1][1],
self._tri_base[1][2],
self._tri_base[2][0],
self._tri_base[2][1],
self._tri_base[2][2],
self._normal_height[0],
self._normal_height[1],
self._normal_height[2],
]
}
fn get_type(&self) -> ShapeType {
ShapeType::TriPrism
}
fn get_bound(&self) -> &dyn IBound {
&self._bound
}
fn get_intersect(&self, other: &dyn IShape) -> (bool, Option<Mat3x1<f64>>) {
if !self.get_bound().intersect(other.get_bound()) {
return (false, None);
} else {
match other.get_type() {
ShapeType::Point => {
let other_shape_data = other.get_shape_data();
let other_point = Mat3x1 {
_val: [
other_shape_data[0],
other_shape_data[1],
other_shape_data[2],
],
};
let n = self._normal_height;
let tests = vec![
(self._tri_base[0], n.scale(-1.).unwrap()),
(self._tri_base2[0], n),
(
self._tri_base[0],
self._tri_base[1]
.minus(&self._tri_base[0])
.unwrap()
.cross(&n)
.unwrap(),
),
(
self._tri_base[1],
self._tri_base[2]
.minus(&self._tri_base[1])
.unwrap()
.cross(&n)
.unwrap(),
),
(
self._tri_base[2],
self._tri_base[0]
.minus(&self._tri_base[2])
.unwrap()
.cross(&n)
.unwrap(),
),
];
let is_inside = tests.iter().all(|(vert, normal)| {
!(other_point.minus(vert).unwrap().dot(normal).unwrap() > 0.)
});
if is_inside {
(true, Some(other_point))
} else {
(false, None)
}
}
ShapeType::Line => {
let other_shape_data = other.get_shape_data();
let a = Mat3x1 {
_val: [
other_shape_data[0],
other_shape_data[1],
other_shape_data[2],
],
};
let b = Mat3x1 {
_val: [
other_shape_data[3],
other_shape_data[4],
other_shape_data[5],
],
};
let n = self._normal_height;
let tests = vec![
(self._tri_base[0], n.scale(-1.).unwrap()),
(self._tri_base2[0], n),
(
self._tri_base[0],
self._tri_base[1]
.minus(&self._tri_base[0])
.unwrap()
.cross(&n)
.unwrap(),
),
(
self._tri_base[1],
self._tri_base[2]
.minus(&self._tri_base[1])
.unwrap()
.cross(&n)
.unwrap(),
),
(
self._tri_base[2],
self._tri_base[0]
.minus(&self._tri_base[2])
.unwrap()
.cross(&n)
.unwrap(),
),
];
let a_is_inside = tests
.iter()
.all(|(vert, normal)| !(a.minus(vert).unwrap().dot(normal).unwrap() > 0.));
let b_is_inside = tests
.iter()
.all(|(vert, normal)| !(b.minus(vert).unwrap().dot(normal).unwrap() > 0.));
if a_is_inside {
return (true, Some(a));
} else if b_is_inside {
return (true, Some(b));
}
let v = b.minus(&a).unwrap();
let mag = v.magnitude().unwrap();
let r = Ray3::init(&[a[0], a[1], a[2]], &[v[0], v[1], v[2]]);
let n1 = self._normal_height;
let n0 = n1.scale(-1.).unwrap();
let n2 = self._tri_base[1]
.minus(&self._tri_base[0])
.unwrap()
.cross(&n1)
.unwrap();
let n3 = self._tri_base[2]
.minus(&self._tri_base[1])
.unwrap()
.cross(&n1)
.unwrap();
let n4 = self._tri_base[0]
.minus(&self._tri_base[2])
.unwrap()
.cross(&n1)
.unwrap();
let facets = vec![
Plane::init(
&[
self._tri_base[0][0] as f64,
self._tri_base[0][1] as f64,
self._tri_base[0][2] as f64,
],
&[n0[0] as f64, n0[1] as f64, n0[2] as f64],
),
Plane::init(
&[
self._tri_base2[0][0] as f64,
self._tri_base2[0][1] as f64,
self._tri_base2[0][2] as f64,
],
&[n1[0] as f64, n1[1] as f64, n1[2] as f64],
),
Plane::init(
&[
self._tri_base[0][0] as f64,
self._tri_base[0][1] as f64,
self._tri_base[0][2] as f64,
],
&[n2[0] as f64, n2[1] as f64, n2[2] as f64],
),
Plane::init(
&[
self._tri_base[1][0] as f64,
self._tri_base[1][1] as f64,
self._tri_base[1][2] as f64,
],
&[n3[0] as f64, n3[1] as f64, n3[2] as f64],
),
Plane::init(
&[
self._tri_base[2][0] as f64,
self._tri_base[2][1] as f64,
self._tri_base[2][2] as f64,
],
&[n4[0] as f64, n4[1] as f64, n4[2] as f64],
),
];
let mut intersect_point = None;
let mut is_inside = false;
for i in facets.iter() {
let res = r.get_intersect(i);
if res.0 {
let collide_point = res.1.unwrap();
let mag2 = collide_point.minus(&a).unwrap().magnitude().unwrap();
let is_point_inside = tests.iter().all(|(vert, normal)| {
!(collide_point.minus(vert).unwrap().dot(normal).unwrap() > 0.)
});
if !is_point_inside || mag2 > mag {
continue;
} else {
is_inside = true;
intersect_point = res.1;
break;
}
}
}
if is_inside {
(true, intersect_point)
} else {
(false, None)
}
}
_ => {
unimplemented!();
}
}
}
}
fn get_support(&self, v: &Mat3x1<f64>) -> Option<Mat3x1<f64>> {
if v.magnitude() != Some(0f64) {
let points = [
self._tri_base[0],
self._tri_base[1],
self._tri_base[2],
self._tri_base2[0],
self._tri_base2[1],
self._tri_base2[2],
];
let furthest = points
.iter()
.map(|x| x.dot(v).unwrap())
.enumerate()
.max_by(|a, b| a.1.partial_cmp(&b.1).unwrap())
.unwrap();
let o = points[furthest.0].clone();
Some(o)
} else {
None
}
}
}
impl IVicinity<f64> for TriPrism {
fn set_vicinity(&mut self, epsilon: f64) {
self._vicinity = epsilon.abs();
}
fn within_vicinity(&self, a: f64, b: f64) -> bool {
if a + self._vicinity >= b && a - self._vicinity <= b {
true
} else {
false
}
}
}