109 lines
3.0 KiB
Rust
109 lines
3.0 KiB
Rust
use glam::{Mat3, Vec3, Vec4};
|
|
|
|
use crate::math;
|
|
|
|
//? Should this be normalised or does that risk introducing fpp errors
|
|
#[derive(Clone, Copy, Debug)]
|
|
pub struct Plane {
|
|
pub normal: Vec3,
|
|
pub offset: f32,
|
|
}
|
|
|
|
impl Plane {
|
|
pub fn from_point_normal(point: Vec3, normal: Vec3) -> Self {
|
|
Self {
|
|
normal,
|
|
offset: point.dot(normal),
|
|
}
|
|
}
|
|
|
|
pub fn into_vec4(&self) -> Vec4 {
|
|
glam::vec4(self.normal.x, self.normal.y, self.normal.z, -self.offset)
|
|
}
|
|
}
|
|
|
|
impl PartialEq for Plane {
|
|
fn eq(&self, other: &Self) -> bool {
|
|
(self.normal.x - other.normal.x).abs() < math::EPSILON
|
|
&& (self.normal.y - other.normal.y).abs() < math::EPSILON
|
|
&& (self.normal.z - other.normal.z).abs() < math::EPSILON
|
|
&& (self.offset - other.offset).abs() < math::EPSILON
|
|
}
|
|
}
|
|
|
|
impl Eq for Plane {}
|
|
|
|
#[derive(Clone, Copy, Debug)]
|
|
pub struct PlaneIntersection {
|
|
pub planes: [Plane; 3],
|
|
}
|
|
|
|
impl PlaneIntersection {
|
|
pub fn from_planes(p1: Plane, p2: Plane, p3: Plane) -> Option<Self> {
|
|
let plane = Self {
|
|
planes: [p1, p2, p3],
|
|
};
|
|
|
|
let determinant = plane.get_matrix().determinant();
|
|
// println!("Determinant: {determinant}");
|
|
if determinant.abs() > math::EPSILON {
|
|
Some(plane)
|
|
} else {
|
|
None
|
|
}
|
|
}
|
|
|
|
pub fn get_intersection_point(&self) -> Vec3 {
|
|
let base_matrix = self.get_matrix();
|
|
let base_det = base_matrix.determinant();
|
|
let offsets = glam::vec3(
|
|
self.planes[0].offset,
|
|
self.planes[1].offset,
|
|
self.planes[2].offset,
|
|
);
|
|
|
|
let mut vs = vec![];
|
|
for idx in 0..3 {
|
|
let mut matrix = base_matrix;
|
|
*(matrix.col_mut(idx)) = offsets;
|
|
vs.push(matrix.determinant() / base_det);
|
|
}
|
|
|
|
Vec3::from_slice(&vs)
|
|
}
|
|
|
|
pub fn in_halfspace_mat(&self, plane: &Plane) -> bool {
|
|
// "Distance" and side of the point defined by plane against the plane defined
|
|
// by the 3 points of the intersection planes.
|
|
//* Note that the sign is dependant on the winding order of the intersection planes
|
|
let m1 = glam::mat4(
|
|
self.planes[0].into_vec4(),
|
|
self.planes[1].into_vec4(),
|
|
self.planes[2].into_vec4(),
|
|
plane.into_vec4(),
|
|
);
|
|
let d1 = m1.determinant();
|
|
|
|
// We can resolve the winding order problem by multiplying by the normal matrix determinant
|
|
let m2 = self.get_matrix().transpose();
|
|
let d2 = m2.determinant();
|
|
let dist = d1 * d2;
|
|
|
|
//* Note: dist is only euclidean accurate if all the normals are normalised :)
|
|
dist < math::EPSILON
|
|
}
|
|
|
|
pub fn in_halfspace(&self, plane: &Plane) -> bool {
|
|
plane.normal.dot(self.get_intersection_point()) - plane.offset <= math::EPSILON
|
|
}
|
|
|
|
fn get_matrix(&self) -> Mat3 {
|
|
glam::mat3(
|
|
self.planes[0].normal,
|
|
self.planes[1].normal,
|
|
self.planes[2].normal,
|
|
)
|
|
.transpose()
|
|
}
|
|
}
|